From 3f05230bab8afef35fa50add10304ba4c4f3f2b8 Mon Sep 17 00:00:00 2001
From: Hilario Tome <hilario.tome@pal-robotics.com>
Date: Mon, 29 Aug 2016 12:15:57 +0200
Subject: [PATCH] Fixed gazebo contact model
---
tor_bringup/config/tor_hardware.yaml | 12 ++++----
tor_description/gazebo/gazebo.urdf.xacro | 12 ++++----
.../robots/tor_lower_body.urdf.xacro | 29 ++++++++++++++++++-
tor_description/urdf/leg/leg.urdf.xacro | 15 +++++++++-
4 files changed, 55 insertions(+), 13 deletions(-)
diff --git a/tor_bringup/config/tor_hardware.yaml b/tor_bringup/config/tor_hardware.yaml
index 7916d06..535234a 100644
--- a/tor_bringup/config/tor_hardware.yaml
+++ b/tor_bringup/config/tor_hardware.yaml
@@ -30,11 +30,13 @@ force_torque:
force_port: force_ankle_right
torque_port: torque_ankle_right
-#imu:
-# base_imu:
-# frame: base_link
-# raw_data:
-# orientation_port: orientation_base
+imu:
+ base_imu:
+ frame: base_link
+ raw_data:
+ orientation_port: orientation_base
+ linear_acceleration_port: linear_acceleration_base
+ angular_velocity_port: angular_velocity_base
#ros_control_component:
# ignore_read_errors: true
diff --git a/tor_description/gazebo/gazebo.urdf.xacro b/tor_description/gazebo/gazebo.urdf.xacro
index d162bf4..4784b4a 100644
--- a/tor_description/gazebo/gazebo.urdf.xacro
+++ b/tor_description/gazebo/gazebo.urdf.xacro
@@ -11,7 +11,7 @@
</gazebo>
<gazebo>
- <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_force">
+ <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
<frameName>base_link</frameName>
<topicName>floating_base_pose</topicName>
</plugin>
@@ -46,7 +46,7 @@
bias means and stddevs to produce biases close to the provided
data. -->
- <!--
+
<rate>
<mean>0.0</mean>
<stddev>2e-4</stddev>
@@ -59,22 +59,22 @@
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
- -->
+ <!--
<rate>
<mean>0.0</mean>
- <stddev>2e-3</stddev>
+ <stddev>0.0</stddev>
<bias_mean>0.000000</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</rate>
<accel>
<mean>0.0</mean>
- <stddev>1.7e-1</stddev>
+ <stddev>0.0</stddev>
<bias_mean>0.0</bias_mean>
<bias_stddev>0.000</bias_stddev>
</accel>
-
+ -->
</noise>
</imu>
diff --git a/tor_description/robots/tor_lower_body.urdf.xacro b/tor_description/robots/tor_lower_body.urdf.xacro
index 5cc159c..6454617 100644
--- a/tor_description/robots/tor_lower_body.urdf.xacro
+++ b/tor_description/robots/tor_lower_body.urdf.xacro
@@ -16,6 +16,7 @@
<xacro:include filename="$(find tor_description)/urdf/leg/leg.urdf.xacro" />
+
<link name="base_link">
<inertial>
<origin xyz="0.0 0.0 0.36" rpy="0 0 0"/>
@@ -39,7 +40,33 @@
</link>
+
+ <!--
+
+ <link name="base_link">
+ <inertial>
+ <origin xyz="0.0 0.0 0.05" rpy="0 0 0"/>
+ <mass value="5.0" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+ </inertial>
+
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://tor_description/meshes/torso_dummy.stl" scale="1 1 1"/>
+ </geometry>
+ </visual>
+
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.05"/>
+ <geometry>
+ <box size="0.1 0.3 0.1"/>
+ </geometry>
+ </collision>
+
+ </link>
+ -->
<xacro:tor_leg prefix="left" reflect="1" />
<xacro:tor_leg prefix="right" reflect="-1" />
@@ -53,7 +80,7 @@
<!-- imu -->
<xacro:tor_imu name="imu" parent="base_link" update_rate="100.0">
- <origin xyz="0 0 0" rpy="0 0 0"/>
+ <origin xyz="0.069 0 0.14647" rpy="3.14 0.0 1.57"/>
</xacro:tor_imu>
</robot>
diff --git a/tor_description/urdf/leg/leg.urdf.xacro b/tor_description/urdf/leg/leg.urdf.xacro
index 65f54bf..f36de3e 100644
--- a/tor_description/urdf/leg/leg.urdf.xacro
+++ b/tor_description/urdf/leg/leg.urdf.xacro
@@ -270,7 +270,8 @@
<mu2>0.9</mu2>
</gazebo>
- <!-- contact model for foot surface -->
+ <!--
+
<gazebo reference="leg_${prefix}_6_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
@@ -281,6 +282,18 @@
<minDepth>0.003</minDepth>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
+
+ -->
+
+ <gazebo reference="leg_${prefix}_6_link">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>1.5</mu1>
+ <mu2>1.5</mu2>
+ <fdir1>1 0 0</fdir1>
+ <maxVel>1.0</maxVel>
+ <minDepth>0.00</minDepth>
+ </gazebo>
<gazebo reference="leg_${prefix}_1_joint">
<implicitSpringDamper>0.1</implicitSpringDamper>
--
GitLab