# Very heavy platforms in Gazebo

Hi all!

I'm trying to simulate a very heavy four wheeled platform in Gazebo. My target weight is 45 tons, however, I actually achieve to simulate "stably" 35 tons. I followed ODE manuals and I tried to implement some of the tricks they give to simulate such platforms. For example, I split the whole platform into several smaller bodies that are connected with fixed joints, to reduce mass ratios between bodies. In addition, I also played with some parameters as Error Reduction Parameter (ERP) and Constraint Force Mixing (CFM). In general, if I don't make joints soft enough, Gazebo crashes. But when I simulate >35 tons platforms, joints are so soft that the platform penetrates the ground and cannot be used. I cannot find any good solution.

Does anybody have any clue about how to simulate those kinds of platforms? Which ODE parameters are crucial and how they should be fixed?

edit retag close merge delete

is it possible for you to post the model and world files? thanks.
( 2011-06-28 02:03:52 -0500 )edit

Sort by » oldest newest most voted

OK. Thanks a lot! So, from my understanding of how ODE works, the problem here arises when the wheels and the ground plane make contact. ODE creates contact joints and the erp and cfm of those contact joints are calculated from the kp and kd of each of the bodies (wheels and ground plane) the way you explained before. Is this correct?

We can change the kp and kd of the ground plane directly in the world file. However, in the same world file we can also set cfm and erp values for the general case inside a <physics:ode> tag. How are those values related? How do those "general" cfm and erp influence the calculation of new contact joints' cfm and erp?

Besides, if I'm correct, I would also have to set the kp and kd values of the wheels. My wheels are modeled in a urdf file. As far as I know from the urdf documentation and tutorials, links do not have any kp and kd parameter. How could I set those parameters, if they have to be set?

Finally, fixing the kp and kd of the ground plane of the world file, I achieved a reasonable solution (kp=1.0e+9 and kd=1.0e+5) that seems to work fine. However, a "small" portion of the wheels are under the floor. Does it have any effect in the behavior of the platform or should I forget about it?

Just in case it might be useful for you, I put the links to download the files needed to make my platform work in Gazebo (I use dropbox for that purpose, since I didn't see here any other way to do it; hope it's fine for you): http://dl.dropbox.com/u/23291294/platform_description/base.gazebo.xacro http://dl.dropbox.com/u/23291294/platform_description/base.transmission.xacro http://dl.dropbox.com/u/23291294/platform_description/experimental_base.urdf.xacro http://dl.dropbox.com/u/23291294/platform_description/empty.world

more

@gazkune could you please shed some light on implementing the joint controllers? I tried with the new ros_control controllers but the joint hardly moves (due to the heavy platform).

( 2014-04-16 16:39:28 -0500 )edit

well it turned out there was a problem with the limits of my joints. http://gazebosim.org/sdf/1.4.html#limit375 says with -1 limits are not enforced but it did not work. With an appropriate value now the joints are moving

( 2014-04-17 22:06:13 -0500 )edit

Without more details, this might be a classical convergence problem for the projected Gauss-Seidel (PGS) algorithm implemented in ODE (quickstep). This happens when the constraint matrix Jinv(M)J' is ill conditioned (large mass ratios, i.e. chasis mass >> wheel mass).

Here are some suggestions to try. For the bodies touching the ground (wheels), try to keep contact_cfm as close to 0 as possible, and contact_erp around 0.2. Given the relationship between kp,kd and cfm,erp below:

contact.surface.soft_erp = h * kp / (h * kp + kd);
contact.surface.soft_cfm = 1.0 / (h * kp + kd);


If time step size h = 0.001 (1ms), following kp and kd values should be a decent starting point:

<kp  value="1.0e+9" />
<kd  value="5.0e+6" />


yielding soft_erp of ~0.17 and soft_cfm of ~1.7e-07 for contcts.

Further, raise number of inner iterations:

<stepIters>500</stepIters>


I tested these values by setting PR2 base mass to 45tons, and was able to simulated it ok.

By the way, are you running the diamondback or unstable release?

John

more

Thanks! I'm running the diamondback release. I will try your solution as soon as possible and give you any feedback.
( 2011-06-28 19:19:38 -0500 )edit

This is something that I would like to change down the road by allowing users to set cfm and erp for contact surfaces directly rather than setting kp and kd.

Currently, when two surfaces come into contact, the contact properties are specified via equivalent stiffness kp and equivalent damping kd coefficients for each contact surface involved. There are two steps to deducing cfm and erp from the two pairs of stiffness and damping parameters (kp1, kp2, kd1 and kd2 for bodies 1 and 2).

First, an overall kp is computed from kp1 and kp2 as if they are two springs in series, and an overall kd is computed from kd1 and kd2 as two dampers in parallel:

kp = 1 / ( 1/kp1 + 1/kp2 )
kd = kd1 + kd2


Then, cfm and erp are computed via following equations (see R. Smith, How to make new Joints in ODE):

cfm = 1 / ( dt * kp + kd )
erp = dt * kp / ( dt * kp + kd )


Given that quickstep is a very stable algorithm compared to worldstep, cfm should be zero for rigid body contacts (a non-zero cfm yields "soft surface" / spring damper dynamics).

erp dictates transient behavior of the contact as the contact tries to repair any constraint violations (due to drift or fixed step size collision issues). Empirically 0.2 is a good approximation for well behaved system, larger erp values tends to induce oscillation or simulation instability; smaller erp values can result in slow penetration repair transients.

For your case, it is more desirable to target:

• cfm ~ 0 (kp >> 1)
• erp ~ 0.2 (kd = dt * kp * ( 1/ erp - 1) )
• iterating your PGS to 0 (stepIters >> 1).

Especially given some of the mass ratios between bodies in your system are large, you might want to experiment making cfm near machine zero.

Hope this makes sense.

more

I tested your solution. The simulation is stable, but a portion of the wheels penetrates the ground. I don't know whether this can influence the behavior of the platform (increasing friction for example). Anyway, I post the urdf of the platform and the world file with the updates suggested by you. Just a note: I changed the kp and kd of the ground plane model in the world file, and the cfm and erp of the wheel joint (is this correct?).

Platform (it is based on the PR2 base):

<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://ros.org/wiki/xacro">

<include filename="$(find prototype_description)/urdf/base.gazebo.xacro" /> <include filename="$(find prototype_description)/urdf/base.transmission.xacro" />

<property name="M_PI" value="3.1415926535897931" />

<property name="caster_offset_x" value="1.5" />
<property name="caster_offset_y" value="0.5" />
<property name="caster_offset_z" value="0.0282" />

<property name="caster_wheel_offset_y" value="0.049" />
<property name="wheel_length" value="0.085" />
<property name="base_mass" value="5000.0" />

<!-- simplified box collision geometry for base -->
<property name="base_size_x" value="4.0" />
<property name="base_size_y" value="2.0" />
<property name="base_size_z" value="0.2" />
<property name="base_collision_size_z" value="0.04" />

<!--                                                      -->
<!--           wheel                                      -->
<!--                                                      -->
<xacro:macro name="experimental_wheel_v0" params="suffix parent reflect"> <!-- The idea is to remove reflect from here -->

<joint name="${parent}_${suffix}_wheel_joint" type="continuous">
<axis xyz="0 1 0" />
<limit effort="500" velocity="100"/>
<safety_controller  k_velocity="10" />
<dynamics damping="0.0" friction="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${parent}_rotation_link"/> <child link="${parent}_${suffix}_wheel_link"/> <cfm>0.00000017</cfm> <erp>0.17</erp> </joint> <link name="${parent}_${suffix}_wheel_link"> <inertial> <mass value="10.44036" /> <!-- original value = 0.44036 --> <origin xyz="0 0 0" /> <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943" /> </inertial> <visual> <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" /> <!-- original rpy="0 0 0"-->
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>

<material name="Wheel_${suffix}" /> </visual> <collision> <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" /> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
</collision>

<!-- extensions -->
<xacro:experimental_wheel_gazebo_v0 parent="${parent}" suffix="${suffix}" />
<xacro:prototype_wheel_transmission_v0 parent="${parent}" suffix="${suffix}" reflect="${reflect}" /> </xacro:macro> <!-- Macro for caster hub only --> <xacro:macro name="experimental_caster_hub_v0" params="suffix parent *origin ref_position" > <joint name="${suffix}_rotation_joint" type="continuous">
<axis xyz="0 0 1" />
<limit effort="1000" velocity="1000"/>
<safety_controller  k_velocity="10" />
<calibration rising="${ref_position}" /> <dynamics damping="0.0" friction="0.0" /> <insert_block name="origin" /> <parent link="${parent}"/>
<child link="${suffix}_rotation_link" /> </joint> <link name="${suffix}_rotation_link">

<inertial>
<mass value="53.473082"/>
<origin xyz="0 0 0.07" />
<inertia  ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943 ...
more

thanks. It doesn't un-xacro for me due to missing files (base.gazebo.xacro, etc.). It would be helpful if I can run the model and test it?
( 2011-06-29 11:53:37 -0500 )edit