Trajectory with both velocity and position

[PrimaryComet]

Hi, I'm looking to evaluate some methods of trajectory generating using the MRS UAV system. The output of these trajectory generators is intended to be a feasible trajectory with position, velocity, acceleration, yaw, and yaw-rate. As I understand, you cannot control (for example) position and velocity simultaneously using the control manager. Is there a way to achieve this natively with your system?

Many thanks for your time

[klaxalk]

The trajectory produced by the "trajectory generation" tool encapsulates the full state information thanks to differential flatness. If you want to specify velocity constraints for each waypoint, you can quite easily modify the "trajectory generation" tool to add these constraints. We have an old branch with such changes https://github.com/ctu-mrs/mrs_uav_trajectory_generation/tree/matous so you can take the commits in there as an example.

[PrimaryComet]

Interesting, I shall have a look at that. However, right now I have another issue with trajectory tracking: It's producing very jittery control of the drone. I have removed my planner and even loading the straight line example trajectory using the default constraints gives me causes the drone to bounds along the trajectory, moving up and down constantly. It seems to be the tracker, as the control reference being generated osculates up and down repeatedly. I've attached a plot of the control reference velocity and the resulting measured velocity (both in the x direction). The trajectory being followed is line_50.txt from your trajectory examples.
.

[klaxalk]

Hey, thanks for finding this! I can replicate the issue, so I will solve it asap. Will keep you posted.

[klaxalk]

Hey @PrimaryComet, I fixed the issue. It is now available on the unstable PPA and tomorrow, if the tests pass, on stable. Thanks for reaching out!

[PrimaryComet]

Thank you, it now works with the trajectory loader!

My current problem is regarding the in-built collision avoidance:

• I have disabled it in the custom config, however it is still enabled upon the simulation starting up, needing an extra rosservice call to disable it
• Even when disabled, sometimes when close to obstacles, the uav stops following my trajectory (and in the control manager diagnostics, I see tracking_trajectory: False. Then it eventually starts tracking the trajectory again, but this causes a collision as the uav attempts to return to the trajectory.

Since I am attempting to evaluated trajectory generators, and safety is one of the key metrics, could you advice on how I might turn all collision avoidance off? I have guessed it is a collision avoidance problem as the issue occurs very close to obstacles.

Many thanks again

[klaxalk]

Hey, there are several mechanisms that could pass as collision avoidance and could interfere with your experiments:

1. The mutual collision avoidance between multiple UAVs. This is turned off via the control_manager/collision_avoidance param.
2. The min_height and max_height check that can be turned off using the uav_manager/min_height_checking and max_height_checking params.
3. The virtual bumper, which is off by default.
4. The safety area, that can be turned off via the world_config.yaml file.

[klaxalk]

Then, if there is still something interfering with your flight, please, check the logs from the "control" tab of tmux, there you will find info on what triggers it.

[PrimaryComet]

Thank you again, that has solved that issue!

However, I now think I have found a potential solution to my first problem. The trajectory generator that I am testing outputs commands which seem to be compatible with your TrackerCommand. That is, it generates position, velocity, acceleration, heading, and heading rate at a frequency sufficient to be used by a controller, rather than a tracker. Is there a method you would suggest for disabling the tracker such that I can publish TrackerCommand messages directly, or is that not possible? I have looked at creating tracker plugin to pass the commands directly but that seems somewhat daunting at the moment, particularly as the tracker plugins are designed to accept one of Position, Velocity, or Trajectory commands.

Thank you again for all your help!

[klaxalk]

Great, I'm glad that you solved that issue.

Right now, there is no way to bypass a tracker with a custom tracker command. I would suggest creating a custom tracker to gain that functionality. You don't have to implement any of the methods for setting any of these references, it is all optional. I recommend starting from our tracker plugin example here https://github.com/ctu-mrs/mrs_core_examples/tree/master/cpp/tracker_plugin.

Tomas

[PrimaryComet]

I have now successfully implemented a custom tracker as you described. I have one more question that will hopefully be my final query:
How complex is it to insert a different model of drone into your simulation, i.e. one that does not come pre-packaged? I am likely to have access to a drone which is significantly smaller than any you include.

Additionally, I cannot for the life of me find where your "waitForRos, waitForControl" elements are defined in session.yml. I would like to add another file to be launched after takeoff, but I cannot figure out where I would even begin to look for these waitForX commands.

Thank you so much again!

[klaxalk]

Don't worry, and keep asking such good questions :-). We are very close to releasing a new version of our Gazebo framework which will do exactly what you request. You will be able to inject your own drone with your own set of sensors, geometry, etc., from a completely external package. When it is ready (in a few weeks), we will provide an example package with a custom "third-party" drone.

These waitFor* functions are installed into /usr/bin by the mrs-uav-shell-additions package (https://github.com/ctu-mrs/mrs_uav_shell_additions). They are just simple Python scripts, feel free to copy them, modify them, and place them close to your tmux session.

Sincerely, Tomas

[PrimaryComet]

Ah good, I shall keep an eye out for those, and I found the waitFor* function I needed thank you, I didn't even need to make one!

The tracker I have implemented that forwards commands directly from the trajectory planner I'm testing is able to track very closely, but I'm still getting some deviation from the trajectory, and I was wondering what I could do to reduce that. Obviously some deviation is expected, but I'm getting a few centimeters of RMS error in the tracking, which causes crashes in some challenging scenarios. I've included an example screen shot below. The main issue in terms of collisions is the tendency of the drone to take the corner wider than the trajectory allows as you can see. [Red = commands sent, blue = path traced by odom]

The trajectory I'm generating in the screenshot has velocity, acceleration, and jerk limits, all of which are lower than the limits set in the constraint manager (I'm currently using your default "medium" constraints). I can reduce the tracking error by lowering the trajectory constraints naturally, but surely the controller should be able to follow these commands since they are within the constraints set by the manager.

[klaxalk]

Hey, the discrepancy between your reference and the odometry is simple. That is the actual control error. What are you providing within the Tracker command? Are you supplying all the derivatives? Our controllers rely on acceleration to compute the feedforward tilt and jerk to compute the feedforward attitude rate. Without these, the active motion of the UAV is only caused by the feedback part of the controller, which only acts when a control error appears. So, there will always be some.

If you do supply all the derivatives and still see a control error, there might be multiple explnantions:
a) the controller is not tuned well, which can be fixed
b) your control reference is not internally feasible (the derivatives do not fit to a multi-integrator point-mass model)
c) your flight is so aggressive that your reference violates the actual UAV constraints (the real ones imposed by the hardware)

By the way, the dynamic constraints are not interfering with you anymore since you wrote your own tracker. It is actually the tracker's job to make sure the control reference does not violate the constraints.

[PrimaryComet]

I am currently providing all derivatives down to Jerk (along with heading and heading rate), as that is all the planner I'm trying to benchmark can output. As we would expect, when I removed the higher order derivates, the performance became worse.

As for the reasons for control error:
a) Is this related to the PID constants that can be configured using the custom configuration file? I have done some limited tests with those and did not find I could improve over the defaults,
b) Just to check I have understood this correctly so I can perform the necessary checks: this would mean that (for example) the provided velocity is not the derivative of the provided position, and as such the controller cannot execute the command,
c) I did think about this, but I have been operating with constraints based upon your slow, medium, and fast options. I found a noticeable control error even when operating at smaller constraints than the "slow" option.

Oh and that's good to know about the dynamic constraints, thank you!

Also I forgot to mention that I'm using the SE3 controller as it has a lower error than the MPC controller.

[klaxalk]

Hey, here is an example of a control error when flying with our tracker (the MPC Tracker) with:

• fast constraints
• emulation of a "3d planner's output" with dynamic replanning
• the control error (/uav1/control_manager/control_error) averages at 10 cm.

Tuning the gains of the Se3Controller could help depending on the scenario. The "soft" gains we provide as default to all our UAVs are rather generic (to fly with all simulation & real-world platforms). You can increase the kpxy and kvxy (position and velocity coefficients of the state feedback, similar to P and D but not the same). You can realistically go to kpxy=12 and kvxy=8. In the real world, you will get punished for this when the state estimate gets noisy. But don't expect anything better than getting to 5 cm control error on average. You can use the rqt_reconfigure app to change the gains in mid-flight to see the results.

b) Just to check I have understood this correctly so I can perform the necessary checks: this would mean that (for example) the provided velocity is not the derivative of the provided position, and as such, the controller cannot execute the command,

You are correct. If these state variables are not derivatives/integrals of each other, then the reference can not be met, and as a result, a control error (in all the state variables) arises. Extreme example: you can not meet a setpoint of velocity=5m/s and constant position.

[PrimaryComet]

Hey, so I have tested a few things.

Firstly, I find that the average total position error is approximately 16 cm. I have tried a little tuning of kpxy and kvxy to reduce it, but so far I only managed to lower it to 13 cm.

Secondly, I find the rms difference between the provided velocity and the computed derivative of position is approximately 0.05 m/s. This is mostly caused by occasional spikes when replanning occurs due to obstacles, so perhaps that is increasing my error. Outside of those spikes, the rms difference is sub 1 mm/s, and those spikes are typically above 0.2 m/s.

Finally, is there a place I can find the constraints imposed by the simulated hardware, to ensure I am not attempting to violate them with the planner?

Many thanks again for all your help