Hi, I’m finding hard time in completely understanding how the TrajectorySetpoint works.
To start I set the OffboardControlMode to position:
msg = OffboardControlMode()
msg.position = True
Then, I want to send a position setpoint together with desired velocity. According to offboard_control, when the position is field is set, velocity and accelerations, if they are not Nan values, are used as feedforward terms for the inner loop controllers. What does this mean? How should I set them?
Here the confusion is between what is trajectory and what is waypoint.
Trajectory should be a continuous stream of reference values for position and possibly velocity, and the drone tries to zero the error between the current position (and velocity) and the reference one(s).
This means that the position reference should not be the fixed to the final one, but instead it should change progressively until reaching the final value. The role of the velocity feedforward term here is to anticipate the position controller. Imagine to be at a certain time in the middle of the trajectory at precisely the right reference position for that time (remember, the position reference is progressively updated). Then in that instant the position controller, measuring zero position error, will output zero velocity setpoint to the velocity controller. The drone will try to stop and miss the next reference position, then you will have reference error and thus velocity setpoint. Now, if this happens at 50Hz, the final behaviour will be the drone following the trajectory with a constant small position error (it will be always a bit “late” w.r.t. the reference. Giving the drone the desired velocity will counter this: even if the position controller says “don’t act, I’m already where a want to be”, the velocity feedforward will force the drone to keep going, anticipating the trajectory.
Using a waypoint with desired velocity is basically trying to set the final point and the velocity to approach it. The final trajectory will be a line then. I think there are other modules and messages that implement this feature.
If you want to keep using trajectory setpoints, you need to have the position reference slowly changing according to the velocity. Fore example, if you want to travel from p_i to p_f at speed v you can do this
pos = p_i + v*t
vel = v
where t is the time since you started moving from p_i to p_f. You stop when you reach p_f
You mentioned there might be other modules for this feature, do you know which one? I tried to have a look at the TrajectoryWaypoint (with VehicleTrajectoryWaypoint) and TrajectoryBezier (with VehicleTrajectoryBezier) but I never got them to work.
In VehicleTrajectoryWaypoint, it’s not clear how to set the parameter uint8 type. Same for the parameters bool point_valid and uint8 type in TrajectoryWaypoint. I wasn’t successfull with the Bezier trajectory either, which looks more straightforward to implement, at least for order 2.
From the documentation path-planning-interface, it looks like they only work for obstacle avoidance, when OBS_COM_AVOID is enabled.
I don’t, sorry.
But as the number of flight modes are limited, I would look into mission mode and what you can do with it, if defining a mission is acceptable for you of course.
