Provide gradients of a MathematicalProgram solution

[RussTedrake]

This came up in a discussion with @hongkai-dai about how to template the RigidBodyPlant dynamics method on AutoDiffXd. Right now, we cannot, because the call to MathematicalProgram in the middle only makes sense for doubles.

But most mathematical programs have well defined gradients. We should be able to provide them. For instance, for the constrained optimization in with f(vdot, q, v) = 0, once we have a solution, then we can evaluate the gradient of that solution (e.g. with respect to vdot) using dfdvdot + dfdq + dfdv = 0.

Even when we have inequality constrained optimization, this should work. f(x) >=0 can be evaluated as f(x) = 0 for all of the constraints that are active at the solution. For optimizations with objectives, we simply use the KKT conditions.

It should be possible to provide some nice interface for this. Perhaps we can even be clever enough that GetSolution(DecisionVariableMatrix) could be templated on scalar type, which returns the solution for double and the gradients for autodiff?

[pvarin]

This would be awesome. We're trying to run dircol on the Kuka arm right now, and this would let us use the dircol solver that's already implemented in Drake. cc @kuindersma

[pvarin]

Actually, @hongkai-dai, where does this fall on your list of priorities? This is pretty high-pri for the Draper project, so let me know if there's anything I can do to help push this through.

[hongkai-dai]

@pvarin OK, I will try to work on this issue this week.

Drake's dircol solver already works. This issue is to find the gradients of the optimal solution, w.r.t some parameters of the optimization problem. For example, a linear program

min c'*x
s.t A*x = b

we want to know how the optimal solution x* would change, if the parameters A, b are changed.

Could you explain why you need this feature?

[pvarin]

Awesome! Thanks!

We'd like to do some dynamic planning on the Kuka arm, which is a RigidBodyTree that loads it's model from a URDF. I've been following the Pendulum example to try to use Drake's dircol solver, and from what I can tell we need the RigidBodyPlant to be templated for AutoDiff types.

If you have any other ideas on how to accomplish this I'd love to know. Thanks!

[hongkai-dai]

Hmm, if that is the case, could you just comment out https://github.com/RobotLocomotion/drake/blob/master/drake/multibody/rigid_body_plant/rigid_body_plant.cc#L242~L313, and compute vdot directly as

M.inverse()*(B*u -c(q, v))

If you do not hit the joint limits (which you can impose a constraint in your optimization, to see the bounds on each joint is tighter than the joint limits.), then you do not have any constraint force, and can compute vdot in closed form above, without using optimization. In that case, you can template RigidBodyPlant for AutoDiff.

I am afraid I have some other tasks piled up now. So we prefer to a hacky way to solve this issue for now.

[amcastro-tri]

I agree with @hongkai-dai here. One important reason you cannot take gradients in RigidBodyPlant is because everything related to collisions only works for double's right now. See the call to ComputeMaximumDepthCollisionPoints which you can see here in the RBT code can only take a KinematicsCache<double>. That essentially is because all the underlying collision detection with Bullet uses double's.

Summarizing, most likely you don't need collision on a first approximation to run direct collocation for Kuka? and if that is the case you can disable the collision code as honkai-dai mentioned (though hacky I know, buts just to try the strategy).

@RussTedrake, how do you guys take gradients through the collision engine in Matlab? what approach do you use when you need say gradients of minimum distance with respect to generalized coordinates?

[rdeits]

@amcastro-tri I've actually been working on a related problem in my work outside of Drake, where I'm taking the gradients of closest-point computations. I ended up implementing the GJK algorithm for generic types, which makes it easy to autodiff all the way through my collision detection code: https://github.com/rdeits/EnhancedGJK.jl

I imagine this code won't actually be helpful to you, but I'm happy to share what I learn as I keep working on this.

[edrumwri]

Robin, just FYI- I have yet to meet anyone who has coded a numerically robust GJK implementation. We've had better mileage with v-clip.

On Thu, Dec 1, 2016 at 1:24 PM, Robin Deits notifications@github.com wrote:

@amcastro-tri https://github.com/amcastro-tri I've actually been working on a related problem in my work outside of Drake, where I'm taking the gradients of closest-point computations. I ended up implementing the GJK algorithm for generic types, which makes it easy to autodiff all the way through my collision detection code: https://github.com/rdeits/ EnhancedGJK.jl

I imagine this code won't actually be helpful to you, but I'm happy to share what I learn as I keep working on this.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/4267#issuecomment-264299740, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwz-K3qhehJdGGQ9lR5aKKpcHzr87xks5rDzsigaJpZM4K9UcT .

[rdeits]

@edrumwri understood, and I haven't even begun to test for numerical robustness. My particular problem is relatively forgiving of errors in individual distance computations, but I am running into issues extracting stable penetration distances when the objects are in collision. Maybe it's time to give V-clip another look...

[RussTedrake]

Huge number of topics discussed here... almost none of them related to this issue. :)

@pvarin - you need autodiff for rigidbodyplant. That is issue #4187 , and is not related to this issue. @amcastro-tri - we should move your discussion there, too.

[hongkai-dai]

This is useful for bi-level optimization, Benoit is likely to work on this over the summer intern.

[RussTedrake]

for bonus points: It would be really awesome if a system that setup and solved a mathematical program inside e.g. a derivatives/update/output method could easily use this to support autodiff. I don't think implementing Solve<T>(prog, ...) is quite enough... and think we should open another issue if/when we want to do this. I just wanted to add the idea here in case it impacts any design decisions when we address this request.

[hongkai-dai]

Just to summarize the math to compute the gradient of the solution here.

Say we have a generic optimization problem

min_x f(x, p)
s.t  g(x, p) = 0

where x is our decision variable, p is some parameters of the constraint/cost. For a given value of p, the optimal solution to the problem is x*, we want to compute the gradient of the optimal solution w.r.t p, namely dx*/dp. Notice that we ignore the inequality constraint here, as we can just pick out the active inequality constraint at the solution x*, and set these inequality to equality.

According to KKT condition, at the optimal solution, it satisfies

∂f/∂x + λᵀ∂g/∂x = 0
g(x, p) = 0

We can take the total derivative of the two functions above, and get

∂²f/∂x² * dx + ∂²f/∂x∂p * dp + λᵀ(∂²g/∂x² * dx + ∂²g/∂x∂p) + (∂g/∂x)ᵀ dλ = 0
∂g/∂x * dx + ∂g/∂p * dp = 0

In order to compute dx/dp, we divide both equations by dp, and obtain the following linear equations on dx/dp, dλ/dp

[∂²f/∂x²+λᵀ∂²g/∂x²,   (∂g/∂x)ᵀ] * [dx/dp] = [-∂²f/∂x∂p - λᵀ∂²g/∂x∂p]
[∂g/∂x,                      0] * [dλ/dp]   [-∂g/∂p]

If we solve this linear equation (assuming the matrix on the LHS is invertible), we get the gradient dx/dp.

It is non-trivial to implement this. We need the second order gradient ∂²f/∂x², ∂²g/∂x², ∂²f/∂x∂p, ∂²g/∂x∂p. I remember Twan has implemented some second order computation using Eigen::AutoDiff<Eigen::AutoDiffXd, Eigen::Dynamic, 1> (nested Eigen autodiff). But I haven't used it for a long time, and Drake's EvaluatorBase doesn't support this nested Eigen autodiff as a scalar type.

[hongkai-dai]

Take inverse kinematics as an example, I think we need several infrastructures in order to compute the gradient of the optimal solution

1. EvaluatorBase need to support the scalar type that can return second order gradient (maybe with nested Eigen::AutoDiff as this scalar type).
2. MultibodyPlant need to support this scalar type, so that we can compute the second order gradient of forward kinematics function.
3. SceneGraph need to support this scalar type, so that we can compute the second order gradient of collision query. (This is very challenging, as SceneGraph doesn't support AutoDiffXd yet).

[RussTedrake]

We did a lot of nesting AutoDiff before -- it certainly works. Hopefully we don't feel like we've architected ourselves out of that. But what might it look like if we only did it for EvaluatorBase constraints that had support for Symbolic first?

[hongkai-dai]

Agreed, we can make EvaluatorBase to work with nested AutoDiffScalar. I just want to outline the infrastructure we need to achieve the eventual goal -- computing the gradient of the motion planning solution.

I can add the scalar type ADS<ADS<Eigen::VectorXd>> to EvaluatorBase and start from there.

[RussTedrake]

FWIW -- this came up again in https://stackoverflow.com/questions/65724598/pydrake-extracting-autodiff-gradients-from-a-robot-controlled-via-differential/