Drake: Need to Implement And Use RigidBodyPlant<T>::DoToAutoDiffXd()

[liangfok]

RigidBodyPlant<T>::DoToAutoDiffXd() is needed by certain advanced optimization-based planners like DircolTrajectoryOptimization. This issue tracks the implementation and application of this feature.

• [x] Spiral 1 - Implement a deep copy of RigidBodyTree<double>. For now, abort if the RigidBodyTree contains any collision elements.
• [ ] Spiral 2 - Implement RigidBodyPlant<double>::DoAutoDiffXd(). This will require a deep copy of its RigidBodyTree<double>.
• [ ] ~Spiral 3 - Create a unit test modeled after drake/examples/Pendulum/test/pendulum_plant_test.cc except using a RigidBodyPlant<double> with a pendulum SDF loaded instead of a PendulumPlant.~ This spiral was deleted based on https://github.com/RobotLocomotion/drake/issues/4897#issuecomment-275002833.
• [ ] Spiral 4 - Create a version of drake/examples/Pendulum/pendulum_run_swing_up.cc that uses a RigidBodyPlant<double> with a pendulum SDF loaded instead of a PendulumPlant.

[jwnimmer-tri]

I'm not sure why Spiral 1 is important (i.e., why Spiral 2 needs a deep copy).

I'm also not sure why this is "Priority: High" and "Assignee: Liangfok"?

[liangfok]

@jwnimmer-tri sorry for the confusion. This is a feature that is needed by some immediate users. I assigned it to myself to allow @amcastro-tri to focus on MBT/MBW. The OP above is based on my rough recollection of what @RussTedrake, @amcastro-tri, and @siyuanfeng-tri told me needed to be done. It likely contains errors. I am currently seeking corrections from them.

[liangfok]

I'm not sure why Spiral 1 is important (i.e., why Spiral 2 needs a deep copy).

I believe a deep copy is needed because two RigidBodyPlant objects cannot own the same tree since it's stored in a unique_ptr.

[RussTedrake]

In a water-cooler conversation, @siyuanfeng-tri and I lamented the fact that we cannot do trajectory optimization, linearization, etc, on rigid body plant yet... despite @amcastro-tri getting it very close. Immediate use would be for trajectory optimization on iiwa, and to replace all of the matlab trajectory optimization examples. We were clear that it might not be top priority, but I'm glad that the blockage is documented; thanks @liangfok. Talking through the solution makes it seem not-so-difficult. cc @naveenoid because his collaborators have been asking for it, too. also @kuindersma .

I would skip "Spiral 3"; i don't see any value in that. Spiral 2 is just a few lines (which admittedly depend on possibly non-trivial template interactions) and Spiral 4 should "just work" if 1 and 2 are done correctly.

[liangfok]

I updated the OP to omit spiral 3. Thanks for the feedback.

[liangfok]

I tried to explicitly instantiate RigidBodyPlant<AutoDiffX> and ran into the following build-time error:

/home/liang/dev/drake-distro-1/drake/multibody/rigid_body_plant/rigid_body_plant.cc:640:20: error: no matching function for call to ‘min(const Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1> >&, double)’
     return std::min(limit_force, 0.);

Looks like RigidBodyPlant needs to be updated to support being templated using AutoDiffX.

This appears relevant: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1223

[RussTedrake]

try replace that line with two lines:

using std::min;
return min(limit_force, 0.);

[liangfok]

Thanks. Using the two lines above fixed the error. The next error I'm encountering seems to be more serious:

/home/liang/dev/drake-distro-2/drake/multibody/rigid_body_plant/rigid_body_plant.cc:402:3: error: static assertion failed: Only support templating on double for now

@amcastro-tri, any idea how hard updating RigidBodyPlant::DoCalcTimeDerivatives() to support AutoDiffX will be? (I assume you implemented the above static_assert.)

Update: @hongkai-dai offered a possible short-cut here: https://github.com/RobotLocomotion/drake/issues/4267#issuecomment-264078942

[jwnimmer-tri]

Per the comment in the code, prerequisites of making that work are #4267 and #4187. In fact, this issue is a duplicate of, and rehashes learnings already captured in #4187.

[liangfok]

Wow. Great find!

[liangfok]

It looks like this issue's spiral 2 may be a duplicate of #4044 as well.

[amcastro-tri]

@liangfok ,

@amcastro-tri, any idea how hard updating RigidBodyPlant::DoCalcTimeDerivatives() to support AutoDiffX will be? (I assume you implemented the above static_assert.)

First off, what breaks if you remove that static assert? though for sure something will break.
An option I think would be to use this SFINAE trick @david-german-tri has been using in System Framework for the transmogrification methods. That allows you to have two versions of a same method (say in this case double and AutoDiff) in the same class, super cool.
Then in this case the AutoDiff (or rather the non-double) version of the code would remove all the collision queries and would only use Eigen's Cholesky solver.

[liangfok]

First off, what breaks if you remove that static assert? though for sure something will break.

Here is the first error I encounter after commenting out the static_assert:

/home/liang/dev/drake-distro-2/drake/multibody/rigid_body_plant/rigid_body_plant.cc:478:59: error: no matching function for call to ‘RigidBodyTree<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1> > >::dynamicsBiasTerm(KinematicsCache<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1> > >&, const BodyToWrenchMap&) const’
       tree_->dynamicsBiasTerm(kinsol, no_external_wrenches);
                                                           ^

[hongkai-dai]

4267 is not the only reason why RBT is not templated on autodiffscalar. I think when we compute the dynamics using the manipulator equation

H(q)*vdot + C(q,v) = B'*u + J'*lambda

to compute xdot = [qdot;vdot] we have to invert the matrix H(q) in the manipulator equation above, to compute vdot. I believe Eigen autodiffscalar does not support the gradient of inverting a matrix.

[amcastro-tri]

The idea is that, assuming no collisions or loop joints, in the AutoDiffScalar version you'd replace drake::solvers::MathematicalProgram with Eigen::LLT which can be templated on T.

[hongkai-dai]

Good to know this. I was not aware that Eigen::LLT works with autodiffscalar.

[amcastro-tri]

Well, I don't think anybody tried :-). But in principle it should.

[amcastro-tri]

@liangfok, What do you think if I take care of the solution of the system in RigidBodyPlant::DoCalcTimeDerivatives() once you provide an RBT::Clone()? I think that'd be an efficient way to split this work.

[liangfok]

@amcastro-tri: Awesome! SGTM.

[amcastro-tri]

Excellent, yes, I know you are super busy working on other issues so I thought the famous divide and conquer strategy would work best in this case.

[liangfok]

@amcastro-tri, now that #4958 is merged, can you update RigidBodyPlant::DoCalcTimeDerivatives() so it can be templated on AutoDiffScalar? I will assign this issue to you for now. Thanks!

[amcastro-tri]

Yes. It's in my TODO list. Though on a time off until Monday so I don't think I'll have an early start with this.

[EricCousineau-TRI]

Resolved (a while ago) by #8224.