运动控制算法
'OpenLoong-Dyn-Control'项目提供了一套基于 MPC（模型预测控制）和 WBC（全身体控制）的仿人机器人运动控制框架，可以部署在 Mujoco 仿真平台上。该项目基于上海人形机器人创新中心的青龙'机器人模型，提供了行走、跳跃、盲踩障碍物等运动示例，且实物样机已实现行走和盲踩障碍功能。其具有易部署（包含主要依赖，简化环境配置）、可扩展（分层模块化设计，便于二次开发）、易理解（代码结构简洁，采用'读取 - 计算 - 写入'逻辑）等特点。
机器人的站立与行走
文件 OpenLoong-Dyn-Control/demo/walk_wbc.cpp 是基于 MuJoCo 的双足机器人仿真控制程序，实现机器人从站立到行走的过程。加载模型并初始化 UI 控制器、动力学求解器、WBC 优先级控制器、步态调度器等模块，通过仿真循环推进时间步。循环中更新传感器数据与机器人状态，经状态估计、运动学动力学计算后，由 WBC 求解关节控制量，结合 PVT 控制生成力矩指令。还包含足端放置规划、期望速度生成，记录仿真数据（如关节状态、基态信息等），并支持可视化交互，实现机器人行走控制与仿真过程监控。
（1）下面代码的功能是创建错误信息缓冲区并初始化默认提示文本，调用 MuJoCo 的 mj_loadXML 函数加载指定路径的仿真场景 XML 模型文件，同时传入错误缓冲区捕获加载异常；基于加载完成的 mjModel 模型实例，调用 mj_makeData 创建对应的 mjData 仿真数据结构，该结构用于存储仿真过程中关节位置、力等动态数据，为后续机器人行走仿真搭建基础的模型和数据载体。
// 错误信息缓冲区，初始值为"无法加载二进制模型"
char error[1000] = "无法加载二进制模型";
// 加载 MuJoCo 仿真场景的 XML 模型文件，参数：模型路径、虚拟文件系统、错误缓冲区、缓冲区长度
mjModel* mj_model = mj_loadXML("../models/scene_board.xml", 0, error, 1000);
// 创建与模型对应的仿真数据结构，存储仿真过程中的动态数据（如关节位置、力等）
mjData* mj_data = mj_makeData(mj_model);
（2）下面代码的功能是作为程序入口定义主函数，用于实例化机器人行走仿真所需的各类核心控制模块（包括 UI 控制器、数据接口、运动学动力学求解器、WBC 控制器、步态调度器等）；初始化机器人站立腿长、期望前进速度等物理参数与步态规划参数，定义足端/手部期望位姿变量并求解腿部逆运动学，初始化 WBC 控制器初始关节位置；为数据日志记录器注册仿真时间、电机状态、基链接位姿等需记录的变量及维度，完成仿真前全量参数与模块的初始化工作。
//************************
// 主函数：程序入口
int main(int argc, const char** argv) {
// 初始化各类核心控制类实例
UIctr uiController(mj_model,mj_data); // MuJoCo 的 UI 控制器（负责仿真可视化、交互）
MJ_Interface mj_interface(mj_model, mj_data); // MuJoCo 数据接口（读取传感器数据、设置执行器指令）
;
;
;
;
;
FootPlacement footPlacement;
;
;
;
stand_legLength = ;
foot_height = ;
xv_des = ;
RobotState.width_hips = ;
footPlacement.kp_vx = ;
footPlacement.kp_vy = ;
footPlacement.kp_wz = ;
footPlacement.stepHeight = ;
footPlacement.legLength=stand_legLength;
model_nv=kinDynSolver.model_nv;
;
;
;
;
;
;
Eigen::Vector3d fe_l_pos_L_des={, , -stand_legLength};
Eigen::Vector3d fe_r_pos_L_des={, , -stand_legLength};
Eigen::Vector3d fe_l_eul_L_des={, , };
Eigen::Vector3d fe_r_eul_L_des={, , };
Eigen::Matrix3d fe_l_rot_des= ((),(),());
Eigen::Matrix3d fe_r_rot_des= ((),(),());
Eigen::Vector<, > hd_l_des{, , , , , , };
Eigen::Vector<, > hd_r_des{, , , , , , };
resLeg=kinDynSolver.(fe_l_rot_des,fe_l_pos_L_des,fe_r_rot_des,fe_r_pos_L_des);
Eigen::VectorXd qIniDes=Eigen::VectorXd::(mj_model->nq,);
qIniDes.(, , mj_model->nq - , ) = resLeg.jointPosRes;
qIniDes.(, , , ) = hd_l_des;
qIniDes.(, , , ) = hd_r_des;
WBC_solv.(qIniDes,RobotState.q);
logger.(, );
logger.(,model_nv);
logger.(,model_nv);
logger.(,);
logger.(,);
logger.(,);
logger.(,);
logger.(,);
logger.(,);
logger.(,);
logger.();
}
