tmp
角度->力矩
def sim_loop(self):
# 当前时间
sim_time = self.data.time
# 目标角度
target_shoulder = 0.0
if sim_time > 1.0:
target_shoulder = -1.57
# 获取当前角度和速度
q_current = self.data.qpos[self.shoulder_id]
qdot_current = self.data.qvel[self.shoulder_id]
# PD 控制计算力矩
Kp = 50.0
Kd = 5.0
tau = Kp * (target_shoulder - q_current) - Kd * qdot_current
# 应用力矩
self.data.ctrl[self.shoulder_id] = tau
xml
<mujoco model="openarm_simple">
<option timestep="0.005" gravity="0 0 -9.81"/>
<visual>
<global offwidth="1920" offheight="1080"/>
</visual>
<worldbody>
<geom type="plane" size="2 2 0.1" rgba=".8 .8 .8 1"/>
<light pos="0 0 3" dir="0 0 -1"/>
<body pos="0 0 0.1" name="base">
<geom type="cylinder" size="0.05 0.1" rgba=".2 .2 .2 1"/>
<body pos="0 0 0.1">
<joint name="shoulder_joint" type="hinge" axis="0 1 0" range="-1.57 1.57" damping="1.0"/>
<geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.03" rgba="0.8 0.1 0.1 1"/>
<body pos="0 0 0.3">
<joint name="elbow_joint" type="hinge" axis="0 1 0" range="-2.0 2.0" damping="1.0"/>
<geom type="capsule" fromto="0 0 0 0 0 0.25" size="0.025" rgba="0.1 0.8 0.1 1"/>
<body pos="0 0 0.25">
<geom type="box" size="0.04 0.01 0.05" rgba="0 0 1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<position joint="shoulder_joint" name="servo_shoulder" kp="50"/>
<position joint="elbow_joint" name="servo_elbow" kp="50"/>
</actuator>
</mujoco>
arm.py
import rclpy
from rclpy.node import Node
import mujoco
import mujoco.viewer
import os
import numpy as np
from ament_index_python.packages import get_package_share_directory
class OpenArmSimNode(Node):
def __init__(self):
super().__init__('openarm_sim_node')
# --- 1. 加载模型 ---
pkg_path = get_package_share_directory('openarm_mujoco')
xml_path = os.path.join(pkg_path, 'meshes', 'openarm_simple.xml')
self.get_logger().info(f"Loading MuJoCo model: {xml_path}")
self.model = mujoco.MjModel.from_xml_path(xml_path)
self.data = mujoco.MjData(self.model)
# 获取执行器ID，方便后续控制
self.shoulder_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "servo_shoulder")
self.elbow_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "servo_elbow")
# --- 2. 启动 Viewer (被动模式) ---
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
# --- 3. 设置仿真循环 (100Hz) ---
self.timer = self.create_timer(0.01, self.sim_loop)
self.get_logger().info("Simulation Started. Arm will lift in 2 seconds...")
def sim_loop(self):
# --- 控制逻辑区域 ---
# 获取当前仿真时间
sim_time = self.data.time
# 简单的状态机：2秒后抬手
target_shoulder = 0.0
target_elbow = 0.0
if sim_time > 2.0:
# 抬手动作：肩部转动 -0.8弧度，肘部转动 -0.5弧度
target_shoulder = -0.8
target_elbow = -0.5
# 将目标角度写入控制输入 (data.ctrl)
self.data.ctrl[self.shoulder_id] = target_shoulder
self.data.ctrl[self.elbow_id] = target_elbow
# --- 物理步进 ---
# 执行一步物理计算
mujoco.mj_step(self.model, self.data)
# --- 同步显示 ---
if self.viewer.is_running():
self.viewer.sync()
else:
self.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = OpenArmSimNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.viewer.close()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
setup.py
from setuptools import setup
import os
from glob import glob
package_name = 'openarm_mujoco'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
# --- 关键：添加这行以安装 meshes 文件夹下的所有 xml ---
(os.path.join('share', package_name, 'meshes'), glob('meshes/*.xml')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='User',
maintainer_email='user@todo.todo',
description='OpenArm MuJoCo Example',
license='TODO',
tests_require=['pytest'],
entry_points={
'console_scripts': [
# 注册节点入口
'arm_control = openarm_mujoco.arm:main'
],
},
)
package.xml
<package format="3">
<name>openarm_mujoco</name>
<version>0.0.0</version>
<description>Demo</description>
<maintainer email="user@todo.todo">User</maintainer>
<license>TODO</license>
<depend>rclpy</depend>
<depend>python3-numpy</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
————————
命令行发布话题
ros2 topic pub /joint_cmd std_msgs/msg/Float64MultiArray "{data: [0,0,0,0.785,0,0]}"
# bridge.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray
import mujoco
import numpy as np
class MuJoCoRosBridge(Node):
def __init__(self):
super().__init__('mujoco_ros_bridge')
# Load MuJoCo model
xml_path = str(self.get_package_share_directory()) + "/models/robot.xml"
self.model = mujoco.MjModel.from_xml_path(xml_path)
self.data = mujoco.MjData(self.model)
# Control buffer
self.cmd = np.zeros(self.model.nu)
# ROS I/O
self.pub = self.create_publisher(JointState, 'joint_state', 10)
self.sub = self.create_subscription(
Float64MultiArray, 'joint_cmd', self.cmd_callback, 10)
# Timer 1 kHz
self.timer = self.create_timer(0.001, self.sim_step)
def get_package_share_directory(self):
from ament_index_python.packages import get_package_share_directory
return get_package_share_directory('mujoco_ros')
def cmd_callback(self, msg):
n = min(len(msg.data), self.model.nu)
self.cmd[:n] = msg.data[:n]
def sim_step(self):
self.data.ctrl[:] = self.cmd
mujoco.mj_step(self.model, self.data)
js = JointState()
js.header.stamp = self.get_clock().now().to_msg()
js.position = self.data.qpos[:self.model.nq].tolist()
js.velocity = self.data.qvel[:self.model.nv].tolist()
js.effort = self.data.actuator_force.tolist()
self.pub.publish(js)
def main():
rclpy.init()
node = MuJoCoRosBridge()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
# cmd_test.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
class CmdTest(Node):
def __init__(self):
super().__init__('cmd_test')
self.pub = self.create_publisher(Float64MultiArray, 'joint_cmd', 10)
self.timer = self.create_timer(0.02, self.send_cmd)
def send_cmd(self):
msg = Float64MultiArray()
msg.data = [0.1, -0.2] # 例如两个关节的 torque
self.pub.publish(msg)
def main():
rclpy.init()
node = CmdTest()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
# mujoco.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='mujoco_ros',
executable='bridge',
output='screen'
),
Node(
package='mujoco_ros',
executable='cmd_test',
output='screen'
)
])
在package.xml添加
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
# setup.py
entry_points={
'console_scripts': [
'bridge = mujoco_ros.bridge:main',
'cmd_test = mujoco_ros.cmd_test:main',
],
},
浙公网安备 33010602011771号