diff --git a/ur10_moveit_config/config/ur10.srdf b/ur10_moveit_config/config/ur10.srdf index 34566c81b0eed68e838f4e68723c7c1afbd342a3..9a78749f0345929f42bf7762619a4b452254bc77 100644 --- a/ur10_moveit_config/config/ur10.srdf +++ b/ur10_moveit_config/config/ur10.srdf @@ -32,7 +32,6 @@ <!--END EFFECTOR: Purpose: Represent information about an end effector.--> <end_effector name="moveit_ee" parent_link="ee_link" group="manipulator" /> <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)--> - <virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" /> <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. --> <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" /> <disable_collisions link1="base_link" link2="upper_arm_link" reason="Never" /> diff --git a/ur_description/launch/test.launch b/ur_description/launch/test.launch index 57e6b9a334e597c2cf4afe41073c4870740947d8..2d58158c79b3ed20fec4aa3c733ab27214ecaab1 100644 --- a/ur_description/launch/test.launch +++ b/ur_description/launch/test.launch @@ -1,7 +1,8 @@ <?xml version="1.0"?> <launch> - <param name="use_gui" value="true"/> - <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> + <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" > + <param name="use_gui" value="true"/> + </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" /> </launch> diff --git a/ur_description/meshes/ur10/collision/Base.stl b/ur_description/meshes/ur10/collision/Base.stl deleted file mode 100644 index 0ac507704937ad22a3a9bfa96b304465986e9073..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Base.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/Forearm.stl b/ur_description/meshes/ur10/collision/Forearm.stl deleted file mode 100644 index 5a2d56815395d3dfabcaa96c88b5f5e8fd9f2f21..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Forearm.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/Shoulder.stl b/ur_description/meshes/ur10/collision/Shoulder.stl deleted file mode 100644 index 4de55907b784289a2a4634253ab2f859f452ea91..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Shoulder.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/UpperArm.stl b/ur_description/meshes/ur10/collision/UpperArm.stl deleted file mode 100644 index 3690528f03f001d12278ddcb2e304dbc51c8ef00..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/UpperArm.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/Wrist1.stl b/ur_description/meshes/ur10/collision/Wrist1.stl deleted file mode 100644 index 6003010b45a24020496d32c8784b91bc0192ce93..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Wrist1.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/Wrist2.stl b/ur_description/meshes/ur10/collision/Wrist2.stl deleted file mode 100644 index b3ea078051c9b61bcf06904ecbf3f0fa4b88f1a9..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Wrist2.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/Wrist3.stl b/ur_description/meshes/ur10/collision/Wrist3.stl deleted file mode 100644 index 01b26bf93baa6c786e76aec5a1a96f7ca93273f9..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/collision/Wrist3.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/collision/cube.dae b/ur_description/meshes/ur10/collision/cube.dae deleted file mode 100644 index 4d6584b3696b08d3503d10bb5b0ab961c9b08888..0000000000000000000000000000000000000000 --- a/ur_description/meshes/ur10/collision/cube.dae +++ /dev/null @@ -1,63 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?> -<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> - <asset> - <contributor> - <author>VCGLab</author> - <authoring_tool>VCGLib | MeshLab</authoring_tool> - </contributor> - <up_axis>Y_UP</up_axis> - <created>Tue Feb 19 15:28:14 2013</created> - <modified>Tue Feb 19 15:28:14 2013</modified> - </asset> - <library_images/> - <library_materials/> - <library_effects/> - <library_geometries> - <geometry id="shape0-lib" name="shape0"> - <mesh> - <source id="shape0-lib-positions" name="position"> - <float_array id="shape0-lib-positions-array" count="24">0.5 0.5 0.5 -0.5 0.5 0.5 0.5 -0.5 0.5 -0.5 -0.5 0.5 0.5 0.5 -0.5 -0.5 0.5 -0.5 0.5 -0.5 -0.5 -0.5 -0.5 -0.5</float_array> - <technique_common> - <accessor count="8" source="#shape0-lib-positions-array" stride="3"> - <param name="X" type="float"/> - <param name="Y" type="float"/> - <param name="Z" type="float"/> - </accessor> - </technique_common> - </source> - <source id="shape0-lib-normals" name="normal"> - <float_array id="shape0-lib-normals-array" count="36">0 0 1 0 0 1 1 0 0 1 0 0 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 -1 0 0 -1 0 0</float_array> - <technique_common> - <accessor count="12" source="#shape0-lib-normals-array" stride="3"> - <param name="X" type="float"/> - <param name="Y" type="float"/> - <param name="Z" type="float"/> - </accessor> - </technique_common> - </source> - <vertices id="shape0-lib-vertices"> - <input semantic="POSITION" source="#shape0-lib-positions"/> - </vertices> - <triangles count="12"> - <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/> - <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/> - <p>0 0 1 0 2 0 3 1 2 1 1 1 0 2 2 2 4 2 6 3 4 3 2 3 0 4 4 4 1 4 5 5 1 5 4 5 7 6 5 6 6 6 4 7 6 7 5 7 7 8 6 8 3 8 2 9 3 9 6 9 7 10 3 10 5 10 1 11 5 11 3 11</p> - </triangles> - </mesh> - </geometry> - </library_geometries> - <library_visual_scenes> - <visual_scene id="VisualSceneNode" name="VisualScene"> - <node id="node" name="node"> - <instance_geometry url="#shape0-lib"> - <bind_material> - <technique_common/> - </bind_material> - </instance_geometry> - </node> - </visual_scene> - </library_visual_scenes> - <scene> - <instance_visual_scene url="#VisualSceneNode"/> - </scene> -</COLLADA> diff --git a/ur_description/meshes/ur10/visual/Base.stl b/ur_description/meshes/ur10/visual/Base.stl deleted file mode 100644 index 0ac507704937ad22a3a9bfa96b304465986e9073..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Base.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/Forearm.stl b/ur_description/meshes/ur10/visual/Forearm.stl deleted file mode 100644 index 5a2d56815395d3dfabcaa96c88b5f5e8fd9f2f21..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Forearm.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/Shoulder.stl b/ur_description/meshes/ur10/visual/Shoulder.stl deleted file mode 100644 index 4de55907b784289a2a4634253ab2f859f452ea91..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Shoulder.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/UpperArm.stl b/ur_description/meshes/ur10/visual/UpperArm.stl deleted file mode 100644 index 3690528f03f001d12278ddcb2e304dbc51c8ef00..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/UpperArm.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/Wrist1.stl b/ur_description/meshes/ur10/visual/Wrist1.stl deleted file mode 100644 index 6003010b45a24020496d32c8784b91bc0192ce93..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Wrist1.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/Wrist2.stl b/ur_description/meshes/ur10/visual/Wrist2.stl deleted file mode 100644 index b3ea078051c9b61bcf06904ecbf3f0fa4b88f1a9..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Wrist2.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/Wrist3.stl b/ur_description/meshes/ur10/visual/Wrist3.stl deleted file mode 100644 index 01b26bf93baa6c786e76aec5a1a96f7ca93273f9..0000000000000000000000000000000000000000 Binary files a/ur_description/meshes/ur10/visual/Wrist3.stl and /dev/null differ diff --git a/ur_description/meshes/ur10/visual/cube.dae b/ur_description/meshes/ur10/visual/cube.dae deleted file mode 100644 index 4d6584b3696b08d3503d10bb5b0ab961c9b08888..0000000000000000000000000000000000000000 --- a/ur_description/meshes/ur10/visual/cube.dae +++ /dev/null @@ -1,63 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?> -<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> - <asset> - <contributor> - <author>VCGLab</author> - <authoring_tool>VCGLib | MeshLab</authoring_tool> - </contributor> - <up_axis>Y_UP</up_axis> - <created>Tue Feb 19 15:28:14 2013</created> - <modified>Tue Feb 19 15:28:14 2013</modified> - </asset> - <library_images/> - <library_materials/> - <library_effects/> - <library_geometries> - <geometry id="shape0-lib" name="shape0"> - <mesh> - <source id="shape0-lib-positions" name="position"> - <float_array id="shape0-lib-positions-array" count="24">0.5 0.5 0.5 -0.5 0.5 0.5 0.5 -0.5 0.5 -0.5 -0.5 0.5 0.5 0.5 -0.5 -0.5 0.5 -0.5 0.5 -0.5 -0.5 -0.5 -0.5 -0.5</float_array> - <technique_common> - <accessor count="8" source="#shape0-lib-positions-array" stride="3"> - <param name="X" type="float"/> - <param name="Y" type="float"/> - <param name="Z" type="float"/> - </accessor> - </technique_common> - </source> - <source id="shape0-lib-normals" name="normal"> - <float_array id="shape0-lib-normals-array" count="36">0 0 1 0 0 1 1 0 0 1 0 0 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 -1 0 0 -1 0 0</float_array> - <technique_common> - <accessor count="12" source="#shape0-lib-normals-array" stride="3"> - <param name="X" type="float"/> - <param name="Y" type="float"/> - <param name="Z" type="float"/> - </accessor> - </technique_common> - </source> - <vertices id="shape0-lib-vertices"> - <input semantic="POSITION" source="#shape0-lib-positions"/> - </vertices> - <triangles count="12"> - <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/> - <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/> - <p>0 0 1 0 2 0 3 1 2 1 1 1 0 2 2 2 4 2 6 3 4 3 2 3 0 4 4 4 1 4 5 5 1 5 4 5 7 6 5 6 6 6 4 7 6 7 5 7 7 8 6 8 3 8 2 9 3 9 6 9 7 10 3 10 5 10 1 11 5 11 3 11</p> - </triangles> - </mesh> - </geometry> - </library_geometries> - <library_visual_scenes> - <visual_scene id="VisualSceneNode" name="VisualScene"> - <node id="node" name="node"> - <instance_geometry url="#shape0-lib"> - <bind_material> - <technique_common/> - </bind_material> - </instance_geometry> - </node> - </visual_scene> - </library_visual_scenes> - <scene> - <instance_visual_scene url="#VisualSceneNode"/> - </scene> -</COLLADA> diff --git a/ur_description/urdf/ur10.transmission.xacro b/ur_description/urdf/ur10.transmission.xacro index 881a12a4efec8255887b11416dc49dfc0a577c52..d4e932573599b9e73d3363d0d50d9344e23c0e2d 100644 --- a/ur_description/urdf/ur10.transmission.xacro +++ b/ur_description/urdf/ur10.transmission.xacro @@ -1,62 +1,62 @@ <?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro"> - <xacro:macro name="ur10_arm_transmission" params="prefix"> + <xacro:macro name="ur10_arm_transmission" params="prefix"> - <transmission name="${prefix}shoulder_pan_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}shoulder_pan_joint"/> - <actuator name="${prefix}shoulder_pan_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> - - <transmission name="${prefix}shoulder_lift_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}shoulder_lift_joint"/> - <actuator name="${prefix}shoulder_lift_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> - - <transmission name="${prefix}elbow_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}elbow_joint"/> - <actuator name="${prefix}elbow_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> - - <transmission name="${prefix}wrist_1_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}wrist_1_joint"/> - <actuator name="${prefix}wrist_1_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> - - <transmission name="${prefix}wrist_2_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}wrist_2_joint"/> - <actuator name="${prefix}wrist_2_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> - - <transmission name="${prefix}wrist_3_trans"> - <type>transmission_interface/SimpleTransmission</type> - <joint name="${prefix}wrist_3_joint"/> - <actuator name="${prefix}wrist_3_motor"> - <hardwareInterface>EffortJointInterface</hardwareInterface> - <mechanicalReduction>1</mechanicalReduction> - </actuator> - </transmission> + <transmission name="${prefix}shoulder_pan_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}shoulder_pan_joint"/> + <actuator name="${prefix}shoulder_pan_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> + + <transmission name="${prefix}shoulder_lift_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}shoulder_lift_joint"/> + <actuator name="${prefix}shoulder_lift_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> + + <transmission name="${prefix}elbow_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}elbow_joint"/> + <actuator name="${prefix}elbow_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> + + <transmission name="${prefix}wrist_1_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}wrist_1_joint"/> + <actuator name="${prefix}wrist_1_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> + + <transmission name="${prefix}wrist_2_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}wrist_2_joint"/> + <actuator name="${prefix}wrist_2_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> + + <transmission name="${prefix}wrist_3_trans"> + <type>transmission_interface/SimpleTransmission</type> + <joint name="${prefix}wrist_3_joint"/> + <actuator name="${prefix}wrist_3_motor"> + <hardwareInterface>EffortJointInterface</hardwareInterface> + <mechanicalReduction>1</mechanicalReduction> + </actuator> + </transmission> - </xacro:macro> + </xacro:macro> </robot> diff --git a/ur_description/urdf/ur10.urdf.xacro b/ur_description/urdf/ur10.urdf.xacro index 3b0dfb59f7b42be9f27161dfee3ac21b83b28402..c317a437496302701d6af874ecd7c6d3b8dafc0e 100644 --- a/ur_description/urdf/ur10.urdf.xacro +++ b/ur_description/urdf/ur10.urdf.xacro @@ -56,230 +56,251 @@ <property name="wrist_2_length" value="${ur10_d5}" /> <property name="wrist_3_length" value="${ur10_d6}" /> - <xacro:macro name="ur10_robot" params="prefix"> - - <link name="${prefix}base_link" > + <xacro:macro name="ur10_robot" params="prefix joint_limited"> + + <link name="${prefix}base_link" > + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Base.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" /> + <material name="UR/Blue" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Base.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" /> + </collision> + + <inertial> + <mass value="${base_mass}" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + </inertial> </link> - <joint name="${prefix}base_joint" type="fixed"> - <parent link="${prefix}base_link" /> - <child link = "${prefix}ur_base_link" /> - <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> - </joint> - - <link name="${prefix}ur_base_link" > - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Base.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" /> - <material name="UR/Blue" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Base.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" /> - </collision> - - <inertial> - <mass value="${base_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="${prefix}shoulder_pan_joint" type="revolute"> - <parent link="${prefix}ur_base_link" /> - <child link = "${prefix}shoulder_link" /> - <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 0.0 1.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> + <joint name="${prefix}shoulder_pan_joint" type="revolute"> + <parent link="${prefix}base_link" /> + <child link = "${prefix}shoulder_link" /> + <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" /> + <axis xyz="0.0 0.0 1.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="330.0" velocity="2.16"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> - <link name="${prefix}shoulder_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae" scale="${scale}"/> - </geometry> + <link name="${prefix}shoulder_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae" scale="${scale}"/> + </geometry> <origin rpy="${pi / 2.0} 0.0 0.0" xyz="0.0 0.0 -0.127"/> <!-- -0.127 ???? --> - <material name="UR/Grey" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.dae" scale="${scale}"/> - </geometry> - <origin rpy="${pi / 2.0} 0.0 0.0" xyz="0.0 0.0 -0.127"/> <!-- -0.127 ???? --> - </collision> - - <inertial> - <mass value="${shoulder_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" /> - </inertial> - </link> - - <joint name="${prefix}shoulder_lift_joint" type="revolute"> - <parent link="${prefix}shoulder_link" /> - <child link = "${prefix}upper_arm_link" /> - <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> - - <link name="${prefix}upper_arm_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset--> - <material name="UR/Blue" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset--> - </collision> - - <inertial> - <mass value="${upper_arm_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" /> - </inertial> - </link> - - <joint name="${prefix}elbow_joint" type="revolute"> - <parent link="${prefix}upper_arm_link" /> - <child link = "${prefix}forearm_link" /> - <origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> - - <link name="${prefix}forearm_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> - <material name="UR/Grey" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> - </collision> - - <inertial> - <mass value="${forearm_mass}" /> - <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="${prefix}wrist_1_joint" type="revolute"> - <parent link="${prefix}forearm_link" /> - <child link = "${prefix}wrist_1_link" /> - <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> - - <link name="${prefix}wrist_1_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" /> <!-- new offset --> - <material name="UR/Blue" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" /> <!-- new offset --> - </collision> - - <inertial> - <mass value="${wrist_1_mass}" /> - <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="${prefix}wrist_2_joint" type="revolute"> - <parent link="${prefix}wrist_1_link" /> - <child link = "${prefix}wrist_2_link" /> - <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 0.0 1.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> - - <link name="${prefix}wrist_2_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> - <material name="UR/Grey" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> - </collision> - - <inertial> - <mass value="${wrist_2_mass}" /> - <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="${prefix}wrist_3_joint" type="revolute"> - <parent link="${prefix}wrist_2_link" /> - <child link = "${prefix}wrist_3_link" /> - <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="10" friction="0.1"/> - </joint> - - <link name="${prefix}wrist_3_link"> - <visual> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" /> <!-- new offset --> - <material name="UR/Blue" /> - </visual> - - <collision> - <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.dae" scale="${scale}"/> - </geometry> - <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" /> <!-- new offset --> - </collision> - - <inertial> - <mass value="${wrist_3_mass}" /> - <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> + <material name="UR/Grey" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.dae" scale="${scale}"/> + </geometry> + <origin rpy="${pi / 2.0} 0.0 0.0" xyz="0.0 0.0 -0.127"/> <!-- -0.127 ???? --> + </collision> + + <inertial> + <mass value="${shoulder_mass}" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" /> + </inertial> + </link> + + <joint name="${prefix}shoulder_lift_joint" type="revolute"> + <parent link="${prefix}shoulder_link" /> + <child link = "${prefix}upper_arm_link" /> + <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> + <axis xyz="0.0 1.0 0.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="330.0" velocity="2.16"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> + + <link name="${prefix}upper_arm_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset--> + <material name="UR/Blue" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset--> + </collision> + + <inertial> + <mass value="${upper_arm_mass}" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" /> + </inertial> + </link> + + <joint name="${prefix}elbow_joint" type="revolute"> + <parent link="${prefix}upper_arm_link" /> + <child link = "${prefix}forearm_link" /> + <origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" /> + <axis xyz="0.0 1.0 0.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> + + <link name="${prefix}forearm_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> + <material name="UR/Grey" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> + </collision> + + <inertial> + <mass value="${forearm_mass}" /> + <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + </inertial> + </link> + + <joint name="${prefix}wrist_1_joint" type="revolute"> + <parent link="${prefix}forearm_link" /> + <child link = "${prefix}wrist_1_link" /> + <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" /> + <axis xyz="0.0 1.0 0.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="3.2"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> + + <link name="${prefix}wrist_1_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" /> <!-- new offset --> + <material name="UR/Blue" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" /> <!-- new offset --> + </collision> + + <inertial> + <mass value="${wrist_1_mass}" /> + <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + </inertial> + </link> + + <joint name="${prefix}wrist_2_joint" type="revolute"> + <parent link="${prefix}wrist_1_link" /> + <child link = "${prefix}wrist_2_link" /> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" /> + <axis xyz="0.0 0.0 1.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="3.2"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> + + <link name="${prefix}wrist_2_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> + <material name="UR/Grey" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" /> <!-- new offset --> + </collision> + + <inertial> + <mass value="${wrist_2_mass}" /> + <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + </inertial> + </link> + + <joint name="${prefix}wrist_3_joint" type="revolute"> + <parent link="${prefix}wrist_2_link" /> + <child link = "${prefix}wrist_3_link" /> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> + <axis xyz="0.0 1.0 0.0" /> + <xacro:unless value="${joint_limited}"> + <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/> + </xacro:unless> + <xacro:if value="${joint_limited}"> + <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="3.2"/> + </xacro:if> + <dynamics damping="10" friction="0.1"/> + </joint> + + <link name="${prefix}wrist_3_link"> + <visual> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" /> <!-- new offset --> + <material name="UR/Blue" /> + </visual> + + <collision> + <geometry> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.dae" scale="${scale}"/> + </geometry> + <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" /> <!-- new offset --> + </collision> + + <inertial> + <mass value="${wrist_3_mass}" /> + <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" /> + <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> + </inertial> + </link> <joint name="${prefix}ee_fixed_joint" type="fixed"> <parent link="${prefix}wrist_3_link" /> diff --git a/ur_description/urdf/ur10_joint_limited_robot.urdf b/ur_description/urdf/ur10_joint_limited_robot.urdf index 3498e38a5a27eddfbde8ffc68e6ac8ad4586bd2b..8e7d341d22bf29e6eb4cb6bfbedac5261ed2f165 100644 --- a/ur_description/urdf/ur10_joint_limited_robot.urdf +++ b/ur_description/urdf/ur10_joint_limited_robot.urdf @@ -36,16 +36,16 @@ <link name="base_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Base.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Base.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 0.0"/> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Base.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Base.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 0.0"/> </collision> <inertial> <mass value="4.0"/> @@ -55,171 +55,187 @@ <joint name="shoulder_pan_joint" type="revolute"> <parent link="base_link"/> <child link="shoulder_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.128"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1273"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="330.0" lower="-3.14159265" upper="3.14159265" velocity="2.16"/> - <dynamics damping="1.2" friction="0.0"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="shoulder_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 -0.127"/> + <!-- -0.127 ???? --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 -0.127"/> + <!-- -0.127 ???? --> </collision> <inertial> - <mass value="3.7"/> + <mass value="7.778"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00193 -0.02561"/> + <origin rpy="0.0 0.0 0.0" xyz="0.00008 0.00244 -0.037"/> </inertial> </link> <joint name="shoulder_lift_joint" type="revolute"> <parent link="shoulder_link"/> <child link="upper_arm_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.1704 0.0"/> + <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.220941 0.0"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-3.14159265" upper="3.14159265" velocity="3.14159265"/> - <dynamics damping="1.2" friction="0.0"/> + <limit effort="330.0" lower="-3.14159265" upper="3.14159265" velocity="2.16"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="upper_arm_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.221 -0.127"/> + <!-- new offset--> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.221 -0.127"/> + <!-- new offset--> </collision> <inertial> - <mass value="8.393"/> + <mass value="12.93"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.024201 0.2125"/> + <origin rpy="0.0 0.0 0.0" xyz="0.00001 0.15061 0.38757"/> </inertial> </link> <joint name="elbow_joint" type="revolute"> <parent link="upper_arm_link"/> <child link="forearm_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.12817 0.60186"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1719 0.612"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="forearm_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.049 -0.739"/> + <!-- new offset --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.049 -0.739"/> + <!-- new offset --> </collision> <inertial> - <mass value="2.275"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0265 0.11993"/> + <mass value="3.87"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00012 0.06112 0.1984"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_1_joint" type="revolute"> <parent link="forearm_link"/> <child link="wrist_1_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.56415"/> + <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.5723"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_1_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="1.570796325 0 0.0" xyz="0.0 -0.049 -1.312"/> + <!-- new offset --> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="1.570796325 0 0.0" xyz="0.0 -0.049 -1.312"/> + <!-- new offset --> </collision> <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.110949 0.01634"/> + <mass value="1.96"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00021 -0.00112 0.02269"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_2_joint" type="revolute"> <parent link="wrist_1_link"/> <child link="wrist_2_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.1149 0.0"/> <axis xyz="0.0 0.0 1.0"/> - <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_2_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.164 -1.312"/> + <!-- new offset --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.164 -1.312"/> + <!-- new offset --> </collision> <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0018 0.11099"/> + <mass value="1.96"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00021 0.00112 0.002269"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_3_joint" type="revolute"> <parent link="wrist_2_link"/> <child link="wrist_3_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1157"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_3_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0" xyz="0.0 0.0857 0.0"/> + <origin rpy="1.570796325 0.0 0" xyz="0.0 -0.164 -1.427"/> + <!-- new offset --> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0" xyz="0.0 0.0857 0.0"/> + <origin rpy="1.570796325 0.0 0" xyz="0.0 -0.164 -1.427"/> + <!-- new offset --> </collision> <inertial> - <mass value="0.1879"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.001159 0.0"/> + <mass value="0.202"/> + <origin rpy="0.0 0.0 0.0" xyz="0 -0.001156 -0.00149"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="ee_fixed_joint" type="fixed"> <parent link="wrist_3_link"/> <child link="ee_link"/> - <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0857 0.0"/> + <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/> </joint> <link name="ee_link"/> <transmission name="shoulder_pan_trans"> @@ -298,4 +314,13 @@ <material value="Gazebo/Grey"/> <turnGravityOff>false</turnGravityOff> </gazebo> + <!-- grounding link (needed for MoveIt! compatability --> + <link name="world"> + </link> + <joint name="world_joint" type="fixed"> + <parent link="world"/> + <child link="base_link"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + </joint> </robot> + diff --git a/ur_description/urdf/ur10_joint_limited_robot.urdf.xacro b/ur_description/urdf/ur10_joint_limited_robot.urdf.xacro index 577465b070a1816cfd11d8fd78333a5643e4bdca..b59ff435290acc69554b25f20ac464b7337f69dd 100644 --- a/ur_description/urdf/ur10_joint_limited_robot.urdf.xacro +++ b/ur_description/urdf/ur10_joint_limited_robot.urdf.xacro @@ -6,11 +6,20 @@ <xacro:include filename="$(find ur_description)/urdf/gazebo.urdf.xacro" /> <!-- ur10 --> - <xacro:include filename="$(find ur_description)/urdf/ur10_joint_limited.urdf.xacro" /> + <xacro:include filename="$(find ur_description)/urdf/ur10.urdf.xacro" /> <!-- arm --> - <xacro:ur10_robot prefix=""> + <xacro:ur10_robot prefix="" joint_limited="true"> </xacro:ur10_robot> + <!-- grounding link (needed for MoveIt! compatability --> + <link name="world" > + </link> + + <joint name="world_joint" type="fixed"> + <parent link="world" /> + <child link = "base_link" /> + <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> + </joint> </robot> diff --git a/ur_description/urdf/ur10_robot.urdf b/ur_description/urdf/ur10_robot.urdf index fb48ec26fb94feddae88fdcbb6f26a9c2b735938..f06a265883679a9500e35f077fca9a7a2113a2ac 100644 --- a/ur_description/urdf/ur10_robot.urdf +++ b/ur_description/urdf/ur10_robot.urdf @@ -36,16 +36,16 @@ <link name="base_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Base.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Base.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 0.0"/> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Base.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Base.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 0.0"/> </collision> <inertial> <mass value="4.0"/> @@ -55,171 +55,187 @@ <joint name="shoulder_pan_joint" type="revolute"> <parent link="base_link"/> <child link="shoulder_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.128"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1273"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="330.0" lower="-6.2831853" upper="6.2831853" velocity="2.16"/> - <dynamics damping="1.2" friction="0.0"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="shoulder_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 -0.127"/> + <!-- -0.127 ???? --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 0.0 -0.127"/> + <!-- -0.127 ???? --> </collision> <inertial> - <mass value="3.7"/> + <mass value="7.778"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00193 -0.02561"/> + <origin rpy="0.0 0.0 0.0" xyz="0.00008 0.00244 -0.037"/> </inertial> </link> <joint name="shoulder_lift_joint" type="revolute"> <parent link="shoulder_link"/> <child link="upper_arm_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.1704 0.0"/> + <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.220941 0.0"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="1.2" friction="0.0"/> + <limit effort="330.0" lower="-6.2831853" upper="6.2831853" velocity="2.16"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="upper_arm_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.221 -0.127"/> + <!-- new offset--> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.221 -0.127"/> + <!-- new offset--> </collision> <inertial> - <mass value="8.393"/> + <mass value="12.93"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.024201 0.2125"/> + <origin rpy="0.0 0.0 0.0" xyz="0.00001 0.15061 0.38757"/> </inertial> </link> <joint name="elbow_joint" type="revolute"> <parent link="upper_arm_link"/> <child link="forearm_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.12817 0.60186"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1719 0.612"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="150.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="150.0" lower="-6.2831853" upper="6.2831853" velocity="3.15"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="forearm_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.049 -0.739"/> + <!-- new offset --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.dae" scale="0.001 0.001 0.001"/> </geometry> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.049 -0.739"/> + <!-- new offset --> </collision> <inertial> - <mass value="2.275"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0265 0.11993"/> + <mass value="3.87"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00012 0.06112 0.1984"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_1_joint" type="revolute"> <parent link="forearm_link"/> <child link="wrist_1_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.56415"/> + <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.5723"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_1_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="1.570796325 0 0.0" xyz="0.0 -0.049 -1.312"/> + <!-- new offset --> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="1.570796325 0 0.0" xyz="0.0 -0.049 -1.312"/> + <!-- new offset --> </collision> <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.110949 0.01634"/> + <mass value="1.96"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00021 -0.00112 0.02269"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_2_joint" type="revolute"> <parent link="wrist_1_link"/> <child link="wrist_2_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.11279 0.0"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.1149 0.0"/> <axis xyz="0.0 0.0 1.0"/> - <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_2_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.164 -1.312"/> + <!-- new offset --> <material name="UR/Grey"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.164 -1.312"/> + <!-- new offset --> </collision> <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0018 0.11099"/> + <mass value="1.96"/> + <origin rpy="0.0 0.0 0.0" xyz="-0.00021 0.00112 0.002269"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="wrist_3_joint" type="revolute"> <parent link="wrist_2_link"/> <child link="wrist_3_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.11279"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1157"/> <axis xyz="0.0 1.0 0.0"/> - <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.6" friction="0.0"/> + <limit effort="54.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/> + <dynamics damping="10" friction="0.1"/> </joint> <link name="wrist_3_link"> <visual> <geometry> - <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.stl"/> + <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0" xyz="0.0 0.0857 0.0"/> + <origin rpy="1.570796325 0.0 0" xyz="0.0 -0.164 -1.427"/> + <!-- new offset --> <material name="UR/Blue"/> </visual> <collision> <geometry> - <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.stl"/> + <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.dae" scale="0.001 0.001 0.001"/> </geometry> - <origin rpy="0.0 0.0 0" xyz="0.0 0.0857 0.0"/> + <origin rpy="1.570796325 0.0 0" xyz="0.0 -0.164 -1.427"/> + <!-- new offset --> </collision> <inertial> - <mass value="0.1879"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.001159 0.0"/> + <mass value="0.202"/> + <origin rpy="0.0 0.0 0.0" xyz="0 -0.001156 -0.00149"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> </link> <joint name="ee_fixed_joint" type="fixed"> <parent link="wrist_3_link"/> <child link="ee_link"/> - <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0857 0.0"/> + <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/> </joint> <link name="ee_link"/> <transmission name="shoulder_pan_trans"> @@ -298,4 +314,13 @@ <material value="Gazebo/Grey"/> <turnGravityOff>false</turnGravityOff> </gazebo> + <!-- grounding link (needed for MoveIt! compatability --> + <link name="world"> + </link> + <joint name="world_joint" type="fixed"> + <parent link="world"/> + <child link="base_link"/> + <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> + </joint> </robot> + diff --git a/ur_description/urdf/ur10_robot.urdf.xacro b/ur_description/urdf/ur10_robot.urdf.xacro index f36ead5b2440fca3ded9de70e8611f152fbc5a7a..5a4e762c8f8dcdf808f9e3c91beb1a4b10911a73 100644 --- a/ur_description/urdf/ur10_robot.urdf.xacro +++ b/ur_description/urdf/ur10_robot.urdf.xacro @@ -9,8 +9,18 @@ <xacro:include filename="$(find ur_description)/urdf/ur10.urdf.xacro" /> <!-- arm --> - <xacro:ur10_robot prefix=""> + <xacro:ur10_robot prefix="" joint_limited="false"> </xacro:ur10_robot> + <!-- grounding link (needed for MoveIt! compatability --> + <link name="world" > + </link> + + <joint name="world_joint" type="fixed"> + <parent link="world" /> + <child link = "base_link" /> + <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> + </joint> + </robot>