Joint limits are not respected

[Atlis]

Hello,

Joint limits (defined in the .urdf file) are not respected when forces (or torques) acting on a given body are too high. In my case, the joints are free to move (no damping, no controller). Also, forces that cause problems are applied by an external torque. I made sure to saturate the torque value to make sure they remain physically possible. In fact, I had to saturate them to even lower values. The problem is also often triggered when the body comes into contact with the ground. Reducing the time step to 0.0001 seconds helps, but it does not completely solve the problem. When, the limits are exceeded, the body is free to rotate, while torque continues to apply. It all goes crazy and the simulation crashes.

Is there something on my side I could do to mitigate the problem?

Thank you!

[jhwangbo]

Joint limits are solved by the contact solver and playing with the parameters will help. You can check the "contact tab" in raisimOgre to figure out if the error is decreasing properly. Can you send me your urdf? I can also check and see if this is a bug or not

[Atlis]

Thank you for the fast reply!

I'm using laikago.cpp as a baseline to test functionalities. To replicate the problem, simply set the lower and upper limits of joints to small values, for instance: <limit effort="55" lower="-0.001" upper="0.001" velocity="28.6"/> and get completely rid of the PD controllers and add external torque (10 Nm should suffice to go pass the limits).

Now, I'm realising that the maximum velocity of 28.6 rad/s is sufficient to go pass the 0.001 rad limit within a time step of 0.001 s (and even 0.0001 s). Could that be the problem? I tried reducing the velocity, but it did not help.

You can check the "contact tab" in raisimOgre to figure out if the error is decreasing properly.

Indeed, the first contact seems to diverge. The contact tab gives a value of 0: -inf and 1: -inf.

Joint limits are solved by the contact solver and playing with the parameters will help.

Indeed, beside the time step, I also played with setERP(), and it helps, but the problem still occurs.

Now, I managed to solve the problem by adding PD controllers to the joints (to simulate stiffness and damping). The simulation is robust now and it does not crash anymore (and I'm totally happy with that). But the PD gains must be kept high (P=200 and D=10, the defaults in laikago.cpp, for instance, is just fine, but P=20 and D=1 will tend to crash).

[jhwangbo]

Such a narrow joint limit gap is not supposed to work. The default erp and solver parameters are not set to work with such an unrealistic scenario. You should just use a fixed joint instead

[Atlis]

By using fixed joints, bodies are no longer accessible by a bodyIdx and therefore, it is not possible to add external forces and torque to them using addExternalForce() and addExternalTorque() since those functions require bodyIdx. Being aware that fixed bodies were "simplified-out", and that I could not use bodyIdx to apply force and torques, I was temped to try "simulating" fixed bodies by limiting the range of the joints.

In cases for which it is required to apply an external force on the fixed bodies, the positions offset can be computed relatively easily and pass to addExternalForce(), although it is not so practical from a user standpoint. But for the application of external torque, the fact that it is currently not possible to specify the location of the torque using addExternalTorque() causes more trouble. Perhaps there is an other way around that I am unaware of, in which case I would really like to know. Thank you very much and good job by the way!

[jhwangbo]

Hi Atlis, It is true that this is not so convenient now. I'll push an update on this. But I want to point out a few points so that it is not confusing when you see the changes.

URDF protocol does not assign a frame on a body. This is what confuses many people. In kinematics/dynamics classes, a body frame is actually the frame of the joint (the one attached to the moving body). But if I call this body frame, many people will assume that it is attached to the COM.

So I call this just CoordinateFrame in RAISIM. If you want to apply force at a specific location, you can attach a fixed joint and add a zero-mass link. Then this joint will appear in the frame list. Currently, you have to perform coordinate transformation yourself to apply the force you want. I'll add a method that you can specify the coordinate frame for setExternalForce. I'll also add a method that returns bodyIdx if you specify a fixed joint name.

[Atlis]

Hi jhwangbo, sorry for the delayed answer. I appreciate the comment. Thanks!