How to realize Trajectory tracking with Vx Vy Vz

Are there any codes that make sure the quadrotor follows the generated trajectory? Thanks.


I suggest to use position instead of velocity. Using velocity you could accumulate position error.
Position references have to be given in the NED reference frame. You can use the set_position_ned() function. You can find it here:

@elBarto Thank you for your advice.
There are two reasons why I don’t use this position method:

  1. Time-parameterized
    The trajectory is generated with time ‘t’,X=f(t),Y=g(t),Z=h(t).It means that I need to reach the specific point at specific time,the position method can not assure this because the quadrotor travels with constant speed. And Vx=d(X)/d(t),Vy=d(Y)/d(t),Vz=d(Z)/d(t),Vx Vy Vz are easy to get.
    when the quadrotor is reaching the waypoint, there will be a short pause in which the quadrotor determines whether the waypoint has arrived. This makes the overall flight discontinuous.

I am looking for a controller that can make correction of speed according to the deviation between the current position and the trajectory to ensure the quadrotor follows the desired trajectory.

@sudo I have created a ROS node for exactly that reason.

It is based on a nonlinear geometric controller and works in offboard mode by sending body_rate commands


I am so excited to see this. Thank you very very much !!:beers::beers:

@sudo Awesome! Contributions are also welcome :slight_smile:

1 Like

try to feed position and velocity at the same time with setpoint_raw. the px4 uses the velocity as feedforward information. this works quit good for me.

Nice work, thanks for it

@sudo Here is a video of it working on a real platform from one of the contributors!

1 Like

@Jaeyoung-Lim The video looks excellent !! I am new in this and I am trying to run your pkg. I can run the sim environment successfully but cant run your launch file.
Terminal reports err as:
while processing /home/gmy/Firmware/launch/posix_sitl.launch:
Invalid tag: mavlink_sitl_gazebo
ROS path [0]=/opt/ros/kinetic/share/ros
ROS path [1]=/home/gmy/test_ws/src
ROS path [2]=/home/gmy/catkin_ws/src
ROS path [3]=/opt/ros/kinetic/share
ROS path [4]=/home/gmy/Firmware
Arg xml is arg default="$(find mavlink_sitl_gazebo)/worlds/" name=“world”/
I am stumbled here

@TobiasM Thank you for your advice. I tried this in Gazebo Simulation. I find that the drone tracks good in X-axis BUT seems that it doesnt work in Y-axis(I abandoned Z, Z is set to 2 ).
Below are part of the position log:

goal.x goal.y goal.z
real.x real.y real.z

14.421766 0.179454 2.000000
14.201502 -0.499506 2.034843

14.475429 0.247687 2.000000
14.244723 -0.445643 2.034098

14.528336 0.315501 2.000000
14.313771 -0.359543 2.032773

14.580449 0.382812 2.000000
14.347980 -0.314730 2.032230

14.631733 0.449536 2.000000
14.416365 -0.225706 2.031084

14.682151 0.515590 2.000000
14.459023 -0.170487 2.030164

14.731667 0.580893 2.000000
14.526984 -0.082826 2.028497

Which launchfile are you running?

Thank you for your quick reply.
roslaunch geometric_controller sitl_trajectory_track_circle.launch
Did I miss anything ?

Could you try switching your firmware directory to ~/sec/Firmware ? unless you tried to manually set the path

Fantastic! I have looked into your code a whole day. But I am a little confused about How to replace circular trajectory with my polynomial trajectory.

Sorry to interrup,which kind of vehicle frame is your controller base on, “十” or “X” ? ?

@sudo It doesn’t matter. The frame geometry are handled in the firmware mixer

Hello @Jaeyoung-Lim !
I have a small query. In one of my projects, I was giving a position setpoint in simulations to track a trajectory and the quad rotor performed pretty well. Won’t this work on real system ?
Thanks much for helping everyone :slight_smile:

@Vrinda It would work as long as your setpoints are moving slowly. As quadrotors are a dynamical systems, it won’t be able to track high acceleration reference points.

For example, if you are moving towards a reference point and the reference point has a velocity in the opposite direction(moving towards the vehicle) If you give only the position setpoint, it will still try to accelerate towards the reference which results in huge overshoots.

1 Like

I get it now ! Thanks much @Jaeyoung-Lim !