视频讲解：Mujoco 检验 KDL 和 Pinocchio 运动学 FK 是否一致_哔哩哔哩_bilibili
代码仓库：https://github.com/LitchiCheng/mujoco-learning
是否好奇过 KDL 和 Pinocchio 等运动学的库的结果是否相同？
今天做个测试同时使用 KDL 和 Pinocchio 进行 FK 来，输入为 data.qpos，然后通过通过 data.xpos 和 fk 后的位姿进行实时比较，如下为完整代码：
import mujoco_viewer
import mujoco
import src.kdl_ik as kdl_ik
import src.casadi_ik as casadi_ik
class CheckFk(mujoco_viewer.CustomViewer):
def __init__(self, rendor_path, arm_path):
super().__init__(rendor_path, 3, azimuth=-45, elevation=-30)
self.arm_path = arm_path
self.ee_body_name = "link7"
# # 初始化逆运动学
self.arm2 = kdl_ik.Kinematics(self.ee_body_name)
urdf_file = "model/franka_panda_urdf/robots/panda_arm.urdf"
self.arm2.buildFromURDF(urdf_file, "link0")
self.arm1 = casadi_ik.Kinematics(self.ee_body_name)
self.arm1.buildFromMJCF(self.arm_path)
def runBefore(self):
self.model.opt.timestep = 0.005
# 设定初始位置
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]
def runFunc(self):
# self.data.qpos[:7] = self.initial_pos[:7]
print("Mujoco body position: ")
print(self.getBodyPosByName(self.ee_body_name))
self.fk_tf1 = self.arm1.fk(self.data.qpos)
self.fk_tf2 = self.arm2.fk(self.data.qpos)
print("FK methed 1 result: ")
print(self.fk_tf1)
print("FK methed 2 result: ")
print(self.fk_tf2)
if __name__ == '__main__':
SCENE_XML_PATH = 'model/franka_emika_panda/scene_pos.xml'
ARM_XML_PATH = 'model/franka_emika_panda/panda_pos.xml'
robot = CheckFk(SCENE_XML_PATH, ARM_XML_PATH)
robot.run_loop()拉取某一个关节，看下末端位置是否一致
换一个方向拉一下看
