diff --git a/ur_description/urdf/ur10.urdf.xacro b/ur_description/urdf/ur10.urdf.xacro index 422f0b2a747ca1dd68dacf1edc5c4aec1016b898..d1f6f8f2de3d5e4c98d5d2bc0da426ebec777cf0 100644 --- a/ur_description/urdf/ur10.urdf.xacro +++ b/ur_description/urdf/ur10.urdf.xacro @@ -60,6 +60,7 @@ <!-- Arbitrary offsets for shoulder/elbow joints --> <xacro:property name="shoulder_offset" value="0.220941" /> <!-- measured from model --> <xacro:property name="elbow_offset" value="-0.1719" /> <!-- measured from model --> + <xacro:property name="upper_arm_inertia_offset" value="0.045" /> <!-- measured from model --> <!-- link lengths used in model --> <xacro:property name="shoulder_height" value="${d1}" /> @@ -150,7 +151,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="${-a2}" mass="${upper_arm_mass}"> - <origin xyz="0.0 0.0 ${-a2/2.0}" rpy="0 0 0" /> + <origin xyz="0.0 ${-upper_arm_inertia_offset} ${-a2/2.0}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -216,7 +217,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -249,7 +250,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -281,8 +282,8 @@ <mesh filename="package://ur_description/meshes/ur10/collision/wrist3.stl"/> </geometry> </collision> - <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.045" length="0.0305" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.0305/2} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> diff --git a/ur_description/urdf/ur3.urdf.xacro b/ur_description/urdf/ur3.urdf.xacro index 5ccf410f3f1c1081591af34874a35df7a16aad65..d1116d979c32b48db3cee1da67e216e5f30a9fd9 100644 --- a/ur_description/urdf/ur3.urdf.xacro +++ b/ur_description/urdf/ur3.urdf.xacro @@ -215,7 +215,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -248,7 +248,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -280,8 +280,8 @@ <mesh filename="package://ur_description/meshes/ur3/collision/wrist3.stl"/> </geometry> </collision> - <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.032" length="0.04" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.02} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> diff --git a/ur_description/urdf/ur5.urdf.xacro b/ur_description/urdf/ur5.urdf.xacro index 8daf417ef21fc1cf502be7fab5a2c7226d47e42e..5e9e54727ed7af6487c8897ac16e133265801b5f 100644 --- a/ur_description/urdf/ur5.urdf.xacro +++ b/ur_description/urdf/ur5.urdf.xacro @@ -198,8 +198,8 @@ <mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.06" length="0.5" mass="${forearm_mass}"> - <origin xyz="0.0 0.0 0.25" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="${-a3}" mass="${forearm_mass}"> + <origin xyz="0.0 0.0 ${-a3/2}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -231,8 +231,8 @@ <mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_1_mass}"> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -264,8 +264,8 @@ <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_2_mass}"> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -297,8 +297,8 @@ <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.0375" length="0.0345" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.0345/2} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> diff --git a/ur_e_description/urdf/ur10e.urdf.xacro b/ur_e_description/urdf/ur10e.urdf.xacro index ca80aa9f86bf2fffa043c14d1e6a3b0e93d330a4..e40d7ac1c77c916796b9a5568cf89350e6ecbf4b 100644 --- a/ur_e_description/urdf/ur10e.urdf.xacro +++ b/ur_e_description/urdf/ur10e.urdf.xacro @@ -215,7 +215,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -248,7 +248,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -280,8 +280,8 @@ <mesh filename="package://ur_e_description/meshes/ur10e/collision/wrist3.stl"/> </geometry> </collision> - <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.045" length="0.05" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.05/2} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> diff --git a/ur_e_description/urdf/ur3e.urdf.xacro b/ur_e_description/urdf/ur3e.urdf.xacro index 726d8f7b7bb33f2a96a82184a4926721bbf13d86..1dd80f97e375bb4ff63d25fe90ad20f6190c2212 100644 --- a/ur_e_description/urdf/ur3e.urdf.xacro +++ b/ur_e_description/urdf/ur3e.urdf.xacro @@ -214,7 +214,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -247,7 +247,7 @@ </geometry> </collision> <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -279,8 +279,8 @@ <mesh filename="package://ur_e_description/meshes/ur3e/collision/wrist3.stl"/> </geometry> </collision> - <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.032" length="0.042" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.021} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> diff --git a/ur_e_description/urdf/ur5e.urdf.xacro b/ur_e_description/urdf/ur5e.urdf.xacro index 9dfc5a4c2896bfdcf303f8c63fa16fe5c4da272a..fe6bada461e1a9e640362162231135636ef9ac1d 100644 --- a/ur_e_description/urdf/ur5e.urdf.xacro +++ b/ur_e_description/urdf/ur5e.urdf.xacro @@ -164,8 +164,8 @@ <mesh filename="package://ur_e_description/meshes/ur5e/collision/upperarm.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.06" length="0.56" mass="${upper_arm_mass}"> - <origin xyz="0.0 0.0 0.28" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="${upper_arm_length}" mass="${upper_arm_mass}"> + <origin xyz="0.0 0.0 ${upper_arm_length/2}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -197,8 +197,8 @@ <mesh filename="package://ur_e_description/meshes/ur5e/collision/forearm.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.06" length="0.5" mass="${forearm_mass}"> - <origin xyz="0.0 0.0 0.25" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="${forearm_length}" mass="${forearm_mass}"> + <origin xyz="0.0 0.0 ${forearm_length/2}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -230,8 +230,8 @@ <mesh filename="package://ur_e_description/meshes/ur5e/collision/wrist1.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_1_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_1_mass}"> + <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -263,8 +263,8 @@ <mesh filename="package://ur_e_description/meshes/ur5e/collision/wrist2.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_2_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_2_mass}"> + <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -296,8 +296,8 @@ <mesh filename="package://ur_e_description/meshes/ur5e/collision/wrist3.stl" /> </geometry> </collision> - <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_3_mass}"> - <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + <xacro:cylinder_inertial radius="0.0375" length="0.0458" mass="${wrist_3_mass}"> + <origin xyz="0.0 ${wrist_3_length - 0.0458/2} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link>