Work on RigidBodyTree

[amcastro-tri]

A checklist to keep track of the planned work on RBT this quarter. I'll keep updating as I move along.

• [ ] Template RBT on <T>. PR first a templatization on a dummy scalar type <T> and replace occurrences of RBT with RBT<T>. Functionality is left intact. Methods are still templated on <Scalar>. Parameters are still double. See #3987 for details.
  • PR #3940 merged.
• [x] Clean up KinematicsCache to break dependency on RigidBody. PR #4378 merged.
• [ ] Provide a physically meaningful API to users of RBT, see issue #4328.
• [x] Move multibody code to a better location, see issue #3481. #4058 merged.
• [x] Refactor drake/multibody files to follow style guide (CamelCase vs snake_case, cpp vs cc). See #4082. #4098 merged.
• [ ] Change the order of declaration of public/private methods/members according to style guide.
• [ ] There seems to be a strange (maybe cyclic?) dependency of ConstraintWrappers.h on RigidBodyTree.h. Or rather
• [ ] Files for drakeIK are in the same directory as RigidBodyTree. Where should they be moved and how (if) would they need to be restructured?
• [ ] Create blocks of methods/documentation based on functionality: 1) Construction, 2) methods to access connectivity (FindBody, parent/child, etc), 3) methods to query state dependent results (RigidBody::get_pose()), 4) global methods such as inverse/forward dynamics.
• [ ] Start using Context instead of KinematicsCache in RBT methods. A first cut would place the entire KinematicsCache in a single cache entry.
• [ ] Revisit method RBT::addFrame(shared_ptr<RigidBodyFrame> frame) and the concept of RigidBodyFrame for RBT.
• [ ] Revisit method RBT::add_model_instance(). The only thing it does is to increment a counter. It doesnt seem to "add a model".
• [ ] Method getRandomConfiguration should return a VectorX<T> instead of VectorXd. Set derivatives to zero.
• [ ] Revisit code in RigidBodyTreeContact.cpp and its dependencies.
• [ ] Replace RBT template methods on Scalar with methods templated on the class' parameter <T>
• [ ] Remove dependency of RigidBodyTree.cpp on parser_common.cc. RigidBodyTree should not depend on parsers and neigher drakeRBM. Evaluate if this file should be removed and where the method AddFloatingJoint should go or even exist.
• [ ] Remove dependency of drakeRBM on any of the parser_* files.
• [ ] Evaluate dependency of drakeRBM on drakeRigidBodyConstraint.
• [ ] Evaluate dependency of drakeRBM on drakeOptimization.
• [ ] Cross-check with the list in #1476 for Matlab's RigidBodyManipulator.

[tkoolen]

methods to query state dependent results

I mentioned this before, but I would really recommend moving these to KinematicsCache, so that the cache elements inside of it no longer need to be exposed, and so that it can be merged back with KinematicsResults again.

[tkoolen]

See also https://github.com/RobotLocomotion/drake/issues/1476.

[amcastro-tri]

Thank you for that list @tkoolen! Regarding your proposal, I was actually thinking more like the KinematicsCache would be this object the user needs to drag around representing the state of the world but the user actually doesn't really know what it is or how to interact with. The user would rather interact with it performing queries on it through RigidBodyTree. For instance, something like const Isometry3<T>& RigidBody<T>::get_pose(const KinematicsCache<T>& cache) const.

In this regard I was thinking to a similar philosophy in System2 where you have the pair System(const)/Context(non-const). In the dynamics world we'd then have the pair RigidBodyTree(const)/KinematicsCache(non-const). Also KinematicsCache will migrate to a System2 context and cache entries representation.

Another thing I like about this is that both API's for Systme2 and dynamics would have similar semantics, homogenizing API's across our code base. What do you think?

[sherm1]

And I would like us to get to the point soon where we are literally using the System 2 Context in the RBTree API (in place of KinematicsCache), so that we don't have two separate caching schemes and a bunch of unnecessary pack/unpack/copy operations.

[tkoolen]

the KinematicsCache would be this object the user needs to drag around representing the state of the world but the user actually doesn't really know what it is or how to interact with

That's the current status quo, and the bad thing about it is that the internals of the cache need to be exposed in this case (this being the main reason for the creation of KinematicsResults).

[SeanCurtis-TRI]

@sherm1 I'm a bit wary of this. We'd talked about the need to be able to run RBT independent of the system 2 architecture. Would passing a System 2 Context instance be consistent with that?

[david-german-tri]

I haven't thought deeply about this, but I have the same knee-jerk reaction as @SeanCurtis-TRI. Surely RBTree can just consume some (set of) mutable pointer(s) to AwesomeFutureKinematicsData, which may or may not point into a System2 cache entry?

[sherm1]

Most of the real value of the Context's caching utility won't be apparent until we are using it in RBTree, where really expensive things get calculated and reused frequently. Also, when an RBTree is used as a component in a System, most of the state variables and computational cost will come from the RBTree. I think it will prove much easier to do this directly in the RBTree API than to implement and maintain an interface layer to keep RBTree Context-free (to coin a phrase :). RBTree already has a similar dependency on KinematicsCache; we should just replace that with Context which is a more general version.