2

I'm working on a biped robot simulation in pydrake. My robot now could stand well or even jump by using inverse dynamics and optimizing contact forces when the step time is 0.0001s. However, the feet penetrate the ground when I set the step time 0.001s. Because time step 0.001s usually used in the real time discrete system .

Is it possible to prevent the interpenetration (between feet and ground when standing) in pydrake? I think the contact I'm using is the penalty method in pydrake, and the parameters related to that are shown below.

    1. The parameters (when step time is 0.0001s):
plant.set_penetration_allowance(0.0001)
plant.set_stiction_tolerance(0.001)
  • Ground set like:
ground_friction = CoulombFriction(static_friction=1,static_friction=1)
plant.RegisterVisualGeometry(plant.world_body(), RigidTransform(),HalfSpace(), "GroundVisualGeometry", diffuse_color=[1., 0.64, 0.0, 0.5])
plant.RegisterCollisionGeometry(plant.world_body(), RigidTransform(), HalfSpace(), "GroundCollisionGeometry", ground_friction)

No penetration here.

Standing when steptime is 0.0001

    1. The parameters (when step time is 0.001s):
plant.set_penetration_allowance(0.001) # if this param smaller than the step time 0.001, an error will be there
plant.set_stiction_tolerance(0.001)
  • Ground parameters are same.

Is it possible to fix the penetration of feet?

Standing when steptime is 0.001

Zhenyuan Fu
  • 21
  • 1
  • 1
    I actually had the same questions as Sean, the actual problem was not clear. You say, "Because time step 0.001s usually used in the real time discrete system". What do you mean by that? is that some sort of requirement in your controller? or are you trying to say that if the time step is smaller then the sim does not run real time? (I'd be surprised, your system looks pretty small?). But Sean's answer on the trade-off between penetration allowance (stiffness) and sim stability is spot-on. – Alejandro May 19 '21 at 14:26
  • Sorry for my question not clear. Since the system of our robots in real time uses the time step 0.001s, I want my time step of simulation in drake same as that in real world. That's why i need to increase the step time from 0.0001s to 0.001s. – Zhenyuan Fu May 20 '21 at 02:34
  • TBH I don't understand what you mean by "our robots in real time uses the time step 0.001s", you mean that you measure state at 1kHz? If so, that doesn't mean that the time step of your sim has to match that frequency. For sim the time step is usually given by how fast is the dynamics you are trying to solve + stability. Your actually physical system is continuous. dt only tells you we are using a discrete approximation of that system. Regardless of measurements. My question is, why is it that dt=1e-4 is bad for you? does it match reality reasonably? – Alejandro May 21 '21 at 13:18

2 Answers2

3

First thing to recognize is that contact will always lead to penetration in Drake. Contact in Drake is akin to a penalty-based method -- the deeper the penetration, the greater the normal force. You can tune various parameters such that for a robot with a given mass, the amount of penetration is reduced, but it will never go to zero.

You can change how much penetration happens by tuning the set_penetration_allowance() value. The bigger that value, the more penetration you'll get. However, the smaller that value gets, the stiffer your system gets and the more expensive (and difficult) it is to simulate. In principle you should not increase this value just because you've changed the discrete time step (although, wildly incompatible penetration allowance and time step can lead to insoluble configurations).

My advice is to set the discrete time step to the value you want, and then gradually decreasing the "penetration allowance" to the largest value that gives you results that are "good" (using whatever "goodness" metric you have).

Out of curiosity, I'm unclear as to what the actual problem is. Is the penetration causing issues? Or is it merely surprising? Certainly, any code that assumes that the soles of the feet can't go below the plane of the ground will be mistaken, so you might have to make accommodations there.

It might help giving advice if you were more clear on what problems the reported penetration is causing. It would better enable the community to understand the problem and provide suggestions drawing from a larger toolbox.

Sean Curtis
  • 1,425
  • 7
  • 8
  • Thanks a lot. To follow your advice, I am trying to decrease the 'set_penetration_allowance()' value. For now, my parameters are smaller as those before (plant.set_penetration_allowance(0.0001) ). But the penetration is still serious and large. In the 5-seconds long simulation, my robot stand and keep the feet penetration. I'm curious about that, once I change the time step back (0.001s), the penetration is gone (or really really small as I showed in the picture above). – Zhenyuan Fu May 20 '21 at 02:09
  • For the simulation, if there is very very large penetration I cannot ignore, the height of the robot is not accurate then it cannot follow my desired COM (center of mass) trajectory. – Zhenyuan Fu May 20 '21 at 03:40
  • What's the total mass of your robot, and how much penetration depth are you observing for a penetration allowance of 0.0001? And how many feet are in contact with the ground for that amount of penetration? – Sean Curtis May 21 '21 at 13:11
0

Luckily, I know what happened to my robot now. The reason of the penetration is the obj format of collision geometry. I am now using a simple cuboid instead of the obj collision in my URDF, then the problem is gone(no penetration)! I think the main reason is because the obj collision might not be perfectly solved in discrete time by the contact function in drake.

Zhenyuan Fu
  • 21
  • 1