Added support for fully-relative trajectories

[johnnwallace]

Current Crazyflie firmware does not allow trajectories to be initiated relative to the drone's current yaw. The relative flag used when starting a trajectory only results in relative position, not yaw. See this issue for more details. Below is a plot of two trajectories initiated relative to each other with the current firmware. The position is initiated relatively, but the yaw setpoint drops back to zero.

I've added support for trajectories to be run fully-relatively. Currently this is only supported for uncompressed trajectories in the forward direction. A similar method could probably be used to support all trajectory modes, but I was only testing the standard trajectories. I found that by starting the trajectories at the drone's current position, rather than its current setpoint (the current firmware does this), leads to better performance with relative trajectories. This was done by informing the high level commander of the drone's state estimate in src/modules/src/stabilizer.c line 297. I left this commented out in order to maintain as much of the current functionality, while still giving the option to use this behavior. It might be worthwhile to add this as a configuration parameter at some point. Below is a plot of two trajectories initiated relatively, using the new firmware.

This was done as part of a project called MonoNav, which uses the trajectories in a motion primitive planner. We believe that there are a number of projects using a similar planner that would benefit from this change. Using the Crazyflie's low level control with asynchronous trajectories leads to much faster and more reliable motion, and enables longer computation without having to update the drone's setpoints. More details can be found in my paper here.

[knmcguire]

I don't mind adding this addition. @whoenig and @jpreiss what do you think?

[jpreiss]

I agree that adding the relative yaw feature is good.

From an API design perspective traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation) is ambiguous about the order of operations, because the rotation and translation are not commutative. One solution would be to add a comment. Another is separate functions for rotation and translation, so the order of operations is obvious from reading the call site.

Implementation detail: the 3x repeated calls to mrotz in that function are are a bit expensive due to the trig functions. It's possible that the compiler can optimize away the 3x calls but I wouldn't count on it. I suggest constructing that matrix as a separate variable.

[knmcguire]

Also there seems to be build issues on this PR as well.

[jpreiss]

Thanks for addressing my comments!

If I understand this feature correctly, it rotates the entire trajectory, including position, such that the initial yaw lines up with the current yaw. This makes sense if, for example, your polynomials are a set of motion primitives that all start out with yaw=0, initial position = 0, and initial velocity in the direction (1, 0, 0). But I'm not sure if this is always good.

The original motivation of relative trajectory was to upload the same polynomial to many CFs, and then have them all fly in formation (like in 1:00 of the Crazyswarm video). If you re-did that experiment with the new version, then all of the figure-8s will be slightly rotated according to each Crazyflie's yaw error at the moment of trajectory start. But since we are rotating positions, the part of trajectory furthest away from the initial position could change significantly with even a few degrees of yaw error. Is that right? If so, then this feature would make it impossible to recreate that flight with the consistent grid spacing seen in the video.

So ideally, it is probably better to make relative yaw a second bit in the radio packet. But of course this creates a lot of work in updating PC-side code... Maybe a param controlling it could be a compromise?

Also, are you sure the rotation logic works correctly if the x-y part of the trajectory polynomial doesn't start at (0, 0) in the x-y plane? Testing with the Python bindings would be a good way to double-check cases like this.

[knmcguire]

Hi @johnnwallace, thanks for fixing the CI issues!

This PR is a bit hanging now. Are you able to address James' comments on this? If not, maybe it is best to close this PR if you need more time to figure something out

[johnnwallace]

@knmcguire @jpreiss sorry for the late responses, I've been out of the lab for the summer so it's been hard be able to test these changes. I tried unsuccessfully to get the Python bindings to work at James's suggestion, so I also haven't had the chance to test the logic that way. I'll try to answer James's questions as best as possible based on my work last semester.

Your understanding of the change is correct. I'm not sure I see the problem with nonzero initial yaw/position, or velocity that is not (1, 0, 0). I certainly agree this change is not suitable for some cases (like the Crazyswarm) due to the magnification of small errors when initializing a trajectory. As I mentioned in the PR description, we can swap between initializing a trajectory relative to the drone's last setpoint vs. initializing relative to its position by commenting/uncommenting line 297 in src/modules/src/stabilizer.c, respectively. For our case, where we wanted smooth chaining of trajectories, it worked best to initialize at last position. There plenty of cases where one might need more precise control of absolute position throughout the entire trajectory, so initializing at last setpoint should be good. I could see that option being set as a parameter quite easily.

If one really wanted relative position but absolute yaw, that functionality could be set with a new parameter as well. That brings up the question whether it should be possible to do relative yaw but absolute position, and how that should be implemented.

I am pretty sure that this would also work with a trajectory polynomial with a nonzero starting position. That position would just be considered in the drone body frame. I haven't tested this case (or other cases like a trajectory polynomial with nonzero yaw or velocity that is not (1, 0, 0)), but I will be able to in the fall. I will also be able to test out parameter functionality if we decide on that. If you all would want to close the PR, that's fine with me.