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

Pinocchio 结合 CasADi 进行 IK 逆运动学及 Mujoco 仿真

news 2025/7/28 13:53:38

视频链接：Pinocchio 结合 CasADi 进行 IK 逆运动学及 Mujoco 仿真_哔哩哔哩_bilibili

代码仓库：GitHub - LitchiCheng/mujoco-learning

上期我们讲到如何安装 Pinocchio 集成 CasADi 的版本，今天我们结合 so100 这个机械臂进行 ik 应用。ik 部分代码参考 github 有如下：

xr_teleoperate/teleop/robot_control/robot_arm_ik.py at main · unitreerobotics/xr_teleoperate

mocap_retarget/src/mocap/src/robot_ik.py at master · ccrpRepo/mocap_retarget

其主要利用 CasADi 构建优化问题，优化目标为最小化位姿误差，如下：

self.translational_error = casadi.Function("translational_error",[self.cq, self.cTf],[casadi.vertcat(self.cdata.oMf[self.ee_id].translation - self.cTf[:3,3])],)self.rotational_error = casadi.Function("rotational_error",[self.cq, self.cTf],[casadi.vertcat(cpin.log3(self.cdata.oMf[self.ee_id].rotation @ self.cTf[:3,:3].T))],)

通过 IPOPT 进行求解，将模型导入和关节数量等进行了优化，只需要导入 URDF 或 MJCF 模型路径即可

     self.opti = casadi.Opti()self.var_q = self.opti.variable(self.model.nq)self.var_q_last = self.opti.parameter(self.model.nq)   # for smoothself.param_tf = self.opti.parameter(4, 4)self.translational_cost = casadi.sumsqr(self.translational_error(self.var_q, self.param_tf))self.rotation_cost = casadi.sumsqr(self.rotational_error(self.var_q, self.param_tf))self.regularization_cost = casadi.sumsqr(self.var_q)self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last)# Setting optimization constraints and goalsself.opti.subject_to(self.opti.bounded(self.model.lowerPositionLimit,self.var_q,self.model.upperPositionLimit))self.opti.minimize(10 * self.translational_cost + 1.0*self.rotation_cost + 0.0 * self.regularization_cost + 0.0 * self.smooth_cost)##### IPOPT #####opts = {'ipopt':{'print_level': 0,'max_iter': 20,'tol': 1e-4,},'print_time':True,# print or not'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F}self.opti.solver("ipopt", opts)

最后我们用其测试下so_arm100 机械臂进行 ik 在 Mojuco 中的效果，完整测试代码如下：

import mujoco
import numpy as np
import mujoco.viewer
import numpy as np
import casadi_ik
import timeSCENE_XML_PATH = 'model/trs_so_arm100/scene.xml'
ARM_XML_PATH = 'model/trs_so_arm100/so_arm100.xml'model = mujoco.MjModel.from_xml_path(SCENE_XML_PATH)
data = mujoco.MjData(model)class CustomViewer:def __init__(self, model, data):self.handle = mujoco.viewer.launch_passive(model, data)self.x = 0.3self.y = 0.0self.z = 0.1self.arm = casadi_ik.Kinematics("Jaw")self.arm.buildFromMJCF(ARM_XML_PATH)def is_running(self):return self.handle.is_running()def sync(self):self.handle.sync()@propertydef cam(self):return self.handle.cam@propertydef viewport(self):return self.handle.viewportdef run_loop(self):status = 0while self.is_running():mujoco.mj_forward(model, data)theta = np.piself.z = self.z + 0.001if self.z > 0.3:self.z = 0.3print(f"Current position: x={self.x}, y={self.y}, z={self.z}")tf = np.array([[1, 0, 0, self.x],[0, np.cos(theta), -np.sin(theta), self.y],[0, np.sin(theta), np.cos(theta), self.z],])tf = np.vstack((tf, [0, 0, 0, 1]))self.dof, info = self.arm.ik(tf)print(f"DoF: {self.dof}, Info: {info}")data.qpos[:6] = self.dof[:6]mujoco.mj_step(model, data)self.sync()time.sleep(0.01)viewer = CustomViewer(model, data)
viewer.cam.distance = 3
viewer.cam.azimuth = 0
viewer.cam.elevation = -30
viewer.run_loop()下·