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.

[ataffanel]

@jpreiss Do you have a comment on this last bit? We'd like to see if we can close this PR or merge it

[jpreiss]

I don't think users should have to recompile the firmware to switch between "relative to current setpoint" and "relative to current state". I agree that using a param to enable/disable the crtpCommanderHighLevelTellState(&state) added in stabilizer.c is a good solution.

[knmcguire]

That would probably be a better solution.

@johnnwallace would you like to make this commit so that we can finalize the review?

[johnnwallace]

yes, I'll work on this @knmcguire @jpreiss @ataffanel

[knmcguire]

Great. Just FYI @johnnwallace, next week we will be doing our release testing and having a tiny feature freeze. If you still want this new functionality to be part of the latest release, you'll have until the end of this week. If you don't mind it being implemented later, then that's fine too but just letting you know.

[johnnwallace]

Sorry for the delay everyone. I've added two boolean parameters (hlCommander.relativeYaw and stabilizer.hlTellState) to control the behavior of relative trajectories. The first controls whether yaw is relative (in addition to position), and the second controls whether the high level commander is informed of vehicle's state. I've also run some testing (only using the PID controller so far) to make sure this works:

First, the behavior of the drone is the same between the current release (2024.10) and this branch when both parameters are 0:

2024.10	this branch

Then, that the behavior of the new settings is as expected:

	relativeYaw = 0	relativeYaw = 1
hlTellState = 0
hlTellState = 1

We can see that relativeYaw controls whether the yaw trajectory is initiated at the drone's current yaw or at its origin in the middle subplots. The effect of hlTellState is most noticeable in the bottom subplots of these figures. We can see that (when hlTellState = 1) the new trajectories are initiated at the drone's current position, rather than its last setpoint (as is the case when hlTellState = 0).

Again, sorry for the delay. Please let me know if there are any other changes or testing you want me to do in order to get this over the line. Also let me know if you'd like the new commits squashed before merging.

@knmcguire @jpreiss @ataffanel

[knmcguire]

I've looked at the changes and it looks good to me. The only problem that I have with it being added to the crtp function now instead of directly in the planner, is that now the function call for plan_start_trajectory() won't be compatible with the Crazyswarm2 sim and the pythonbindings.

Currently those two projects are not in a generic release-testing process, we usually advise people to have the latest firmware /software on anything. So if this new function call seems to be fine and logical to @whoenig as well, then we can go ahead and merge this.

[gemenerik]

@johnnwallace is it ready for us to look at it again?

[johnnwallace]

@gemenerik yes, was just fixing some build issues.

[knmcguire]

Codewise it checks out!

However before merge let's flight test it maybe? I won't be at the office so that will be next week.

[knmcguire]

I finally managed to test fly it but I'm having trouble of activating the relative yaw. So in the autonomous high level example with the figure 8 (see here in the cflib) before the trajectory is started, I set these function where I set both parameters.

def relative_heading(cf):
    cf.param.set_value('stabilizer.hlTellState', '1')
    cf.param.set_value('hlCommander.relativeYaw', '1')

I might be missing something but I haven't been able to test the functionality yet now due to this