Keep the end-effector rotation fixed throughout the execution of the trajectory about mplib HOT 6 CLOSED

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

from mplib.

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

from mplib.

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

from mplib.

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.

from mplib.

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.

from mplib.

Todo:

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

from mplib.