Mujoco和ROS联合使用_mujoco ros

在mujoco中创建一个位置控制的机械臂，同时在Ros的rviz中打开，用gui拖动关节，同时在mujoco的程序中订阅joint-states话题，把关节位置控制施加到mujoco模型上，这样就可以实现mujoco和ros联动

import mujoco as mj
import numpy as np
from mujoco_base import MuJoCoBase
from mujoco.glfw import glfw
import rospy
import rospkg
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import Pose, Twist
from sensor_msgs.msg import JointState
import threading


class RobotSim(MuJoCoBase):
    def __init__(self, xml_path):
        super().__init__(xml_path)
        self.simend = 1000.0
        print(self.model.nv)
        print(self.model.nq)
        print(len(self.data.ctrl))
        # print(self.data.qpos)

        rospy.Subscriber('/joint_states', JointState, self.controlCallback)

        mj.mj_step(self.model, self.data)

        self.opt.flags[mj.mjtVisFlag.mjVIS_CONTACTFORCE] = True # enable contact force visualization

        viewport_width, viewport_height = glfw.get_framebuffer_size(self.window)
        viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)
        mj.mjv_updateScene(self.model, self.data, self.opt, None, self.cam, mj.mjtCatBit.mjCAT_ALL.value, self.scene)
        mj.mjr_render(viewport, self.scene, self.context)

    def controlCallback(self, data):
        # print(data.position)
        # np.copyto(self.data.ctrl, data.position)
        self.data.ctrl[:7] = data.position

    def reset(self):
        # Set camera configuration
        self.cam.azimuth = 89.608063
        self.cam.elevation = -11.588379
        self.cam.distance = 3.0
        self.cam.lookat = np.array([0.0, 0.0, 1.])

    def simulate(self):
        while not glfw.window_should_close(self.window):
            simstart = self.data.time
            # 60 fps
            while (self.data.time - simstart <= 1.0/60.0 and not self.pause_flag):
                mj.mj_step(self.model, self.data)

            if self.data.time >= self.simend:
                break

            # get framebuffer viewport
            viewport_width, viewport_height = glfw.get_framebuffer_size(self.window)
            viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)

            # update scene and render
            mj.mjv_updateScene(self.model, self.data, self.opt, None, self.cam,
                                mj.mjtCatBit.mjCAT_ALL.value, self.scene)
            mj.mjr_render(viewport, self.scene, self.context)
            glfw.swap_buffers(self.window)
            glfw.poll_events()

        glfw.terminate()


# def thread_job():
#     rospy.spin()


def main():
    np.set_printoptions(formatter={"float": lambda x: "{0:0.3f}".format(x)})
    rospy.init_node('robot_sim', anonymous=True)
    # add_thread = threading.Thread(target = thread_job)
    # add_thread.start()
    rospack = rospkg.RosPack()
    rospack.list()
    # robot_desc_path = rospack.get_path('arm')
    # xml_path = robot_desc_path + "/mjcf/scene.xml"
    xml_path = "../mjcf/scene.xml"
    sim = RobotSim(xml_path)
    sim.reset()
    sim.simulate()


if __name__ == "__main__":
    main()
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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

有趣的是，照理说订阅话题就需要执行spin或spinOnce来执行回调函数，因为rospy没有spinOnce，所以就单独开一个线程来执行rospy.spin
后来，无意中把线程函数注释掉了，发现程序仍然可以运行，依然可以订阅到ros话题
好神奇，暂时不知道为什么