Spot rotating when trajectory pose commands are not in body reference frame about spot_ros HOT 4 OPEN

I'm not sure how I feel about this. While I acknowledge that this is a useful thing, I'm not sure I want to make it the default behaviour of the trajectory command, since that is a straight interface to the SDK, which I think is the main intent of this driver.

Perhaps implementing something like a forwards_trajectory (perhaps with a better name) actionserver where we can provide this behaviour would make sense.

from spot_ros.

I do see this, but I'm not saying that it should be the behaviour for all instances: only when the quaternion has not been filled in. I don't think issue is being introduced by the SDK itself: from memory, I think that if you give spot a command in the ODOM_FRAME_NAME using RobotCommandBuilder, then it will navigate to it in such a way that it is facing forward. I can check the way the SDK responds to this tomorrow.

The reason I think this is necessary is because the issue is not coming from the sdk, its coming from the transformation that tf2_geometry_msgs.do_transform_pose is doing: if you transform a pose message with orientation [0, 0, 0, 0], it doesn't come out with orientation [0, 0, 0, 0], before getting handed off to _send_trajectory_command.

From playing around with it it seems that a [0,0,0,0] quaternion will always get converted into a quaternion which converts to [0,0,pi] roll pitch yaw, hence the spinning.

from spot_ros.

The facing of the robot depends entirely on the specification of the goal point, I think. If you specify a point where it would be more efficient for it to go backwards, then it will do so.

In your spinning example, you were sending goals in a line in the same non-odom frame? I guess your observation of the conversion would match up with that, since the orientation of the robot would be rotated by pi at each point along the line. Fixing the conversion to make it a unit quaternion would at least remove the spinning behaviour, but it would still point along the positive x axis at each point on the line, I think.

from spot_ros.

Yep, all the goals were being sent in the odom frame.

When the goal is given to spot, a yaw is being specified: on line 1335 of wrapper.py in spot_wrapper, the goal is set by specifiying x, y, yaw and frame name. The goal heading is being computed from the quaternion on line 1007 spot_ros using math_helpers.Quat.

Interestingly for go to goal stuff the spot sdk uses commands in the odom frame, so any spot_ros commands in odom are being converted from odom to body, then back from body to odom.

from spot_ros.