RigidBodyManipulator cleanup list

[tkoolen]

My list of things that should be cleaned up:

• [x] Rename RigidBodyManipulator to RigidBodyTree.
• [ ] Create RigidBodyManipulator and TimeSteppingRigidBodyManipulator, which hold on to a RigidBodyTree
• [ ] Create RigidBodyForceElements in C++, but instead of having the RigidBodyTree hold on to a bunch of RigidBodyForceElements, have them be separate systems that can be concatenated with a RigidBodyManipulator or TimeSteppingRigidBodyManipulator. To enable this, also need to add a method to RigidBodyManipulator and TimeSteppingRigidBodyManipulator which creates additional inputs corresponding to an external wrench exerted upon one of its bodies.
• [ ] Replace ints as frame and rigid body identifiers in RigidBodyTree methods such as forwardKin by RigidBodyFrame. Robin and I came up with a design for this; need to discuss a little more and implement.
• [x] split up forwardKin into transformPoints, relativeQuaternion, relativeRollPitchYaw. Create separate Jacobian methods for each. This is to improve the user interface and improve efficiency (can return fixed-size matrices instead of dynamic). In addition, it is needed for TrigPoly support, as there should not be any atan2 calls and such used in rotation conversions at compile time.
• [ ] move RigidBodyTree methods to KinematicsCache? See discussion in https://github.com/RobotLocomotion/drake/pull/1304.
• [ ] add TrigPoly instantiation of a subset of RigidBodyTree methods (probably relativeTransform, forwardKin, forwardKinJacobian, geometricJacobian, massMatrix, inverseDynamics, centerOfMass, centerOfMassJacobian, *MomentumMatrix, ...). For inverseDynamics, we'll have to split off the friction forces since they use e.g. abs, which can't be implemented for TrigPoly.
• [ ] Move documentation to from Matlab to C++. We should also have documentation in Matlab though, and it should not take any effort to keep it up to date. Ideally, we would have something that parses the documentation at compile time and writes .m files containing only the documentation (see e.g. the .m file for the mtimes Matlab built in).
• [ ] Replace Matlab RigidBodyTree methods with mex implementations. The idea is to no longer have any wrapping functions, but to have users call the mex implementation directly.
• [ ] get rid of most of drakeGradientUtil. Should no longer be necessary after everything is using AutoDiff gradients.
• [x] get rid of GradientVar. Get rid of the last usages and delete. Should no longer be necessary after everything is using AutoDiff gradients.
• [ ] possibly use structs containing matrices f and df as the interface for mex gradients instead of always converting inputs to TaylorVar and using mexCallMatlab to create TaylorVars as return arguments. This is a source of overhead for mex RBM method gradients, see https://github.com/RobotLocomotion/drake/pull/1434.
• [x] add a getZeroConfiguration which does do the right thing for quaternion parameterized floating joints (would probably fix https://github.com/RobotLocomotion/drake/issues/1378)
• [ ] split off collision methods from RigidBodyTree and put into new class CollisionWorld as discussed in design meeting
• [x] add option to not include velocity terms in inverseDynamics. E.g GravityCompensationTorqueConstraint needs gravitational torques only, and right now the awkward workaround is to redo kinematics with the velocity vector set to zeros. This is pretty inefficient.
• [ ] mexify createKinematicsCache* mex functions.
• [ ] fix several instances where KinematicsCaches should be passed into mex functions instead of created on the fly in those mex functions, e.g. bodyMotionControlmex, supportDetectmex, ...
• [x] improve efficiency of transformSpatialInertia. Needed to compute the inertias in world frame and composite rigid body inertias for massMatrix and inverseDynamics, and currently pretty slow. Should at least exploit the fact that the inertia matrices are symmetric (but they have even more structure that could be exploited).

[tkoolen]

I've got a branch where I've removed GradientVar. It's currently blocking on https://github.com/RobotLocomotion/drake/pull/1671 and https://github.com/RobotLocomotion/drake/pull/1648; I'll need to resolve the Eigen issue first (hoping for a response from the Eigen people and a quick patch).

This is the last cleanup thing I'm planning to do for a while.

[RussTedrake]

@tkoolen -- i think we need to update this, and potentially break it into pieces so that we can hand the relevant pieces over to the dynamics team.

[RussTedrake]

I think we should merge this with @amcastro-tri's list (and probably close this issue).

[amcastro-tri]

This is a great summary @tkoolen. I think, correct me if I am wrong, our long term goal is to provide a fully capable RBT (or its system 2.0 counterpart RigidBodyPlant) in C++ with a thin wrapper around it in Matlab.

[RussTedrake]

That’s correct.

[jwnimmer-tri]

Note that #6369 will make some of the above TODO items moot. @amcastro-tri Would you like to update the list to be only non-MEX / non-MATLAB issues, and/or transfer them into your other MBT uber-TODO list issue, or something?