MuJoCo 机械臂仿真实战：传感器数据获取及使用、机械臂进行 IK 和 FK 运动学控制末端移动

Mujoco 仿真中传感器数据（SensorData）获取及使用

原文链接：

https://www.eeworld.com.cn/anDafX5

代码链接：

https://github.com/LitchiCheng/mujoco-learning

在 Mujoco 中进行 RL 或者仿真，针对一个body或者一些关节甚至是交互数据，如力、速度等，部分可以通过计算或者直接读取 data 获得，但 Mujoco 已经提供了较为方便的传感器类型及方式。

mjData->sensordata 传感器数据数组，存储所有仿真传感器的当前读数

内置多种传感器，其数据都会存储在 sensordata 中：

关节位置/速度传感器（JOINTPOS, JOINTVEL）

陀螺仪/加速度计（GYRO, ACCELEROMETER）

力/力矩传感器（FORCE, TORQUE）

触摸传感器（TOUCH）

磁场传感器（MAGNETOMETER）

摄像头/深度传感器（通过 CAMERA 相关机制）

用户自定义传感器（USER）

更具体传感器及xml参考，可以 refer

https://mujoco.readthedocs.io/en/stable/XMLreference.html#sensor

修改下mujoco-learning工程中

model/franka_emika_panda/panda_vel.xml，增加一个sensor记录下joint1的关节速度

<sensor>     <jointvel name="joint1_vel" joint="joint1"/>   </sensor>  

测试的代码，通过控制关节1的变化，同时监控关节1的传感器数据，以及使用plot进行在线绘图观察

import src.mujoco_viewer as mujoco_viewer import src.matplot as mp classPandaEnv(mujoco_viewer.CustomViewer):   def__init__(self, path):       super().__init__(path, 3, azimuth=-45, elevation=-30)       self.path = path   defgetSensorDataByName(self, sensor_name):         sensor_id =self.model.sensor(sensor_name).id         sensor_type =self.model.sensor(sensor_name).type         sensor_dim =self.model.sensor(sensor_name).dim[0]       # 获取数据在数组中的起始位置         adr =self.model.sensor_adr[sensor_id]       # 读取相应维度的数据         sensor_values =self.data.sensordata[adr:adr+sensor_dim]       returnsensor_values, sensor_type, sensor_dim   defrunBefore(self):       self.initial_pos =self.model.key_qpos[0].copy()       foriinrange(self.model.nq):           self.data.qpos[i] =self.initial_pos[i]       self.plot_manager = mp.MultiChartRealTimePlotManager()       self.plot_manager.addNewFigurePlotter("j1_sensor_vel", "j1_sensor_vel", row=0, col=0)       self.plot_manager.addNewFigurePlotter("j1_qvel", "j1_qvel", row=1, col=0)   defrunFunc(self):         current_joint1_vel,_,_ =self.getSensorDataByName("joint1_vel")       self.plot_manager.updateDataToPlotter("j1_sensor_vel", "j1_sensor_vel", current_joint1_vel[0])       self.plot_manager.updateDataToPlotter("j1_qvel", "j1_qvel",self.data.qvel[0]) if__name__ == "__main__":     env = PandaEnv("./model/franka_emika_panda/scene_pos.xml")     env.run_loop()

Mujoco 使用

KDL 对机械臂进行 IK 和 FK 运动学控制末端移动

原贴链接：

https://www.eeworld.com.cn/avzfPaL

代码仓库：

https://github.com/LitchiCheng/mujoco-learning

前面介绍过通过 UV 进行安装PyKDL的方法以及一些注意事项，今天我们用 KDL 来进行机械臂IK的测试，来控制 Franka机械臂的末端 ee_center_body 沿着 X 方向进行移动，需要注意的是，Mujoco显示需要使用xml，KDL目前只支持URDF，可以参考我的其他分享，如何将MJCF和URDF手动对齐。

需要提前创建好各个求解器，求解器也有很多种，这里使ChainIkSolverPos_NR，需要设置迭代次数和收敛精度阈值。

defcreateSolver(self):      self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.chain)      self.ik_vel_solver = PyKDL.ChainIkSolverVel_pinv(self.chain)        max_iterations = 500        eps = 1e-6      self.ik_pos_solver = PyKDL.ChainIkSolverPos_NR(          self.chain,self.fk_solver,self.ik_vel_solver, max_iterations, eps        )      self.jac_solver = PyKDL.ChainJntToJacSolver(self.chain)

如下为ik函数，可以传入当前关节位置，防止关节突变等，传入的tf为4x4的齐次变换矩阵的位姿。

   def ik(self, tf , current_arm_motor_q = None, current_arm_motor_dq = None):        num_joints =self.chain.getNrOfJoints()      ifcurrent_arm_motor_q is None:            q_init = PyKDL.JntArray(num_joints)      else:            q_init = PyKDL.JntArray(num_joints)          foriinrange(num_joints):                q_init[i] = current_arm_motor_q[i]        q_out = PyKDL.JntArray(num_joints)        ## 提取平移向量（前3行第4列）        trans = PyKDL.Vector(tf[0, 3], tf[1, 3], tf[2, 3])        ## 提取旋转矩阵（前3行前3列），构造PyKDL.Rotation        # Rotation构造参数：xx, xy, xz, yx, yy, yz, zx, zy, zz（行优先展开）        rot = PyKDL.Rotation(            tf[0, 0], tf[0, 1], tf[0, 2],            tf[1, 0], tf[1, 1], tf[1, 2],            tf[2, 0], tf[2, 1], tf[2, 2]        )        frame = PyKDL.Frame(rot, trans)        dof = []        status =self.ik_pos_solver.CartToJnt(q_init, frame, q_out)      ifstatus >= 0:            dof = [q_out[i]foriinrange(num_joints)]            info = {"success": True}      else:            dof = q_init            info = {"success": False, "error_code": status}      returndof, info

如下为fk函数，将当前关节空间映射到末端笛卡尔坐标系下的位姿。

def fk(self, q):        num_joints = self.chain.getNrOfJoints()        q_kdl = PyKDL.JntArray(num_joints)        for i inrange(num_joints):            q_kdl[i] = q[i]        end_frame = PyKDL.Frame()        self.fk_solver.JntToCart(q_kdl, end_frame)        tf = np.eye(4, dtype=np.float64)        rot_mat = np.array([            [end_frame.M[0,0], end_frame.M[0,1], end_frame.M[0,2]],            [end_frame.M[1,0], end_frame.M[1,1], end_frame.M[1,2]],            [end_frame.M[2,0], end_frame.M[2,1], end_frame.M[2,2]]        ], dtype=np.float64)        tf[:3, :3] = rot_mat        tf[:3, 3] = np.array([end_frame.p.x(), end_frame.p.y(), end_frame.p.z()])        return tf

如下为完整的测试代码，需要注意如上代码封装在kdl_ik中，需要import进来，另外注意URDF中是否包含对应Link name，最后通过打印Mujoco的位姿以及fk后的位姿，可以看到位置接近（这里mujoco的body pos和urdf应该有offset）

import mujocoimport numpyasnpimport mujoco_viewerimport src.kdl_ikaskdl_ikimport timeimport osimport utils class RobotController(mujoco_viewer.CustomViewer):        def __init__(self, scene_path, arm_path):      super().__init__(scene_path, distance=3.5, azimuth=180, elevation=-90)      self.scene_path = scene_path      self.arm_path = arm_path       self.ee_body_name = "ee_center_body"        # 初始化逆运动学      self.arm = kdl_ik.Kinematics(self.ee_body_name)        urdf_file = "model/franka_panda_urdf/robots/panda_arm.urdf"      self.arm.buildFromURDF(urdf_file, "link0")      self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY,self.ee_body_name)      self.last_dof = None     def runBefore(self):      self.model.opt.timestep = 0.001        # 设定初始位置      self.initial_pos =self.model.key_qpos[0]          print("Initial position: ",self.initial_pos)      foriinrange(self.model.nq):          self.data.qpos[i] =self.initial_pos[i]        # mujoco.mj_forward(self.model, self.data)        ee_pos =self.data.body(self.end_effector_id).xpos.copy()        # self.x = ee_pos[0]        # self.y = ee_pos[1]        # self.z = ee_pos[2]      self.x = 0.1      self.y = 0.1      self.z = 0.5      self.last_dof =self.data.qpos[:9].copy()     def runFunc(self):      self.x += 0.001      ifself.x > 0.5:          self.x = 0.5          self.data.qpos[:7] =self.last_dof[:7]          self.data.qpos[7:] = [0,0]      else:            tf = utils.transform2mat(self.x,self.y,self.z, np.pi, 0, 0)          self.dof, info =self.arm.ik(tf, current_arm_motor_q=self.last_dof)          ifinfo["success"]:              self.last_dof =self.dof              self.data.qpos[:7] =self.dof[:7]              self.data.qpos[7:] = [0,0]                # self.data.ctrl[:7] = self.dof[:7]                print("ee pos",self.getBodyPosByName(self.ee_body_name))                print("fk",self.arm.fk(self.dof[:7]))        time.sleep(0.01) if__name__ == "__main__":    SCENE_XML_PATH = 'model/franka_emika_panda/scene_pos.xml'    ARM_XML_PATH = 'model/franka_emika_panda/panda_pos.xml'    robot = RobotController(SCENE_XML_PATH, ARM_XML_PATH)    robot.run_loop()