Mujoco 检验 KDL 和 Pinocchio 运动学 FK 是否一致

视频讲解: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()

拉取某一个关节,看下末端位置是否一致

换一个方向拉一下看

相关推荐
培培说证1 分钟前
大专商务数据分析与应用可以报考哪些职业证书?
人工智能·数据挖掘·数据分析
pursuit_c4 分钟前
关于 Personal Agent 的思考碎片
人工智能
数字工作台7 分钟前
给 LangGraph 加一道可审计记忆:替换默认 Checkpointer,实现防篡改 + 崩溃不丢数据
python·agent
龙腾AI白云11 分钟前
具身智能的空间导航与未知环境探索
大数据·数据库·人工智能·机器学习·知识图谱
snakeshe101018 分钟前
大模型 Function Call 实战入门:原理、环境搭建与多轮调用完整教程
人工智能
DKPT19 分钟前
Python 的 Web 框架:选型选错的人,都踩了同一个坑
python
用户3600555790019 分钟前
Kestrel_VLLM - 43 · 默认化前的一次解耦:一个遮蔽 bug 与一次口径污染
人工智能
狒狒AI26 分钟前
DeepSeek Harness 插件生态拆解:31,259 个仓库与 93% 装不上的口径差
人工智能
AI前端工程狮27 分钟前
我把 MCP 长任务拆成三个工具,token 账单涨了 7 倍
javascript·人工智能·程序员
阿里云大数据AI技术29 分钟前
如何用 STAROps Elasticsearch 智能助手看穿“搜索变慢”
人工智能·agent