Setting a rotation in LinearRelativeMotion and LinearMotion

[TimSchneider42]

Hi,
I am experiencing strange behavior when I am setting a target rotation in LinearRelativeMotion or LinearMotion.

E.g. by calling

robot.move(LinearRelativeMotion(Affine(0.0, 0.0, -0.1, 0.0, 0.0, 0.2)))

I expect the robot to move 10cm and rotate its gripper by around 11.5 degrees. However, what I am experiencing in around 25% of all cases is a continuous rotation of the gripper way above 90°, which I have to abort to prevent the robot from crashing into itself. Do you have any idea how to debug this problem?

My Franka is on version 4.2.1 and I am running the latest frankx version in PyPI (0.2.0).

Best,
Tim

[TimSchneider42]

Hi,

after digging a bit in the code, I figured out that the issue stems from the cartesian trajectory generation in WaypointMotionGenerator. This class is using ruckig to generate a desired trajectory of end-effector positions and orientations. The issue is that ruckig is used to compute a trajectory directly on the Euler angles of the orientation. However, this is not a good idea, as Euler angles are ambiguous and discontinuous in some places.

Specifically, the problem I was facing was caused by ruckig trying to compute a trajectory between the orientations $(0, 0, -\pi - \epsilon)^T$ and $(0, 0, \pi + \epsilon)^T$ and ending up suggesting to do a full $360^\circ$ rotation, not realizing that these two orientations are right next to each other.

I believe this problem can be fixed by computing the trajectory on the rotation vector of the relative orientation of the target pose in the frame of the initial pose. The reason why I think this works is that if we assume we start at 0 (which we do if we represent everything in the frame of the initial pose), there are no discontinuities in this representation. Furthermore, the rotation vector is exactly the integral of the angular velocities, as long as we only rotate around the same axis (hence, take no detours). I am unsure, however, what the implications are if we start this trajectory generation in a situation where the robot is already moving and, hence, already rotates around an axis that is different from the one it needs to rotate around to reach the goal. In that case, the resulting angular trajectory might be slightly suboptimal, but I expect this approach still to work reasonably well.

I have implemented the suggested change and tested it on a Franka Panda. So far everything seems to work, but it might need some more testing.

Best,
Tim