Contents
- 1 Model Preparation
- 1.1 Converting a SolidWorks Robot Model to URDF
- 1.2 Viewing the Model in ROS
- 1.3 Adding the Robotiq 2f-85 Gripper
- 1.4 Creating a MoveIt Configuration
- 2 Importing the Model into MuJoCo
- 2.1 Modifying the URDF
- 2.2 Generating the Base Model
- 3 Common MuJoCo Commands
- 3.1 Model Loading and Initialization
- 3.2 Querying and Setting Basic State
The model configured in this experiment is open source at Github: HaofeiMa/E05_Robotiq-2f-85
1 Model Preparation
1.1 Converting a SolidWorks Robot Model to URDF
This simulation experiment uses the Han’s E05 robot and the Robotiq 2f-85 gripper
This section follows this video
First, download the robot model and convert it to URDF.
(1) Install the sw_urdf_exporter plugin
Download the sw_urdf_exporter plugin: http://wiki.ros.org/sw_urdf_exporter. Grab the latest release; it also supports older SolidWorks versions.
Close SolidWorks.
Run sw2urdfSetup.exe and install it.
(2) Add reference axes (rotation axes) for the robot
Select six cylindrical faces in this way to define the six rotation-axis directions.
(3) Export URDF
Check whether Tools → File (at the bottom) → Export as URDF is available. If it is, open it directly. If not, open Tools → Add-Ins and enable both Sw2URDF plugin checkboxes at the bottom.
Follow the steps below to create base_link and link1-6
Then click Preview and Export
Then click Next and Export URDF and Mesh. It saves the URDF model as a ROS package in the location you configured.
After creation, make sure the last coordinate frame is centered on the flange at the robot end, because you need this frame when attaching the gripper later. If it is not, adjust the frame position manually and regenerate the URDF.
1.2 Viewing the Model in ROS
If you do not need Gazebo, skip directly to Section 2 (2 Importing the Model into MuJoCo)
Create a workspace
mkdir -p catkin_robot/src
cd catkin_robot/src
catkin_init_workspaceCopy the package into the src directory
Return to the catkin_robot directory and build the workspace
cd ..
catkin_makeRun the test program
source devel/setup.bash
roslaunch e05 display.launchSet Fixed Frame to base_link, click Add in the lower-left corner, and add a Robot Model. The robot should appear correctly in RViz. Drag the slider to move each robotic arm joint.
1.3 Adding the Robotiq 2f-85 Gripper
(1) Prepare the gripper environment
Go to the workspace src directory
cd src
git clone https://github.com/ros-industrial/robotiq.gitIn your robotic arm package’s urdf folder, create a new common.gazebo.xacro file
cd src/e05/urdf
gedit common.gazebo.xacroAdd the following content
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
<gazebo>
<plugin name="ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
</robot>
For convenience, at the end of e05.urdf I added an ee_link
<link name="ee_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.01" /> <!-- You can use any simple geometry like a sphere for visualization -->
</geometry>
<material name="">
<color rgba="1 0 0 1" /> <!-- Choose a color for visualization -->
</material>
</visual>
</link>
<joint name="ee_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="link6" />
<child link="ee_link" />
</joint>Create another xacro file (for example, my robotic arm package is named e05)
gedit e05.xacroAdd the following content. Note: modify your robotic arm package name in the Gazebo支持 and E05机械臂 sections, and set the parent on the first line of the 夹爪与机械臂连接 section—I connected it to link6, the end link, here.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="e05">
<xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
<!-- E05机械臂 -->
<xacro:include filename="$(find e05)/urdf/e05.urdf" />
<!-- Gazebo 支持 -->
<xacro:include filename="$(find e05)/urdf/common.gazebo.xacro" />
<!-- 加载gazebo中需要使用的模型 -->
<!-- macros for transmission -->
<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- Transmissions for ros control -->
<xacro:transmission_block joint_name="joint1"/>
<xacro:transmission_block joint_name="joint2"/>
<xacro:transmission_block joint_name="joint3"/>
<xacro:transmission_block joint_name="joint4"/>
<xacro:transmission_block joint_name="joint5"/>
<xacro:transmission_block joint_name="joint6"/>
<!-- <xacro:include filename="$(find e05)/urdf/e05.gazebo.xacro" /> -->
<!-- 机器人固定在世界坐标系下 -->
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" />
</joint>
<!-- Robotiq 2F-85夹爪 -->
<xacro:include filename="$(find robotiq_2f_85_gripper_visualization)/urdf/robotiq_arg2f_85_macro.xacro" />
<xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
<gazebo>
<plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
<arm>
<!-- <arm_name>应该是一个单独的名字，不能和别的任何关节同名 -->
<arm_name>ur5_gripper</arm_name>
<!-- <palm_link>是和手指相连的关节 -->
<palm_link>link6</palm_link>
<!-- <gripper_link>是会检测碰撞的关节 -->
<gripper_link>gripper_finger1_finger_tip_link</gripper_link>
<gripper_link>gripper_finger2_finger_tip_link</gripper_link>
<gripper_link>gripper_finger2_knuckle_link</gripper_link>
<gripper_link>gripper_finger1_knuckle_link</gripper_link>
<gripper_link>gripper_finger1_inner_knuckle_link</gripper_link>
<gripper_link>gripper_finger2_inner_knuckle_link</gripper_link>
</arm>
<forces_angle_tolerance>150</forces_angle_tolerance>
<!-- 检测频率 -->
<update_rate>130</update_rate>
<!-- 检测为抓取状态的接触次数阈值 -->
<grip_count_threshold>2</grip_count_threshold>
<max_grip_count>8</max_grip_count>
<!-- 释放时的容忍度，超过这个就会把物体放下。数值越大，需要把夹爪打开更大才能释放物体 -->
<release_tolerance>0.005</release_tolerance>
<disable_collisions_on_attach>true</disable_collisions_on_attach>
<contact_topic>__default_topic__</contact_topic>
</plugin>
</gazebo>
<!-- 将夹爪实例化，并设置夹爪和机械臂的关系（连接在tool0上） -->
<xacro:robotiq_85_gripper prefix="" parent="ee_link" >
<origin xyz="0 0 0" rpy="0 ${-pi/2} 0"/>
</xacro:robotiq_85_gripper>
<!-- 相机实例化，然后设置仿真位置 -->
<!-- <xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>
<xacro:realsense_d435 sensor_name="d435" parent_link="tool0" rate="10">
<origin rpy="0 ${-pi/2} 0 " xyz="-0.1 0 0"/>
</xacro:realsense_d435> -->
</robot>Modify the original launch file. For convenience later, I renamed the original display.launch to display_e05_with_gripper.launch. The main change is this line: <param name="robot_description" command="$(find xacro)/xacro '$(find e05)/urdf/e05.xacro'" /> —point it at the xacro file you just created.
<launch>
<arg name="model" />
<!-- <param name="robot_description" textfile="$(find e05)/urdf/e05.xacro" /> -->
<param name="robot_description" command="$(find xacro)/xacro '$(find e05)/urdf/e05.xacro'" />
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find e05)/urdf.rviz" />
</launch>(2) View the robot in RViz
cd catkin_motion
catkin_make
source devel/setup.bash
roslaunch e05 display_e05_with_gripper.launchClick Add in the lower-left corner and add the RobotModel.
Select base_link for Fixed Frame on the left to see the robot.
(3) View the robot in Gazebo
Create a gazebo_e05_with_gripper.launch file with content like the following:
<launch>
<arg name="gui" default="true" doc="Starts gazebo gui" />
<arg name="paused" default="false" doc="Starts gazebo in paused mode" />
<!-- 启动仿真环境 后续有环境修改，可以替换此处的worlds/empty.world，改为例如"$(find ur_gazebo)/worlds/table_custom.world" -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="$(find gazebo_ros)/launch/empty_world.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>
<!-- 加载TF -->
<node name="tf_footprint_base" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 base_link base_footprint 40" />
<!-- 启动机器人 -->
<include file="$(find e05)/launch/display_e05_with_gripper.launch" />
<!-- 将 robot_description 发送到 gazebo 中生成机器人 -->
<!-- <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find e05)/urdf/e05.urdf -urdf -model e05" output="screen" /> -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0" respawn="false" output="screen" />
<node name="fake_joint_calibration" pkg="rostopic" type="rostopic" args="pub /calibrated std_msgs/Bool true"/>
</launch>Launch the launch file and you should see the robot in Gazebo.
The MoveIt setup below is not needed for MuJoCo. Skip directly to Section 2 (Importing the Model into MuJoCo)
1.4 Creating a MoveIt Configuration
This section follows this article.
Start the MoveIt Setup Assistant
rosrun moveit_setup_assistant moveit_setup_assistantSelect Create New Moveit Configuration Package, choose your e05.xacro file, and click Load Files
The second item on the left, Self-Collisions, checks for collisions. Make sure you enable every pair that could collide; otherwise you will run into errors later.
The third item, Virtual Joints, is usually not needed either.
The fourth item, Planning Groups, is the most important. Configure it by clicking Add Group and setting up the robot and the end gripper separately.
Robotic arm:
- For Group Name, manipulator is usually fine
- For the kinematics solver, choose kdl
- For the motion planner, the default RRT Star is fine
- Click
Add Kin. Chain, set Baselink to base_link, and Tiplink to ee_link
For the end gripper, set Group Name to gripper; leave the other options unchanged.
The fifth item, Robot Poses, can store common poses so you can quickly move the robot to them, for example:
- up: robot upright in the initial pose
- pick: robot ready to grasp
- open: gripper open
- close: gripper closed
The sixth item, End Effectors, can be configured as shown below
Click Controllers, then use auto-generate in the upper-left corner
Click the second-to-last item, Author Information, and fill in a name and email. They do not have to be real, but the package cannot be generated without them.
Finally, click Generate Package (create an e05_moveit folder under src and generate into that folder).
Test whether RViz can control the robot
catkin_make
source devel/setup.bash
roslaunch e05_moveit demo.launchTest whether Gazebo responds correctly
roslaunch e05_moveit demo_gazebo.launch
During testing I ran into the robotic arm plans and executes correctly in RViz, but the Gazebo arm does not move. See this article for the fix.
2 Importing the Model into MuJoCo
2.1 Modifying the URDF
At the top of the existing model’s xacro or urdf file, add the following tag
<mujoco>
<compiler
meshdir="../meshes_mujoco/"
balanceinertia="true"
discardvisual="false" />
</mujoco>Create a folder e05/meshes_mujoco and put all model STL files in it.
Command to generate a urdf file from the xacro file (skip this if you already have a urdf file):
rosrun xacro xacro --inorder e05.xacro > e05.urdfCheck the urdf file:
check_urdf e05.urdfVisualize in RViz:
roslaunch e05 display.launch model:=path/to/your/urdf/file2.2 Generating the Base Model
After confirming the URDF model is correct, go to the MuJoCo executable directory and run the conversion command
cd ~/.mujoco/mujoco210/binThe official docs say you can convert to three model formats: .mjb/.txt/.xml. We usually use the xml format.
./compile /path/to/model.urdf /path/to/model.xmlTest the generated base model
./simulate /path/to/model.xmlYou should see the robot appear in the simulation environment. It may collapse immediately, but that is because actuator and related settings are not configured yet.
Because the gripper kept failing, I replaced it with the gripper from vikashplus/robotiq_sim.
After adding it, the e05_mujoco.xml code looks like this:
<mujoco model="e05">
<compiler angle="radian" meshdir="../meshes_mujoco/" />
<size njmax="500" nconmax="100" />
<!-- 加载 E05 机械臂模型与基本配置 -->
<asset>
<mesh name="base_link" file="e05/base_link.STL" />
<mesh name="link1" file="e05/link1.STL" />
<mesh name="link2" file="e05/link2.STL" />
<mesh name="link3" file="e05/link3.STL" />
<mesh name="link4" file="e05/link4.STL" />
<mesh name="link5" file="e05/link5.STL" />
<mesh name="link6" file="e05/link6.STL" />
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<!-- <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/> -->
<!-- <material name="geom" texture="texgeom" texuniform="true"/> -->
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
</asset>
<default>
<default class="E05">
<joint damping='200' />
</default>
<default class="E05e">
<joint damping='100'/>
</default>
</default>
<!-- 加载 Robotiq-2f-85 夹爪与基本配置 -->
<include file="robotiq-2f-85-assets.xml"/>
<!-- 机器人本体 -->
<worldbody>
<!-- 仿真环境设置定义 -->
<!-- <body pos="-0.16 0.90 0.02" euler="4.71238898 0 3.14159265">
<body euler="0 -0.82 0">
<camera name="workbench_camera" euler="0 0 0" fovy="60" pos="0 0 0.5" />
</body>
</body> -->
<light diffuse="0.6 0.6 0.6" pos="0 0 3"/>
<light diffuse="0.6 0.6 0.6" pos="0 -0.3 3" dir="0 0.2 -0.8" directional="true"/>
<!-- <light cutoff="100" diffuse="0.5 0.5 0.5" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/> -->
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
<!-- 整个运动链 -->
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.501961 0.501961 0.501961 1" mesh="base_link" />
<geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="base_link" />
<body name="link1" pos="0 0 0.0735">
<inertial pos="-0.0218175 -1.34618e-05 0.0953928" quat="0.696024 -0.147787 -0.14991 0.686467" mass="2.58559" diaginertia="0.0143616 0.0141793 0.00506835" />
<joint class="E05" name="joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.772549 0.752941 0.733333 1" mesh="link1" />
<geom type="mesh" rgba="0.772549 0.752941 0.733333 1" mesh="link1" />
<body name="link2" pos="0 0 0.1465" quat="0.5 -0.5 -0.5 0.5">
<inertial pos="4.42659e-06 -0.15814 0.130501" quat="0.491565 0.508215 -0.508335 0.491606" mass="1.21311" diaginertia="0.0159807 0.0154676 0.00149746" />
<joint class="E05" name="joint2" pos="0 0 0" axis="0 0 -1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.792157 0.819608 0.933333 1" mesh="link2" />
<geom type="mesh" rgba="0.792157 0.819608 0.933333 1" mesh="link2" />
<body name="link3" pos="0 -0.38 0" quat="0.707107 0 0 0.707107">
<inertial pos="-0.0414786 1.14213e-05 0.0194164" quat="0.601987 0.371892 0.373044 0.600122" mass="1.53239" diaginertia="0.00608826 0.00587729 0.00215529" />
<joint class="E05" name="joint3" pos="0 0 0" axis="0 0 -1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.866667 0.866667 0.890196 1" mesh="link3" />
<geom type="mesh" rgba="0.866667 0.866667 0.890196 1" mesh="link3" />
<body name="link4" pos="0 0 0" quat="0.5 -0.5 0.5 -0.5">
<inertial pos="3.8238e-05 -0.0589656 -0.257081" quat="0.988248 -0.152858 -0.000415939 -0.000860755" mass="0.475189" diaginertia="0.00538322 0.0052745 0.000778119" />
<joint class="E05e" name="joint4" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.772549 0.752941 0.733333 1" mesh="link4" />
<geom type="mesh" rgba="0.772549 0.752941 0.733333 1" mesh="link4" />
<body name="link5" pos="0 0 -0.42" quat="0.707105 -0.707108 0 0">
<inertial pos="1.42418e-06 0.0365906 -0.0159722" quat="0.527641 0.849468 0.000113207 -7.90953e-05" mass="0.811919" diaginertia="0.00229898 0.00217056 0.000742684" />
<joint class="E05e" name="joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.792157 0.819608 0.933333 1" mesh="link5" />
<geom type="mesh" rgba="0.792157 0.819608 0.933333 1" mesh="link5" />
<body name="link6" pos="0 0.155 0" quat="0.707105 -0.707108 0 0">
<inertial pos="-4.84703e-06 0.00109667 -0.0032406" quat="0.999926 0.0120786 2.3554e-05 -0.00120144" mass="0.75038" diaginertia="0.00129697 0.00122959 0.00051245" />
<joint class="E05e" name="joint6" pos="0 0 0" axis="0 0 -1" limited="true" range="-3.14159 3.14159" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.776471 0.756863 0.737255 1" mesh="link6" />
<geom type="mesh" rgba="0.776471 0.756863 0.737255 1" mesh="link6" />
<geom size="0.01" contype="0" conaffinity="0" group="1" rgba="1 0 0 1" />
<!-- 导入 Robotiq 2f-85 运动链 -->
<include file="robotiq-2f-85-chain.xml"/>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<!-- E05 机械臂驱动 -->
<position name="joint1" ctrllimited="true" ctrlrange="-3.14159 3.14159" joint="joint1" kp="4000"/>
<position name="joint2" ctrllimited="true" ctrlrange="-1.57 1.57" joint="joint2" kp="3000"/>
<position name="joint3" ctrllimited="true" ctrlrange="-3.14159 3.14159" joint="joint3" kp="3000"/>
<position name="joint4" ctrllimited="true" ctrlrange="-3.14159 3.14159" joint="joint4" kp="2000"/>
<position name="joint5" ctrllimited="true" ctrlrange="-3.14159 3.14159" joint="joint5" kp="2000"/>
<position name="joint6" ctrllimited="true" ctrlrange="-3.14159 3.14159" joint="joint6" kp="2000"/>
</actuator>
</mujoco>The result after adding it looks like this:
Note: add a damping attribute to every joint tag, for example
damping='200', otherwise the model will keep jittering and flickering and become uncontrollable.
Organize the file structure, add a table and small blocks, move the robot position and camera viewpoint, and you end up with the simulation environment below. The full code is open source on Github:
3 Common MuJoCo Commands
3.1 Model Loading and Initialization
Load the model:
model = mujoco_py.load_model_from_path("path/to/.xml")Create a MuJoCo simulation instance:
sim = mujoco_py.MjSim(model)Rendering setup:
# 创建渲染器
viewer = mujoco_py.MjViewer(sim)
# 设置模拟环境视角
lookat = [1.27998563, 0.68635066, 0.55350562]
for idx in range(3):
viewer.cam.lookat[idx] = lookat[idx]
viewer.cam.distance = 1.4547035766991275
viewer.cam.azimuth = 134.95215311004816
viewer.cam.elevation = -32.488038277512022Initial pose setup:
initial_qpos = {
'joint1': np.pi/2,
'joint2': 0,
'joint3': np.pi/2,
'joint4': 0,
'joint5': np.pi/2,
'joint6': 0,
'robotiq_2f_85_right_driver_joint': 0,
'object0:joint': [1, 0.45, 0.425, 1., 0., 0., 0.],
}
for name, value in initial_qpos.items():
sim.data.set_joint_qpos(name, value)
sim.forward()Main loop:
while True:
sim.step()
viewer.render()3.2 Querying and Setting Basic State
(1) body
Print body position and pose
body_idx = sim.model.body_name2id("link6")
# 打印 body 坐标
print(sim.data.body_xpos[body_idx])
# 打印 body 四元数
print(sim.data.body_xquat[body_idx])(2) site
Print site position and pose
print(sim.data.get_site_xpos('site_name'))
print(sim.data.get_site_xquat('site_name'))(3) joint
Print joint values:
joint_idx = model.joint_name2id("joint_name")
print(sim.data.qpos[model.jnt_qposadr[joint_idx]])Set joint values
sim.data.set_joint_qpos("joint_name", value)(4) actuator
Print actuator state
print(sim.data.ctrl)Set actuator values
sim.data.ctrl[actuator_index] = value(5) mocap
Print mocap position and pose:
print(sim.data.mocap_pos)
print(sim.data.mocap_quat)Set mocap position and pose
# 方法一
sim.data.mocap_pos[:] = np.array([x, y, z])
sim.data.mocap_quat[:] = np.array([quat_1, quat_2, quat_3, quat_4])
# 方法二
sim.data.set_mocap_pos('mocap_name', np.array([x, y, z]))
sim.data.set_mocap_quat('mocap_name', np.array([quat_1, quat_2, quat_3, quat_4]))References:
Comments