search

DAIRLab / dairlib

MIT License
66 stars 26 forks source link

Changing number of Mode constraints failing due Jacobian size check #216

Closed ShaneRozenLevy closed 3 years ago

ShaneRozenLevy commented 3 years ago

I'm currently running dircon on a multi mode sequence where the planar biped goes from a double support phase to a single flight phase. The code fails with error: what(): Failure at multibody/kinematic/kinematic_evaluator_set.cc:121 in EvalFullJacobian(): condition 'J->rows() == count_full()' failed.

In the kinematic_evaluator_set's CalcTimeDerivativesWithForce the Jacobian is initialized as static, thus when the number of constraints change from 4 to 0, the number of rows in the Jacobian doesn't change. As a result it fails when check in EvalFullJacobian. A similar error happens in dircon_opt_constraints.cc's DirconCollocationConstraint.

My proposed fix is to change line 98 of dircon_opt_constrain.cc to drake::MatrixX<T> J(evaluators_.count_full(), plant_.num_velocities()); and change line 229 of kinematic_evaluator_set to MatrixX<T> J(count_full(), plant_.num_velocities());.

mposa commented 3 years ago

Thanks for submitting all of these issues! Feel free to file pull requests, if you prefer.

On this one, I'm not quite seeing the bug. Aren't the Jacobian's being initialized to the proper size? The use of static is a (relatively minor) performance optimization to avoid extra memory allocation within those high-frequency methods.

I suspect there's an issue with how you're handling multiple modes, that could perhaps use better documentation or an error message. Each mode should be associated with it's own unique KinematicEvaluatorSet. Are you modifying a KinematicEvaluatorSet after using it?

ShaneRozenLevy commented 3 years ago

The Jacobians do not seem to be initialized to the correct size. If for the double support phase the Jacobian has 6 rows it will still have 6 rows for the flight phase.

Here is an example script/branch that fails. I added printouts with the number of rows and constraints: https://github.com/KodlabPenn/dairlib/blob/issue_216_example/examples/PlanarWalker/run_jump_dircon.cc

Here is the same script, but I have applied my change (though with no printouts): https://github.com/KodlabPenn/dairlib/blob/biped_jump/examples/PlanarWalker/run_jump_dircon.cc

mposa commented 3 years ago

Thanks! I see the issue now, due to my own misunderstanding of how static worked in this setting. PR incoming.