mujoco-learning/ik_kdl_panda.py at main · LitchiCheng/mujoco-learning · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
import mujoco
import numpy as np
import src.mujoco_viewer as mujoco_viewer
import src.kdl_kinematic as kdl_kinematic
import time
import os
import src.utils as 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_kinematic.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.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]
        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.3
        self.y = 0.0
        self.z = 0.4
        self.last_dof = self.data.qpos[:9].copy()

    def runFunc(self):
        self.x += 0.0001
        if self.x > 0.5:
            self.x = 0.5
        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.ctrl[:7] = self.dof[:7]
            # print("ee pos", self.getBodyPoseByName(self.ee_body_name))
            # print("fk", self.arm.fk(self.dof[:7])
        else:
            print("ik failed")
            self.data.ctrl[:7] = self.last_dof[:7]

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()