Search code examples
drake

Quaternion elements go to infinity when giving non-zero input to discrete time system


I am trying to run a simulation of a floating base robot in discrete time. However, when I give a non-zero input to the robot, the quaternion elements go to infinity. The issue can be reproduced in this Colab notebook. I do not have the same issue in continuous time, however running the simulation in continuous time takes much longer (10 minutes rather than a couple of seconds) due to the contact points at the robot foot.


Solution

  • It sounds like your system is unstable (or very close to instability). The continuous-time solver taking a long time is normally an indication that the error-controlled integrator is taking exceedingly small steps. I suspect there is a problem in your dynamics description. Perhaps some inertias that are zero or nearly zero?