11.5.2 机器人跳跃
文件OpenLoong-Dyn-Control/demo/jump_mpc.cpp实现了双足机器人跳跃控制的仿真功能，集成了UI控制、数据接口、运动学动力学求解器、MPC力控制器及PVT关节控制器。通过主循环执行仿真步骤，更新传感器数据与机器人状态，分阶段（准备、起跳、上升、下降、着陆）实现跳跃控制逻辑，利用MPC计算力指令并转换为关节力矩，结合PVT控制输出力矩，同时记录了仿真时间、电机状态、位姿等数据，完成了机器人跳跃全过程的仿真与控制。
（1）下面代码的功能是定义全局控制时间步长常量，创建MuJoCo模型加载的错误信息缓存数组，加载指定路径下的XML场景模型文件并编译，同时基于加载的模型创建MuJoCo仿真数据结构，为后续机器人跳跃仿真搭建基础的仿真模型和数据载体。
python
// 全局常量定义
const double dt = 0.001; // 控制时间步长（秒）
// MuJoCo模型加载与编译
char error[1000] = "无法加载二进制模型"; // 模型加载错误信息缓存
mjModel *mj_model = mj_loadXML("../models/scene.xml", 0, error, 1000); // 加载XML场景模型
mjData *mj_data = mj_makeData(mj_model); // 创建MuJoCo仿真数据结构（2）下面代码的功能是初始化机器人跳跃仿真所需的各类核心功能模块，包括MuJoCo可视化UI控制器、MuJoCo数据交互接口、机器人运动学动力学求解器、MPC力控制器、PVT关节控制器、机器人状态数据总线和数据日志记录器；获取机器人关节总自由度并定义双腿关节力矩指令矩阵；完成GLFW可视化库初始化、机器人刚体视角跟踪启用及仿真演示窗口创建，为仿真运行提供组件支撑。
python
int main(int argc, char **argv) {
// ===================== 初始化各类核心模块 =====================
// UI控制器（用于MuJoCo可视化）
UIctr uiController(mj_model, mj_data);
// MuJoCo数据接口（传感器/执行器数据交互）
MJ_Interface mj_interface(mj_model, mj_data);
// 机器人运动学/动力学求解器（加载URDF模型）
Pin_KinDyn kinDynSolver("../models/AzureLoong.urdf");
// MPC力控制器（输入控制时间步）
MPC mpc_force(dt);
// PVT关节控制器（输入仿真步长+关节配置文件）
PVT_Ctr pvtCtr(mj_model->opt.timestep, "../common/joint_ctrl_config.json");
// 机器人状态数据总线（输入关节自由度）
DataBus RobotState(kinDynSolver.model_nv);
// 数据日志记录器（指定日志文件路径）
DataLogger logger("../record/datalog.log");
// 获取机器人关节总自由度（减去浮动基6个自由度）
int model_nv = kinDynSolver.model_nv;
// 关节力矩指令（12维，对应双腿关节）
Eigen::Matrix<double, 12, 1> Uje;
// ===================== 初始化GLFW可视化窗口 =====================
uiController.iniGLFW(); // 初始化GLFW库
uiController.enableTracking(); // 启用机器人第1个刚体的视角跟踪
// 创建仿真窗口（参数：窗口名、是否保存视频；注：保存视频时15秒原始文件可达2.5GB）
uiController.createWindow("机器人跳跃演示", false);（3）下面代码的功能是配置数据日志记录的各类关键参数项，涵盖仿真时间、电机状态、机器人位姿、足端位置/力指令等维度；初始化机器人足端和手部在机体坐标系下的期望位姿（位置、欧拉角、旋转矩阵），求解腿和手关节的逆运动学；初始化跳跃控制核心参数，包括起跳时间、准备时长、跳跃状态机、站立/跳跃高度等，为分阶段跳跃控制做好参数和数据记录准备。
python
// ===================== 初始化数据日志项 =====================
logger.addIterm("simTime", 1); // 仿真时间（1维）
logger.addIterm("motor_pos_des", model_nv - 6); // 电机期望位置（关节自由度-6）
logger.addIterm("motor_pos_cur", model_nv - 6); // 电机当前位置
logger.addIterm("motor_vel_cur", model_nv - 6); // 电机当前速度
logger.addIterm("motor_tor_des", model_nv - 6); // 电机期望扭矩
logger.addIterm("motor_tor_out", model_nv - 6); // 电机输出扭矩
logger.addIterm("rpyVal", 3); // 机器人基姿滚转/俯仰/偏航（3维）
logger.addIterm("gpsVal", 3); // 机器人基位置（3维）
logger.addIterm("fe_l_pos_L_des", 3); // 左脚端期望位置（机体坐标系）
logger.addIterm("fe_r_pos_L_des", 3); // 右脚端期望位置（机体坐标系）
logger.addIterm("fe_l_pos_W", 3); // 左脚端当前位置（世界坐标系）
logger.addIterm("fe_r_pos_W", 3); // 右脚端当前位置（世界坐标系）
logger.addIterm("Ufe", 12); // 足端力/力矩指令（12维）
logger.finishItermAdding(); // 完成日志项添加
// ===================== 初始化足端/手部期望位姿 =====================
// 足端期望位置（世界坐标系）
Eigen::Vector3d fe_l_pos_W_des, fe_r_pos_W_des;
// 左脚端期望位置（机体坐标系，初始值）
Eigen::Vector3d fe_l_pos_L_des = {-0.018, 0.113, -1.01};
// 右脚端期望位置（机体坐标系，初始值）
Eigen::Vector3d fe_r_pos_L_des = {-0.018, -0.116, -1.01};
// 左脚端期望欧拉角（机体坐标系）
Eigen::Vector3d fe_l_eul_L_des = {-0.000, -0.008, -0.000};
// 右脚端期望欧拉角（机体坐标系）
Eigen::Vector3d fe_r_eul_L_des = {0.000, -0.008, 0.000};
// 欧拉角转旋转矩阵（左脚端期望姿态）
Eigen::Matrix3d fe_l_rot_des = eul2Rot(fe_l_eul_L_des(0), fe_l_eul_L_des(1), fe_l_eul_L_des(2));
// 欧拉角转旋转矩阵（右脚端期望姿态）
Eigen::Matrix3d fe_r_rot_des = eul2Rot(fe_r_eul_L_des(0), fe_r_eul_L_des(1), fe_r_eul_L_des(2));
// 足端基准位置（机体坐标系）
Eigen::Vector3d fe_l_pos_L_base, fe_r_pos_L_base;
// 左手端期望位置（机体坐标系）
Eigen::Vector3d hd_l_pos_L_des = {-0.02, 0.32, -0.159};
// 右手端期望位置（机体坐标系）
Eigen::Vector3d hd_r_pos_L_des = {-0.02, -0.32, -0.159};
// 左手端期望欧拉角（机体坐标系）
Eigen::Vector3d hd_l_eul_L_des = {-1.253, 0.122, -1.732};
// 右手端期望欧拉角（机体坐标系）
Eigen::Vector3d hd_r_eul_L_des = {1.253, 0.122, 1.732};
// 欧拉角转旋转矩阵（左手端期望姿态）
Eigen::Matrix3d hd_l_rot_des = eul2Rot(hd_l_eul_L_des(0), hd_l_eul_L_des(1), hd_l_eul_L_des(2));
// 欧拉角转旋转矩阵（右手端期望姿态）
Eigen::Matrix3d hd_r_rot_des = eul2Rot(hd_r_eul_L_des(0), hd_r_eul_L_des(1), hd_r_eul_L_des(2));
// 求解腿关节逆运动学（输入足端期望姿态/位置）
auto resLeg = kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des);
// 求解手关节逆运动学（输入手端期望姿态/位置）
auto resHand = kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des);
// ===================== 跳跃控制参数初始化 =====================
double startJumpingTime = 8.5; // 起跳开始时间（秒）
double prepareTime = 3; // 准备阶段时长（秒）
uint16_t jump_state = 0; // 跳跃状态机：0-起跳 3-上升 4-下降 5-落地
double stand_z = -0.8; // 站立时足端z轴位置（机体坐标系）
double jump_z = 0.2; // 跳跃高度（米）
double count = 0.0; // 落地阶段计时
mjtNum simstart = mj_data->time; // 仿真起始时间
double simTime = mj_data->time; // 当前仿真时间
double simEndTime = 13; // 仿真结束时间（4）下面代码的功能是实现机器人跳跃仿真的主循环逻辑，外层循环保证可视化窗口未关闭时持续运行，内层循环控制60Hz可视化刷新率并推进MuJoCo仿真步；先更新传感器数据、计算运动学/动力学信息及足端接触力，再按时间和状态机分阶段执行跳跃控制（准备、起跳准备、起跳加速、上升、下降、落地），完成MPC力控指令转关节扭矩、PVT关节控制计算及扭矩指令下发至MuJoCo，同时记录每一步的关键仿真数据，是跳跃控制的核心执行逻辑。
python
// ===================== 主仿真循环 =====================
while (!glfwWindowShouldClose(uiController.window)) {
simstart = mj_data->time;
// 保证60Hz的可视化刷新率（内部按MuJoCo步长推进仿真）
while (mj_data->time - simstart < 1.0 / 60.0 && uiController.runSim) {
mj_step(mj_model, mj_data); // 推进MuJoCo仿真一步
simTime = mj_data->time; // 更新当前仿真时间
// --------------------- 传感器数据更新 ---------------------
mj_interface.updateSensorValues(); // 读取MuJoCo传感器数据
mj_interface.dataBusWrite(RobotState); // 将传感器数据写入数据总线
// --------------------- 运动学/动力学计算 ---------------------
kinDynSolver.dataBusRead(RobotState); // 从数据总线读取机器人状态
kinDynSolver.computeJ_dJ(); // 计算雅克比矩阵及雅克比导数
kinDynSolver.computeDyn(); // 计算动力学（惯性矩阵/科氏力等）
kinDynSolver.dataBusWrite(RobotState); // 将计算结果写入数据总线
// 构建站立时的雅克比矩阵（12维：双腿各6维）
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Jac_stand(12, 12);
Jac_stand.setZero();
Jac_stand.block(0, 0, 6, 12) = RobotState.J_l.block(0, model_nv-12, 6, 12);
Jac_stand.block(6, 0, 6, 12) = RobotState.J_r.block(0, model_nv-12, 6, 12);
// 提取关节扭矩（排除浮动基6个自由度）
Eigen::VectorXd torJoint;
torJoint = Eigen::VectorXd::Zero(model_nv-6);
for (int i = 0; i < model_nv-6; i++) {
torJoint[i] = RobotState.motors_tor_cur[i];
}
// 估计左右足端接触力（基于动力学和雅克比矩阵）
Eigen::Vector<double,6> FLest, FRest;
Eigen::VectorXd tauAll;
tauAll = Eigen::VectorXd::Zero(model_nv);
tauAll.block(6,0,model_nv-6,1) = torJoint;
FLest = -pseudoInv_SVD(RobotState.J_l * RobotState.dyn_M.inverse() * RobotState.J_l.transpose())
* (RobotState.J_l * RobotState.dyn_M.inverse() * (tauAll - RobotState.dyn_Non) + RobotState.dJ_l * RobotState.dq);
FRest = -pseudoInv_SVD(RobotState.J_r * RobotState.dyn_M.inverse() * RobotState.J_r.transpose())
* (RobotState.J_r * RobotState.dyn_M.inverse() * (tauAll - RobotState.dyn_Non) + RobotState.dJ_r * RobotState.dq);
// --------------------- 分阶段控制逻辑 ---------------------
if (simTime <= prepareTime) {
// 阶段1：准备阶段（前3秒）- 保持初始位姿
fe_l_pos_L_des = RobotState.fe_l_pos_L; // 左脚端跟随当前位置
fe_r_pos_L_des = RobotState.fe_r_pos_L; // 右脚端跟随当前位置
// 足端期望位置（世界坐标系）= 基姿旋转 * 机体坐标系位置
fe_l_pos_W_des = RobotState.base_rot * fe_l_pos_L_des;
fe_r_pos_W_des = RobotState.base_rot * fe_r_pos_L_des;
// 设置电机期望位置（逆解结果）、速度/扭矩为0
RobotState.motors_pos_des = eigen2std(resLeg.jointPosRes + resHand.jointPosRes);
RobotState.motors_vel_des.assign(model_nv - 6, 0);
RobotState.motors_tor_des.assign(model_nv - 6, 0);
} else if (simTime < startJumpingTime && simTime > prepareTime) {
// 阶段2：起跳准备（3~8.5秒）- 足端z轴下降到站立位置
fe_l_pos_L_des(2) = Ramp(fe_l_pos_L_des(2), stand_z, 0.1 * dt);
fe_r_pos_L_des(2) = Ramp(fe_r_pos_L_des(2), stand_z, 0.1 * dt);
// 重新求解腿/手逆运动学
auto resLeg = kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des);
auto resHand = kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des);
// 记录站立时的基准位置
RobotState.base_pos_stand = RobotState.base_pos;
RobotState.pfeW_stand.block<3, 1>(0, 0) = fe_l_pos_W_des;
RobotState.pfeW_stand.block<3, 1>(3, 0) = fe_r_pos_W_des;
// 设置电机期望位置，速度/扭矩为0
RobotState.motors_pos_des = eigen2std(resLeg.jointPosRes + resHand.jointPosRes);
RobotState.motors_vel_des.assign(model_nv - 6, 0);
RobotState.motors_tor_des.assign(model_nv - 6, 0);
// 跟踪当前基姿欧拉角/位置/角速度/线速度
for (int j = 0; j < 3; j++) RobotState.js_eul_des(j) = RobotState.base_rpy(j);
for (int j = 0; j < 3; j++) RobotState.js_pos_des(j) = RobotState.base_pos(j);
for (int j = 0; j < 3; j++) RobotState.js_omega_des(j) = RobotState.base_omega_W(j);
for (int j = 0; j < 3; j++) RobotState.js_vel_des(j) = RobotState.dq(j);
RobotState.legState = DataBus::DSt; // 设置腿部状态为"站立"
} else if (simTime >= startJumpingTime) {
// 阶段3：跳跃阶段（8.5秒后）- 状态机控制
// 计算期望跳跃速度（z轴：基于自由落体公式 v=√(2gh)）
double jump_vel_des[3] = {0.0, 0.0, sqrt(2.0 * 9.8 * jump_z)};
// 起跳加速度时间（达到期望速度的时间）
double jump_acc_t = 2.0 * (0.9 + stand_z) / (jump_vel_des[2]);
// 起跳期望欧拉角（小角度俯仰）
double jump_eul_des[3] = {0.0, 0.0 / 180.0 * 3.1415926, 0.0};
if (jump_state == 0) {
// 子状态0：起跳加速
mpc_force.enable(); // 启用MPC力控制
// 设置MPC权重矩阵（状态权重L/输入权重K）
Eigen::Matrix<double, 1, nx> L_diag;
Eigen::Matrix<double, 1, nu> K_diag;
L_diag << 50.0, 50.0, 1.0, // 欧拉角权重
50.0, 50.0, 200.0, // CoM位置权重
0.1, 0.1 , 0.1, // 角速度权重
0.01, 0.1, 20.0; // CoM速度权重
K_diag << 1.0, 1.0, 0.1, // 左脚力权重
10.0, 10.0, 10.0, // 左脚力矩权重
1.0, 1.0, 0.1, // 右脚力权重
10.0, 10.0, 10.0, 1.0; // 右脚力矩权重
mpc_force.set_weight(1e-6, L_diag, K_diag);
// 设置期望欧拉角（起跳姿态）
RobotState.js_eul_des(0) = jump_eul_des[0];
RobotState.js_eul_des(1) = jump_eul_des[1];
RobotState.js_eul_des(2) = jump_eul_des[2];
// 斜坡函数逼近期望跳跃速度
RobotState.js_vel_des(0) = Ramp(RobotState.js_vel_des(0), jump_vel_des[0],
fabs(jump_vel_des[0] / (jump_acc_t) * dt));
RobotState.js_vel_des(1) = 0.0;
RobotState.js_vel_des(2) = Ramp(RobotState.js_vel_des(2), jump_vel_des[2],
fabs(jump_vel_des[2] / jump_acc_t * dt));
// 积分更新期望位置
RobotState.js_pos_des(2) = RobotState.js_pos_des(2) + RobotState.js_vel_des(2) * dt;
// 足端位置跟随当前基姿
fe_l_pos_L_des = RobotState.base_rot * RobotState.fe_l_pos_L;
fe_r_pos_L_des = RobotState.base_rot * RobotState.fe_r_pos_L;
// 加速时间到，切换到上升阶段
if (simTime > startJumpingTime + jump_acc_t) {
jump_state = 3;
mpc_force.disable(); // 关闭MPC
// 记录当前足端位置作为基准
RobotState.pfeW0.block<3, 1>(0, 0) = fe_l_pos_W_des;
RobotState.pfeW0.block<3, 1>(3, 0) = fe_r_pos_W_des;
}
} else if (jump_state == 3) {
// 子状态3：上升阶段 - 足端z轴回落至站立位置
mpc_force.disable(); // 关闭MPC
// 斜坡函数回落足端z轴位置
fe_l_pos_W_des[2] = Ramp(fe_l_pos_W_des[2], stand_z, 5.0 * dt);
fe_r_pos_W_des[2] = Ramp(fe_r_pos_W_des[2], stand_z, 5.0 * dt);
// 世界坐标系转机体坐标系
fe_l_pos_L_des = RobotState.base_rot.inverse() * fe_l_pos_W_des;
fe_r_pos_L_des = RobotState.base_rot.inverse() * fe_r_pos_W_des;
// 逆运动学求解（补偿基姿俯仰角）
auto resLeg = kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des);
auto resHand = kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des);
Eigen::Matrix<double, 31, 1> IKRes;
IKRes = resLeg.jointPosRes + resHand.jointPosRes;
IKRes(model_nv-6 - 8) += RobotState.base_rpy(1); // 补偿左腿关节
IKRes(model_nv-6 - 2) += RobotState.base_rpy(1); // 补偿右腿关节
// 上升速度降低到阈值，切换到下降阶段
if (RobotState.dq(2) < 0.1) {
jump_state = 4;
RobotState.pfeW0.block<3, 1>(0, 0) = RobotState.base_rot * RobotState.fe_l_pos_L;
RobotState.pfeW0.block<3, 1>(3, 0) = RobotState.base_rot * RobotState.fe_r_pos_L;
}
// 启用PVT位置/速度控制，设置电机指令
pvtCtr.enablePV();
RobotState.motors_pos_des = eigen2std(IKRes);
RobotState.motors_vel_des.assign(model_nv - 6, 0);
RobotState.motors_tor_des.assign(model_nv - 6, 0);
} else if (jump_state == 4) {
// 子状态4：下降阶段 - 足端x轴前伸，准备落地
mpc_force.disable(); // 关闭MPC
// 足端x轴前伸0.2米
fe_l_pos_W_des[0] = Ramp(fe_l_pos_W_des[0], RobotState.pfeW0[0] + 0.2, fabs(10.0 * dt));
fe_r_pos_W_des[0] = Ramp(fe_r_pos_W_des[0], RobotState.pfeW0[3] + 0.2, fabs(10.0 * dt));
// 世界坐标系转机体坐标系（仅更新x轴）
Eigen::Vector3d fe_l_pos_L_des_tmp, fe_r_pos_L_des_tmp;
fe_l_pos_L_des_tmp = RobotState.base_rot.inverse() * fe_l_pos_W_des;
fe_r_pos_L_des_tmp = RobotState.base_rot.inverse() * fe_r_pos_W_des;
fe_l_pos_L_des[0] = fe_l_pos_L_des_tmp[0];
fe_r_pos_L_des[0] = fe_r_pos_L_des_tmp[0];
// 逆运动学求解（补偿基姿俯仰角）
auto resLeg = kinDynSolver.computeInK_Leg(fe_l_rot_des, fe_l_pos_L_des, fe_r_rot_des, fe_r_pos_L_des);
auto resHand = kinDynSolver.computeInK_Hand(hd_l_rot_des, hd_l_pos_L_des, hd_r_rot_des, hd_r_pos_L_des);
Eigen::MatrixXd IKRes;
IKRes = resLeg.jointPosRes + resHand.jointPosRes;
IKRes(model_nv-6 - 8) += RobotState.base_rpy(1);
IKRes(model_nv-6 - 2) += RobotState.base_rpy(1);
// 足端接触力超过阈值，切换到落地阶段
if (FLest(2) > 1000 && FRest(2) > 1000) {
jump_state = 5;
RobotState.pfeW0.block<3, 1>(0, 0) = RobotState.base_rot * RobotState.fe_l_pos_L;
RobotState.pfeW0.block<3, 1>(3, 0) = RobotState.base_rot * RobotState.fe_r_pos_L;
RobotState.js_pos_des(0) = RobotState.base_pos(0);
RobotState.js_pos_des(1) = RobotState.base_pos(1);
RobotState.js_pos_des(2) = RobotState.base_pos(2);
}
// 启用PVT控制，设置电机指令
pvtCtr.enablePV();
RobotState.motors_pos_des = eigen2std(IKRes);
RobotState.motors_vel_des.assign(model_nv - 6, 0);
RobotState.motors_tor_des.assign(model_nv - 6, 0);
} else if (jump_state == 5) {
// 子状态5：落地阶段 - MPC控制保持平衡
mpc_force.enable(); // 启用MPC
// 重新设置MPC权重（落地阶段更关注位置/力控制）
Eigen::Matrix<double, 1, nx> L_diag;
Eigen::Matrix<double, 1, nu> K_diag;
L_diag << 2.0, 10.0, 1.0, // 欧拉角权重
100.0, 100.0, 200.0, // CoM位置权重
1e-4, 1e-4, 1e-4, // 角速度权重
0.5, 0.01, 0.5; // CoM速度权重
K_diag << 0.1, 0.1, 0.1, // 左脚力权重
0.01, 0.01, 1.0, // 左脚力矩权重
0.1, 0.1, 0.1, // 右脚力权重
0.01, 0.01, 1.0, 1.0;// 右脚力矩权重
mpc_force.set_weight(1e-6, L_diag, K_diag);
// 落地阶段期望位置调整，姿态/速度归零
double tt = 0.4;
RobotState.js_pos_des(2) = Ramp(RobotState.js_pos_des(2), 1.08, 0.1 * dt);
RobotState.js_eul_des.setZero();
RobotState.js_omega_des.setZero();
RobotState.js_vel_des.setZero();
// 落地计时
if (count < tt / dt) count = count + 1.0;
}
}
// --------------------- MPC力控转关节扭矩 ---------------------
mpc_force.dataBusRead(RobotState); // 读取机器人状态到MPC
mpc_force.cal(); // 执行MPC计算
mpc_force.dataBusWrite(RobotState); // 将MPC输出写入数据总线
if (mpc_force.get_ENA()) {
// MPC启用时：足端力指令转关节扭矩
Uje.setZero();
Uje = Jac_stand.transpose() * (-1.0) * RobotState.fe_react_tau_cmd.block<nu - 1, 1>(0, 0);
// 关节扭矩限幅（保护电机）
double jTor_max[6] = {400.0, 100.0, 400.0, 400.0, 80.0, 20.0};
double jTor_min[6] = {-400.0, -100.0, -400.0, -400.0, -80.0, -20.0};
for (int i = 0; i < 6; i++) {
Limit(Uje(i), jTor_max[i], jTor_min[i]); // 左腿扭矩限幅
Limit(Uje(i + 6), jTor_max[i], jTor_min[i]);// 右腿扭矩限幅
}
// 禁用对应关节的PVT位置控制，直接下发MPC扭矩指令
for (int i = 0; i < 12; i++) {
pvtCtr.disablePV(model_nv-6-12 + i);
RobotState.motors_tor_des[model_nv-6-12 + i] = Uje(i);
}
} else {
// MPC禁用时：足端力指令归零
RobotState.fe_react_tau_cmd.setZero();
}
// --------------------- PVT关节控制器 ---------------------
pvtCtr.dataBusRead(RobotState); // 读取期望/当前关节状态
if (simTime <= startJumpingTime) {
// 起跳前：低增益PVT控制（防止震荡）
pvtCtr.calMotorsPVT(110.0 / 1000.0 / 180.0 * 3.1415);
} else {
// 跳跃阶段：默认增益PVT控制
pvtCtr.calMotorsPVT();
}
pvtCtr.dataBusWrite(RobotState); // 将PVT输出扭矩写入数据总线
// --------------------- 执行器指令下发 ---------------------
mj_interface.setMotorsTorque(RobotState.motors_tor_out); // 扭矩指令下发到MuJoCo
// --------------------- 数据日志记录 ---------------------
logger.startNewLine(); // 新建日志行
logger.recItermData("simTime", simTime);
logger.recItermData("motor_pos_des", RobotState.motors_pos_des);
logger.recItermData("motor_pos_cur", RobotState.motors_pos_cur);
logger.recItermData("motor_vel_cur", RobotState.motors_vel_cur);
logger.recItermData("motor_tor_des", RobotState.motors_tor_des);
logger.recItermData("motor_tor_out", RobotState.motors_tor_out);
logger.recItermData("rpyVal", RobotState.rpy);
logger.recItermData("gpsVal", RobotState.base_pos);
logger.recItermData("fe_l_pos_L_des", fe_l_pos_L_des);
logger.recItermData("fe_r_pos_L_des", fe_r_pos_L_des);
logger.recItermData("fe_l_pos_W", RobotState.fe_l_pos_W);
logger.recItermData("fe_r_pos_W", RobotState.fe_r_pos_W);
logger.recItermData("Ufe", RobotState.fe_react_tau_cmd.block<nu - 1, 1>(nu * 0, 0));
logger.finishLine(); // 结束当前日志行
}
// 仿真时间到达结束时间，退出循环
if (mj_data->time >= simEndTime) break;
// 更新可视化场景
uiController.updateScene();
}