437 lines
16 KiB
XML
437 lines
16 KiB
XML
<?xml version="1.0" ?>
|
|
<!-- =================================================================================== -->
|
|
<!-- | This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro | -->
|
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
|
<!-- =================================================================================== -->
|
|
<robot name="xarm6">
|
|
<gazebo>
|
|
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
|
|
<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
|
|
<parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
|
|
</plugin>
|
|
</gazebo>
|
|
<link name="world"/>
|
|
<joint name="world_joint" type="fixed">
|
|
<parent link="world"/>
|
|
<child link="link_base"/>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
</joint>
|
|
<ros2_control name="UFRobotSystem" type="system">
|
|
<hardware>
|
|
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
|
<param name="hw_ns">xarm</param>
|
|
<param name="velocity_control">False</param>
|
|
<param name="prefix">P</param>
|
|
<param name="robot_ip">R</param>
|
|
<param name="report_type">normal</param>
|
|
<param name="dof">6</param>
|
|
<param name="baud_checkset">True</param>
|
|
<param name="default_gripper_baud">2000000</param>
|
|
<param name="robot_type">lite</param>
|
|
<param name="add_gripper">False</param>
|
|
</hardware>
|
|
<joint name="joint1">
|
|
<command_interface name="position">
|
|
<param name="min">-6.283185307179586</param>
|
|
<param name="max">6.283185307179586</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
<joint name="joint2">
|
|
<command_interface name="position">
|
|
<param name="min">-2.61799</param>
|
|
<param name="max">2.61799</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
<joint name="joint3">
|
|
<command_interface name="position">
|
|
<param name="min">-0.061087</param>
|
|
<param name="max">5.235988</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
<joint name="joint4">
|
|
<command_interface name="position">
|
|
<param name="min">-6.283185307179586</param>
|
|
<param name="max">6.283185307179586</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
<joint name="joint5">
|
|
<command_interface name="position">
|
|
<param name="min">-2.1642</param>
|
|
<param name="max">2.1642</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
<joint name="joint6">
|
|
<command_interface name="position">
|
|
<param name="min">-6.283185307179586</param>
|
|
<param name="max">6.283185307179586</param>
|
|
</command_interface>
|
|
<command_interface name="velocity">
|
|
<param name="min">-3.14</param>
|
|
<param name="max">3.14</param>
|
|
</command_interface>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<!-- <state_interface name="effort"/> -->
|
|
</joint>
|
|
</ros2_control>
|
|
<material name="White">
|
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
|
</material>
|
|
<material name="Silver">
|
|
<color rgba="0.753 0.753 0.753 1.0"/>
|
|
</material>
|
|
<link name="link_base">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
|
|
<mass value="1.65393501783165"/>
|
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="link1">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
|
|
<mass value="1.169"/>
|
|
<inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint1" type="revolute">
|
|
<origin rpy="0 0 0" xyz="0 0 0.2435"/>
|
|
<parent link="link_base"/>
|
|
<child link="link1"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link2">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
|
|
<mass value="1.192"/>
|
|
<inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint2" type="revolute">
|
|
<origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
|
|
<parent link="link1"/>
|
|
<child link="link2"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link3">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
|
|
<mass value="0.930"/>
|
|
<inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint3" type="revolute">
|
|
<origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
|
|
<parent link="link2"/>
|
|
<child link="link3"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link4">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
|
|
<mass value="1.31"/>
|
|
<inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint4" type="revolute">
|
|
<origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
|
|
<parent link="link3"/>
|
|
<child link="link4"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link5">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
|
|
<mass value="0.784"/>
|
|
<inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
|
|
</geometry>
|
|
<material name="White"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint5" type="revolute">
|
|
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
|
|
<parent link="link4"/>
|
|
<child link="link5"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link6">
|
|
<inertial>
|
|
<origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
|
|
<mass value="0.180"/>
|
|
<inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
|
|
</geometry>
|
|
<material name="Silver"/>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint6" type="revolute">
|
|
<origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
|
|
<parent link="link5"/>
|
|
<child link="link6"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
|
<dynamics damping="1.0" friction="1.0"/>
|
|
</joint>
|
|
<link name="link_eef"/>
|
|
<joint name="joint_eef" type="fixed">
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<parent link="link6"/>
|
|
<child link="link_eef"/>
|
|
</joint>
|
|
<transmission name="tran1">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint1">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor1">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<transmission name="tran2">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint2">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor2">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<transmission name="tran3">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint3">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor3">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<transmission name="tran4">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint4">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor3">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<transmission name="tran5">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint5">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor5">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<transmission name="tran6">
|
|
<type>transmission_interface/SimpleTransmission</type>
|
|
<joint name="joint6">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
</joint>
|
|
<actuator name="motor6">
|
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
|
<mechanicalReduction>100</mechanicalReduction>
|
|
</actuator>
|
|
</transmission>
|
|
<gazebo reference="link_base">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link1">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link2">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link3">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link4">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link5">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<gazebo reference="link6">
|
|
<selfCollide>true</selfCollide>
|
|
</gazebo>
|
|
<joint name="eef_joint" type="fixed">
|
|
<parent link="link_eef"/>
|
|
<child link="pen_link"/>
|
|
</joint>
|
|
<link name="pen_link">
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<geometry>
|
|
<cylinder length="0.2" radius="0.005"/>
|
|
</geometry>
|
|
<material name="Cyan">
|
|
<color rgba="0.0 1.0 1.0 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<box size="0 0 0"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
|
|
<gazebo>
|
|
<plugin filename="libgazebo_ros_p3d.so" name="pen_position">
|
|
<!-- <alwaysOn>true</alwaysOn> -->
|
|
<ros>
|
|
<remapping>odom:=pen_position</remapping>
|
|
</ros>
|
|
<frame_name>world</frame_name>
|
|
<!-- <body_name>pen_link</body_name> -->
|
|
<body_name>link6</body_name>
|
|
<!-- topic name is always /odom -->
|
|
<!-- <topic_name>pen_position</topic_name> -->
|
|
<!-- Update rate in Hz -->
|
|
<update_rate>10</update_rate>
|
|
<!-- <xyzOffsets>0 0 0</xyzOffsets> -->
|
|
<!-- <rpyOffsets>0 0 0</rpyOffsets> -->
|
|
</plugin>
|
|
</gazebo>
|
|
</robot>
|
|
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
|
|
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
|
|
<xacro:lite6_urdf prefix=''/> -->
|
|
<!--
|
|
</robot> -->
|