Keep the end-effector rotation fixed throughout the execution of the trajectory

[Lexseal]

A lot of tasks require steady hands. For example, when grasping a glass of water or a plate of cookies, the robot needs to keep the end effector level throughout the execution of some movements.

Add a parameter inside the plan API to indicate whether we want end-effector rotation to be fixed for the trajectory.

[Lexseal]

OMPL supports this and has a set of APIs associated with constrained planning. Will go ahead and hook it up soon.

[Lexseal]

Also, add the ability to fix some joints during planning.

[Lexseal]

To support this there are some issues:

1. How do you pass the constraint function and its jacobian from python to c++? There are some ways to do it but might be very involved

2. If we only support certain types of constraints, then we can have two types of constraints:

   • rotation lock on some particular joints individually
   • end effector rotation lock

   The first is easy to implement, but the second might be involved. In particular, we would first turn the joint values to end effector pose with FK. Then, we multiply it some matrix to remove the translation. Now, suppose we want to make the hand level, we are basically allowing it to rotate only about a single axis. To describe such a constraint, we need to extract its coordinate frame and require one axis to have a certain dot product with some world axis. Finally, we need to hand-calculate the jacobian. The part that worries me is the to rotation matrix part. Need to discuss this

[Lexseal]

Constrained planning does not support compound state space, which is what we are using. In order to proceed, need more pointers from @Colin97 how to convert our current state space into a constraint-compatible one.

The OMPL is not the easiest to use either, but one guy added general constraint planning to moveit in summer 2020 so can take a look at how they support it.

[Lexseal]

Design of the API:

Users probably don't want constrained planning all the time. For example, when the robot is not holding anything. As such, the constraint should be on a per-planning basis. For now, let's support this level constraint only.

For the public-facing plan API in planner.py, user can optionally supply a vector they want the end effector to align with and the angle of alignment. If these two parameters are both present, pass them in and the corresponding c++ plan will make new constrained space.

[Lexseal]

Todo:

change to not full jacobian optimize the traj expose general constraints to python learn how ompl does constraint planning