How to resolve initial Jerk

[shubham-shahh]

Hi,
I @Jaeyoung-Lim I am able to run mavros_controllers with ardupilot sucessfully. but currently I am facing some issues, below, I have added a video describing the problem. I want to know what causes initial jerk and how can I resolve it.

Thanks

[Jaeyoung-Lim]

@shubham-shahh Do you have a log? I suspect it has something to do with the feedforward references that is being passed

[shubham-shahh]

Hi, I am sorry, I might not have logs for the same flight. but I made a new video, this time I had a way better flight after reducing max_acc from 5 to 4, attctrl_constant to 0.55. also, I reduced the maximum yaw rate given by the path planner to 0.52 which was earlier 3.14 because of Iris's limitation. here's the video and the logs

few things I noted, The aggressive pitch in the beginning of the flight, still it has a hard time taking corners and I cannot go more aggressive to use the trajectory tracker at full potential
I want to make sure, I got the mavros_controllers params right
for this flight, I used
max_acc=4.0
attctrl_constant = 0.55
normalizedthrust_constant=0.06
normalizedthrust_offset=0.001
and I have visualised the gains in the video
and what do you mean by feedforward referenced? can you please tell me more about that?

please let me know if i should make any changes in the params I mentioned.

[Jaeyoung-Lim]

@shubham-shahh When passing a trajectory to the controller, normally it is better to send velocity, acceleration alongside the position so that the controller can deal with setpoints changing dynamically.

It also seems like you are using Ardupilot, which might mean that there might be something going on on the firmware side.

[shubham-shahh]

Hi, yes, you are right, I am using ardupilot. I am passing, position, velocity, acceleration, yaw, yaw rate to the controllers.

[Jaeyoung-Lim]

@shubham-shahh Have you tried removing yaw and yaw rate?

[shubham-shahh]

@shubham-shahh Have you tried removing yaw and yaw rate?

Hi, I'm curious how will that help, I've not tried that yet, but I'll try that and update this thread, thanks

[shubham-shahh]

Hi @Jaeyoung-Lim, I tried your suggestion and turned off yaw and yaw rate, it worked fine but that is not the desired behaviour, since copter's heading plays a vital role for obstacle avoidance. though I think with the attitude controller tuning of copter, I can solve it.
I want to try this setup in real world and after reading all the issues/discusssions in this repo I still couldn't find a concrete way to find values for params such as

• attctrl_constant
• normalizedthrust_constant
• normalizedthrust_offset

I went through the report which is implemented in following fucnction

mavros_controllers/geometric_controller/src/geometric_controller.cpp

Line 436 in d3e0fa0

Eigen::Vector4d geometricCtrl::attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc,

and also the paper implemented for following function, except body rates are used in this case

mavros_controllers/geometric_controller/src/geometric_controller.cpp

Line 494 in d3e0fa0

Eigen::Vector4d geometricCtrl::geometric_attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc,

I developed the idea of the underlying concepts but it would be great if you could share your knowledge of all the params mentioned above along with some ways you used to find those params and tune them

please correct me, if there's a mistake in things I mentioned above

thanks and regards

[Jaeyoung-Lim]

Hi @Jaeyoung-Lim, I tried your suggestion and turned off yaw and yaw rate, it worked fine but that is not the desired behaviour, since copter's heading plays a vital role for obstacle avoidance. though I think with the attitude controller tuning of copter, I can solve it.

I am not sure if this is true. I would try to find out why exactly the problem is happening. Could you elaborate on how you made the conclusion that it is a tuning issue? There is also a mode where you can set the yaw to be always in the direction of velocity

[shubham-shahh]

Hi @Jaeyoung-Lim, I tried your suggestion and turned off yaw and yaw rate, it worked fine but that is not the desired behaviour, since copter's heading plays a vital role for obstacle avoidance. though I think with the attitude controller tuning of copter, I can solve it.

I am not sure if this is true. I would try to find out why exactly the problem is happening. Could you elaborate on how you made the conclusion that it is a tuning issue? There is also a mode where you can set the yaw to be always in the direction of velocity

Hi @Jaeyoung-Lim , Thanks for the response, from the logs attached above, I compared the desired Roll/Pitch/Yaw rate with attained Roll/Pitch/Yaw rate and there's a difference, visualised in the picture attached

I am referring to tuning of ardupilot's attitude controller.

I tried using the velocity yaw feature, but the copter goes haywire so I am working to resolve that

what are your thoughts?

thanks

[shubham-shahh]

The velocity_yaw feature is making the copter crash because it is giving super high yaw rates which the iris is not able to handle

[shubham-shahh]

Hi @Jaeyoung-Lim, I tried your suggestion and turned off yaw and yaw rate, it worked fine but that is not the desired behaviour, since copter's heading plays a vital role for obstacle avoidance. though I think with the attitude controller tuning of copter, I can solve it.

I am not sure if this is true. I would try to find out why exactly the problem is happening. Could you elaborate on how you made the conclusion that it is a tuning issue? There is also a mode where you can set the yaw to be always in the direction of velocity

Hi @Jaeyoung-Lim , Thanks for the response, from the logs attached above, I compared the desired Roll/Pitch/Yaw rate with attained Roll/Pitch/Yaw rate and there's a difference, visualised in the picture attached

I am referring to tuning of ardupilot's attitude controller.

I tried using the velocity yaw feature, but the copter goes haywire so I am working to resolve that

what are your thoughts?

thanks

Hi @Jaeyoung-Lim, any update on this?

[Jaeyoung-Lim]

@shubham-shahh Sorry for the late reply,

I think the current implementation of this repo assumes that you have either a yaw rate reference or a yaw reference. You can also tune down the yaw reference if it is making the vehicle crash (although, this should be something the autopilot should be able to handle, if it is capable of dealing with saturation).

I would try to check if this is coming from the autopilot or this repository. If there is something wrong with the geometric_controller, I can help resolve the issue

[shubham-shahh]

Hi @Jaeyoung-Lim, Thanks for the response. I have yaw-rate coming for the trajectory planner. In the velocity_yaw feature, yaw rate calculation might result in huge changes which are not compatible with iris in simulation (as seen in the previous yaw rate graph I shared).

so I have an update on this, I have tuned the ardupilot's attitude controller in SITL and things are looking good with the trajectory yaw rate as well. I am planning to take this setup in real world, I want to know how can I tune all the params such as

• attctrl_constant
• normalizedthrust_constant

and any advised way of tuning the geometric controllers other than hand-tuning?

thanks for all the help

[Jaeyoung-Lim]

@shubham-shahh As long as you have the autopilot safe from huge yaw rate calculations, This hand tuning should not be a problem. You can try to eyeball the constants from your vehicle's dynamic properties but not sure how this is done in ardupilot

What is missing is that you are getting yaw rates from a high level controller, while the low level control (geometric_controller) is assuming that it can choose it's own yaw rate.

[shubham-shahh]

@Jaeyoung-Lim, okay I will try the hand-tuning method and will look into finding a way to get the constants with ardupilot

can we place some upper bounds on the yaw rates the geometric controller can give out? In real world experimentations, I think this will be really critical

thanks

[Jaeyoung-Lim]

@shubham-shahh Sure why not - would tou be able to make the addition?

[shubham-shahh]

@Jaeyoung-Lim, sure, I'll look into it and add a PR once I implement it

thanks for all the support

[Jaeyoung-Lim]

@Jaeyoung-Lim Any updates regarding the problem?

[shubham-shahh]

Hi @Jaeyoung-Lim
I haven't worked on it for a while, but soon I'll conclude it. I wanted to ask you one thing,
In your opinion, shall I add a max limit here?

or shall I add it as a constraint in the controller itself?

thanks

[shubham-shahh]

Hi @Jaeyoung-Lim I haven't worked on it for a while, but soon I'll conclude it. I wanted to ask you one thing, In your opinion, shall I add a max limit here?

or shall I add it as a constraint in the controller itself?

thanks

Hi @Jaeyoung-Lim, any update on this?

[Jaeyoung-Lim]

@shubham-shahh sorry, I think you need to do both. Controller should be able to be robust against degenerate signals and your planner should not generate those signals to start with

[shubham-shahh]

@shubham-shahh sorry, I think you need to do both. Controller should be able to be robust against degenerate signals and your planner should not generate those signals to start with

Okay, thanks for the response, I'll look into it

[halil93ibrahim]

Hi @shubham-shahh, I am also having problems with Ardupilot SITL. Do you suggest any particular versions of Ardupilot and mavros_controller to start working with?

[shubham-shahh]

Hi @shubham-shahh, I am also having problems with Ardupilot SITL. Do you suggest any particular versions of Ardupilot and mavros_controller to start working with?

The latest version works best but you should aim for at least 4.2.0. please describe your issue in more detail

[halil93ibrahim]

Thanks for the prompt response! I am using SITL in Webots simulations, which restricts me with an old version. I will first try to set everything with the latest versions, and then try the controller again.

[shubham-shahh]

Thanks for the prompt response! I am using SITL in Webots simulations, which restricts me with an old version. I will first try to set everything with the latest versions, and then try the controller again.

Ardupilot doesn't support SET_ATTITUDE_TARGET message to use rate control in older versions.

[halil93ibrahim]

Ardupilot doesn't support SET_ATTITUDE_TARGET message to use rate control in older versions.

Hi again, I updated Ardupilot and now I can command SET_ATTITUDE_TARGET using rostopic mavros/setpoint_raw/attitude.

I fixed unsupported FCU. Sorry for disturbing too much. Now, the quadrotor has crash when the geometric controller launches.
But I found your old discussions about it, trying to find the solution there.

SOLVED [Now, I am getting an error message: "MODE: Unsupported FCU" when I launch geometric controller. Can you help me with that? What should I change when I launch it?]

[shubham-shahh]

Ardupilot doesn't support SET_ATTITUDE_TARGET message to use rate control in older versions.

Hi again, I updated Ardupilot and now I can command SET_ATTITUDE_TARGET using rostopic mavros/setpoint_raw/attitude.

I fixed unsupported FCU. Sorry for disturbing too much. Now, the quadrotor has crash when the geometric controller launches. But I found your old discussions about it, trying to find the solution there.

SOLVED [Now, I am getting an error message: "MODE: Unsupported FCU" when I launch geometric controller. Can you help me with that? What should I change when I launch it?]

Hi, So, you are facing issues because of this code block
since ardupilot doesn't have any mode called "OFFBOARD" the equivalent there is GUIDED. you can either change the mode there, or disable
sim_enable_ param from the launch file, and start mavros controllers after takeoff

Initially, you should set

<param name="Kp_x" value="1.0" />
<param name="Kp_y" value="1.0" />
<param name="Kp_z" value="10.0" />
<param name="Kv_x" value="1.0" />
<param name="Kv_y" value="1.0" />
<param name="Kv_z" value="1.0" />
<param name="attctrl_constant" value="0.3"/>
<param name="normalizedthrust_constant" value="0.06"/>
<param name="normalizedthrust_offset" value="0.00"/>

and from here you should claim your way to tuning, the default tuning params are too aggressive for iris in ardupilot simulation

[halil93ibrahim]

Thanks for the help.

since ardupilot doesn't have any mode called "OFFBOARD" the equivalent there is GUIDED. you can either change the mode there

After this fix, it stopped giving "mode offboard" error, but still, it cannot follow the trajectory. It diverges in one direction and crushes to the ground.

I tried also smaller and higher values than the controller parameters that you provided since I am using the sitl ardupilot model in Webots simulation, different than Iris. The result is the same.

I checked the mavros/setpoint_raw/attitude command with joy-stick and it's working fine. If you can provide any other tips for debugging I would appreciate it.

[shubham-shahh]

Thanks for the help.

since ardupilot doesn't have any mode called "OFFBOARD" the equivalent there is GUIDED. you can either change the mode there
After this fix, it stopped giving "mode offboard" error, but still, it cannot follow the trajectory. It diverges in one direction and crushes to the ground.

I tried also smaller and higher values than the controller parameters that you provided since I am using the sitl ardupilot model in Webots simulation, different than Iris. The result is the same.

I checked the mavros/setpoint_raw/attitude command with joy-stick and it's working fine. If you can provide any other tips for debugging I would appreciate it.

if you can describe your issue with more description, we might be able to help. you can also share your SITL logs so I can check, what's causing the issue for you

[halil93ibrahim]

Here is a video: https://drive.google.com/file/d/1sou5qn184Q_H0i065SfsUBuNHKzslAxR/view?usp=sharing
After taking off, I run the geometric controller and it does crash.
I am not sure if it is the correct one, but this is the last log from ardupilot/logs directory: https://drive.google.com/file/d/1ocSZTwe3sajbI4_0lsz2X-59IJ3GrSjU/view?usp=sharing

Please let me know if I could share something more; I am not very expert on those.

[Jaeyoung-Lim]

@halil93ibrahim Have you tried giving it a fixed reference and see if the vehicle is controlled?

For me it looks like the inputs from the controllers are not being updated

[halil93ibrahim]

Hi, I am not sure how should I try giving a fixed reference.
In this video, I have disabled trajectory_publisher node in the launch file. Is it what you mean? https://drive.google.com/file/d/1-mTsYekKFkWlYdtiXVC1QjcdNEczm01V/view?usp=sharing

[shubham-shahh]

Hi @halil93ibrahim, After analyzing your logs, from the below images we can clearly see, that your copter is not able to attain attitude rates in Roll, Pitch, and Yaw rate, and Yaw control looks really bad

ng)

[shubham-shahh]

Hi @halil93ibrahim, After analyzing your logs, from the below images we can clearly see, that your copter is not able to attain attitude rates in Roll, Pitch, and Yaw rate, and Yaw control looks really bad ng)

What you can try is, either tune your copter's controller aggressively or add constraints to the controller (which I may add in next PR as discussed with @Jaeyoung-Lim )