[ROS2] Jerky motion (in joint space) when planning with orientation constraints

[amalnanavati]

When planning with orientation constraints, the resulting robot arm motion feels jerkier. It is smooth in cartesian space, but when sitting in the wheelchair, it feels like some joints change motion suddenly, causing the wheelchair to vibrate.

This issue involves:

1. Investigating this.
2. Addressing this. One possible way to address this is Ruckig Trajectory Smoothening, although we have found that it doesn't work in sim, so if we re-implement it it should only be for real.

[amalnanavati]

These are the results of visualizing the trajectories as implemented in #99 . Done on t0b1 connected to the real robot.

AbovePlate --> RestingPosition (kinematic plan w/out path constraints):

RestingPosition --> StagingPosition (kinematic plan w/ orientation path constraints):

StagingPosition --> Mouth (cartesian plan):

Mouth --> StagingPose (cartesian plan):

StagingPose --> RestingPosition (kinematic plan w/ orientation path constraints):

This reveals that the plans with orientation path constraints do, in fact, have a sudden change from negative to positive acceleration (e.g., high jerk) during their trajectory.

To look into this, I'll first see how Ruckig modifies the curve and decide where to go from there.