Drake: Simplify RigidBodyFrame API

[amcastro-tri]

RigidBodyFrame's should represent exactly what they are:

1. A reference to the body they are attached to.
2. A transformation X_BF that gives the pose of the frame in the frame of the body to which it is attached.

However, a number of constructors break this abstraction right now, namely the ones not taking a RigidBody or even the default constructor.

There should only be a single constructor signature for RigidBodyFrame:

RigidBodyFrame(const std::string& name, const RigidBody<T>& body,
                            const Eigen::Isometry3d& X_BF);

Notice a RigidBodyFrame only takes a const reference to a body meaning that a body should not be modified through this reference. This also implies the method RigidBodyFrame::get_mutable_rigid_body() should be removed.

In addition:

• There is no need to create pointers to RigidBodyFrame's (even less of a need to create shared_ptr's) . Shared pointers need to be removed from the API.
• RigidBodyFrame's should only be created through the RBT i.e.: RBT::add_frame(const RigidBodyFrame frame), where a constructor by copy is just enough given how small this class is.
• Users should not set the frame index by hand and must be removed (or hidden) from the API. Only RBT::add_frame(const RigidBodyFrame frame) can set the frame's unique id (namely the index in its array of frames.

Assigning @liangfok to this since many of these changes have a lot to do with using RigidBodyFrame as an IR in the parsers breaking the basic abstraction needed in the dynamics engine.

[sherm1]

To keep in mind for later: If we will use RBFrames for important things like joint, geometry, or force-element placement we will likely want to parameterize their poses, with the parameter values in the Context.

[amcastro-tri]

@sherm1, I am working on #4406 right now. We will coordinate to merge that PR first.

[amcastro-tri]

After fixing the TODO's introduced in #4406 this class will simplify to:

template <typename T>
class RigidBodyFrame {
 public:
  RigidBodyFrame(const std::string& name, 
                 const RigidBody<T>& body,
                 const Eigen::Isometry3d& X_BF);

  const std::string& get_name() const;

  const RigidBody<T>& get_rigid_body() const;

  const Eigen::Isometry3d& get_transform_to_body() const;

  int get_frame_index() const;

  bool is_attached_to(const RigidBody<T>& body);

 private:
  std::string name_;
  const RigidBody<T>& body_;
  Eigen::Isometry3d X_BF;
  int frame_index_{0};
};

[liangfok]

Have you considered using an int rigid_body_index instead of a const RigidBody<T>& body?

[liangfok]

Have you thought about making RigidBodyFrame a struct? Here are the rules:

https://google.github.io/styleguide/cppguide.html#Structs_vs._Classes

[amcastro-tri]

Yes, I have and that actually was my first proposal. But @sherm1's concern above made us think that for future development it might be useful to have this class fully templated on T.

[liangfok]

I see. Interesting. Maybe it should be a goal, then, to eliminate the use of rigid_body_index throughout drake/multibody in favor of const RigidBody<T>&?

[liangfok]

BTW, I think:

  std::string name_;
  const RigidBody<T>& body_;
  Eigen::Isometry3d X_BF;
  int frame_index_{0};

should be:

  const std::string name_;
  const RigidBody<T>& body_;
  const Eigen::Isometry3d X_BF_;
  const int frame_index_{0};

[amcastro-tri]

Maybe it should be a goal, then, eliminate the use of rigid_body_index through out drake/multibody in favor of const RigidBody&?

This statement is probably too strong at this point. What actually worries me more is to have things like rigid_or_frame_id. Once I have a nicer API for methods needing these "frame_or_body_id"'s we can get rid of them, but not till then. You can see an example of where I'm moving towards to here. That change however does not collide with indexes as they are right now as long as the users cannot mess up with them (my top priority in this regard).

BTW, I think ... should be...

Yes, I completely agree.

Have you thought about making RigidBodyFrame a struct?

Yes, not strong about it right now. It is true a struct would be enough right now to carry this concept of `Frame = RigidBody + Pose in body's frame". I would keep it a class right now and we can make it a struct later if we think that'd be best.

The top priority right now would be to fix the list of TODO's in rigid_body_frame.h introduced in #4406. As long as we control how these things are used and users cannot mess around with indexes and shared pointers we are good.

[liangfok]

BTW, have you thought about making RigidBody a child class of RigidBodyFrame? Doesn't this make some level of sense since RigidBody _is_ a frame plus things like dimensions / mass / inertia?

[amcastro-tri]

I would keep things simple.

The top priority right now would be to fix the list of TODO's in rigid_body_frame.h introduced in #4406.

[liangfok]

FTR, the reason I ask is because there currently exists rigid_body_or_frame_id_foo logic, which implies to me that if one were to inherit from another, the odd bar_or_baz logic would disappear.

[amcastro-tri]

You can see an example of where I'm moving towards to here.

where that points to the implementation already here in one of my branches, I just don't have a clean infrastructure or the unit tests for it.

[liangfok]

:shipit: !

[sherm1]

BTW, have you thought about making RigidBody a child class of RigidBodyFrame? Doesn't this make some level of sense since RigidBody is a frame plus things like dimensions / mass / inertia?

This is technically correct, but I've seen it be somewhat awkward to work out in practice because RBFrame has an offset from a "parent" frame to which it is rigidly attached, while RigidBody (allegedly is-a RigidBodyFrame) has no such parent frame. It usually ends up requiring a more complicated hierarchy. If you do want to do something like that I suggested discussing with @mitiguy first -- he has strong opinions & lots of past success organizing multibody system classes.

[mitiguy]

@amcastro-tri I think it would be helpful to have a conversation about this when you are here.

[amcastro-tri]

cc'ing @siyuanfeng-tri since he will be providing very soon the first RBT methods using a RigidBodyFrame based API instead of body_or_frame_id logic.

[sherm1]

@amcastro-tri, I'm assigning this to you. Please feel free to close it if this is done.