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列赋值为Ab
data.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_f
centroidalMomentumRate.template head<3>() += contactForceInWorldFrame;
centroidalMomentumRate.template tail<3>().noalias() += positionComToContactPointInWorldFrame.cross(contactForceInWorldFrame);
} // end of i loop
for (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 mass
centroidalMomentumRate /= 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 PreComputation
matrix_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 frame
basePose = qPinocchio.head<6>();
const auto basePosition = basePose.head<3>();
const auto baseOrientation = basePose.tail<3>();
// Base Velocity in world frame
const auto& A = getCentroidalMomentumMatrix(pinocchioInterface_); //获取动量矩阵Ab
const 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 frame
baseAcceleration.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) {
// handles
const auto& info = mapping_.getCentroidalModelInfo();
const auto& model = pinocchioInterface_.getModel();
auto& data = pinocchioInterface_.getData();
// desired
Vector6 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;
}
// measured
vector_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 augmentation
const vector_t pdFeedback = pGains.cwiseProduct(qDesired - qMeasured) + dGains.cwiseProduct(vDesired - vMeasured);
// feedforward plus PD on acceleration level
const 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 momentum
normalizedLinearMomentumRateDerivativeQ_.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 rates
computeNormalizedCentroidalMomentumRateGradients(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;
}