diff --git a/tor_description/meshes/torso/base_link.STL b/tor_description/meshes/torso/base_link.STL index cecb7b204000f1c99b0e3dc9dcfc1255e0bfb088..e301a40a245e7fab2df4d6169763e92887ae6948 100644 Binary files a/tor_description/meshes/torso/base_link.STL and b/tor_description/meshes/torso/base_link.STL differ diff --git a/tor_description/meshes/torso/base_link_collision.STL b/tor_description/meshes/torso/base_link_collision.STL index 8c343eee474583d835306871314afb50c329a18b..9b477ec3df64d383afa31d123c1d78478130772c 100644 Binary files a/tor_description/meshes/torso/base_link_collision.STL and b/tor_description/meshes/torso/base_link_collision.STL differ diff --git a/tor_description/urdf/torso/torso.urdf.xacro b/tor_description/urdf/torso/torso.urdf.xacro index feca5282a6b4e031a7d21d785dd1f2287b81380b..8e03c1c18229cb167b7175f6743407a51688a180 100644 --- a/tor_description/urdf/torso/torso.urdf.xacro +++ b/tor_description/urdf/torso/torso.urdf.xacro @@ -13,6 +13,7 @@ <!--File includes--> <xacro:include filename="$(find tor_description)/urdf/deg_to_rad.xacro" /> <xacro:include filename="$(find tor_description)/urdf/torso/torso.transmission.xacro" /> + <xacro:include filename="$(find tor_description)/urdf/sensors/imu.urdf.xacro" /> <!--Constant parameters--> <xacro:property name="torso_max_vel" value="5.4" /> @@ -78,17 +79,17 @@ <joint name="${name}_1_joint" type="revolute"> <parent link="${name}_2_link"/> <child link="${name}_1_link"/> - <origin xyz="0.02000 0.00000 0.0" + <origin xyz="0.0 0.0 0.0" rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/> <axis xyz="0 1 0" /> - <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}" + <limit lower="${-45.0 * deg_to_rad}" upper="${15.0 * deg_to_rad}" effort="${torso_max_effort}" velocity="${torso_max_vel}"/> <dynamics damping="1.0" friction="1.0"/> <!-- <safety_controller k_position="20" k_velocity="20" - soft_lower_limit="${-75.0 * deg_to_rad + torso_eps}" - soft_upper_limit="${ 75.0 * deg_to_rad - torso_eps}" /> --> + soft_lower_limit="${-15.0 * deg_to_rad + torso_eps}" + soft_upper_limit="${ 45.0 * deg_to_rad - torso_eps}" /> --> </joint> <!--************************--> @@ -106,7 +107,7 @@ <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> - <mesh filename="package://tor_description/meshes/torso/base_link.STL" scale="1 1 1"/> + <mesh filename="package://tor_description/meshes/torso/base_link_2.STL" scale="1 1 1"/> </geometry> <material name="LightGrey" /> </visual> @@ -122,7 +123,6 @@ <joint name="${name}_2_joint" type="revolute"> <parent link="${name}_1_link"/> <child link="base_link"/> - <!-- <child link="${name}_2_link"/> --> <origin xyz="0.0 0.0 -0.2012" rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/> <axis xyz="0 0 1" /> @@ -137,14 +137,10 @@ </joint> + <xacro:tor_imu name="imu" parent="${name}_2_link" update_rate="100.0"> + <origin xyz="0.04925 0 0.078" rpy="${180.0*deg_to_rad} 0 ${-90.0*deg_to_rad}"/> + </xacro:tor_imu> -<!-- <joint name="torso_1_to_base_link" type="fixed"> - <parent link="${name}_1_link"/> - <child link="base_link"/> - <origin xyz="0.0 0.0 0.0" - rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/> - <axis xyz="0 0 0" /> - </joint> --> <gazebo reference="${name}_1_link"> <mu1>0.9</mu1> @@ -154,6 +150,10 @@ <mu1>0.9</mu1> <mu2>0.9</mu2> </gazebo> + <gazebo reference="base_link"> + <mu1>0.9</mu1> + <mu2>0.9</mu2> + </gazebo> <gazebo reference="${name}_1_joint"> <implicitSpringDamper>1</implicitSpringDamper>