ros_gz
ros_gz copied to clipboard
Adding geometry to UR5e Flange Link creates .world file "lump"?
Environment
- OS Version: Ubuntu 22.04
- Gazebo Fortress Binary Build - installed via the https://gazebosim.org/docs/latest/ros_installation & https://gazebosim.org/docs/fortress/install_ubuntu install instructions
Description
- Expected behavior: Taking the ur_descriptions lines [386-391] URDF and adding visual and collision geometry to the flange_link connected to the wrist_3_link to enable the flange_link to be the body_frame for a sensor should result in an auto-generated .world file like:
<joint name='wrist_3-flange' type='fixed'>
<pose relative_to='robot2/wrist_3_link'>0 0 0 0 0 0</pose>
<parent>robot2/wrist_3_link</parent>
<child>robot2/flange_link</child>
</joint>
<link name='flange_link'>
<pose relative_to='flange_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1.219</mass>
<inertia>
<ixx>1.1e-09</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.1e-09</iyy>
<iyz>0</iyz>
<izz>1.1e-09</izz>
</inertia>
</inertial>
<collision name='flange_link_collision'>
<pose>0 0 0 1.57079 -1.57079 3.14159</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='flange_link_visual'>
<pose>0 0 0 1.57079 -1.57079 3.14159</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
- Actual behavior: And instead produces a .world file of:
<link name='wrist_3_link'>
<pose relative_to='wrist_3_joint'>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 -0.0229 0 -0 0</pose>
<mass>0.187902</mass>
<inertia>
<ixx>9.89062e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.89062e-05</iyy>
<iyz>0</iyz>
<izz>0.000132118</izz>
</inertia>
</inertial>
<collision name='wrist_3_link_collision'>
<pose>0 0 -0.0989 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/user/repos/ws/install/ur_description/share/ur_description/meshes/ur5e/collision/wrist3.stl</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wrist_3_link_fixed_joint_lump__flange_link_collision_1'>
<pose>0 0 0 1.57079 -1.57079 3.14159</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='robot2/wrist_3_link_visual'>
<pose>0 0 -0.0989 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/user/repos/ws/install/ur_description/share/ur_description/meshes/ur5e/visual/wrist3.dae</uri>
</mesh>
</geometry>
</visual>
<visual name='wrist_3_link_fixed_joint_lump__flange_link_visual_1'>
<pose>0 0 0 1.57079 -1.57079 3.14159</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
Steps to reproduce
- Copy the ur_macro.xacro file and make the following edit:
<link name="${tf_prefix}wrist_3_link">
<xacro:get_visual_params name="wrist_3" type="visual_offset"/>
<visual>
<origin xyz="0 0 ${visual_params}" rpy="${pi/2} 0 0"/>
<geometry>
<xacro:get_mesh name="wrist_3" type="visual"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 ${visual_params}" rpy="${pi/2} 0 0"/>
<geometry>
<xacro:get_mesh name="wrist_3" type="collision"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}" mass="${wrist_3_mass}">
<origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>
<!-- Modd'd ROS-Industrial 'tool0' frame - all-zeros tool frame -->
<link name="${tf_prefix}flange_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<inertial>
<mass value="2e-06"/>
<inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
</inertial>
</link>
.....
<joint name="${tf_prefix}wrist_3_joint" type="revolute">
<parent link="${tf_prefix}wrist_2_link" />
<child link="${tf_prefix}wrist_3_link" />
<origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
<axis xyz="0 0 1" />
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
<xacro:if value="${safety_limits}">
<safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
</xacro:if>
<dynamics damping="0" friction="0"/>
</joint>
<!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
<joint name="${tf_prefix}wrist_3-flange" type="fixed">
<parent link="${tf_prefix}wrist_3_link" />
<child link="${tf_prefix}flange_link" />
<origin xyz="0 0 0" rpy="0 ${-pi/2.0} ${-pi/2.0}" />
<axis xyz="0 0 1.0"/>
<dynamics damping="0" friction="0"/>
</joint>
- Autogenerate a .world gazebo file and launch gazebo using the following commands:
# Gazebo nodes
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"]
),
)
# Spawn robot
gazebo_spawn_robot = Node(
package="gazebo_ros",
executable="spawn_entity.py",
name="spawn_ur",
arguments=["-entity", "armatrix", "-topic", "robot_description"],
output="screen",
)
- Save off the .world file generated by using the Save As... menu in the Gazebo UI.
Output
(Output is same as above)