Add Path Constraints

[amalnanavati]

Description

MoveIt2 allows planning with path constraints (e.g., see here) but pymoveit2 does not currently support it. This PR does the following:

1. Separates the logic to create constraints from the logic to set goal constraints, so that the constraint creation functions can be reused.
2. Extends the API to allow for setting orientation, position, and joint constraints as path constraints (in addition to the existing option of setting them as goal constraints).
3. Adds an example showcasing the orientation path constraint capability.

Note that this PR is joint with panda_ign_moveit2#29, which enables OMPL Constrained Planning for efficient planning with orientation path constraints.

Testing

Config:

• Pull the panda config with orientation path constraints enabled: panda_ign_moveit2#29
• Pull this branch
• Re-build your workspace
• Launch the simulated panda arm: ros2 launch panda_moveit_config ex_fake_control.launch.py

Testing:

• Check the motion without orientation constraints: ros2 run pymoveit2 ex_orientation_path_constraint.py --ros-args -p use_orientation_constraint:=False
• Check the motion with orientation constraints, and verify that the end-effector stays flatter than it did without the constraints: ros2 run pymoveit2 ex_orientation_path_constraint.py --ros-args -p use_orientation_constraint:=True
• Verify that planning to a goal pose still works: ros2 run pymoveit2 ex_pose_goal.py. (Note that planning to a joint goal was already tested in the above examples)

Future Work

1. Limiting position constraints to a sphere made sense for goal constraints, but no longer makes sense for path constraints (e.g., look at the tutorial, that uses a box as opposed to a sphere). Thus, future work should generalize position constraints to allow for any Solid Primitive.
2. We currently don't have examples showcasing the use of position and joint path constraints. They should work in theory, given that they reuse the same code used for position/joint goal constraints and orientation path constraints, but that has not been tested. Future work includes adding examples for position and joint path constraints.

[AndrejOrsula]

Thank you very much for this work! I think it looks good! 😄