1. ModelHelperFunctions.cpp
1.1 updateCentroidalDynamics() : 质心动力学更新
template <typename SCALAR_T>
void updateCentroidalDynamics(PinocchioInterfaceTpl<SCALAR_T>& interface, const CentroidalModelInfoTpl<SCALAR_T>& info,const Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1>& q) {using vector3_t = Eigen::Matrix<SCALAR_T, 3, 1>;using matrix3_t = Eigen::Matrix<SCALAR_T, 3, 3>;using matrix6_t = Eigen::Matrix<SCALAR_T, 6, 6>;const auto& model = interface.getModel();auto& data = interface.getData();switch (info.centroidalModelType) {case CentroidalModelType::FullCentroidalDynamics: {pinocchio::computeCentroidalMap(model, data, q);pinocchio::updateFramePlacements(model, data);break;}case CentroidalModelType::SingleRigidBodyDynamics: {//从广义坐标 q 中提取的欧拉角const vector3_t eulerAnglesZyx = q.template segment<3>(3);// 将欧拉角导数映射到全局角速度的映射矩阵const matrix3_t mappingZyx = getMappingFromEulerAnglesZyxDerivativeToGlobalAngularVelocity(eulerAnglesZyx);//基坐标系到世界坐标系的旋转矩阵const matrix3_t rotationBaseToWorld = getRotationMatrixFromZyxEulerAngles(eulerAnglesZyx);//质心相对于基坐标系的位置信息，变换到世界坐标系const vector3_t comToBasePositionInWorld = rotationBaseToWorld * info.comToBasePositionNominal;//质心位置的反对称矩阵，用于计算惯性耦合项const matrix3_t skewSymmetricMap = skewSymmetricMatrix(comToBasePositionInWorld);//用于计算角动量相关的中间矩阵const matrix3_t mat1 = rotationBaseToWorld * info.centroidalInertiaNominal;const matrix3_t mat2 = rotationBaseToWorld.transpose() * mappingZyx;//刚体动力学下的质心动量映射矩阵，6x6 矩阵matrix6_t Ab = matrix6_t::Zero();//template显性的指定矩阵块大小//topLeftCorner左上角3*3块，diagonal：提取对角线元素，array：将对角线视为数组逐元素操作Ab.template topLeftCorner<3, 3>().diagonal().array() = info.robotMass;//noalias()：优化矩阵赋值操作Ab.template topRightCorner<3, 3>().noalias() = info.robotMass * skewSymmetricMap * mappingZyx;Ab.template bottomRightCorner<3, 3>().noalias() = mat1 * mat2;//Ag前3行是线动量部分，后3行是角动量部分data.Ag = Eigen::Matrix<SCALAR_T, -1, -1>::Zero(6, info.generalizedCoordinatesNum);//左侧6列赋值为Abdata.Ag.template leftCols<6>() = Ab;data.com[0] = q.template head<3>() - comToBasePositionInWorld;pinocchio::forwardKinematics(model, data, q);pinocchio::updateFramePlacements(model, data);break;}default: {throw std::runtime_error("The chosen centroidal model type is not supported.");break;}}
}
-
将欧拉角导数映射到全局角速度的映射矩阵：
https://blog.csdn.net/subtitle_/article/details/131915276
旋转顺序Z -> Y -> X，对应角度\(\alpha\),\(\beta\),\(\gamma\),
-
Ab(data.Ag)矩阵的公式：
参考文献：D. E. Orin, A. Goswami, and S. Lee, “Centroidal dynamics of a humanoid robot,”
1.2 updateCentroidalDynamicsDerivatives() : 更新质心动力学导数
data.dFdq.setZero(6, info.generalizedCoordinatesNum);
data.dFdq.template middleCols<3>(3) = getCentroidalMomentumZyxGradient(interface, info, q, v);
pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 6, 3> getCentroidalMomentumZyxGradient(const PinocchioInterfaceTpl<SCALAR_T>& interface,const CentroidalModelInfoTpl<SCALAR_T>& info,const Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1>& q,const Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1>& v) {using matrix_t = Eigen::Matrix<SCALAR_T, Eigen::Dynamic, Eigen::Dynamic>;using vector3_t = Eigen::Matrix<SCALAR_T, 3, 1>;using matrix3_t = Eigen::Matrix<SCALAR_T, 3, 3>;const auto& data = interface.getData();const auto m = info.robotMass;const vector3_t eulerAngles = q.template segment<3>(3);const vector3_t eulerAnglesDerivatives = v.template segment<3>(3);//从欧拉角导数到全局角速度的映射矩阵const auto T = getMappingFromEulerAnglesZyxDerivativeToGlobalAngularVelocity(eulerAngles);//从基座到世界坐标系的旋转矩阵const auto R = getRotationMatrixFromZyxEulerAngles(eulerAngles);matrix3_t Ibaseframe, Iworldframe;vector3_t rbaseframe, rworldframe;switch (info.centroidalModelType) {case CentroidalModelType::FullCentroidalDynamics: {Iworldframe = data.Ig.inertia().matrix();Ibaseframe.noalias() = R.transpose() * (Iworldframe * R);rworldframe = q.template head<3>() - data.com[0];rbaseframe.noalias() = R.transpose() * rworldframe;break;}case CentroidalModelType::SingleRigidBodyDynamics: {Ibaseframe = info.centroidalInertiaNominal; //归一化的基座惯性张量Iworldframe.noalias() = R * (Ibaseframe * R.transpose()); //世界坐标惯性张量rbaseframe = info.comToBasePositionNominal; //质心位置rworldframe.noalias() = R * rbaseframe; //世界坐标质心位置break;}default: {throw std::runtime_error("The chosen centroidal model type is not supported.");}}const auto S = skewSymmetricMatrix(rworldframe); //r_com 反对称矩阵const auto dT = getMappingZyxGradient(eulerAngles); //映射矩阵 T 的梯度const auto dR = getRotationMatrixZyxGradient(eulerAngles);// 旋转矩阵R的梯度std::array<matrix3_t, 3> dS;for (size_t i = 0; i < 3; i++) {const vector3_t dr = dR[i] * rbaseframe;dS[i] = skewSymmetricMatrix(dr); //世界坐标质心位置关于欧拉角的梯度}matrix_t dhdq = matrix_t::Zero(6, 3);for (size_t i = 0; i < 3; i++) {const vector3_t T_eulerAnglesDev = T * eulerAnglesDerivatives; const vector3_t dT_eulerAnglesDev = dT[i] * eulerAnglesDerivatives; const matrix3_t dR_I_Rtrans = dR[i] * Ibaseframe * R.transpose();dhdq.template block<3, 1>(0, i).noalias() = m * (S * dT_eulerAnglesDev);dhdq.template block<3, 1>(0, i).noalias() += m * (dS[i] * T_eulerAnglesDev);dhdq.template block<3, 1>(3, i).noalias() = (dR_I_Rtrans + dR_I_Rtrans.transpose()) * T_eulerAnglesDev;dhdq.template block<3, 1>(3, i).noalias() += Iworldframe * dT_eulerAnglesDev;}if (info.centroidalModelType == CentroidalModelType::FullCentroidalDynamics) {const auto jointVelocities = v.tail(info.actuatedDofNum);const vector3_t comLinearVelocityInWorldFrame = (1.0 / m) * (data.Ag.topRightCorner(3, info.actuatedDofNum) * jointVelocities);const vector3_t comAngularVelocityInWorldFrame =Iworldframe.inverse() * (data.Ag.bottomRightCorner(3, info.actuatedDofNum) * jointVelocities);const vector3_t linearMomentumInBaseFrame = m * (R.transpose() * comLinearVelocityInWorldFrame);const vector3_t angularMomentumInBaseFrame = Ibaseframe * (R.transpose() * comAngularVelocityInWorldFrame);for (size_t i = 0; i < 3; i++) {dhdq.template block<3, 1>(0, i).noalias() += dR[i] * linearMomentumInBaseFrame;dhdq.template block<3, 1>(3, i).noalias() += dR[i] * angularMomentumInBaseFrame;}}return dhdq;
}
- 其中dh/dq的公式为：
1.3 质心（CoM）到特定接触点的平移雅可比矩阵
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, Eigen::Dynamic> getTranslationalJacobianComToContactPointInWorldFrame(const PinocchioInterfaceTpl<SCALAR_T>& interface, const CentroidalModelInfoTpl<SCALAR_T>& info, size_t contactIndex) {const auto& model = interface.getModel();auto data = interface.getData();Eigen::Matrix<SCALAR_T, 6, Eigen::Dynamic> jacobianWorldToContactPointInWorldFrame;jacobianWorldToContactPointInWorldFrame.setZero(6, info.generalizedCoordinatesNum);pinocchio::getFrameJacobian(model, data, info.endEffectorFrameIndices[contactIndex], pinocchio::LOCAL_WORLD_ALIGNED,jacobianWorldToContactPointInWorldFrame);Eigen::Matrix<SCALAR_T, 3, Eigen::Dynamic> J_com = getCentroidalMomentumMatrix(interface).template topRows<3>() / info.robotMass;return (jacobianWorldToContactPointInWorldFrame.template topRows<3>() - J_com);
}
1.4 动量的变化率，也就是一阶导
/**********************动量的变化率，也就是一阶导,这里就是求的h_{com}的导数*********************************************************/
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 6, 1> getNormalizedCentroidalMomentumRate(const PinocchioInterfaceTpl<SCALAR_T>& interface,const CentroidalModelInfoTpl<SCALAR_T>& info,const Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1>& input) {const Eigen::Matrix<SCALAR_T, 3, 1> gravityVector(SCALAR_T(0.0), SCALAR_T(0.0), SCALAR_T(-9.81));Eigen::Matrix<SCALAR_T, 6, 1> centroidalMomentumRate;centroidalMomentumRate << info.robotMass * gravityVector, Eigen::Matrix<SCALAR_T, 3, 1>::Zero();for (size_t i = 0; i < info.numThreeDofContacts; i++) {const auto contactForceInWorldFrame = centroidal_model::getContactForces(input, i, info); //获取接触力const auto positionComToContactPointInWorldFrame = getPositionComToContactPointInWorldFrame(interface, info, i); //r_fcentroidalMomentumRate.template head<3>() += contactForceInWorldFrame;centroidalMomentumRate.template tail<3>().noalias() += positionComToContactPointInWorldFrame.cross(contactForceInWorldFrame);} // end of i loopfor (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) {const auto contactForceInWorldFrame = centroidal_model::getContactForces(input, i, info);const auto contactTorqueInWorldFrame = centroidal_model::getContactTorques(input, i, info);const auto positionComToContactPointInWorldFrame = getPositionComToContactPointInWorldFrame(interface, info, i);centroidalMomentumRate.template head<3>() += contactForceInWorldFrame;centroidalMomentumRate.template tail<3>().noalias() +=positionComToContactPointInWorldFrame.cross(contactForceInWorldFrame) + contactTorqueInWorldFrame;} // end of i loop// normalize by the total masscentroidalMomentumRate /= info.robotMass;return centroidalMomentumRate;
}
2. CentroidalModelPinocchioMapping.cpp
2.1 getPinocchioJointVelocity 获取浮动基座速度
/********************获取浮动基座速度**********************************************************/
template <typename SCALAR>
auto CentroidalModelPinocchioMappingTpl<SCALAR>::getPinocchioJointVelocity(const vector_t& state, const vector_t& input) const -> vector_t {const auto& model = pinocchioInterfacePtr_->getModel();const auto& data = pinocchioInterfacePtr_->getData();const auto& info = centroidalModelInfo_;assert(info.stateDim == state.rows());assert(info.inputDim == input.rows());const auto& A = getCentroidalMomentumMatrix(*pinocchioInterfacePtr_);const Eigen::Matrix<SCALAR, 6, 6> Ab = A.template leftCols<6>(); //取左侧6列关于浮动基座的部分const auto Ab_inv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);const auto jointVelocities = centroidal_model::getJointVelocities(input, info).head(info.actuatedDofNum);Eigen::Matrix<SCALAR, 6, 1> momentum = info.robotMass * centroidal_model::getNormalizedMomentum(state, info);if (info.centroidalModelType == CentroidalModelType::FullCentroidalDynamics) {momentum.noalias() -= A.rightCols(info.actuatedDofNum) * jointVelocities;}vector_t vPinocchio(info.generalizedCoordinatesNum);vPinocchio.template head<6>().noalias() = Ab_inv * momentum;vPinocchio.tail(info.actuatedDofNum) = jointVelocities;return vPinocchio;
}
- computeFloatingBaseCentroidalMomentumMatrixInverse():
2.2 getOcs2Jacobian 动力学公式内的关节速度和浮动速度对状态和输入的偏导
/****主要动力学公式内的关节速度和浮动速度对状态和输入的偏导**************************************************************************/
template <typename SCALAR>
auto CentroidalModelPinocchioMappingTpl<SCALAR>::getOcs2Jacobian(const vector_t& state, const matrix_t& Jq, const matrix_t& Jv) const-> std::pair<matrix_t, matrix_t> {const auto& model = pinocchioInterfacePtr_->getModel();const auto& data = pinocchioInterfacePtr_->getData();const auto& info = centroidalModelInfo_;assert(info.stateDim == state.rows());// Partial derivatives of joint velocities//关节角速度对输入的偏导matrix_t jointVelocitiesDerivativeInput = matrix_t::Zero(info.actuatedDofNum, info.inputDim);jointVelocitiesDerivativeInput.rightCols(info.actuatedDofNum).setIdentity();// Partial derivatives of the floating base variables//浮动基座速度对状态和输入的偏导// TODO: move getFloatingBaseCentroidalMomentumMatrixInverse(Ab) to PreComputationmatrix_t floatingBaseVelocitiesDerivativeState = matrix_t::Zero(6, info.stateDim);matrix_t floatingBaseVelocitiesDerivativeInput = matrix_t::Zero(6, info.inputDim);const auto& A = getCentroidalMomentumMatrix(*pinocchioInterfacePtr_);const Eigen::Matrix<SCALAR, 6, 6> Ab = A.template leftCols<6>();const auto Ab_inv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);floatingBaseVelocitiesDerivativeState.leftCols(6) = info.robotMass * Ab_inv;using matrix6x_t = Eigen::Matrix<SCALAR, 6, Eigen::Dynamic>;matrix6x_t dhdq(6, info.generalizedCoordinatesNum);switch (info.centroidalModelType) {case CentroidalModelType::FullCentroidalDynamics: {pinocchio::translateForceSet(data.dHdq, data.com[0], dhdq.const_cast_derived());for (size_t k = 0; k < model.nv; ++k) {dhdq.template block<3, 1>(pinocchio::Force::ANGULAR, k) +=data.hg.linear().cross(data.dFda.template block<3, 1>(pinocchio::Force::LINEAR, k)) / data.Ig.mass();}dhdq.middleCols(3, 3) = data.dFdq.middleCols(3, 3);const auto Aj = A.rightCols(info.actuatedDofNum);floatingBaseVelocitiesDerivativeState.rightCols(info.generalizedCoordinatesNum).noalias() = -Ab_inv * dhdq;floatingBaseVelocitiesDerivativeInput.rightCols(info.actuatedDofNum).noalias() = -Ab_inv * Aj;break;}case CentroidalModelType::SingleRigidBodyDynamics: {dhdq = data.dFdq;floatingBaseVelocitiesDerivativeState.middleCols(6, 6).noalias() = -Ab_inv * dhdq.leftCols(6);break;}default: {throw std::runtime_error("The chosen centroidal model type is not supported.");}}matrix_t dvdx = matrix_t::Zero(info.generalizedCoordinatesNum, info.stateDim);dvdx.template topRows<6>() = floatingBaseVelocitiesDerivativeState;matrix_t dvdu = matrix_t::Zero(info.generalizedCoordinatesNum, info.inputDim);dvdu << floatingBaseVelocitiesDerivativeInput, jointVelocitiesDerivativeInput;matrix_t dfdx = matrix_t::Zero(Jq.rows(), centroidalModelInfo_.stateDim);dfdx.middleCols(6, info.generalizedCoordinatesNum) = Jq;dfdx.noalias() += Jv * dvdx;const matrix_t dfdu = Jv * dvdu;return {dfdx, dfdu};
}3. CentroidalModelRbdConversions.cpp
3.1 computeBaseKinematicsFromCentroidalModel 计算机体运动学
输入：状态state；输入input；关节角加速度jointAccelerations，
输出：基座位置basePose(从state获取)；基座速度baseVelocity（getPinocchioJointVelocity获取）；基座加速度
基座加速度公式：
\(A_b \ddot{q}_b = \dot{h}_{com} - \dot{A_b} \dot{q_b} - A_j \ddot{q_j}\)
\(\ddot{q_j}\)一般不考虑，等0
void CentroidalModelRbdConversions::computeBaseKinematicsFromCentroidalModel(const vector_t& state, const vector_t& input,const vector_t& jointAccelerations, Vector6& basePose,Vector6& baseVelocity, Vector6& baseAcceleration) {const auto& model = pinocchioInterface_.getModel();auto& data = pinocchioInterface_.getData();const auto& info = mapping_.getCentroidalModelInfo();const auto qPinocchio = mapping_.getPinocchioJointPosition(state);updateCentroidalDynamics(pinocchioInterface_, info, qPinocchio);// Base Pose in world framebasePose = qPinocchio.head<6>();const auto basePosition = basePose.head<3>();const auto baseOrientation = basePose.tail<3>();// Base Velocity in world frameconst auto& A = getCentroidalMomentumMatrix(pinocchioInterface_); //获取动量矩阵Abconst Matrix6 Ab = A.template leftCols<6>();const auto Ab_inv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);const auto Aj = A.rightCols(info.actuatedDofNum);const vector_t vPinocchio = mapping_.getPinocchioJointVelocity(state, input); //获取基座速度baseVelocity.head<3>() = vPinocchio.head<3>();const Vector3 derivativeEulerAnglesZyx = vPinocchio.segment<3>(3);baseVelocity.tail<3>() = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(baseOrientation, derivativeEulerAnglesZyx);const auto Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); // 获取动量矩阵A的导数Vector6 centroidalMomentumRate = info.robotMass * getNormalizedCentroidalMomentumRate(pinocchioInterface_, info, input);centroidalMomentumRate.noalias() -= Adot * vPinocchio;centroidalMomentumRate.noalias() -= Aj * jointAccelerations.head(info.actuatedDofNum);const Vector6 qbaseDdot = Ab_inv * centroidalMomentumRate;// Base Acceleration in world framebaseAcceleration.head<3>() = qbaseDdot.head<3>();baseAcceleration.tail<3>() =getGlobalAngularAccelerationFromEulerAnglesZyxDerivatives<scalar_t>(baseOrientation, derivativeEulerAnglesZyx, qbaseDdot.tail<3>());
}
3.2 computeCentroidalStateFromRbdModel 将刚体模型rbdState转成质心模型state
输入rbdState：
- 基座的位置、欧拉角
- 关节角度
- 基座的线速度、角速度
- 关节速度
输出state： - 归一化动量
- 广义坐标（基座位置和关节角度）
/*******将刚体模型转换为质心模型****************/
vector_t CentroidalModelRbdConversions::computeCentroidalStateFromRbdModel(const vector_t& rbdState) {const auto& model = pinocchioInterface_.getModel();auto& data = pinocchioInterface_.getData();const auto& info = mapping_.getCentroidalModelInfo();vector_t qPinocchio(info.generalizedCoordinatesNum);qPinocchio.head<3>() = rbdState.segment<3>(3); //基座位置qPinocchio.segment<3>(3) = rbdState.head<3>(); //基座姿态qPinocchio.tail(info.actuatedDofNum) = rbdState.segment(6, info.actuatedDofNum);//关节角度vector_t vPinocchio(info.generalizedCoordinatesNum);vPinocchio.head<3>() = rbdState.segment<3>(info.generalizedCoordinatesNum + 3); //基座线速度vPinocchio.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(qPinocchio.segment<3>(3), rbdState.segment<3>(info.generalizedCoordinatesNum)); //基座欧拉角导数//关节速度vPinocchio.tail(info.actuatedDofNum) = rbdState.segment(info.generalizedCoordinatesNum + 6, info.actuatedDofNum);updateCentroidalDynamics(pinocchioInterface_, info, qPinocchio);const auto& A = getCentroidalMomentumMatrix(pinocchioInterface_); //获得动量矩阵vector_t state(info.stateDim);centroidal_model::getNormalizedMomentum(state, info).noalias() = A * vPinocchio / info.robotMass; //得到规划化后的动量centroidal_model::getGeneralizedCoordinates(state, info) = qPinocchio; //状态位置赋值return state;
}
3.3 computeRbdTorqueFromCentroidalModelPD 根据期望计算最终的控制力矩
/***根据期望计算最终控制的力矩*************************************************************************/
vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(const vector_t& desiredState, const vector_t& desiredInput,const vector_t& desiredJointAccelerations,const vector_t& measuredRbdState, const vector_t& pGains,const vector_t& dGains) {// handlesconst auto& info = mapping_.getCentroidalModelInfo();const auto& model = pinocchioInterface_.getModel();auto& data = pinocchioInterface_.getData();// desiredVector6 desiredBasePose, desiredBaseVelocity, desiredBaseAcceleration;//将质心模型的期望状态和输入转换为基座的期望位姿、速度、加速度computeBaseKinematicsFromCentroidalModel(desiredState, desiredInput, desiredJointAccelerations, desiredBasePose, desiredBaseVelocity,desiredBaseAcceleration);vector_t qDesired(info.generalizedCoordinatesNum), vDesired(info.generalizedCoordinatesNum), aDesired(info.generalizedCoordinatesNum);qDesired << desiredBasePose, centroidal_model::getJointAngles(desiredState, info);vDesired << desiredBaseVelocity, centroidal_model::getJointVelocities(desiredInput, info);aDesired << desiredBaseAcceleration, desiredJointAccelerations;//将世界坐标期望力转到末端坐标系的力和力矩pinocchio::container::aligned_vector<pinocchio::Force> fextDesired(model.njoints, pinocchio::Force::Zero());for (size_t i = 0; i < info.numThreeDofContacts; i++) {const auto frameIndex = info.endEffectorFrameIndices[i];const auto jointIndex = model.frames[frameIndex].parent;const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation();const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose();const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info);fextDesired[jointIndex].linear() = contactForce;fextDesired[jointIndex].angular() = translationJointFrameToContactFrame.cross(contactForce);}for (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) {const auto frameIndex = info.endEffectorFrameIndices[i];const auto jointIndex = model.frames[frameIndex].parent;const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation();const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose();const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info);const Vector3 contactTorque = rotationWorldFrameToJointFrame * centroidal_model::getContactTorques(desiredInput, i, info);fextDesired[jointIndex].linear() = contactForce;fextDesired[jointIndex].angular() = translationJointFrameToContactFrame.cross(contactForce) + contactTorque;}// measuredvector_t qMeasured(info.generalizedCoordinatesNum), vMeasured(info.generalizedCoordinatesNum);qMeasured.head<3>() = measuredRbdState.segment<3>(3);qMeasured.segment<3>(3) = measuredRbdState.head<3>();qMeasured.tail(info.actuatedDofNum) = measuredRbdState.segment(6, info.actuatedDofNum);vMeasured.head<3>() = measuredRbdState.segment<3>(info.generalizedCoordinatesNum + 3);vMeasured.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(qMeasured.segment<3>(3), measuredRbdState.segment<3>(info.generalizedCoordinatesNum));vMeasured.tail(info.actuatedDofNum) = measuredRbdState.segment(info.generalizedCoordinatesNum + 6, info.actuatedDofNum);// PD feedback augmentationconst vector_t pdFeedback = pGains.cwiseProduct(qDesired - qMeasured) + dGains.cwiseProduct(vDesired - vMeasured);// feedforward plus PD on acceleration levelconst vector_t aAugmented = aDesired + pdFeedback;return pinocchio::rnea(model, data, qDesired, vDesired, aAugmented, fextDesired);
}
- 将质心模型的期望状态和输入转换为基座的期望位姿、速度、加速度
- 将世界坐标期望力转到末端坐标系的力和力矩
- PD控制器补偿到关节加速度
- 运动动力学公式计算各关节的控制力矩
4 PinocchioCentroidalDynamics.cpp
4.1 获取质心动力学的f值：
vector_t PinocchioCentroidalDynamics::getValue(scalar_t time, const vector_t& state, const vector_t& input) {const auto& interface = *pinocchioInterfacePtr_;const auto& info = mapping_.getCentroidalModelInfo();assert(info.stateDim == state.rows());vector_t f(info.stateDim);f << getNormalizedCentroidalMomentumRate(interface, info, input), mapping_.getPinocchioJointVelocity(state, input);return f;
}
4.2 computeNormalizedCentroidalMomentumRateGradients 计算质心动量导数对状态和输入的偏导
/*******计算质心动量导数对状态和输入的偏导**********************************************************************/
void PinocchioCentroidalDynamics::computeNormalizedCentroidalMomentumRateGradients(const vector_t& state, const vector_t& input) {const auto& interface = *pinocchioInterfacePtr_;const auto& info = mapping_.getCentroidalModelInfo();assert(info.stateDim == state.rows());assert(info.inputDim == input.rows());// compute partial derivatives of the center of robotMass acceleration and normalized angular momentumnormalizedLinearMomentumRateDerivativeQ_.setZero(3, info.generalizedCoordinatesNum); //线动量率对状态的偏导normalizedAngularMomentumRateDerivativeQ_.setZero(3, info.generalizedCoordinatesNum);//角动量率对状态的偏导normalizedLinearMomentumRateDerivativeInput_.setZero(3, info.inputDim); //线动量率对输入的偏导normalizedAngularMomentumRateDerivativeInput_.setZero(3, info.inputDim);//角动量率对输入的偏导Matrix3 f_hat, p_hat;for (size_t i = 0; i < info.numThreeDofContacts; i++) {const Vector3 contactForceInWorldFrame = centroidal_model::getContactForces(input, i, info);f_hat = skewSymmetricMatrix(contactForceInWorldFrame) / info.robotMass;const auto J = getTranslationalJacobianComToContactPointInWorldFrame(interface, info, i);normalizedAngularMomentumRateDerivativeQ_.noalias() -= f_hat * J;normalizedLinearMomentumRateDerivativeInput_.block<3, 3>(0, 3 * i).diagonal().array() = 1.0 / info.robotMass;p_hat = skewSymmetricMatrix(getPositionComToContactPointInWorldFrame(interface, info, i)) / info.robotMass;normalizedAngularMomentumRateDerivativeInput_.block<3, 3>(0, 3 * i) = p_hat;}for (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) {const size_t inputIdx = 3 * info.numThreeDofContacts + 6 * (i - info.numThreeDofContacts);const Vector3 contactForceInWorldFrame = centroidal_model::getContactForces(input, i, info);f_hat = skewSymmetricMatrix(contactForceInWorldFrame) / info.robotMass;const auto J = getTranslationalJacobianComToContactPointInWorldFrame(interface, info, i);normalizedAngularMomentumRateDerivativeQ_.noalias() -= f_hat * J;normalizedLinearMomentumRateDerivativeInput_.block<3, 3>(0, inputIdx).diagonal().array() = 1.0 / info.robotMass;p_hat = skewSymmetricMatrix(getPositionComToContactPointInWorldFrame(interface, info, i)) / info.robotMass;normalizedAngularMomentumRateDerivativeInput_.block<3, 3>(0, 3 * inputIdx) = p_hat;normalizedAngularMomentumRateDerivativeInput_.block<3, 3>(0, 3 * inputIdx + 3).diagonal().array() = 1.0 / info.robotMass;}
}
4.3 getLinearApproximation 线性化近似
/****线性近似，求df/dx,df/du*******************************************************************************/
VectorFunctionLinearApproximation PinocchioCentroidalDynamics::getLinearApproximation(scalar_t time, const vector_t& state,const vector_t& input) {const auto& info = mapping_.getCentroidalModelInfo();assert(info.stateDim == state.rows());assert(info.inputDim == input.rows());auto dynamics = ocs2::VectorFunctionLinearApproximation::Zero(info.stateDim, info.stateDim, info.inputDim);dynamics.f = getValue(time, state, input); // Partial derivatives of the normalized momentum ratescomputeNormalizedCentroidalMomentumRateGradients(state, input);matrix_t dfdq = matrix_t::Zero(info.stateDim, info.generalizedCoordinatesNum); //对状态求偏导matrix_t dfdv = matrix_t::Zero(info.stateDim, info.generalizedCoordinatesNum); //对输入求偏导//先将动量一阶导对状态的偏导赋值dfdq.topRows<6>() << normalizedLinearMomentumRateDerivativeQ_, normalizedAngularMomentumRateDerivativeQ_;dfdv.bottomRows(info.generalizedCoordinatesNum).setIdentity();//计算关节角速度和浮动基座速度对状态和输入的偏导std::tie(dynamics.dfdx, dynamics.dfdu) = mapping_.getOcs2Jacobian(state, dfdq, dfdv);// Add partial derivative of f with respect to u since part of f depends explicitly on the inputs (contact forces + torques)//将动量一阶导对输入的偏导加入矩阵中dynamics.dfdu.topRows<3>() += normalizedLinearMomentumRateDerivativeInput_;dynamics.dfdu.middleRows(3, 3) += normalizedAngularMomentumRateDerivativeInput_;return dynamics;
}
.png)
