Workflow for "Through-Contact" Trajectory Optimization in Drake

user_15272904 0
Viewed 203

I would like ask what would be an appropriate workflow for solving a trajectory optimization problem that takes into account contact interactions between different bodies in an MBP in Drake. Problems like the ones addressed in this paper. I've tried using DirectCollocation class but apparently it does not consider contact forces as optimization variables. Is there anyway to incorporate them into the mathematical program generated by DirectCollocation? Thanks.

drake
1 Answers

There are several things to consider when you plan through contact

  1. The decision variables are not only the state/control any more, but should include the contact forces.
  2. The constraints are not only the dynamic constraint x[n+1] = f(x[n], u[n]), but x[n+1] = f(x[n], u[n], λ[n]) where λ is the contact force, and also the complementarity constraint 0≤ λ ⊥ ϕ(q) ≥ 0
  3. Instead of using direct collocation (which assumes the state trajectory being a piecewise cubic spline), Michael's paper uses just backward Euler integration, which assumes piecewise linear interpolation.

So you would need the following features:

  1. Add the contact forces as decision variables.
  2. Add the complementarity constraints.
  3. Use backward Euler integration instead of DirectCollocationConstraint.

Unfortunately Drake doesn't have an implementation doing these yet. The closest match inside Drake is in this folder, and the starting point could be the StaticEquilibriumProblem. This class does feature 1 and part of feature 2 (it has the complementarity constraint, but only static equilibrium constraint instead of dynamic constraint), but doesn't do feature 3 (the backward Euler step).

user_19738610
Related