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

视频链接：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 smooth self.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 goals self.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 mujocoimport numpy as npimport mujoco.viewerimport numpy as npimport casadi_ikimport 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.3 self.y = 0.0 self.z = 0.1 self.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() @property def cam(self): return self.handle.cam @property def viewport(self): return self.handle.viewport def run_loop(self): status = 0 while self.is_running(): mujoco.mj_forward(model, data) theta = np.pi self.z = self.z + 0.001 if self.z > 0.3: self.z = 0.3 print(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 = 3viewer.cam.azimuth = 0viewer.cam.elevation = -30viewer.run_loop()下·

变化位置 Z