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 mujoco
import numpy as np
import mujoco.viewer
import numpy as np
import casadi_ik
import time

SCENE_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 = 3
viewer.cam.azimuth = 0
viewer.cam.elevation = -30
viewer.run_loop()下·

变化位置 Z