Support Polygon Concept

[mxgrey]

I'm about to implement a support polygon concept, which is commonly used for kinematic and dynamic planning with balancing constraints. The support polygon itself will just be a std::vector<Eigen::Vector3d>, which I think I might call a SupportGeometry instead, because really the "support polygon" will be the convex hull of the projections of those points onto a plane.

I wanted to open up this discussion before I got too far along with the implementation to see if anyone has thoughts or feelings on the matter.

As of right now, my plan is to have EndEffectors contain SupportGeometry data, and to not have SupportGeometry information inside of BodyNodes. The main reason I want to leave SupportGeometry out of BodyNode is because I feel like BodyNode has a significant enough role as it is, and it would be better to leave this new role in the hands of the EndEffector class. I think that would make things less cluttered.

I would also intend to give the EndEffector class either an enumeration flag or a boolean flag saying whether it is currently in support mode (i.e. whether a whole body IK solver is allowed to use it for support). This would make it easy to switch between right-foot support solving and left-foot support solving, or even quadrupedal solving.

[jslee02]

SupportGeometry sounds interesting!

Your plan looks SupportGeometry will be created from single EndEffector. (Am I right?) I think some SupportGeometry can be defined over mutiple BodyNodes (or EndEffector). A bipedal robot, for instance, will have a support polygon defined by contacting feet and the ground. In that case, it would be good to be able to create a SupportGeometry from multiple BodyNodes (or EndEffectors) and a plane.

[mxgrey]

Yes, part of what I intend to do is be able to build up a support polygon from the support geometries of multiple end effectors. This is very easy to do since I can just mash together the support geometries of all the active end effectors and compute their convex hull.

The function would just take in a std::vector<Eigen::Vector3d> that represents the support points and a single Eigen::Vector3d that represents the plane's normal, then it will project the support points onto the plane and compute the 2D convex hull.

[mxgrey]

To go along with the Support Polygon concept, I intend to create a "Balance" constraint class. It would be a kinematic constraint (not a dynamic constraint), and it would inherit optimizer::Function. Would it be more appropriate to put this class in the constraint namespace or the dynamics namespace?

[mxgrey]

Finished in #461