Spot rotating when trajectory pose commands are not in body reference frame

[matthew-rt]

If you send a command to Spot in a reference frame other than body, but only fill out the x and y values, then when spot moves to the new reference frame it will rotate, as seen in the attached gif.

In the gif, the robot is being told to navigate to several points along a line, but the spinning is making it quite inefficient.

I think this is happening because tf2_geometry_msgs.do_transform_pose catches the empty quaternion [0,0,0,0], populates it with something (I'm assuming [0,0,0,1]), and then applies the transform.

I think the functionality of being able to give spot an x and y goal in an arbitrary frame, without needing to specify the goal orientation, and have it navigate there without unnecessary spinning is desirable. I think its also a reasonable assumption that if not otherwise specified, spot should arrive at its goal facing forward.

I would like to solve this issue by adding a line to the function _transform_pose_to_body_frame() which checks to see if the quaternion provided is empty ([0,0,0,0]), and if so, calls a function which will compute the orientation such that if spot is travelling from point a to point b, it will have an orientation facing along the line a->b.

I've already implemented this in a separate script, but let me know if you don't think this would be a desirable addition.

[heuristicus]

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.

[matthew-rt]

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.

[heuristicus]

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.

[matthew-rt]

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.