701 lines
17 KiB
XML
701 lines
17 KiB
XML
<?xml version="1.0"?>
|
|
<robot name="wx250s_custom" xmlns:xacro="http://ros.org/wiki/xacro">
|
|
|
|
<material name="interbotix_black">
|
|
<texture filename="package://interbotix_xsarm_descriptions/meshes/interbotix_black.png"/>
|
|
</material>
|
|
|
|
|
|
<link name="world"/>
|
|
<joint name="fixed" type="fixed">
|
|
<parent
|
|
link="world"/>
|
|
<child
|
|
link="wx250s_custom/base_link"/>
|
|
</joint>
|
|
|
|
|
|
<link name="wx250s_custom/base_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/base.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/base.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.0380446000 0.0006138920 0.0193354000"/>
|
|
<mass value="0.538736"/>
|
|
<inertia
|
|
ixx="0.0021150000"
|
|
iyy="0.0006921000"
|
|
izz="0.0025240000"
|
|
ixy="-0.0000163500"
|
|
ixz="0.0000006998"
|
|
iyz="0.0000464200"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="waist" type="revolute">
|
|
<axis xyz="0 0 1"/>
|
|
<limit
|
|
effort="10"
|
|
lower="-3.141582653589793"
|
|
upper="3.141582653589793"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0.072"/>
|
|
<parent
|
|
link="wx250s_custom/base_link"/>
|
|
<child
|
|
link="wx250s_custom/shoulder_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/shoulder_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 -0.003"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/shoulder.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 -0.003"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/shoulder.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0000223482 0.0000414609 0.0066287000"/>
|
|
<mass value="0.480879"/>
|
|
<inertia
|
|
ixx="0.0003790000"
|
|
iyy="0.0005557000"
|
|
izz="0.0005889000"
|
|
ixy="0.0000000022"
|
|
ixz="-0.0000003561"
|
|
iyz="0.0000012245"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="shoulder" type="revolute">
|
|
<axis xyz="0 1 0"/>
|
|
<limit
|
|
effort="20"
|
|
lower="-1.8849555921538759"
|
|
upper="-1.9896753472735356"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0.03865"/>
|
|
<parent
|
|
link="wx250s_custom/shoulder_link"/>
|
|
<child
|
|
link="wx250s_custom/upper_arm_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/upper_arm_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/upper_arm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/upper_arm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0171605000 0.0000002725 0.1913230000"/>
|
|
<mass value="0.430811"/>
|
|
<inertia
|
|
ixx="0.0034630000"
|
|
iyy="0.0035870000"
|
|
izz="0.0004566000"
|
|
ixy="-0.0000000001"
|
|
ixz="-0.0000000002"
|
|
iyz="0.0004272000"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="elbow" type="revolute">
|
|
<axis xyz="0 1 0"/>
|
|
<limit
|
|
effort="15"
|
|
lower="-2.1467549799530254"
|
|
upper="1.605702911834783"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.04975 0 0.25"/>
|
|
<parent
|
|
link="wx250s_custom/upper_arm_link"/>
|
|
<child
|
|
link="wx250s_custom/upper_forearm_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/upper_forearm_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/upper_forearm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/upper_forearm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.1079630000 0.0001158760 0"/>
|
|
<mass value="0.234589"/>
|
|
<inertia
|
|
ixx="0.0000397100"
|
|
iyy="0.0008878000"
|
|
izz="0.0008880000"
|
|
ixy="0.0000023528"
|
|
ixz="0"
|
|
iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="forearm_roll" type="revolute">
|
|
<axis xyz="1 0 0"/>
|
|
<limit
|
|
effort="2"
|
|
lower="-3.141582653589793"
|
|
upper="3.141582653589793"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.175 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/upper_forearm_link"/>
|
|
<child
|
|
link="wx250s_custom/lower_forearm_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/lower_forearm_link">
|
|
<visual>
|
|
<origin
|
|
rpy="3.141592653589793 0 0"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/lower_forearm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="3.141592653589793 0 0"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/lower_forearm.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="3.141592653589793 0 0"
|
|
xyz="0.0374395000 0.0052225200 0"/>
|
|
<mass value="0.220991"/>
|
|
<inertia
|
|
ixx="0.0000636900"
|
|
iyy="0.0001677000"
|
|
izz="0.0001834000"
|
|
ixy="-0.0000229200"
|
|
ixz="0"
|
|
iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="wrist_angle" type="revolute">
|
|
<axis xyz="0 1 0"/>
|
|
<limit
|
|
effort="5"
|
|
lower="-1.7453292519943295"
|
|
upper="2.1467549799530254"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.075 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/lower_forearm_link"/>
|
|
<child
|
|
link="wx250s_custom/wrist_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/wrist_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/wrist.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/wrist.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0423600000 -0.0000106630 0.0105770000"/>
|
|
<mass value="0.084957"/>
|
|
<inertia
|
|
ixx="0.0000308200"
|
|
iyy="0.0000282200"
|
|
izz="0.0000315200"
|
|
ixy="0.0000000191"
|
|
ixz="0.0000000023"
|
|
iyz="0.0000025481"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<!-- Include the gripper if used -->
|
|
|
|
|
|
|
|
<joint name="wrist_rotate" type="revolute">
|
|
<axis xyz="1 0 0"/>
|
|
<limit
|
|
effort="1"
|
|
lower="-3.141582653589793"
|
|
upper="3.141582653589793"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.065 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/wrist_link"/>
|
|
<child
|
|
link="wx250s_custom/gripper_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/gripper_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.02 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.02 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0216310000 0.0000002516 0.0114100000"/>
|
|
<mass value="0.072885"/>
|
|
<inertia
|
|
ixx="0.0000253700"
|
|
iyy="0.0000183600"
|
|
izz="0.0000167400"
|
|
ixy="0.0000000000"
|
|
ixz="0.0000000000"
|
|
iyz="0.0000004340"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="ee_arm" type="fixed">
|
|
<axis xyz="1 0 0"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.043 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/gripper_link"/>
|
|
<child
|
|
link="wx250s_custom/ee_arm_link"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/ee_arm_link">
|
|
<inertial>
|
|
<mass value="0.001"/>
|
|
<inertia
|
|
ixx="0.001"
|
|
iyy="0.001"
|
|
izz="0.001"
|
|
ixy="0"
|
|
ixz="0"
|
|
iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="gripper" type="continuous">
|
|
<axis xyz="1 0 0"/>
|
|
<limit
|
|
effort="1"
|
|
velocity="3.141592653589793"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.0055 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/ee_arm_link"/>
|
|
<child
|
|
link="wx250s_custom/gripper_prop_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/gripper_prop_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.0685 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_prop.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.0685 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_prop.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0008460000 -0.0000016817 0.0000420000"/>
|
|
<mass value="0.00434"/>
|
|
<inertia
|
|
ixx="0.0000005923"
|
|
iyy="0.0000011156"
|
|
izz="0.0000005743"
|
|
ixy="0.0000000000"
|
|
ixz="0.0000003195"
|
|
iyz="-0.0000000004"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<!-- If the AR tag is being used, then add the AR tag mount -->
|
|
|
|
|
|
|
|
|
|
<!-- If the gripper bar is being used, then also add the gripper bar -->
|
|
|
|
|
|
|
|
<joint name="gripper_bar" type="fixed">
|
|
<axis xyz="1 0 0"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/ee_arm_link"/>
|
|
<child
|
|
link="wx250s_custom/gripper_bar_link"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/gripper_bar_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.063 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_bar.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="-0.063 0 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_bar.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 0 1.5707963267948966"
|
|
xyz="0.0096870000 0.0000008177 0.0049620000"/>
|
|
<mass value="0.034199"/>
|
|
<inertia
|
|
ixx="0.0000074125"
|
|
iyy="0.0000284300"
|
|
izz="0.0000286000"
|
|
ixy="-0.0000000008"
|
|
ixz="-0.0000000006"
|
|
iyz="-0.0000013889"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="ee_bar" type="fixed">
|
|
<axis xyz="1 0 0"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.023 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/gripper_bar_link"/>
|
|
<child
|
|
link="wx250s_custom/fingers_link"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/fingers_link">
|
|
<inertial>
|
|
<mass value="0.001"/>
|
|
<inertia
|
|
ixx="0.001"
|
|
iyy="0.001"
|
|
izz="0.001"
|
|
ixy="0"
|
|
ixz="0"
|
|
iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<!-- If the gripper fingers are being used, add those as well -->
|
|
|
|
|
|
|
|
<joint name="left_finger" type="prismatic">
|
|
<axis xyz="0 1 0"/>
|
|
<limit
|
|
effort="5"
|
|
lower="0.015"
|
|
upper="0.037"
|
|
velocity="1"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/fingers_link"/>
|
|
<child
|
|
link="wx250s_custom/left_finger_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/left_finger_link">
|
|
<visual>
|
|
<origin
|
|
rpy="3.141592653589793 3.141592653589793 0"
|
|
xyz="0 0.005 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_finger.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="3.141592653589793 3.141592653589793 0"
|
|
xyz="0 0.005 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_finger.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="3.141592653589793 3.141592653589793 1.5707963267948966"
|
|
xyz="0.0138160000 0.0000000000 0.0000000000"/>
|
|
<mass value="0.016246"/>
|
|
<inertia
|
|
ixx="0.0000047310"
|
|
iyy="0.0000015506"
|
|
izz="0.0000037467"
|
|
ixy="-0.0000004560"
|
|
ixz="0.0000000000"
|
|
iyz="0.0000000000"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="right_finger" type="prismatic">
|
|
<axis xyz="0 1 0"/>
|
|
<limit
|
|
effort="5"
|
|
lower="-0.037"
|
|
upper="-0.015"
|
|
velocity="1"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/fingers_link"/>
|
|
<child
|
|
link="wx250s_custom/right_finger_link"/>
|
|
<dynamics
|
|
friction="0.1"
|
|
damping="0.1"/>
|
|
|
|
<mimic
|
|
joint="left_finger"
|
|
multiplier="-1"
|
|
offset="0"/>
|
|
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/right_finger_link">
|
|
<visual>
|
|
<origin
|
|
rpy="0 3.141592653589793 0"
|
|
xyz="0 -0.005 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_finger.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
<material name="interbotix_black"/>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
rpy="0 3.141592653589793 0"
|
|
xyz="0 -0.005 0"/>
|
|
<geometry>
|
|
<mesh
|
|
filename="${mesh_directory}/gripper_finger.stl"
|
|
scale="0.001 0.001 0.001"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<origin
|
|
rpy="0 3.141592653589793 1.5707963267948966"
|
|
xyz="0.0138160000 0.0000000000 0.0000000000"/>
|
|
<mass value="0.016246"/>
|
|
<inertia
|
|
ixx="0.0000047310"
|
|
iyy="0.0000015506"
|
|
izz="0.0000037467"
|
|
ixy="0.0000004560"
|
|
ixz="0.0000000000"
|
|
iyz="0.0000000000"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="ee_gripper" type="fixed">
|
|
<axis xyz="1 0 0"/>
|
|
<origin
|
|
rpy="0 0 0"
|
|
xyz="0.128 0 0"/>
|
|
<parent
|
|
link="wx250s_custom/fingers_link"/>
|
|
<child
|
|
link="wx250s_custom/ee_gripper_link"/>
|
|
</joint>
|
|
|
|
<link name="wx250s_custom/ee_gripper_link">
|
|
<inertial>
|
|
<mass value="0.001"/>
|
|
<inertia
|
|
ixx="0.001"
|
|
iyy="0.001"
|
|
izz="0.001"
|
|
ixy="0"
|
|
ixz="0"
|
|
iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
|
|
</robot>
|