InverseKinematics for MultiBodyTree

[hongkai-dai]

As @amcastro-tri is writing MultiBodyTree, it is probably a good time to re-write our IK code. The major reason why I want to re-write this code are

1. RigidBodyConstraint is not really a Constraint sub-class. It also has this property tspan, which is confusing and not necessary.
2. InverseKinematics is not a MathematicalProgram sub-class. It only accepts q as decision variables, and its cost is fixed to (q- q*)ᵀ Q (q- q*). We would like to add additional cost, and maybe additional variables.

Plan

• Create a class KinematicsProblem as a subclass of MathematicalProgram. KinematicsProblem can add kinematics constraints. We will create two sub-classes of KinematicsProblem, InverseKinematics and InverseKinematicsSequence. The inheritance graph looks like

            MathematicalProgram 
                      |
                      |
             KinematicsProblem
                      |
            ----------------------
            |                    |
  InverseKinematics     InverseKinematicsSequence

• KinematicsProblem has methods to add kinematics constraints. For example

  // Add the constraint that a point Q on the body B, with body index `body_index`, to be within a bounding box p_RQ_lowerbound <= p_RQ <= p_RQ_upperbound, where p_RQ is the position of Q in the reference frame R, whose ID is `reference_frame_id`.
  Binding<Constraint> KinematicsProblem::AddPositionConstraint(int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ, int reference_frame_id, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_lowerbound, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_upperbound);

  so we get rid of the RigidBodyConstraint class, but just have some Evaluator object, that evaluates a kinematics expression.

• An InverseKinematics class solves the IK problem for a single posture. An InverseKinematicsSequence class solves the IK problem for a sequence of postures.

@avalenzu @amcastro-tri @RussTedrake

[amcastro-tri]

This is just awesome @hongkai-dai, please let me know anyway I can be of help with this. As I expressed to you before, from just a purely OOP design perspective, I think InverseKinematics is not a MathematicalProgram but has a (or uses) MathematicalProgram internally. I tell you more, what if one day we want to try out someone else's solver? we could change it under the hood (so swap the MathematicalProgram solver) and still have a reasonable IK. Are you sure you want to make public all off MathematicalProgram's API's through IK?

[amcastro-tri]

Another suggestion, why not to make the solvers to take a KinematicsProblem? Take as an analogy a linear algebra problem. You have the problem being described by your matrix object A (the KinematicsProblem) and then you have a series of solvers that can solve A (InverseKinematics, InverseKinematicsSequence). So what about:

              KinematicsSolver
                     |
           ----------------------
           |                    |
InverseKinematicsSolver     InverseKinematicsSequenceSolver

Then you could do:

MultibodyTree<T> model;
// Add stuff to your model...
KinematicsProblem problem(model);
// Add constraints and costs to your problem...
InverseKinematicsSolver ik_solver(problem);
InverseKinematicsSolution solution = ik_solver.solve();

[jwnimmer-tri]

I would also suggest avoiding KinematicsProgram inheriting from MathematicalProgram. Having bespoke subclasses of MathematicalProgram will only make it more difficult to maintain the MP class over time. (We should probably mark MP as final now to enforce this.) Also, using inheritance for "narrowing" violates the subtyping principle.

If you want to give KP users access to MP helper methods, then give KP an accessor that returns the MP, or add selective forwarding methods if you don't want to expose everything.

[hongkai-dai]

Thanks @amcastro-tri and @jwnimmer-tri for the suggestion. @avalenzu and I had a discussion, and we decide to take the following steps

• We will create a file add_kinematics_constraint.h, which declares functions to add kinematics constraints to a MathematicalProgram object. For example, the API could look like this

  // Adds the kinematics constraint to the optimization program, that a point Q on the body B, should be within a bounding box p_RQ_lowerbound <= p_RQ <= p_RQ_upperbound;
  Binding<Constraint> AddPositionConstraint(MathematicalProgram* prog, MultiBodyTree<double>* robot, const Eigen::Ref<const VectorXDecisionVariable>& q, int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ, int reference_body_index, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_lowerbound, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_upperbound);

  These functions can be used in different scenarios, for example, adding kinematics constraints to a trajectory optimization problem, or an inverse kinematics problem.

• We will create a class InverseKinematics which has a MathematicalProgram object. It also owns the MultibodyTree. It can call functions to add kinematics constraint. These functions are just wrappers, that internally call the corresponding functions in add_kinematics_constraint.h. The function definition in inverse_kinematics.cc looks like this

  #include "add_kinematics_constraint.h"
  Binding<Constraint> InverseKinematics::AddPositionConstraint(int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ, int reference_body_index, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_lowerbound, const Eigen::Ref<const Eigen::Vector3d>& p_RQ_upperbound) {
    return AddPositionConstraint(prog_, tree_, q_, body_index, p_BQ, reference_body_index, p_RQ_lowerbound, p_RQ_upperbound);
  }

  Note that InverseKinematics class has a MathematicalProgram object. This prevents the user to call some functions in MathematicalProgram that should not be used in an IK problem, such as adding binary variables, or adding positive semidefinite constraint.

• We will create some Evaluator class, that evaluates the kinematics expressions, such as "the position of a point Q (rigidly attached to body B), in a reference frame R".

[RussTedrake]

@jwnimmer-tri -- fyi, the trajectory optimization classes already subclass mathematical program (and I think that is a very natural thing)