1. 启动
共启动3个节点
mobile_manipulator_mpc_node //mpc问题构建，计算
mobile_manipulator_dummy_mrt_node //仿真，承接MPC的输出，发布Observation, 对于仿真来讲，状态发布也是反馈
mobile_manipulator_target //交互发布target
2. MobileManipulatorMpcNode.cpp
MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); //问题构建
auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, interface.getReferenceManagerPtr()); //目标点接收在这里
ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(),
interface.getOptimalControlProblem(), interface.getInitializer()); //DDP
MPC_ROS_Interface mpcNode(nodeHandle,mpc, robotName); //MPC线程
3. MobileManipulatorInterface.cpp
MobileManipulatorInterface::MobileManipulatorInterface
{
//一些初始化
//pinocchio接口，这里注意一下，createPinocchioInterface会把底盘的x,y,theta，这三个自由度作为一个复合joint加入到pinocchio模型中
pinocchioInterfacePtr_.reset(new PinocchioInterface(createPinocchioInterface(urdfFile, modelType, removeJointNames)));
manipulatorModelInfo_ = mobile_manipulator::createManipulatorModelInfo(*pinocchioInterfacePtr_, modelType, baseFrame, eeFrame);
ddpSettings_ = ddp::loadSettings(taskFile, "ddp"); //从文件里读取配置参数
mpcSettings_ = mpc::loadSettings(taskFile, "mpc");
referenceManagerPtr_.reset(new ReferenceManager);
//问题构建:
//二次输入型cost
problem_.costPtr->add("inputCost", getQuadraticInputCost(taskFile));
//关节限位约束
problem_.softConstraintPtr->add("jointLimits", getJointLimitSoftConstraint(*pinocchioInterfacePtr_, taskFile));
//末端约束
problem_.stateSoftConstraintPtr->add("endEffector", getEndEffectorConstraint(*pinocchioInterfacePtr_, taskFile, "endEffector",
usePreComputation, libraryFolder, recompileLibraries));
//最终末端约束
problem_.finalSoftConstraintPtr->add("finalEndEffector", getEndEffectorConstraint(*pinocchioInterfacePtr_, taskFile, "finalEndEffector",
usePreComputation, libraryFolder, recompileLibraries));
//自碰撞约束
problem_.stateSoftConstraintPtr->add(
"selfCollision", getSelfCollisionConstraint(*pinocchioInterfacePtr_, taskFile, urdfFile, "selfCollision", usePreComputation,
libraryFolder, recompileLibraries));.
//运动学
problem_.dynamicsPtr.reset(
new WheelBasedMobileManipulatorDynamics(manipulatorModelInfo_, "dynamics", libraryFolder, recompileLibraries, true));
//预计算
problem_.preComputationPtr.reset(new MobileManipulatorPreComputation(*pinocchioInterfacePtr_, manipulatorModelInfo_));
//rollout
rolloutPtr_.reset(new TimeTriggeredRollout(*problem_.dynamicsPtr, rolloutSettings));
initializerPtr_.reset(new DefaultInitializer(manipulatorModelInfo_.inputDim));
}
末端约束
std::unique_ptr<StateCost> MobileManipulatorInterface::getEndEffectorConstraint(const PinocchioInterface& pinocchioInterface,
const std::string& taskFile, const std::string& prefix,
bool usePreComputation, const std::string& libraryFolder,
bool recompileLibraries)
{
if (usePreComputation)
{
//MobileManipulatorPinocchioMapping和MobileManipulatorPinocchioMappingCppAd都是MobileManipulatorPinocchioMappingTpl，类型不一样而已
//MobileManipulatorPinocchioMappingTpl继承PinocchioStateInputMapping，主要功能是定制获取关节位置，速度，雅可比
MobileManipulatorPinocchioMapping pinocchioMapping(manipulatorModelInfo_);
//主要功能是获取末端位置，速度，角度误差等接口
PinocchioEndEffectorKinematics eeKinematics(pinocchioInterface, pinocchioMapping, {manipulatorModelInfo_.eeFrame});
constraint.reset(new EndEffectorConstraint(eeKinematics, *referenceManagerPtr_));
}
else
{
MobileManipulatorPinocchioMappingCppAd pinocchioMappingCppAd(manipulatorModelInfo_);
PinocchioEndEffectorKinematicsCppAd eeKinematics(pinocchioInterface, pinocchioMappingCppAd, {manipulatorModelInfo_.eeFrame},
manipulatorModelInfo_.stateDim, manipulatorModelInfo_.inputDim,
"end_effector_kinematics", libraryFolder, recompileLibraries, false);
constraint.reset(new EndEffectorConstraint(eeKinematics, *referenceManagerPtr_));
}
std::vector<std::unique_ptr<PenaltyBase>> penaltyArray(6);
std::generate_n(penaltyArray.begin(), 3, [&] { return std::make_unique<QuadraticPenalty>(muPosition); });
std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::make_unique<QuadraticPenalty>(muOrientation); });
return std::make_unique<StateSoftConstraint>(std::move(constraint), std::move(penaltyArray));
}
4. MPC_ROS_interface.cpp
//构造函数主要创建了观测的订阅和mpc结果的发布，以及mpc的reset
MPC_ROS_Interface()
{
// Observation subscriber
mpcObservationSubscriber_ = nodeHandle_->create_subscription<ocs2_msgs::msg::MpcObservation>(topicPrefix_ + "_mpc_observation", 1, std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, std::placeholders::_1));
// MPC publisher
mpcPolicyPublisher_ = nodeHandle_->create_publisher<ocs2_msgs::msg::MpcFlattenedController>(topicPrefix_ + "_mpc_policy", 1);
// MPC reset service server
mpcResetServiceServer_ = nodeHandle_->create_service<ocs2_msgs::srv::Reset>(topicPrefix_ + "_mpc_reset", std::bind(&MPC_ROS_Interface::resetMpcCallback, this, std::placeholders::_1,std::placeholders::_2));
// start thread for publishing
#ifdef PUBLISH_THREAD
publisherWorker_ = std::thread(&MPC_ROS_Interface::publisherWorker, this);
#endif
}
void MPC_ROS_Interface::mpcObservationCallback(const std::shared_ptr<ocs2_msgs::msg::MpcObservation> msg)
{
//启动MPC
bool controllerIsUpdated = mpc_.run(currentObservation.time, currentObservation.state);
copyToBuffer(currentObservation);
}
ocs2_msgs::msg::MpcFlattenedController MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution,
const CommandData& commandData,
const PerformanceIndex& performanceIndices)
{
ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg; //自定义topic类型，发布mpc相关计算
uint8 controller_type # what type of controller is this
MpcObservation init_observation # plan initial observation
MpcTargetTrajectories plan_target_trajectories # target trajectory in cost function
MpcState[] state_trajectory # optimized state trajectory from planner
MpcInput[] input_trajectory # optimized input trajectory from planner
float64[] time_trajectory # time trajectory for stateTrajectory and inputTrajectory
uint16[] post_event_indices # array of indices indicating the index of post-event time in the trajectories
ModeSchedule mode_schedule # optimal/predefined MPC mode sequence and event times
ControllerData[] data # the actual payload from flatten method: one vector of data per time step
MpcPerformanceIndices performance_indices # solver performance indices
//填充数据
mpcPolicyMsg.time_trajectory = primalSolution.timeTrajectory_;
mpcPolicyMsg.post_event_indices = primalSolution.postEventIndices_;
mpcState.value = primalSolution.stateTrajectory_;
mpcInput.value[j] = primalSolution.inputTrajectory_
mpcPolicyMsg.data.emplace_back(ocs2_msgs::msg::ControllerData());
primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, policyMsgDataPointers);
}
5. GaussNewtonDDP.cpp
void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime)
{
// set cost desired trajectories
for (auto& ocp : optimalControlProblemStock_) {
ocp.targetTrajectoriesPtr = &this->getReferenceManager().getTargetTrajectories();
}
initializeConstraintPenalties(); // initialize penalty coefficients
bool initialSolutionExists = initializePrimalSolution(); // true if the rollout is not purely from the Initializer
initializeDualSolutionAndMetrics();
// DDP main loop
while (true)
{
//constructs the LQ problem around the nominal trajectories
approximateOptimalControlProblem();
//solves the LQ problem
avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost);
//calculate controller and store the result in unoptimizedController_
calculateController();
//based on the current LQ solution updates the optimized primal and dual solutions
//更新原始解和对偶解，运行搜索策略来找到最佳步长，搜索成功后更新对偶解
takePrimalDualStep(lqModelExpectedCost);
// check convergence
std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence(
!initialSolutionExists, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back());
//条件完成
if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_)
{
break;
}
else
{
// update the constraint penalty coefficients
updateConstraintPenalties(performanceIndex_.equalityConstraintsSSE);
}
}
}
approximateOptimalControlProblem()
void GaussNewtonDDP::approximateOptimalControlProblem()
{
//对最优化问题进行二次近似，dynamics，cost，Equality constraints，Lagrangians
approximateIntermediateLQ(nominalDualData_.dualSolution, nominalPrimalData_);
//对过程cost进行二次近似
while ((timeIndex = nextTimeIndex_++) < NE)
{
ocs2::approximatePreJumpLQ(optimalControlProblemStock_[taskId], time, state, multiplier, modelData);
}
//对终端cost进行近似
modelData = ocs2::approximateFinalLQ(optimalControlProblemStock_[0], time, state, multiplier);
}
solveSequentialRiccatiEquations()
scalar_t SLQ::solveSequentialRiccatiEquations(const ScalarFunctionQuadraticApproximation& finalValueFunction)
{
const size_t N = nominalPrimalData_.primalSolution.timeTrajectory_.size();
nominalDualData_.riccatiModificationTrajectory.resize(N);
nominalDualData_.projectedModelDataTrajectory.resize(N);
if (N > 0) {
// perform the computeRiccatiModificationTerms for partition i
nextTimeIndex_ = 0;
nextTaskId_ = 0;
auto task = [this, N]() {
int timeIndex;
const matrix_t SmDummy = matrix_t::Zero(0, 0);
// get next time index is atomic
while ((timeIndex = nextTimeIndex_++) < N) {
computeProjectionAndRiccatiModification(nominalPrimalData_.modelDataTrajectory[timeIndex], SmDummy,
nominalDualData_.projectedModelDataTrajectory[timeIndex],
nominalDualData_.riccatiModificationTrajectory[timeIndex]);
}
};
runParallel(task, settings().nThreads_);
}
return solveSequentialRiccatiEquationsImpl(finalValueFunction);
}
computeProjectionAndRiccatiModification()
void GaussNewtonDDP::computeProjectionAndRiccatiModification(const ModelData& modelData, const matrix_t& Sm, ModelData& projectedModelData,riccati_modification::Data& riccatiModification) const
{
// compute the Hamiltonian's Hessian
riccatiModification.time_ = modelData.time;
riccatiModification.hamiltonianHessian_ = computeHamiltonianHessian(modelData, Sm);
//计算约束投影
computeProjections(riccatiModification.hamiltonianHessian_, modelData.stateInputEqConstraint.dfdu,
riccatiModification.constraintRangeProjector_, riccatiModification.constraintNullProjector_);
// 通过投影来转换LQ模型数据，考虑状态输入等式约束
projectLQ(modelData, riccatiModification.constraintRangeProjector_, riccatiModification.constraintNullProjector_, projectedModelData);
// compute deltaQm, deltaGv, deltaGm
searchStrategyPtr_->computeRiccatiModification(projectedModelData, riccatiModification.deltaQm_, riccatiModification.deltaGv_,riccatiModification.deltaGm_);
}