当前位置：首页 > backend >正文

legged_control足式机器人控制框架及代码解析（三）：legged_interface NMPC接口

backend 2025/6/17 19:43:47

欢迎交流私信～某乎ID：OpenRobotSL
在这里插入图片描述

EndEffectorLinearConstraint.h/cpp

一个用于腿式机器人末端执行器位置和线速度的线性约束最终类，继承自StateInputConstraint，约束形式为：

g(xee, vee) = Ax * xee + Av * vee + b

For defining constraint of type g(xee), set Av to matrix_t(0, 0)
For defining constraint of type g(vee), set Ax to matrix_t(0, 0)

同时提供getLinearApproximation 方法，求得约束函数在当前状态和输入附近的线性近似，这对梯度基优化和控制设计非常重要。

FrictionConeConstraint.h/cpp

《四足机器人_于宪元》eq(3.11)摩擦锥约束

/**
• 实现了约束 h(t,x,u) >= 0
• 
• frictionCoefficient * (Fz + gripperForce) - sqrt(Fx * Fx + Fy * Fy + regularization) >= 0
• 
• 夹持力（gripper force）将摩擦锥的原点沿 z 方向向下移动了一个等于夹持力大小的量。
• 这使得在没有对该脚施加常规法向力的情况下，能够产生切向力，或者以最多等于夹持力的大小“拉动”该脚。
• 
• 正则化项（regularization）防止当 Fx = Fz = 0 时约束的梯度/二阶导数趋于无穷大。它还为摩擦锥创建了一个抛物线形的安全边界。例如，当 Fx = Fy = 0 时，约束的零点将位于 Fz = 1/frictionCoefficient * sqrt(regularization) 而不是 Fz = 0。
*/

在这里插入图片描述

在这里插入图片描述
h(F)=frictionCoefficient * (Fz + gripperForce) - sqrt(Fx * Fx + Fy * Fy + regularization)表示摩擦锥函数。

局部力与世界力 local force w.r.t. forces in world frame

头文件中强调了两种力，足端局部坐标系下的local force和forces in world frame，并根据力的不同，根据链式法则计算不同的一阶导与二阶导，由此计算得到摩擦锥函数h(F)相对于系统状态x与系统输入u的线性近似与二次近似。其中F表示足端局部坐标系下的局部力，输入u中的f就是世界坐标系下的足端力，在代码中两者之间的变换关系由t_R_w表示。在这里，默认t_R_w为单位阵，即默认足端局部坐标系与世界坐标系一致，代码中标注了[FrictionConeConstraint] setSurfaceNormalInWorld() is not implemented!。

需要明确的是，摩擦锥函数h(F)是以接触面的法向方向（z轴）作为参考，即Fz是沿接触面的法向方向的分量，Fx和Fy是接触面的切向方向，可以认为该函数始终是基于局部坐标系表达。而在NMPC要优化的系统输入u中的f是基于世界坐标系的，原则上也要经过转换和链式法则才能计算函数值与导数。默认t_R_w为单位阵是为了简化，但不代表以后机器人每个足端坐标系都与世界系一致，例如倾斜地形，足端接触角度变化或者多足机器人使用不同触点法向等，在这些情况下u中的足端接触力f必须投影到局部坐标系下。因此，虽然此处设为单位阵，但整个代码框架设计上始终以“局部力”为准，是为了抽象成通用约束接口，适用于任意足端姿态和地形条件。

/*** @brief   链式法则计算摩擦锥函数相对于输入的一阶导数dCone_du和二阶导数d2Coned_du2* @param   coneLocalDerivatives dCone_dF和d2Cone_dF2* @param   localForceDerivatives dF_du* @return  FrictionConeConstraint::ConeDerivatives */FrictionConeConstraint::ConeDerivatives FrictionConeConstraint::computeConeConstraintDerivatives(const ConeLocalDerivatives &coneLocalDerivatives, const LocalForceDerivatives &localForceDerivatives) const{ConeDerivatives coneDerivatives;// First order derivativesconeDerivatives.dCone_du.noalias() = coneLocalDerivatives.dCone_dF.transpose() * localForceDerivatives.dF_du;// Second order derivativesconeDerivatives.d2Cone_du2.noalias() =localForceDerivatives.dF_du.transpose() * coneLocalDerivatives.d2Cone_dF2 * localForceDerivatives.dF_du;return coneDerivatives;}

该函数是使用链式法则计算摩擦锥函数相对于输入的一阶导数dCone_du和二阶导数d2Coned_du2。推导过程如下：

在这里插入图片描述

摩擦锥函数的一阶线性近似

/*** @brief   计算摩擦锥约束函数的线性近似，f(x,u) = dfdx dx + dfdu du + f* @param   time* @param   state* @param   input* @param   preComp* @return  VectorFunctionLinearApproximation 摩擦锥约束函数f，对状态的导数dfdx和对输入的导数dfdu*/VectorFunctionLinearApproximation FrictionConeConstraint::getLinearApproximation(scalar_t time, const vector_t &state,const vector_t &input,const PreComputation &preComp) const{const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_); // 获取世界坐标系下索引为contactPointIndex_的足端接触点的力 3x1const vector3_t localForce = t_R_w * forcesInWorldFrame; // 转化成localForce 在这里还是=forcesInWorldFrameconst auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame);const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce); // 计算dConedF和d2ConedF2const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives); // 计算dCone_du和d2Cone_du2VectorFunctionLinearApproximation linearApproximation;linearApproximation.f = coneConstraint(localForce);linearApproximation.dfdx = matrix_t::Zero(1, state.size()); // 摩擦锥函数对状态的导数，这里是0，表示状态对力无直接影响linearApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives);return linearApproximation;}

摩擦锥函数的二阶近似

/*** @brief   计算摩擦锥约束函数的二次近似，f[i](x,u) = 1/2 dx' dfdxx[i] dx + du' dfdux[i] dx + 1/2 du' dfduu[i] du + dfdx[i,:] dx + dfdu[i,:] du + f[i]* @param   time* @param   state* @param   input* @param   preComp* @return  VectorFunctionQuadraticApproximation*/VectorFunctionQuadraticApproximation FrictionConeConstraint::getQuadraticApproximation(scalar_t time, const vector_t &state,const vector_t &input,const PreComputation &preComp) const{const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_); // 获取世界坐标系下索引为contactPointIndex_的足端接触点的力 3x1const vector3_t localForce = t_R_w * forcesInWorldFrame;const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame);const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce); // 计算dConedF和d2ConedF2const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives); // 计算dCone_du和d2Cone_du2VectorFunctionQuadraticApproximation quadraticApproximation; // 二阶近似quadraticApproximation.f = coneConstraint(localForce);quadraticApproximation.dfdx = matrix_t::Zero(1, state.size()); // 摩擦锥函数对状态的梯度，这里是0，表示状态对力无直接影响quadraticApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives); // 摩擦锥函数对输入的导数dhdu，对输入的梯度// emplace_back表示直接在vector的末尾添加一个元素，避免了额外的拷贝或移动操作quadraticApproximation.dfdxx.emplace_back(frictionConeSecondDerivativeState(state.size(), coneDerivatives)); // 摩擦锥函数对状态的二阶导数ddhdxdx，对状态x的HessianquadraticApproximation.dfduu.emplace_back(frictionConeSecondDerivativeInput(input.size(), coneDerivatives)); // 对输入u的Hessian，ddhduduquadraticApproximation.dfdux.emplace_back(matrix_t::Zero(input.size(), state.size())); // ddhdudx，状态和输入的混合二阶项，这里是0return quadraticApproximation;}

在非线性优化或数值求解中，常常需要对目标函数或约束函数做二阶近似（Quadratic Approximation），其通用形式可以写成二阶泰勒展开：

在这里插入图片描述
上述公式对应到代码中，对于单个约束f[i]而言，其近似形式在注释中用增量dx和du表示为：

在这里插入图片描述

LeggedSelfCollisionConstraint.h

腿式机器人自碰撞约束

NormalVelocityConstraintCppAd.h/cpp

NormalVelocityConstraintCppAd是一个专门用于对末端执行器法向速度进行约束的类，适用于 CppAd（算法微分）环境下。它将具体计算工作委托给LeggedRobotPreComputation::getEeNormalVelocityConstraintConfigs()，并通过预计算数据更新约束配置。

在腿式机器人控制中，当机器人处于摆动阶段（非支撑相）时，常常需要限制末端执行器的z向速度，以保证落地时的安全和控制精度。该类提供的约束和导数信息可以被优化器使用，实现对机器人运动的严格约束。

核心调用类SwingTrajectoryPlanner中getZvelocityConstraint()和getZpositionConstraint()，以此生成z向位置与速度约束。

ZeroForceConstraint.h/cpp

ZeroForceConstraint类主要用于在非接触状态下强制机器人接触点的受力为零，即摆动相足底反力约束，f=0。

通过查询 SwitchedModelReferenceManager 的接触标志，只有在目标接触点处于非接触状态时才激活该约束。利用 centroidal_model::getContactForces 从输入中提取指定接触点的 3D 力，作为约束函数的值。

ZeroVelocityConstraintCppAd.h/cpp

ZeroVelocityConstraintCppAd类是专门针对末端执行器位置和线速度的零速度约束的自动微分(CppAD)版本，本质上是对EndEffectorLinearConstraint的一个包装，其作用在于：（1）将底层约束计算模块的约束数量固定为3；（2）根据接触状态决定约束是否激活，在接触时激活，要求足端零速，不打滑。

类中重载的isActive、getValue和getLinearApproximation函数均只是简单地调用底层对象的方法，保证接口统一，便于上层算法调用。

SwingTrajectoryPlanner.h/cpp

足底摆动轨迹规划器，用于规划机器人的摆动（swing）轨迹，即在足部离开地面（take-off）到触地（touch-down）期间，产生平滑的高度（Z方向）轨迹。

该模块考虑了不同阶段（摆动和支撑）的高度约束，通过对不同足部的接触状态进行判断，利用预定义的参数配置生成CubicSpline曲线，用于后续控制器或优化器保证足部运动平滑且符合设计要求。

查询接口

通过查找函数在当前足leg的事件时间数组findIndexInTimeArray中确定当前处于哪个阶段index，然后调用对应阶段的CubicSpline曲线的velocity/position函数，返回对应的垂直速度/高度。

  /*** @brief   查询接口，给定时间和足部索引，调用对应阶段的CubicSpline曲线的velocity(time)函数，返回垂直速度* @param   leg 足部索引* @param   time 给定时间* @return  scalar_t 返回垂直速度*/scalar_t SwingTrajectoryPlanner::getZvelocityConstraint(size_t leg, scalar_t time) const{const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time);return feetHeightTrajectories_[leg][index].velocity(time);}/*** @brief   查询接口，给定时间和足部索引，调用对应阶段的CubicSpline曲线的position(time)函数，返回高度位置* @param   leg* @param   time* @return  scalar_t 返回高度位置*/scalar_t SwingTrajectoryPlanner::getZpositionConstraint(size_t leg, scalar_t time) const{const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time);return feetHeightTrajectories_[leg][index].position(time);}

update()系列函数

update(modeSchedule, terrainHeight)

/*** @brief   更新摆动轨迹规划器的内部状态，根据给定的模式序列和地形高度，为每个足部生成摆动和接触阶段的高度序列。* 如果只给定一个统一的地形高度，则为每个足部在所有阶段构造一个高度序列（lift-off和touch-down高度均为terrainHeight），然后调用下一级的update()* @param   modeSchedule 步态模式序列* @param   terrainHeight 地形高度*/void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, scalar_t terrainHeight){// 创建一个大小为modeSequence.size()的高度序列terrainHeightSequence，所有值均为terrainHeight// 用于为每个足部生成相同的lift-off和touch-down高度序列const scalar_array_t terrainHeightSequence(modeSchedule.modeSequence.size(), terrainHeight);feet_array_t<scalar_array_t> liftOffHeightSequence;liftOffHeightSequence.fill(terrainHeightSequence);feet_array_t<scalar_array_t> touchDownHeightSequence;touchDownHeightSequence.fill(terrainHeightSequence);update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence);}

update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence)

 /*** @brief   针对每个足部，遍历所有阶段，对每个阶段的lift-off高度和touch-down高度取最大值，构造出一个maxHeightSequence数组。* 然后调用第三个重载版本，将最大高度作为额外信息传递* @param   modeSchedule 步态模式序列* @param   liftOffHeightSequence lift-off高度序列* @param   touchDownHeightSequence touch-down高度序列*/void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, const feet_array_t<scalar_array_t> &liftOffHeightSequence,const feet_array_t<scalar_array_t> &touchDownHeightSequence){scalar_array_t heightSequence(modeSchedule.modeSequence.size()); // 初始化一个大小为modeSequence.size()的高度序列feet_array_t<scalar_array_t> maxHeightSequence;for (size_t j = 0; j < numFeet_; j++) // 遍历每个足部{for (int p = 0; p < modeSchedule.modeSequence.size(); ++p) // 遍历所有步态阶段{heightSequence[p] = std::max(liftOffHeightSequence[j][p], touchDownHeightSequence[j][p]); // 取lift-off和touch-down高度的最大值}maxHeightSequence[j] = heightSequence;}update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence, maxHeightSequence);}

update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence, maxHeightSequence)

/*** @brief   最终执行的更新函数，根据给定的模式序列和高度序列，为每个足部生成摆动和接触阶段的高度序列。* @param   modeSchedule 步态模式序列* @param   liftOffHeightSequence lift-off高度序列* @param   touchDownHeightSequence touch-down高度序列* @param   maxHeightSequence 最大高度序列*/void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, const feet_array_t<scalar_array_t> &liftOffHeightSequence,const feet_array_t<scalar_array_t> &touchDownHeightSequence,const feet_array_t<scalar_array_t> &maxHeightSequence){const auto &modeSequence = modeSchedule.modeSequence;const auto &eventTimes = modeSchedule.eventTimes;const auto eesContactFlagStocks = extractContactFlags(modeSequence); // 根据模式序列提取每个模式对应的各足部的接触状态feet_array_t<std::vector<int>> startTimesIndices; // 每个足部存储每个阶段的摆动相起始时间索引feet_array_t<std::vector<int>> finalTimesIndices; // 每个足部存储每个阶段的摆动相结束时间索引for (size_t leg = 0; leg < numFeet_; leg++){// 遍历每个足部，求解leg足部每个阶段摆动相的起始时间和结束时间索引std::tie(startTimesIndices[leg], finalTimesIndices[leg]) = updateFootSchedule(eesContactFlagStocks[leg]);}for (size_t j = 0; j < numFeet_; j++) // 遍历每个足部{feetHeightTrajectories_[j].clear();feetHeightTrajectories_[j].reserve(modeSequence.size());for (int p = 0; p < modeSequence.size(); ++p) // 遍历j足部的所有阶段{if (!eesContactFlagStocks[j][p])                                                  // 摆动相{                                                                                 // for a swing legconst int swingStartIndex = startTimesIndices[j][p];                            // 对于j足部的第p个阶段，摆动相的起始时间索引const int swingFinalIndex = finalTimesIndices[j][p];                            // 对于j足部的第p个阶段，摆动相的结束时间索引checkThatIndicesAreValid(j, p, swingStartIndex, swingFinalIndex, modeSequence); // 检查事件时间索引是否有效const scalar_t swingStartTime = eventTimes[swingStartIndex]; // j足p阶段，实际摆动相的起始时间const scalar_t swingFinalTime = eventTimes[swingFinalIndex]; // j足p阶段，实际摆动相的结束时间// 计算缩放因子scaling = min(1, (swingFinalTime - swingStartTime) / swingTimeScale)const scalar_t scaling = swingTrajectoryScaling(swingStartTime, swingFinalTime, config_.swingTimeScale);// 创建两个CubicSpline::Node对象 {时间，位置，速度}// liftOff节点：时间为swingStartTime、高度为对应liftOffHeight、速度为scaling * liftOffVelocity}// touchDown节点：时间为swingFinalTime、高度为对应touchDownHeight、速度为scaling * touchDownVelocityconst CubicSpline::Node liftOff{swingStartTime, liftOffHeightSequence[j][p], scaling * config_.liftOffVelocity};const CubicSpline::Node touchDown{swingFinalTime, touchDownHeightSequence[j][p], scaling * config_.touchDownVelocity};// 计算中间高度 = 地形高度 + scaling * 摆动相高度const scalar_t midHeight = maxHeightSequence[j][p] + scaling * config_.swingHeight;// emplace_back在容器的末尾直接构造对象，避免了额外的拷贝或移动操作// 为j足部的第p个阶段构造一个CubicSpline对象，包含liftOff、midHeight、touchDown三个节点feetHeightTrajectories_[j].emplace_back(liftOff, midHeight, touchDown);}else{ // for a stance leg 支撑相生成一条常数轨迹，高度保持不变// Note: setting the time here arbitrarily to 0.0 -> 1.0 makes the assert in CubicSpline fail 在这里将时间任意设置为0.0 -> 1.0会使CubicSpline中的断言失败const CubicSpline::Node liftOff{0.0, liftOffHeightSequence[j][p], 0.0};const CubicSpline::Node touchDown{1.0, liftOffHeightSequence[j][p], 0.0};feetHeightTrajectories_[j].emplace_back(liftOff, liftOffHeightSequence[j][p], touchDown);}}feetHeightTrajectoriesEvents_[j] = eventTimes; // 将每个足部的事件时间序列保存到feetHeightTrajectoriesEvents_中}}

辅助函数

updateFootSchedule():

对于传入的某足 contactFlagStock（指定j足部的接触状态序列，布尔数组，描述各阶段是否处于接触），遍历所有阶段，对于摆动腿，调用 findIndex() 得到当前swing相的起飞和着地时间索引，并存入startTimeIndexStock和finalTimeIndexStock数组中，表示leg足部每个阶段摆动相的起始时间和结束时间索引。

extractContactFlags():

根据模式序列modeSequence提取每个足部在各个阶段的接触标志（true表示支撑，相应足部处于接触状态）。

findIndex():

给定足部索引index和足部接触状态序列contactFlagStock，返回{startTimesIndex，finalTimesIndex}，也就是{摆动足的起跳时间索引，摆动足的触地时间索引}。

checkThatIndicesAreValid():

检查给定的起飞和着地时间索引是否在合法范围内（不能小于 0 或超出eventTimes数组长度），否则输出错误信息并抛出异常。

swingTrajectoryScaling():

计算摆动轨迹的缩放因子，用于调整摆动轨迹参数，使得当摆动阶段持续时间较短时，足部的抬升高度和速度能够相应缩小，避免规划出不切实际或过于激进的轨迹。当实际摆动阶段的时间小于预设的时间尺度时，scaling会小于1，从而按比例降低起飞和着地时的速度（如liftOffVelocity、touchDownVelocity）以及摆动高度（swingHeight），这样可以确保在较短的摆动阶段内，不会要求足部完成过快或过高的运动。

LeggedRobotInitializer.h/cpp

定义了LeggedRobotInitializer最终类，继承自Initializer。

该类用于腿式机器人初始化，用于提供优化初始猜测（状态、输入），主要功能是根据当前时间和状态利用参考管理器获取接触状态，然后计算一个用于补偿的输入，同时对状态中的归一化动量部分根据设定决定是否置零。

SwitchedModelReferenceManager.h/cpp

SwitchedModelReferenceManager充当了机器人参考信息的集中管理器，继承自ReferenceManager，负责管理模式调度和目标轨迹的生成。它整合了步态调度（GaitSchedule）与足部摆动轨迹规划（SwingTrajectoryPlanner），并通过modifyReferences更新目标轨迹和模式调度，同时提供 getContactFlags等接口供其他模块查询当前各足部的接触状态。

步态调度器（GaitSchedule）决定了机器人在不同时间段的运动模式。
摆动轨迹规划器（SwingTrajectoryPlanner）则利用这些模式和事件时间生成每个足部在摆动阶段的高度轨迹。
SwitchedModelReferenceManager则将这两个模块整合，并在修改参考信息时，先更新模式调度，再更新足部轨迹，确保整个系统的一致性。

 /*** @brief   根据当前时间区间重新规划模式和足部轨迹，确保生成的目标轨迹和运动模式与实际规划时间段匹配，从而为后续的控制器提供准确的参考信息* @param   initTime 给定的初始时间* @param   finalTime 给定的结束时间* @param   initState 给定的初始状态，未使用* @param   targetTrajectories 目标轨迹，未使用* @param   modeSchedule 模式调度*/void SwitchedModelReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t &initState,TargetTrajectories &targetTrajectories, ModeSchedule &modeSchedule){// 计算时间范围const auto timeHorizon = finalTime - initTime;// 根据给定的时间范围更新步态调度modeSchedule = gaitSchedulePtr_->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon);const scalar_t terrainHeight = 0.0; // 固定地面高度为0swingTrajectoryPtr_->update(modeSchedule, terrainHeight); // 根据步态调度modeSchedule和地面高度terrainHeight更新摆动轨迹规划器}

在该函数中，为什么要获取 (initTime - timeHorizon) 到 (finalTime + timeHorizon) 范围内的模式调度，该范围指的是什么？

当前规划的时间区间为[initTime, finalTime]，那么timeHorizon就是这段区间的长度：timeHorizon = finalTime - initTime。而代码中请求模式调度信息的时间范围为[initTime - timeHorizon, finalTime + timeHorizon]，这表示不仅包含了当前规划区间，还在前后扩展了一个与规划区间长度相同的缓冲区间。那么为什么要这么做，可以从以下几点考虑：

保证上下文信息的完整性。模式调度（如支撑相和摆动相）往往在切换时刻发生。如果只考虑[initTime, finalTime]内的信息，可能无法捕捉到与当前规划区间相邻的切换事件。这些切换事件对生成平滑、连续的轨迹非常关键。例如，摆动轨迹的生成往往需要知道抬脚前后的一些信息；
保证平滑过渡。扩展的时间区间作为缓冲区可以帮助算法在区间边缘也能“看到”前后阶段的状态，从而更平滑地衔接不同阶段的轨迹，避免突变。
保证轨迹规划的鲁棒性。当系统状态或模式切换信息存在不确定性时，额外的时间缓冲区有助于对未来和过去状态进行一定预见，确保整个规划过程更加鲁棒。

LeggedRobotPreComputation.h/cpp

LeggedRobotPreComputation类作为预计算回调，在控制循环中根据当前状态、输入和时间更新机器人运动学与动力学的缓存数据，例如提前计算雅可比、惯量等，同时计算末端执行器（足部）的线性约束配置。这些预计算数据对于后续求解最优控制问题（如成本、约束等）至关重要。继承自PreComputation，这是一个通用接口，允许在规划或控制过程中缓存必要的数据，从而避免重复计算。

/*** @brief   预计算的核心函数，用于更新机器人足端接触点法向速度约束配置，更新Pinocchio模型与运动学数据，为后续控制优化提供准确的动力学信息* @param   request 当前计算请求* @param   t 当前时刻* @param   x 当前状态* @param   u 当前输入*/void LeggedRobotPreComputation::request(RequestSet request, scalar_t t, const vector_t &x, const vector_t &u){// 检查请求中是否包含成本Cost、约束Constraint或软约束SoftConstraint，如果不包含直接返回，避免不必要的计算if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint)){return;}// lambda to set config for normal velocity constraints 定义一个lambda函数，用于设置法向速度约束的配置auto eeNormalVelConConfig = [&](size_t footIndex){EndEffectorLinearConstraint::Config config;// 根据当前时刻t和足端索引footIndex获取足端的法向速度约束，取负值，并存入config.b// 约束公式为：g(xee, vee) = Ax * xee + Av * vee + b >= 0，当实际速度达到目标值时约束满足，故取负值// (vector_t(1) << ...).finished()语法是Eigen的逗号初始化方法，用于构造一个只有一个元素的向量config.b = (vector_t(1) << -swingTrajectoryPlannerPtr_->getZvelocityConstraint(footIndex, t)).finished();// 速度约束系数Av设置为[0, 0, 1]，表示只对z轴方向的速度进行约束，只考虑足端在z轴方向的运动config.Av = (matrix_t(1, 3) << 0.0, 0.0, 1.0).finished();if (!numerics::almost_eq(settings_.positionErrorGain, 0.0)) // 判断位置误差增益是否为0，如果不为0，说明系统希望将位置误差也引入约束{// config.b被定义为标量，故只有索引0// 对config.b(0)再减去一个项，该项等于positionErrorGain乘以当前足部在Z方向上的位置约束config.b(0) -= settings_.positionErrorGain * swingTrajectoryPlannerPtr_->getZpositionConstraint(footIndex, t);// 设置config.Ax为1×3矩阵[0, 0, positionErrorGain]，将位置误差引入约束计算中config.Ax = (matrix_t(1, 3) << 0.0, 0.0, settings_.positionErrorGain).finished();}return config;};if (request.contains(Request::Constraint)) // 如果request中包含约束Constraint{for (size_t i = 0; i < info_.numThreeDofContacts; i++) // 对于每个足端接触点{eeNormalVelConConfigs_[i] = eeNormalVelConConfig(i); // 对于足端i，调用lambda函数，设置法向速度约束配置}}const auto &model = pinocchioInterface_.getModel();auto &data = pinocchioInterface_.getData();vector_t q = mappingPtr_->getPinocchioJointPosition(x); // 从状态向量x中提取广义坐标位置qif (request.contains(Request::Approximation)) // 判断是否需要计算近似信息，例如优化问题中的一阶线性近似和二阶近似{pinocchio::forwardKinematics(model, data, q); // 计算当前广义坐标位置q下的正向运动学pinocchio::updateFramePlacements(model, data); // 更新各个坐标系的位姿pinocchio::updateGlobalPlacements(model, data); // 更新机器人在全局坐标系下的位姿pinocchio::computeJointJacobians(model, data); // 计算每个关节的雅可比矩阵updateCentroidalDynamics(pinocchioInterface_, info_, q); // 根据当前广义坐标q，更新质心动力学信息（质心位置、动量等）vector_t v = mappingPtr_->getPinocchioJointVelocity(x, u); // 从状态x和输入u中提取出广义坐标速度v，用于计算动力学导数// Updates the centroidal momentum derivatives (such as in data.dHdq) for the FullCentroidalDynamics model and the SingleRigidBodyDynamics Model// 根据当前广义坐标位置q和速度v，更新质心动力学导数例如梯度，Hessian等updateCentroidalDynamicsDerivatives(pinocchioInterface_, info_, q, v);}else // 如果不需要近似信息，则只计算正向运动学{pinocchio::forwardKinematics(model, data, q);pinocchio::updateFramePlacements(model, data);}}

其中，auto eeNormalVelConConfig = [&](size_t footIndex){}定义了一个lambda函数，其作用是根据当前时间t和指定的足部索引footIndex计算一个末端执行器法向速度约束的配置config，其类型为EndEffectorLinearConstraint::Config。它利用摆动轨迹规划器swingTrajectoryPlanner提供的当前时刻下足部z方向的速度和位置约束信息，并结合系统设置中的位置误差增益，构造出完整的约束配置。

什么是lambda函数？

Lambda 函数是一种匿名函数，也称为闭包（closure），它可以在需要函数对象的地方直接定义并使用，而无需先定义一个单独的命名函数。在 C++11 及以后的标准中，lambda 函数被广泛支持，具有以下特点：

匿名性：Lambda 函数没有名称，可以直接在代码中定义和使用，通常用于需要短小函数体的场合。
内联定义：可以在需要函数对象的地方立即定义 lambda 函数，这使得代码更简洁、可读性更高。
捕获外部变量：lambda 函数可以捕获其定义时所在作用域中的变量（按值或按引用），从而在函数体中使用这些变量，这种机制使得 lambda 函数具有闭包的特性。
可调用对象：Lambda 函数可以像普通函数一样被调用，还可以传递给算法或存储到函数对象中。

实例：

#include <iostream>
#include <vector>
#include <algorithm>int main() {std::vector<int> numbers = {1, 2, 3, 4, 5};// 定义一个 lambda 函数，按引用捕获所有外部变量auto printNumbers = [&]() {for (int num : numbers) {std::cout << num << " ";}std::cout << std::endl;};// 调用 lambda 函数printNumbers();// 使用 lambda 函数进行排序std::sort(numbers.begin(), numbers.end(), [](int a, int b) {return a > b; // 降序排序});// 重新调用打印函数printNumbers();return 0;
}

在这里插入图片描述
在函数LeggedRobotPreComputation::request()中，摆动腿z向约束（位置，速度，分别通过swingTrajectoryPlannerPtr_->getZvelocityConstraint()和swingTrajectoryPlannerPtr_->getZpositionConstraint()获得）设置到eeNormalVelConConfigs_中。

eeNormalVelConConfigs_在NormalVelocityConstraintCppAd.h/cpp中被调用，由此生成摆动腿z向速度约束，因此在下面最优控制问题配置setupOptimalControlProblem()函数中，只看到如下三个约束。实际上摆动腿z向位置和速度约束包含在了NormalVelocityConstraintCppAd中。

│   ├─ Constraints:
│   │   ├─ Friction cone
│   │   ├─ Zero force / velocity / normal velocity
│   │   └─ Self collision

！！！！！！！！！！NMPC中有一个约束就是摆动腿z向约束（位置、速度），摆动腿轨迹不是规划得到的，而是在NMPC模型中给定期望状态（期望状态由卡尔曼滤波估计得到）后优化得到状态x，经过状态x正解得到足端轨迹，该轨迹满足摆动腿z向约束。！！！！！！！！！！！！！！！！！！

！！！！！！！！！！于宪元论文中调用MPC之前需要进行落足点位置规划（即足底轨迹规划），是因为他论文中构建MPC问题是简化后的带约束QP凸优化问题，只优化输入u，其系统状态方程是根据单刚体动力学模型建立的，简化后预测方程中的系统矩阵Ak与输入矩阵Bk只与期望轨迹，落足点坐标以及当前系统状态有关，因此必须进行落足点位置规划。而legged_control代码构建的是NMPC问题，围绕质心动力学模型构建，同时优化状态轨迹x与系统输入u。

LeggedRobotQuadraticTrackingCost.h

类LeggedRobotStateInputQuadraticCost

类LeggedRobotStateInputQuadraticCost继承自类QuadraticStateInputCost，用于计算中间状态的二次型状态与输入成本，也就是state cost。

 /***@brief   构造函数，成员变量初始化* @param   Q 状态误差的权重矩阵*@param   R 输入误差的权重矩阵* @param   info 质心模型信息，之后用于计算补偿输入等*@param   referenceManager 步态轨迹管理器，用于在运行时获取当前的接触状态或其它参数*/
LeggedRobotStateInputQuadraticCost(matrix_t Q, matrix_t R, CentroidalModelInfo info,const SwitchedModelReferenceManager &referenceManager): QuadraticStateInputCost(std::move(Q), std::move(R)), info_(std::move(info)), referenceManagerPtr_(&referenceManager) {}

构造当前time下的二次型代价函数，为状态的跟踪误差加权平方和与控制输入的加权平方和 $L = 0.5(x-x_n)'Q(x-x_n) + 0.5(u-u_n)'R(u-u_n) + (u-u_n)'P(x-x_n)$ ，在legged_controll四足控制框架下，二次型代价函数只包含这两项，不包含状态-输入耦合项。另外，在类QuadraticStateInputCost中还存在一项状态-输入耦合项，公式如下：

$L = 0.5(x-x_n)'Q(x-x_n) + 0.5(u-u_n)'R(u-u_n) + (u-u_n)'P(x-x_n)$

其中， $0.5(x-x_n)'Q(x-x_n)$ 表示状态误差项，用来惩罚当前状态 $x$ 偏离期望状态 $x_n$ ；权重矩阵 $Q$ 半正定，越大对状态偏差的惩罚越重。

$0.5(u-u_n)'R(u-u_n)$ 表示输入误差项，用来惩罚输入 $u$ 偏离参考输入 $u_n$ ；权重矩阵 $R$ 必须正定，避免输入无限大。

$u-u_n)'P(x-x_n)$ 表示状态-输入耦合项，双线性形式， $P$ 表示耦合权重。这个项不单独惩罚输入或状态，而是惩罚“某种输入-状态组合”。举个例子，假设某个状态偏差往某个方向偏离，就希望输入也要往某种方向偏离，否则就加惩罚，也就是说方向相同会减轻惩罚，方向对立会加重惩罚（因为这个项没有平方，可能正也可能负）。
在这里插入图片描述

  /*** @brief   计算状态和输入的偏差，在动态规划或优化过程中被反复调用，用于计算离目标轨迹的偏离程度* @param   time 当前时间，函数内部会根据时间获取对应的参考状态和接触状态* @param   state 当前系统状态* @param   input 当前控制输入* @param   targetTrajectories 包含预期轨迹信息的对象，通过它可以获得时间对应的目标状态* @return  std::pair<vector_t, vector_t> 返回状态偏差和输入偏差的pair*/std::pair<vector_t, vector_t> getStateInputDeviation(scalar_t time, const vector_t &state, const vector_t &input,const TargetTrajectories &targetTrajectories) const override{const auto contactFlags = referenceManagerPtr_->getContactFlags(time); // 获取当前时间的接触状态const vector_t xNominal = targetTrajectories.getDesiredState(time); // 获取当前时间的目标状态const vector_t uNominal = weightCompensatingInput(info_, contactFlags);return {state - xNominal, input - uNominal};}

该函数用来计算状态 $x$ 和输入 $u$ 的偏差，该偏差用于构造二次型代价函数。其中，函数weightCompensatingInput()如下：

/** Computes an input with zero joint velocity and forces which equally distribute the robot weight between contact feet. */
// 计算一个输入，其中关节速度为零，并将机器人重量在接触足之间均匀分配的力。
inline vector_t weightCompensatingInput(const CentroidalModelInfoTpl<scalar_t>& info, const contact_flag_t& contactFlags) {const auto numStanceLegs = numberOfClosedContacts(contactFlags); // 计算接触足的数量vector_t input = vector_t::Zero(info.inputDim);if (numStanceLegs > 0) {const scalar_t totalWeight = info.robotMass * 9.81; // 机器人总重力const vector3_t forceInInertialFrame(0.0, 0.0, totalWeight / numStanceLegs); // 重力平均分配到每个接触足上for (size_t i = 0; i < contactFlags.size(); i++) {if (contactFlags[i]) {centroidal_model::getContactForces(input, i, info) = forceInInertialFrame;}}  // end of i loop}return input;
}

可以看出，在期望控制输入 $u$ 中，后12个关节速度分量均为0，前12个足端接触力分量根据是否存在接触足来均匀分配机器人重量在接触足之间的力，这样保证每个与地面接触的足承担均等的重力分量，从而在理想情况下实现重力补偿。设置这样的 $u$ 通常基于以下原因：

静平衡参考。当机器人处于平衡或静止状态时，理想情况下关节不应有任何额外的运动（速度为零）。因此在nominal输入中，关节速度设置为0能够确保系统处于静止状态，从而简化控制设计。
重力补偿。为了维持机器人稳定站立，需要通过足端施加与机器人重力大小相等的支撑力。由于不同时间的接触状态可能发生改变（例如部分足离地），系统计算每个接触足应承担的重力份额，并均匀分配。这样，可以保证即使接触状态发生变化，机器人依然可以通过调整足端受力来实现重力补偿和稳定性。
提供最小能量参考输入。关节速度不动，不做功，是最节能的参考。

类LeggedRobotStateQuadraticCost

类LeggedRobotStateQuadraticCost继承自类QuadraticStateCost，只涉及状态的二次型成本，不包含输入部分。用于计算terminal cost，末端成本反映系统在最终时刻离目标状态的偏差。

LeggedInterface.h/cpp

OCS2学习记录（1）—— Tutorial - OCS2 - Farbod Farshidian

腿式机器人的控制接口类LeggedInterface,它集成了机器人动力学、成本函数、约束、自碰撞检查、参考轨迹管理等模块，是一个典型的MPC接口框架的一部分，继承自ocs2::RobotInterface。主要功能有：

加载模型、任务和参考轨迹配置；
构建动力学模型；
添加成本函数和约束；
配置MPC、DDP、SQP和IPM等求解器；
提供OptimalControlProblem接口供求解器使用；
提供Pinocchio、Rollout、ReferenceManager等关键模块指针。

NMPC最优控制问题配置

setupOptimalControlProblem()函数构建顺序：

加载机器人模型；
设置参考轨迹；
构建动力学系统；
添加成本函数；
添加硬/软约束（如摩擦锥、自碰撞）；
配置 rollout 和初始化器。

setupOptimalControlProblem()
│
├─ setupModel()          → Pinocchio + CentroidalModelInfo
├─ setupReferenceManager() → 步态 + 摆动轨迹
├─ Load initialState
│
├─ 创建 OptimalControlProblem
│   ├─ Dynamics (AD)
│   ├─ Cost: Q, R
│   ├─ Constraints:
│   │   ├─ Friction cone
│   │   ├─ Zero force / velocity / normal velocity
│   │   └─ Self collision
│
├─ setupPreComputation()
├─ setup Rollout
└─ setup Initializer

/*** @brief   设置最优控制问题* @param   taskFile 任务文件 task.info* @param   urdfFile* @param   referenceFile* @param   verbose 是否显示详细信息*/void LeggedInterface::setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile,bool verbose){setupModel(taskFile, urdfFile, referenceFile, verbose); // 创建模型// Initial stateinitialState_.setZero(centroidalModelInfo_.stateDim); // 初始化状态向量 24*1 四足x=[h_6x1; q_18*1] h表示归一化质心动量，q表示广义坐标 loadData::loadEigenMatrix(taskFile, "initialState", initialState_); // 从task.info文件中加载初始状态// 创建参考轨迹管理器setupReferenceManager(taskFile, urdfFile, referenceFile, verbose);// Optimal control problemproblemPtr_ = std::make_unique<OptimalControlProblem>(); // 创建一个空的最优控制问题对象，后续往里添加模型、成本函数、约束等// Dynamics 设置动力学模型std::unique_ptr<SystemDynamicsBase> dynamicsPtr;dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(*pinocchioInterfacePtr_, centroidalModelInfo_, "dynamics", modelSettings_);problemPtr_->dynamicsPtr = std::move(dynamicsPtr); // 将动力学模型添加到最优控制问题中// Cost terms 添加成本函数problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, verbose));// Constraint terms// friction cone settings 读取摩擦锥配置scalar_t frictionCoefficient = 0.7;RelaxedBarrierPenalty::Config barrierPenaltyConfig;std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(taskFile, verbose); // 构建摩擦力软约束// 遍历四条腿，针对每个足端接触点设置约束，这些都是MPC控制支撑腿和摆动腿行为的关键物理约束for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++){const std::string &footName = modelSettings_.contactNames3DoF[i]; // 获取每个足端接触点的名称std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr = getEeKinematicsPtr({footName}, footName); // 构造足端运动学if (useHardFrictionConeConstraint_) // 是否使用硬约束{// 使用硬约束problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient));}else // 使用软约束{problemPtr_->softConstraintPtr->add(footName + "_frictionCone",getFrictionConeSoftConstraint(i, frictionCoefficient, barrierPenaltyConfig));}// 添加摆动相足端零力等式约束，在非接触状态下强制机器人接触点的受力为零，即摆动相足底反力约束，f=0problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", std::unique_ptr<StateInputConstraint>(new ZeroForceConstraint(*referenceManagerPtr_, i, centroidalModelInfo_)));// 添加接触状态下足端零速等式约束，在足底接触状态下要求足端线速度为0problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i));// 添加接触状态下足端法向速度等式约束，要求足端法向速度为预规划轨迹的z向速度problemPtr_->equalityConstraintPtr->add(footName + "_normalVelocity",std::unique_ptr<StateInputConstraint>(new NormalVelocityConstraintCppAd(*referenceManagerPtr_, *eeKinematicsPtr, i)));}// Self-collision avoidance constraint 添加自碰撞约束problemPtr_->stateSoftConstraintPtr->add("selfCollision",getSelfCollisionConstraint(*pinocchioInterfacePtr_, taskFile, "selfCollision", verbose));// 为自动微分/导数计算预处理：（1）提前计算雅可比、惯量等；（2）减少优化过程中重复计算开销setupPreComputation(taskFile, urdfFile, referenceFile, verbose);// Rollout 设置rollout模块rolloutPtr_ = std::make_unique<TimeTriggeredRollout>(*problemPtr_->dynamicsPtr, rolloutSettings_);// Initialization 设置初始解初始化器，用于提供优化初始猜测（状态、输入），帮助OCS2在第一次运行时更快收敛constexpr bool extendNormalizedNomentum = true;initializerPtr_ = std::make_unique<LeggedRobotInitializer>(centroidalModelInfo_, *referenceManagerPtr_, extendNormalizedNomentum);}

rollout

; Rollout settings rollout模块负责利用数值积分方法对系统动力学进行前向仿真，也就是从当前状态出发，根据给定的控制输入生成下一时刻状态
rollout
{AbsTolODE                       1e-5 ; 绝对误差容限，在ODE积分中，绝对误差容限用于衡量积分器在每一步中允许的绝对误差上限RelTolODE                       1e-3 ; 相对误差容限,用于衡量积分器在每一步中允许的相对误差timeStep                        0.015 ; 时间步长integratorType                  ODE45 ; 积分器类型,指定使用的数值积分方法，这里选择了ODE45，即Dormand-Prince自适应Runge-Kutta方法maxNumStepsPerSecond            10000 ; 每秒最大积分步数,限制在仿真过程中，每模拟1秒所允许的内部积分步数上限checkNumericalStability         false ; 检查数值稳定性
}

在最优控制问题中，rollout 模块负责利用数值积分方法对系统动力学进行前向仿真，也就是从当前状态出发，根据给定的控制输入生成下一时刻状态。这个仿真在优化器中用于评估系统行为（例如在 DDP 或 SQP 中的 forward pass）。

前向仿真利用系统的动力学模型对未来状态进行预测，从而构成了模型预测控制（MPC）中的预测方程。在 MPC 中，使用前向仿真来模拟在当前控制输入下系统的未来演化，这为后续成本计算、约束检测以及控制更新提供了依据。

DDP Differential Dynamic Programming

非线性模型预测控制微分动态规划（NMPC-DDP）详解（一）
非线性模型预测控制微分动态规划（NMPC-DDP）详解（二）

DDP，Differential Dynamic Programming（微分动态规划），是一种用于求解非线性最优控制问题（nonlinear optimal control）的高效迭代算法，在机器人控制、轨迹优化和模型预测控制（MPC）中非常常用。DDP 通过“前向仿真 + 局部线性/二次近似 + 反向传播更新”的方式，逐步逼近最优控制策略，适用于复杂非线性系统的最优控制问题。

在四足机器人控制中：

DDP/SLQ 是核心的轨迹优化器，基于模型预测当前状态到未来一段时间的最优动作；
目标是生成接触力、关节控制命令，使机器人平衡、步态协调、能耗低、跟踪轨迹精确；
在实际控制中它通常作为 MPC 的内部求解器，每个控制周期执行一次 DDP 更新（通常只跑 1～2 次迭代，以满足实时性）。

; DDP settings 
ddp
{;; 算法与多线程参数algorithm                       SLQ ; 使用SLQ算法，它是DDP的一种变体，利用线性-二次近似求解连续时间最优控制问题nThreads                        3 ; 设置并行计算时使用的线程数量threadPriority                  50 ; 设置线程的调度优先级;; 迭代与终止条件maxNumIterations                1 ; 最大迭代次数，表示DDP求解器每次调用时仅执行一次迭代minRelCost                      1e-1 ; 相对成本改善的最小容忍度,当连续迭代中成本改善低于0.1（即10%）时，可以认为已经收敛constraintTolerance             5e-3 ; 约束允许的最大违反误差,在求解过程中，如果约束违反程度低于0.005，则认为满足约束；这个参数用于判断是否需要增加约束惩罚;; 调试与显示设置displayInfo                     falsedisplayShortSummary             false ; 控制求解器在运行时是否打印详细信息或简短总结checkNumericalStability         false ; 是否检查数值稳定性debugPrintRollout               falsedebugCaching                    false ; 用于调试前向仿真和缓存机制的额外信息输出，一般关闭以提高性能;; 数值积分参数设置，ODE参数AbsTolODE                       1e-5 ; 数值积分器的绝对误差容限RelTolODE                       1e-3 ; 相对误差容限maxNumStepsPerSecond            10000 ; 限制每秒钟最多使用的积分步数timeStep                        0.015 ; 时间步长backwardPassIntegratorType      ODE45 ; 在DDP算法的backward pass（求解Riccati方程时）的积分方法，这里采用ODE45（Dormand-Prince自适应Runge-Kutta方法）;; 约束惩罚参数constraintPenaltyInitialValue   20.0 ; 初始的约束惩罚因子值constraintPenaltyIncreaseRate   2.0 ; 每次迭代中，约束惩罚因子增加的倍率preComputeRiccatiTerms          true ; 指示是否提前计算Riccati递归中的中间项,预计算可以加快backward pass的求解速度，尤其在问题维度较高时useFeedbackPolicy               false ; 是否在求解器中使用反馈增益更新控制输入; 更新策略和线搜索参数strategy                        LINE_SEARCH ; 优化更新时采用线搜索lineSearch{minStepLength                 1e-2 ; 线搜索时允许的最小步长maxStepLength                 1.0 ; 最大步长hessianCorrectionStrategy     DIAGONAL_SHIFT ; 当Hessian（或二次型近似）不正定时，采用对角线修正策略，即通过添加一个正值到对角线，确保Hessian正定，从而使得二次规划问题可解hessianCorrectionMultiple     1e-5 ; Hessian修正时添加到对角线的乘子}
}

从参数中可以知道，采用的是SLQ算法，属于DDP的一种变体。另一个选项是ILQR，ocs2_ddp中默认采用SLQ。SLQ（Sequential Linear Quadratic）是一种基于 DDP 思路的非线性轨迹优化方法，将非线性轨迹优化问题，反复近似为线性动力学 + 二次 cost 的 LQR 子问题，通过 Riccati backward pass 得到控制律，并进行 rollout，再迭代直到收敛。（1）它保留了 DDP 的局部二次 cost和局部线性dynamics；（2）用 Q-function 二阶近似 + Riccati backward pass 推导控制律；（3）相比 DDP，不需要二阶动力学导数（即 Hessian），因此计算更快、更稳定。SLQ 是为了避免高阶导数，牺牲一点理论精度，用线性 dynamics + 二次 cost 组合，近似原始非线性问题，更适合大系统、实时优化。

在这里插入图片描述

DDP是从贝尔曼方程的形式出发，展开整个Action-Value function ，这包含了cost和系统dynamic的复合项。 $V_{k+1}(f(x_k, u_k))$ 是嵌套复合函数，需要用链式法则求二阶偏导Hessian，难以计算。

SQP

在 OCS2 中的 SQP 是基于 Multiple Shooting 的结构，它将整个时间域划分为多个子区间，在每个区间上优化状态与控制输入，并在子区间之间强制满足动力学一致性。

; Multiple_Shooting SQP settings
sqp
{nThreads                              3 ; 并行使用的线程数。加速梯度计算、仿真等过程dt                                    0.015 ; 时间离散步长（秒）。与 MPC 控制周期相同，一般在 10～20ms 范围内sqpIteration                          1 ; 每个控制周期最多允许的 SQP 迭代次数。1 表示“冷启动”一次迭代，适用于实时 MPCdeltaTol                              1e-4 ; 变量变化的停止阈值：若两次迭代间控制输入/状态变化很小，停止迭代g_max                                 1e-2 ; 最大允许的约束违例（容忍度），用于不等式约束g_min                                 1e-6 ; 最小约束激活判据，小于这个值的约束会被忽略为非激活inequalityConstraintMu                0.1 ; 不等式约束惩罚函数（log barrier）中的权重参数mu,越大惩罚越强inequalityConstraintDelta             5.0 ; Log barrier松弛因子, 越大越“软”，影响优化器数值稳定性projectStateInputEqualityConstraints  true ;     是否在每轮迭代后将状态和输入投影到等式约束可行空间（用于保证动力学一致性）printSolverStatistics                 true ; 是否在控制台打印每轮 SQP 的统计信息（如成本下降、时间）printSolverStatus                     false ; 是否打印每次迭代的状态（如是否收敛、步长等）printLinesearch                       false ; 是否打印线搜索细节（用于调试 step size）useFeedbackPolicy                     false ; 是否在控制策略中使用反馈控制律（通常在LQ解中得到）。关闭表示使用纯前馈integratorType                        RK2 ; 使用的积分器类型，这里是 2 阶 Runge-Kutta。适用于实时但精度适中问题threadPriority                        50 ; 设置线程优先级，用于多线程求解的调度优化
}

其中，

sqpIteration, deltaTol, g_max, g_min：控制 SQP 的收敛速度与精度，对实时性能和数值稳定性影响极大。

较小的 deltaTol和 g_max 提高精度，但计算时间更长。

inequalityConstraintMu, inequalityConstraintDelta：影响log barrier 类似方法处理不等式约束的方式。常用于摩擦锥、自碰撞等软约束。

较大的 mu 和较小的 delta 更接近硬约束，但数值敏感。

integratorType = RK2：控制系统的积分方法，影响预测精度与稳定性；

nThreads, threadPriority：用于加速 rollout 和梯度计算，是高性能计算的重要部分。

printSolverStatistics, printSolverStatus, printLinesearch：控制求解过程的输出信息。调试阶段建议打开，部署阶段可以关闭减少开销。

IPM

Multiple Shooting 内点法（IPM）求解器的配置文件，属于 OCS2 框架中用于求解非线性约束最优控制问题的核心模块之一。这些参数控制 IPM（Interior Point Method）在求解过程中对迭代流程、数值稳定、约束处理、屏蔽边界问题等方面的行为。

; Multiple_Shooting IPM settings Multiple Shooting 内点法（IPM）求解器 的配置文件
ipm
{; 基本设置nThreads                              3 ; 使用的线程数,加速仿真、Jacobian/Hessian 计算dt                                    0.015 ; 时间离散步长（秒）ipmIteration                          1 ; 每轮最多迭代次数,典型用法为 MPC，每轮执行一次迭代（实时）deltaTol                              1e-4 ; 状态输入变化容忍度,若控制量变化小于此阈值，则认为已收敛g_max                                 10.0 ; 最大允许的约束违反量（容忍度），用于不等式约束，超过视为严重违反，触发惩罚g_min                                 1e-6 ; 最小约束激活判据，小于这个值的约束会被忽略为非激活; 调试与显示设置computeLagrangeMultipliers            true ; 是否求解拉格朗日乘子（λ），用于导出 KKT 条件、灵敏度分析printSolverStatistics                 true ;     每轮输出 IPM 收敛信息（残差、代价等）printSolverStatus                     false ; 是否输出每轮迭代细节（是否收敛、步长等）printLinesearch                       false ; 是否打印线搜索细节（用于调试 step size）; 控制策略useFeedbackPolicy                     false ; 是否使用反馈控制（LQR）策略integratorType                        RK2 ; 使用的积分器类型，这里是 2 阶 Runge-Kutta。适用于实时但精度适中问题threadPriority                        50 ; 设置线程优先级，用于多线程求解的调度优化; 内点法核心参数，Barrier Function 参数设置，在求解器中用于处理不等式约束initialBarrierParameter               1e-4 ; 初始障碍参数mu_0，越大初期对约束违例惩罚越强targetBarrierParameter                1e-4 ; 期望障碍参数mu_target，控制最终精度barrierLinearDecreaseFactor           0.2 ; 每轮障碍系数线性缩小比例,mu_k+1 = 0.2 * mu_kbarrierSuperlinearDecreasePower       1.5 ; 超线性减小因子,若设置超线性下降，则使用mu_k+1 = mu_k^1.5 barrierReductionCostTol               1e-3 ; 若成本下降小于此值则降低mu,防止在未收敛时降低 barrier 导致震荡barrierReductionConstraintTol         1e-3 ; 若约束违反小于此值则降低mu,同上，作为barrier减小的判据; 边界管理fractionToBoundaryMargin              0.995 ; 离约束边界的安全距离比率,确保变量不会过度接近边界（避免数值发散）usePrimalStepSizeForDual              false ; 是否用主变量步长控制对偶变量步长,可能影响收敛速度和稳定性; 变量初始值边界initialSlackLowerBound                1e-4 ; slack变量最小初始值，防止 barrier log(0) 崩溃initialDualLowerBound                 1e-4 ; 对偶变量最小初始值initialSlackMarginRate                1e-2 ; slack初始值乘安全边界系数initialDualMarginRate                 1e-2 ; dual初始值乘安全边界系数
}

内部配置函数

模型与控制器相关

setupModel()：根据urdf文件创建PinocchioInterface对象，根据task文件创建CentroidalModelInfo对象，从task.info判断是哪种动力学模型，(SRBD or FRBD)。

setupReferenceManager()：根据task文件创建SwingTrajectoryPlanner对象，根据reference文件创建referenceManagerPtr_对象。

setupPreComputation()：配置预计算模块preComputationPtr，在控制循环中根据当前状态、输入和时间更新机器人运动学与动力学的缓存数据，同时计算末端执行器（足部）的线性约束，也就是z向位置与速度约束。这些预计算数据对于后续求解最优控制问题（如成本、约束等）至关重要。

获取/构造各模块的辅助函数

std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;用于加载步态调度，包括初始模式调度、默认模式序列模板。

std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile, const CentroidalModelInfo &info, bool verbose);用于构造二次型成本函数，读取Q矩阵和R矩阵。下面解释状态成本矩阵Q和输入权重矩阵R。

状态成本矩阵Q

通过函数getBaseTrackingCost()读取task.info文件获取。scaling 1e+0：整个 Q 矩阵将乘以该缩放因子（这里是 1.0）。

; standard state weight matrix 标准状态权重矩阵，状态成本矩阵，对角阵，24*24，惩罚状态偏离目标
；决定系统在优化中如何惩罚状态偏离目标值。权重越大，系统越倾向于保持该状态接近参考值
Q
{scaling 1e+0;; Normalized Centroidal Momentum: [linear, angular] ;; 对应状态向量质心动量部分(0,0)   15.0     ; vcom_x(1,1)   15.0     ; vcom_y(2,2)   100.0    ; vcom_z 保持z稳定性，抗下落(3,3)   10.0     ; L_x / robotMass 角动量/机器人质量(4,4)   30.0     ; L_y / robotMass(5,5)   30.0     ; L_z / robotMass;; Base Pose: [position, orientation] ;; 对应状态向量浮动基坐标部分(6,6)   1000.0   ; p_base_x 基座位置，权重非常高，保持位置稳定尤其z(7,7)   1000.0   ; p_base_y(8,8)   1500.0   ; p_base_z(9,9)   100.0    ; theta_base_z 控制姿态稳定，尤其是pitch：theta_base_y/roll：theta_base_x(10,10) 300.0    ; theta_base_y(11,11) 300.0    ; theta_base_x;; Leg Joint Positions: [LF, LH, RF, RH] ;; 对应状态向量各关节位置部分(12,12) 5.0     ; LF_HAA(13,13) 5.0     ; LF_HFE(14,14) 2.5     ; LF_KFE(15,15) 5.0     ; LH_HAA(16,16) 5.0     ; LH_HFE(17,17) 2.5     ; LH_KFE(18,18) 5.0     ; RF_HAA(19,19) 5.0     ; RF_HFE(20,20) 2.5     ; RF_KFE(21,21) 5.0     ; RH_HAA(22,22) 5.0     ; RH_HFE(23,23) 2.5     ; RH_KFE
}

输入权重矩阵R

读取task.info加载R

; control weight matrix 输入成本矩阵 惩罚控制输入的大小，使系统控制更加“平滑”、“省力”。权重大 = 惩罚大 = 不鼓励该输入快速变化
；与inputDim 输入向量维度一致 24*24
R
{scaling 1e-3      ; 整体惩罚相较Q较小，表示允许适度用力;; Feet Contact Forces: [LF, RF, LH, RH] 控制接触力的大小，对足端接触力的惩罚 ;; (0,0)   1.0       ; left_front_force(1,1)   1.0       ; left_front_force(2,2)   1.0       ; left_front_force(3,3)   1.0       ; right_front_force(4,4)   1.0       ; right_front_force(5,5)   1.0       ; right_front_force(6,6)   1.0       ; left_hind_force(7,7)   1.0       ; left_hind_force(8,8)   1.0       ; left_hind_force(9,9)   1.0       ; right_hind_force(10,10) 1.0       ; right_hind_force(11,11) 1.0       ; right_hind_force;; foot velocity relative to base: [LF, LH, RF, RH] (uses the Jacobian at nominal configuration) ;;；；非常强地约束摆动腿的相对速度 —— 典型用于控制摆动轨迹平稳，对足端速度的惩罚 ；；(12,12) 5000.0    ; x (13,13) 5000.0    ; y(14,14) 5000.0    ; z(15,15) 5000.0    ; x(16,16) 5000.0    ; y(17,17) 5000.0    ; z(18,18) 5000.0    ; x(19,19) 5000.0    ; y(20,20) 5000.0    ; z(21,21) 5000.0    ; x(22,22) 5000.0    ; y(23,23) 5000.0    ; z
}

/*** @brief   输出输入权重矩阵r_24x24* @param   taskFile 任务文件* @param   info 质心动力学模型信息* @return  matrix_t */matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info){const size_t totalContactDim = 3 * info.numThreeDofContacts; // 3*4=12const auto &model = pinocchioInterfacePtr_->getModel();auto &data = pinocchioInterfacePtr_->getData();const auto q = centroidal_model::getGeneralizedCoordinates(initialState_, centroidalModelInfo_);pinocchio::computeJointJacobians(model, data, q);pinocchio::updateFramePlacements(model, data);matrix_t base2feetJac(totalContactDim, info.actuatedDofNum); // feet相对于base的雅可比矩阵 12*12for (size_t i = 0; i < info.numThreeDofContacts; i++) // 遍历四个脚{matrix_t jac = matrix_t::Zero(6, info.generalizedCoordinatesNum); // 6*18pinocchio::getFrameJacobian(model, data, model.getBodyId(modelSettings_.contactNames3DoF[i]), pinocchio::LOCAL_WORLD_ALIGNED, jac); // 获取每个足端接触点坐标系在LOCAL_WORLD_ALIGNED坐标系下的雅可比矩阵base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum); // 取jac的前3行后12列，存储到base2feetJac中，对应腿各关节的雅可比线速度部分，不包含浮动基}matrix_t rTaskspace(info.inputDim, info.inputDim); // 24*24loadData::loadEigenMatrix(taskFile, "R", rTaskspace); // 从task.info文件中加载Rmatrix_t r = rTaskspace;// Joint velocitiesr.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) =base2feetJac.transpose() * rTaskspace.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) *base2feetJac;return r;}

NMPC中输入矩阵u=[fc_12x1; vj_12x1]24x1，由足端接触力和各关节速度组成，前12行是在任务空间中，后12行是在关节空间中。

而task.info文件中的R矩阵rTaskspace全是任务空间的，对角阵，上12x12是对接触力的惩罚，下12x12是对足端速度的惩罚。此时就需要对rTaskspace的下对角阵进行空间转换。

matrix_t r = rTaskspace;
// Joint velocities
r.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) =base2feetJac.transpose() * rTaskspace.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) *base2feetJac;

这两条代码表明，更新后r矩阵的上12x12矩阵不变，但因为输入矩阵u后12行是关节输入，并不了解足端速度，因此需要将任务空间中的足端速度目标投影回关节空间中，即：
在这里插入图片描述

获得Q矩阵和修正R矩阵后，return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info, *referenceManagerPtr_);

摩擦锥硬约束和软约束

static std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string &taskFile, bool verbose);

从task.info中读取摩擦系数frictionCoefficient和软约束惩罚项系数RelaxedBarrierPenalty::Config。其中摩擦系数frictionCoefficient用来构造摩擦锥硬约束，RelaxedBarrierPenalty::Config用来构造摩擦软约束惩罚项。

frictionConeSoftConstraint // 摩擦系数和软约束惩罚项
{frictionCoefficient    0.3; relaxed log barrier parametersmu                     0.1delta                  5.0
}

在这里插入图片描述
RelaxedBarrierPenalty::Config定义详见类RelaxedBarrierPenalty，上图为构造松弛对数罚函数公式（内点罚函数法），用于将不等式约束 $h \geq 0$ 以软惩罚的形式加入最优控制问题中，要求迭代过程中优化变量始终不能违反约束 $h \geq 0$ 。它常用于优化中替代硬约束，让优化器在违反约束时施加代价，但不至于数值崩溃，适用于像摩擦锥约束、足端位置约束、自碰撞约束等场景。

在图示公式中， $\mu$ 表示惩罚系数，决定惩罚强度，数值越大惩罚强度越大； $\delta$ 表示松弛因子，决定软化的范围，越大表示约束违反时惩罚越宽容。

当 $h>\delta$ 时，用标准对数罚函数，快速增大成本接近边界；

当 $h≤\delta$ 时，进入软区间，用二次多项式来光滑地过渡，避免无限梯度（数值发散）。

这样 $h \to 0 +$ （接近违反约束）时：（1）代价函数是光滑的；（2）一阶导数是有限的；（3）惩罚仍然强烈，鼓励优化器保持 $h \geq 0$ 。

std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient);

根据给定的足端接触点索引contactPointIndex和摩擦系数frictionCoefficient，构造线性摩擦锥硬约束，详见FrictionConeConstraint.h/cpp。

std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config &barrierPenaltyConfig);

根据给定的足端接触点索引contactPointIndex、摩擦系数frictionCoefficient和摩擦软约束惩罚项配置参数barrierPenaltyConfig，构造摩擦锥软约束，通过松弛对数罚函数法将硬约束 $h \geq 0$ 以软惩罚的形式加入最优控制问题中，表明接触力必须落在摩擦锥内，但允许轻微违反（如在动态动作中）。

在这里插入图片描述

 /*** @brief   构造摩擦锥软约束* @param   contactPointIndex 足端接触点索引* @param   frictionCoefficient 摩擦系数，用来构造摩擦锥约束* @param   barrierPenaltyConfig 软约束惩罚项配置* @return  std::unique_ptr<StateInputCost> */std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,const RelaxedBarrierPenalty::Config &barrierPenaltyConfig){return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient),std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));}

StateInputSoftConstraint构造函数在ocs2_core中StateInputSoftConstraint类中，此类使用链式法则计算约束惩罚的二阶近似值。在未提供约束的二阶近似值的情况下，它采用仅依赖于一阶近似的高斯-牛顿近似技术。一般来说，惩罚函数可以是时间函数。工具箱提供了一些常用的惩罚函数，例如 Relaxed-Barrier 和 Squared-Hinge 惩罚函数。

原始约束为摩擦系数构成的索引index对应足端的摩擦锥约束h。

/*** Constructor.* @param [in] constraintPtr:  指向将作为软约束强制执行的约束的指针 A pointer to the constraint which will be enforced as soft constraints.* @param [in] penaltyPtrArray: 指向约束惩罚函数的指针数组 An array of pointers to the penalty function on the constraint.*/StateInputSoftConstraint(std::unique_ptr<StateInputConstraint> constraintPtr, std::vector<std::unique_ptr<PenaltyBase>> penaltyPtrArray);/*** Constructor.* @note 允许约束数量可变，并对每个约束使用相同的惩罚函数 This allows a varying number of constraints and uses the same penalty function for each constraint.* @param [in] constraintPtr: 指向将作为软约束强制执行的约束的指针 A pointer to the constraint which will be enforced as soft constraints.* @param [in] penaltyFunction: 指向约束惩罚函数的指针 A pointer to the penalty function on the constraint.*/StateInputSoftConstraint(std::unique_ptr<StateInputConstraint> constraintPtr, std::unique_ptr<PenaltyBase> penaltyFunction);

构造一个支持自动微分(CppAD)的足端运动学接口

/*** @brief   为给定的足端（end-effector）构造一个支持自动微分CppAD版本的运动学接口对象，用于计算足端位置、速度以及相关雅可比矩阵* @param   footNames 指定要计算运动学的足端名称* @param   modelName 模型名称* @return  std::unique_ptr<EndEffectorKinematics<scalar_t>> */std::unique_ptr<EndEffectorKinematics<scalar_t>> LeggedInterface::getEeKinematicsPtr(const std::vector<std::string> &footNames,const std::string &modelName){std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr;// 在自动微分框架中，需要使用特定的数值类型（如 CppAD::AD<double>）来进行雅可比、梯度等的计算，// 这里完成数据类型的转换在自动微分框架中，需要使用特定的数值类型（如 CppAD::AD<double>）来进行雅可比、梯度等的计算，这里完成数据类型的转换const auto infoCppAd = centroidalModelInfo_.toCppAd(); // 将存储在当前对象中的质心动力学模型信息转换为适用于自动微分（CppAD）的版本// 利用刚刚转换后的infoCppAd创建一个CentroidalModelPinocchioMappingCppAd对象// 这个映射对象用于在Pinocchio（机器人动力学库）和质心模型之间建立联系，便于在自动微分下进行一致的模型计算const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);// 定义lambda函数，用作速度更新回调函数。按引用捕获infoCppAd，确保回调内部可以使用自动微分版本的质心模型信息// 当前系统状态state类型为ad_vector_t，表示自动微分向量auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state, PinocchioInterfaceCppAd &pinocchioInterfaceAd){const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd); // 从当前状态中提取广义坐标q，类型是自动微分类型，ad_vector_tupdateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q); // 更新自动微分版本的质心动力学模型};// 创建一个PinocchioEndEffectorKinematicsCppAd对象，传入必要的参数eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, footNames,centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,velocityUpdateCallback, modelName, modelSettings_.modelFolderCppAd,modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));return eeKinematicsPtr;}

自动微分 (CppAD)：CppAD 是 C++ 中一个常用的自动微分库。它通过对整个计算过程进行“记录”（也称为“跟踪”或“tape recording”），然后再反向传播或正向传播计算导数，从而得到精确的梯度、雅可比矩阵等。自动微分与符号微分不同，它不生成符号表达式；与数值微分不同，它不会受步长选择等问题影响，因此在很多控制和优化问题中非常有用。这里将质心动力学模型信息转换为适用于自动微分的版本，就是为了能够利用 CppAD 等工具在后续计算中准确、有效地求导，为控制器和优化器提供必要的梯度信息。

足端零速度约束

std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t> &eeKinematics, size_t contactPointIndex);

用于构造接触时的足端零速度约束g(xee, vee) = Ax * xee + Av * vee + b，线性约束函数。在足底接触状态下要求足端线速度为0，不打滑。

 /*** @brief   构造接触时足端零速度约束* @param   eeKinematics 足端运动学对象* @param   contactPointIndex 足端接触点索引* @return  std::unique_ptr<StateInputConstraint> */std::unique_ptr<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t> &eeKinematics,size_t contactPointIndex){// 定义一个lambda函数，用于配置足端线性约束的参数。参数为位置误差增益positionErrorGain，返回一个EndEffectorLinearConstraint::Config对象auto eeZeroVelConConfig = [](scalar_t positionErrorGain){// g(xee, vee) = Ax * xee + Av * vee + b  线性约束函数 EndEffectorLinearConstraint::Config config;config.b.setZero(3);config.Av.setIdentity(3, 3); // vee = bif (!numerics::almost_eq(positionErrorGain, 0.0)) // 如果位置误差增益不为0，考虑Ax * xee来补偿位置误差{config.Ax.setZero(3, 3);config.Ax(2, 2) = positionErrorGain; // 只将Ax(2, 2)设置为位置误差增益，表明只对末端在z方向上的位置偏差施加惩罚}return config;};return std::unique_ptr<StateInputConstraint>(new ZeroVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex,eeZeroVelConConfig(modelSettings_.positionErrorGain)));}

当位置误差增益不为0时，为什么只考虑Ax(2, 2)？

只考虑Ax(2, 2) = positionErrorGain表明只对末端z方向上的位置偏差施加惩罚。通常，在腿式机器人接触建模中，足端与地面接触时，最关键的要求之一是控制法向（通常是 z 轴）运动，足端在法向（z 方向）运动会导致接触破坏（穿透或脱离地面），因此非常关键，必须严格控制；对于 x 和 y 方向，虽然也希望控制零滑动，但通常由其他机制（例如摩擦约束）或者已经通过速度直接约束（Av = I）来处理。

Ax(2, 2) = positionErrorGain保证了如果足端在垂直方向上存在位置偏差，这个误差会转化为一个对应的速度补偿项，从而帮助系统更快纠正这种偏差，确保接触稳定。

构造自碰撞约束

std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, const std::string &taskFile, const std::string &prefix, bool verbose);

用于构造自碰撞约束。读取task.info文件中的selfCollision参数，先根据安全距离和自碰撞link对构造自碰撞硬约束h，然后根据mu和delta系数构造一个松弛惩罚函数p(h)。

selfCollision   // 自碰撞约束参数
{; Self Collision raw object pairs 定义原始的碰撞对象对，也就是直接用物体标识（如模型中定义的几何体ID）进行碰撞检测；这里为空，说明没有单独定义原始碰撞对象对，后续自碰撞检测主要依靠link名称对collisionObjectPairs{}; Self Collision pairs 指定需要进行自碰撞检测的机器人关节或连杆对，通常用两个link的名称组成一对。检测时会计算这两个link之间的距离collisionLinkPairs{[0] "LF_calf, RF_calf"[1] "LH_calf, RH_calf"[2] "LF_calf, LH_calf"[3] "RF_calf, RH_calf"[4] "LF_FOOT, RF_FOOT"[5] "LH_FOOT, RH_FOOT"[6] "LF_FOOT, LH_FOOT"[7] "RF_FOOT, RH_FOOT"}；安全距离阈值，单位mminimumDistance  0.05; relaxed log barrier parameters 松弛对数罚函数参数，对安全距离不足的情况施加代价mu      1e-2delta   1e-3
}

参考文献

OCS2学习记录（1）—— Tutorial - OCS2 - Farbod Farshidian
非线性模型预测控制微分动态规划（NMPC-DDP）详解（一）
非线性模型预测控制微分动态规划（NMPC-DDP）详解（二）
四足机器人控制算法——建模，控制与实践
于宪元《基于稳定性的仿生四足机器人控制系统设计》
开源 NMPC-WBC 足式机器人控制框架： legged_control

查看全文

http://www.xdnf.cn/news/14185.html