OCS2_mobile_manipulator案例详解

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(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 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> penaltyArray(6);
  std::generate_n(penaltyArray.begin(), 3, [&] { return std::make_unique(muPosition); });
  std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::make_unique(muOrientation); });

  return std::make_unique(std::move(constraint), std::move(penaltyArray));
}

4. MPC_ROS_interface.cpp

//构造函数主要创建了观测的订阅和mpc结果的发布,以及mpc的reset
MPC_ROS_Interface()
{
  // Observation subscriber
  mpcObservationSubscriber_ = nodeHandle_->create_subscription(topicPrefix_ + "_mpc_observation", 1, std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, std::placeholders::_1));

  // MPC publisher
  mpcPolicyPublisher_ = nodeHandle_->create_publisher(topicPrefix_ + "_mpc_policy", 1);

  // MPC reset service server
  mpcResetServiceServer_ = nodeHandle_->create_service(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 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_);
}

你可能感兴趣的:(OCS2_mobile_manipulator案例详解)