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