Improved HL commander - spiral & linear segment

[matejkarasek]

This PR extends the functionality of the HL commander by adding 1) a linear GOTO command and 2) a spiral segment.
To function correctly, it requires this cflib PR: bitcraze/crazyflie-lib-python#470

1. GOTO command now has an extra 'linear' argument. When set to true, the crazyflie will go to the setpoint with constant velocity, instead of a smooth acceleration and deceleration.
   This change will require updates of cfclient, and other apps using the GOTO...
   Alternatively, we could introduce e.g. a GOTOLINEAR or GOTO2 commands. @whoenig Please share your thoughts on this as this would affect the crazyswarm also.
   Edit 2024-09-13:
   The new functionality is introduced in a GOTO2 command, former GOTO is kept for now but will be deprecated

2. SPIRAL command, which allows to fly along an arc or spiral segment approximated by splines. For segments <90 degrees this is a very good approximation of an arc or a spiral. The maneuver has a constant angular velocity, the axis of the spiral is defined relative to the current x,y,z,yaw setpoint (such that all the parameters fit into a single packet). Internally, the piecewise_plan_7th_order_no_jerk planner is reused.

The parameters are described below:

• phi: spiral angle (rad)
• r0: initial radius (m)
• rF: final radius (m)
• ascent: altitude gain (m)
• duration_s: time it should take to reach the end of the spiral (s)
• sideways: true if crazyflie should spiral sideways instead of forward
• clockwise: true if crazyflie should spiral clockwise instead of counter-clockwise
  

[matejkarasek]

Here a comparison of the xy trajectories between ideal circular arcs and the HL commander segments from the current implementation:

90 degrees

180 degrees

360 degrees

[knmcguire]

@whoenig or @jpreiss. Do you have time to pitch in on this functionality? It goes with the cflib (see this PR bitcraze/crazyflie-lib-python#470) but this would also require a change in the cpp link in crazyswarm2 then as well

[knmcguire]

The current build issues is because of the cflib not being up to date, but this is a bit of an API breaking change (or rather crtp). Wonder if it might be better to make this another type of message? Let's wait to see what the others say.

[whoenig]

Generally, CRTP breaking changes are not allowed. We can add a new message and deprecate the old one (and eventually remove the old one). Any (CRTP) API change should also bump up the "protocol version", so that clients can easily know which functionality might be available.

Otherwise I do like these additional features, of course!

[matejkarasek]

Thanks for the quick pre-review!
Ok, so I will introduce a GOTO2 instead

[matejkarasek]

So I added a GOTO2 while keeping GOTO in the firmware.

Tested on a crazyflie 2.1

• with old cfclib (crtpCommanderHighLevelGoTo, current master)
• and with updated cfclib (crtpCommanderHighLevelGoTo2, Improved HL commander - spiral & linear segment crazyflie-lib-python#470)

Also increased the CRTP protocol version by 1.

What is the policy with deprecation/removal, i.e. how long do we keep the deprecated message?

[ataffanel]

Our current policy about depreciation would be depreciate messages for at least one version before dropping it. The intent is that if a client is developed for version n it should work on version n+1. We have never really used this rule though and the python client is not (yet) checking this version. So I would depreciate packets only if the old packet existence really causes a problem.

[gemenerik]

These are great features to have, thanks for the PRs.

One downside is that the spiral definition feels a bit unintuitive. Users need to calculate to know where the drone will end up, and it may take some playing around - or looking at your drawings - to see what parameters must be changed to achieve the desired arc. That said, this may be natural given the precise arc definition this function provides, as opposed to something like a "curve towards point." So while it's harder to design with, it fits the nature of this function.

There are also some edge cases where I wonder if they were considered, even if they may not be fair expectations:

• What happens with angles greater than 2π? It seems to work but can behave unpredictably, what should happen?
• What about negative radii? I had initially thought that a positive and negative radius might create an S-curve, which may not be a fair expectation. I also noticed that setting both radii to negative values appears to cause the drone to fly backwards.

One thing that would be cool would be to put the spiral in the vertical plane, or a tilted plane. Just something we could look at in the future.

[gemenerik]

This is implemented, correct?

[matejkarasek]

Yes, I missed this, will remove it...

[gemenerik]

Can be removed

[matejkarasek]

Thanks for the review and for testing this @gemenerik!

One downside is that the spiral definition feels a bit unintuitive. Users need to calculate to know where the drone will end up, and it may take some playing around - or looking at your drawings - to see what parameters must be changed to achieve the desired arc.

Yes, I agree it is not very intuitive... I wanted to fit all the parameters into one message, so the size limit made me reduce the number of parameters.... the result is that the definition is relative to the current setpoint...

So you may need to do a bit of math, but in practice, we typically use 90- or 180-degree segments, often in a loop with multiple iterations, and so the calculation of the final position is not so difficult...

What happens with angles greater than 2π? It seems to work but can behave unpredictably, what should happen?

Actually, I have not tried this :)
We can limit the angle to +/-2pi as I don't think there is much of a use to go beyond that...

What about negative radii? I had initially thought that a positive and negative radius might create an S-curve, which may not be a fair expectation. I also noticed that setting both radii to negative values appears to cause the drone to fly backwards.

Again, one thing I have not tested :)
While a combination of positive and negative radiuses can produce some interesting shapes, let's allow only positive values to make the result predictable...
To fly backward, a negative angle can be used.

Agree with the vertical/tilted spiral, it was one thing I initially considered, but decided to leave that for later as we didn´t have an urgent need for it :) Also, this would require an additional set of parameters to define the axis of rotation... So now, that is always z axis...

[gemenerik]

So you may need to do a bit of math, but in practice, we typically use 90- or 180-degree segments, often in a loop with multiple iterations, and so the calculation of the final position is not so difficult...

Fair point!

Actually, I have not tried this :) We can limit the angle to +/-2pi as I don't think there is much of a use to go beyond that...
...
Again, one thing I have not tested :) While a combination of positive and negative radiuses can produce some interesting shapes, let's allow only positive values to make the result predictable... To fly backward, a negative angle can be used.

I agree, let's limit to +/-2pi and positive radii. Then together with the minor comments above it's good for merging as far as I'm concerned.

Agree with the vertical/tilted spiral, it was one thing I initially considered, but decided to leave that for later as we didn´t have an urgent need for it :) Also, this would require an additional set of parameters to define the axis of rotation... So now, that is always z axis...

OK something to keep in mind for the future!