Mujoco 模型 MJCF 和 URDF 如何手动对齐（Pinocchio验证）
原贴链接：
https://www.eeworld.com.cn/aOO0qfL
代码仓库：
https://github.com/LitchiCheng/mujoco-learning
在Mujco中的模型为mjcf的xml，其他仿真ROS2中，urdf也是xml文件，一般可以通过一些工具进行相互转换，但转换的结果却常常不正确，有几种情况：
- 模型直接运行不了，报错
- 模型的mesh文件丢失，显示不对
- 模型的link和body等对应不上
所以手动修改MJCF和URDF作为保底手段，还是要理解哪些是重大部分和必定要对齐的地方，前提是这两份模型的差异不是很大（手动狗头，不然改到吐），还有就是理解其中要对齐的方向是什么（是否只思考运动学等等）。
下面介绍下其中几个关键的字段：
MCJF中的Body
<body name="hand" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834">再者就是位置关系，如MCJF中位置定义在body字段中，而URDF定义在link之间的joint，如下以添加一个新的link和body为例，MJCF添加新的body（ee_center_body），位置为pos=”0 0 0.105，姿态为quat=”1 0 0 0″
<body name="ee_center_body" pos="0 0 0.105" quat="1 0 0 0"> <site name='ee_center_site' size="0.01" group="3"/></body>URDF添加新的link，同时添加joint，位置为 xyz=”0 0 0.105″，姿态为rpy=”0 0 0″
<!-- 添加ee_center_body,和mjcf一致 --> <link name="ee_center_body"> <inertial> <origin rpy="0 0 0" xyz="0 0 0"/> <mass value="0.0"/> <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/> </inertial> </link> <joint name="joint9" type="fixed"> <origin rpy="0 0 0" xyz="0 0 0.105"/> <parent link="hand"/> <child link="ee_center_body"/> <axis xyz="0 0 0"/> </joint>
下面使用pinocchio进行验证
from pathlib import Pathfrom sys import argv
import pinocchio as pinimport numpy as np
def test_urdf_match_with_mjcf(model): print("model name: " + model.name) print("lowerLimits: " + str(model.lowerPositionLimit)) print("upperLimits: " + str(model.upperPositionLimit))
data = model.createData() q_list = [ 1.97125175, -0.37236355, 1.64044676, -0.67488302, 2.38533178, 0.72726866, -0.95481822] q = np.array(q_list, dtype=np.float64) # Perform the forward kinematics over the kinematic tree pin.forwardKinematics(model, data, q) # Print out the placement of each joint of the kinematic tree for name, oMi in zip(model.names, data.oMi): print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))
if __name__ == '__main__': print("mjcf test") model_mjcf = pin.RobotWrapper.BuildFromMJCF("model/franka_emika_panda/panda_pos.xml").model test_urdf_match_with_mjcf(model_mjcf) print("") print("urdf test") model_urdf = pin.buildModelFromUrdf("model/franka_panda_urdf/robots/panda_arm.urdf") test_urdf_match_with_mjcf(model_urdf)model.nq需要保持一致，mjcf多了两个finger，删除finger也可以，或者传入qpos的时候创建9×1的维度即可。
Mujoco 基础获取模型中所有 body 的 name, id 以及位姿
原贴链接：
https://www.eeworld.com.cn/aHuL4CS
代码仓库：
https://github.com/LitchiCheng/mujoco-learning
今天介绍下一个很常用的接口：获取模型中所有的 body 的 name，id，以及位姿。
常常做测试时会遇到获取某个 body 的位姿以及利用该位姿，我们可以通过指定 body，先获取 id，再通过 id 获取 ee 的世界坐标系下的位姿，然后将位置作为对运动学的一个目标，同时可以在模型当中添加一些可视化的元素，更直观的来看自己的代码或者说这个实验是否达到预期，如下为常用来的接口或字段：
- body的数量
model.nbody
- 通过id获取名称
mujoco.mj_id2name
- 获取父 id
model.body_parentid
- 获取指定 id 的位置
data.body(body_id).xpos
如下代码为获取所有body的id、以及位姿
import mujocoimport timeimport mujoco_viewerimport numpy as np
class Test(mujoco_viewer.CustomViewer): def __init__(self, path): super().__init__(path, 3, azimuth=-45, elevation=-30) self.path = path
def runBefore(self): for body_id in range(self.model.nbody): # 参数说明：model=模型，obj_type=对象类型（body），obj_id=body ID body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, body_id) # 获取父 body ID parent_body_id = self.model.body_parentid[body_id] # print(f"{body_id:<10} {body_name:<20} {parent_body_id:<15}") pos = self.data.body(body_id).xpos quat = self.data.body(body_id).xquat print(f"id:{body_id}, name: {body_name}, Position: {pos}, Quaternion: {quat}")
def runFunc(self): placeholder = 0
test = Test("./model/franka_emika_panda/scene_pos.xml")test.run_loop()也可以通过在viewer中显示body看到实际的位置。
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，需要设置迭代次数和收敛精度阈值。
def createSolver(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为4×4的齐次变换矩阵的位姿。
def ik(self, tf , current_arm_motor_q = None, current_arm_motor_dq = None): num_joints = self.chain.getNrOfJoints() if current_arm_motor_q is None: q_init = PyKDL.JntArray(num_joints) else: q_init = PyKDL.JntArray(num_joints) for i in range(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) if status >= 0: dof = [q_out[i] for i in range(num_joints)] info = {"success": True} else: dof = q_init info = {"success": False, "error_code": status} return dof, info如下为fk函数，将当前关节空间映射到末端笛卡尔坐标系下的位姿。
def fk(self, q): num_joints = self.chain.getNrOfJoints() q_kdl = PyKDL.JntArray(num_joints) for i in range(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 numpy as npimport mujoco_viewerimport src.kdl_ik as kdl_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) for i in range(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 if self.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) if info["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()
© 版权声明
文章版权归作者所有，未经允许请勿转载。
相关文章
暂无评论...