Improve correction of positional constraint violation in dynamics simulation

[jslee02]

Currently, DART corrects positional violation using artificial velocity with ERP. High ERP may correct the violation fast, but it can make the system unstable by adding too much artificial energy. On the other hand, low ERP may not correct the violation sufficiently fast. This makes DART requires more error tolerance than other physics engines in the gazebo unit tests including this joint limit test.

DART probably need to correct the positional constraint violation directly by solving LCP twice for both of positional and velocity constraint violations as ODE does (it will sacrifice the performance). Or, we should consider using better constraint handling method if possible.

comment