CODEWITHSUNDEEP
pythondrake
Zhenyuan Fu
Zhenyuan Fu

Reputation: 21

Pydrake: About Penetration in discrete time simulation

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.

plant.set_penetration_allowance(0.0001)
plant.set_stiction_tolerance(0.001)

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

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)

Is it possible to fix the penetration of feet?

Standing when steptime is 0.001

Upvotes: 2

Views: 189

Answers (2)

Zhenyuan Fu
Zhenyuan Fu

Reputation: 21

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.

Upvotes: 0

Sean Curtis
Sean Curtis

Reputation: 1883

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.

Upvotes: 3

Related Questions