这个文件主要是对最优问题的构造。
1. setupOptimalConrolProblem
void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile, const std::string& urdfFile,const std::string& referenceFile, bool verbose)
{//pinocchio接口pinocchioInterfacePtr_.reset(new PinocchioInterface(centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames)));//质心动力学centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo(*pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile),centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), modelSettings_.contactNames3DoF,modelSettings_.contactNames6DoF); // 足迹规划auto swingTrajectoryPlanner =std::make_unique<SwingTrajectoryPlanner>(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);// 参考器初始化referenceManagerPtr_ =std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose), std::move(swingTrajectoryPlanner));// 最优化问题构造：problemPtr_.reset(new OptimalControlProblem);//动力学 problemPtr_->dynamicsPtr = new LeggedRobotDynamicsAD(*pinocchioInterfacePtr_, centroidalModelInfo_, modelName, modelSettings_);//costproblemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, false));std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr;eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, {footName},centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,velocityUpdateCallback, footName, modelSettings_.modelFolderCppAd,modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));//约束problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient));problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i));problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));problemPtr_->equalityConstraintPtr->add(footName + "_normalVelocity",getNormalVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));//预计算problemPtr_->preComputationPtr.reset(new LeggedRobotPreComputation(*pinocchioInterfacePtr_, centroidalModelInfo_,*referenceManagerPtr_->getSwingTrajectoryPlanner(), modelSettings_));//rolloutrolloutPtr_.reset(new TimeTriggeredRollout(*problemPtr_->dynamicsPtr, rolloutSettings_));//initializationinitializerPtr_.reset(new LeggedRobotInitializer(centroidalModelInfo_, *referenceManagerPtr_, extendNormalizedMomentum));
}
2. 最优问题构建
getBaseTrackingCost()
{
return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info, *referenceManagerPtr_);
}
getFrictionConeConstraint()
{FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex,centroidalModelInfo_);
}
getFrictionConeSoftConstraint()
{
return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient),std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
}
getZeroForceConstraint()
{
return std::make_unique<ZeroForceConstraint>(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_);
}
