When the wrist is stowed and an operator tries to center the wrist, whether or not the wrist collides with the mast depends on non-deterministic factors. Specifically, if the joint trajectory server executes yaw first (as it tends to do in 3030), then it won't collide. However, if it executes pitch first (as it seems to do on 2054), then it will collide.
This is a good motivation to send a trajectory with more waypoints than we currently use when centering wrist. There are several ways to do this:
If the gripper is in a "close to mast" state (as determined by arm length, wrist yaw, and wrist pitch), add an intermediate waypoint that pitches the wrist down and yaws the wrist out.
First command the wrist yaw movement and then command the wrist pitch and roll movement.
Linearly interpolate between the start wrist configuration and the goal one, and for the first 1/5th of that motion only move the yaw.
I prefer the first option. The benefit of the second is that it doesn't depend on the current configuration. But the downside of the second and third is that doing yaw first is not guarenteed to be collision free, in case the user pitched the wrist up while near the mast.
When the wrist is stowed and an operator tries to center the wrist, whether or not the wrist collides with the mast depends on non-deterministic factors. Specifically, if the joint trajectory server executes yaw first (as it tends to do in
3030), then it won't collide. However, if it executes pitch first (as it seems to do on2054), then it will collide.This is a good motivation to send a trajectory with more waypoints than we currently use when centering wrist. There are several ways to do this:
I prefer the first option. The benefit of the second is that it doesn't depend on the current configuration. But the downside of the second and third is that doing yaw first is not guarenteed to be collision free, in case the user pitched the wrist up while near the mast.