Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Lee controller #1343

Merged
merged 18 commits into from
May 16, 2024
Merged

Add Lee controller #1343

merged 18 commits into from
May 16, 2024

Conversation

Khaledwahba1994
Copy link
Contributor

This adds the Lee controller from

Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
Geometric Tracking Control of a Quadrotor UAV on SE(3)
CDC 2010

which is similar to Mellinger, but has provable stability guarantees.

Tested with manual flight (cfclient), trajectory tracking (crazyswarm2), and simulation (crazyswarm2 simulator).

@knmcguire
Copy link
Member

Thanks for this PR! It is a big change so we will need time to review this, so hopefully that will be possible in the next couple of weeks. Also I think we need to discuss internally as this is now the 5th controller as well in the firmware so I'll mark it for triage

Copy link
Member

@knmcguire knmcguire left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll soon have time to test this out, however I see that the dwm1000 lib submodule has changed commit hash... was this the intention? I see something similar with the system id PR as well.

Copy link
Member

@knmcguire knmcguire left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I also was able to test this out and it seems to work, however it is very aggressively tuned. If I use a crazyflie without the thrust upgrade kit it flips immediately upon take off from the cfclient. Any chance that this can be tuned down a bit as a default value?

Also it would be nice to add the controller to the controller page of the documentation (source in the /doc folder)

@whoenig
Copy link
Contributor

whoenig commented Feb 21, 2024

  • submodule: certainly not intentional and I am also not sure how this even happened. I should be able to revert this.
  • Flight tests: How did you fly? Manually with a controller in cfclient? That's at least how we also tested it (but we are very poor pilots, so perhaps we didn't notice how aggressive this is). Otherwise our main testing was with a mocap and trajectory tracking. Everything we tested is on a "stock" Crazyflie with the standard battery and standard motors. We have slightly different gains for the upgraded motors.
  • Documentation: yes, good idea!

@knmcguire
Copy link
Member

I tested it with the command based flight control panel with just a takeoff and a forward and up and down motion.

I'll try again with another crazyflie perhaps see if it is not a motor issue, but then it makes sense that it's a bit wobbly with the upgraded motors if those needed to be updated

@jpreiss
Copy link
Contributor

jpreiss commented Feb 23, 2024

Is there any outside contribution that could help move this along? Having a "geometric PID" controller with SI outputs in the firmware would be useful for a current research project. Would also prefer a lower-gain tune.

@whoenig
Copy link
Contributor

whoenig commented Feb 23, 2024

I did the "smaller" changes (docs, reverting the unrelated submodule, fixing a build error caused by recent changes in master). @Khaledwahba1994 and @den-schmidt, could you double check the gains and perhaps fly with cfclient and the flow deck (no mocap) to verify the observed flipping behavior? My guess is that something went wrong when merging our internal config files to the default values in the firmware.

@knmcguire
Copy link
Member

Thanks for the changes. I'll be able to test it out this wednesday again to double check any hardware faults. Could you perhaps also show me where I can find/know about the gains for the upgraded motors?

@knmcguire
Copy link
Member

knmcguire commented Feb 28, 2024

Alright I've done another test where I use the cfclient command-based flight controller on the flight control tab for a simple take-off and landing (uses high-level commander functions) with lighthouse

This is the PID controller on the same firmware on this PR:

20240228_133218.mp4

This is the Lee controller

20240228_133244.mp4

It doesn't flip at take off but it has a weird hop after landing and it flips afterwards. This is pretty repeatable for every landing for my crazyflie. Also the position hold it also shakes a little, indicating that the gains are still high.

@jpreiss
Copy link
Contributor

jpreiss commented Apr 23, 2024

I am really excited to start using this controller and forget about massThrust forever! But I am concerned that these lines introduce an unnecessary coupling between the quadrotor's moments of inertia and the controller gains:

self->u = vadd4(
vneg(veltmul(self->KR, eR)),
vneg(veltmul(self->Komega, omega_error)),
vneg(veltmul(self->KI, self->i_error_att)),
vcross(self->omega, veltmul(self->J, self->omega)));
control->controlMode = controlModeForceTorque;
control->torque[0] = self->u.x;
control->torque[1] = self->u.y;
control->torque[2] = self->u.z;

Here we define torque = gain * error, so the gains will need to be scaled a lot for larger quadrotors.

As I argued in #537 (comment) and #537 (comment), using mass/inertia-normalized units means the PID gains have the interpretation of "aggressiveness" independent of the quadrotor properties. For example, the KR gain means "if I am 1 radian away from the desired attitude, I should accelerate at KR radians/sec/sec towards the desired attitude."

Even though the Bitcraze team decided to go with SI units, we can still retain most of the benefits by working with the normalized model within the controller and converting to thrust/torque only at the interface boundaries. Note that you are already doing this for the thrust part:

struct vec F_d = vadd4(
acc_d,
veltmul(self->Kpos_D, vel_e),
veltmul(self->Kpos_P, pos_e),
veltmul(self->Kpos_I, self->i_error_pos));
struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
struct mat33 R = quat2rotmat(q);
struct vec z = vbasis(2);
control->thrustSi = self->mass*vdot(F_d , mvmul(R, z));

I propose that the attitude part should look similar.

@knmcguire
Copy link
Member

@jpreiss we have indeed expressed our interest of going to go for SI units but implementation has been quite on hold and currently there are other priorities, so it's fine of adding it in legacy way.

What is currently blocking this PR is the weird behavior we see that still needed to be investigated. The bare minimum we'd like to see is a stable take off and landing from the cfclient's command flight panel with all positioning systems, and currently we are seeing issues with the weird hop and flip at the end with lighthouse.

@jpreiss
Copy link
Contributor

jpreiss commented Apr 24, 2024

Sorry if my previous comment sounded like a request for a big change - I only meant to request a couple of lines change so this controller sets torque = inertia_matrix * (KR * error + ...) instead of torque = (KR * error + ...).

As a data point, I tested this controller in a mocap system and it worked OK, but I did see find the attitude control a bit jittery. I think it's unusual that the attitude integral gain KI is larger than the proportional gain KR - this is very different from the Mellinger controller tuning, even though the controllers are nearly the same architecture. But I wasn't able to significantly improve upon this tuning with few rounds of trial and error.

@knmcguire
Copy link
Member

Ah oke. I'll mark this for our next triage then to see about the next steps. I'm not sure how to fix it either, but if mocap works maybe we can be a bit more lenient. However, we are a bit worried about the surplus of controllers that is now difficult for us to maintain...

We'll let you know the outcome!

Copy link
Member

@knmcguire knmcguire left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So sorry for the delay.

We have agreed internally that we will merge this as this is a very good valued addition to the firmware. We will put a pin in the dicussion on how to handle the current overload of controllers but we won't bring that worry in for now. Just be aware to maintain this controller to test it out every once in a while if it still works well in master.

Also, just let us know before if you'd like to implement another controller to see what we have decided to do for the controllers.

Thanks and sorry again for the wait!

@knmcguire knmcguire merged commit a25e8fb into bitcraze:master May 16, 2024
24 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants