Unexpected behavior with raw trajectories

[natesimon]

Hello! I have 7 different turning trajectories in the raw format. My goal is to chain them (one after another) for navigation.

Here is the trajectory I am flying:

traj = [
    [1.0,0.007796,0.501326,-0.019784,0.098986,-0.153218,-0.979962,1.368393,-0.482004,2.8e-05,-0.002699,0.066181,0.182636,1.402138,-2.828051,1.924088,-0.456612,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1e-06,-8.2e-05,3.456345,-0.019215,-2.746276,-0.263982,1.362492,-0.389284],
]

And the command to fly the same trajectory twice:

    commander.start_trajectory(trajectory_id, 1.0, relative=True)
    time.sleep(duration)
    commander.start_trajectory(trajectory_id, 1.0, relative=True)
    time.sleep(duration)

However, I am having 2 problems:

1. When I run the same trajectory twice in a row, the yaw is not relative (despite setting relative=True).
   As you can see below, the position is relative (roughly, the X/Y values are doubled), but the yaw aims to go from 0->80->25->80 in both cases, instead of 0->160. This results in an awkward motion.
   

2. If I plot the desired and actual values vs. time, I am curious why the trajectory has a "delay" in the X and Y values. I would expect a faster increase in forward velocity. I believe one source of error is that this trajectory expects an initial velocity of 0.5 m/s. However, when I use send_hover_setpoint to set an initial velocity, the drone suffers an error and is unable to use the trajectory.

Is there a limitation against using the commander framework, and the high-level commander framework, simultaneously?

For example, this causes an error: (using both send_hover_setpoint and start_trajectory).

    speedup_start_time = time.time()
    while time.time() - speedup_start_time < 0.5:
        cf.commander.send_hover_setpoint(0.5, 0., 0., 0.4)
        time.sleep(0.1)
    print("Starting traj.")
    # relative = True
    traj_start_time = time.time()
    commander.start_trajectory(trajectory_id, 1.0, relative=True)
    time.sleep(duration)
    commander.start_trajectory(trajectory_id, 1.0, relative=True)
    time.sleep(duration)

Any ideas that might help to fix these issues are greatly appreciated!

[krichardsson]

When I run the same trajectory twice in a row, the yaw is not relative (despite setting relative=True).

I think the simple answer is that the relative only is used for the position and not the yaw. You can check out the implementation in the firmware where the trajectory is started

If I plot the desired and actual values vs. time, I am curious why the trajectory has a "delay" in the X and Y values. I would expect a faster increase in forward velocity.

It depends on which controller you use, I guess you use the PID controller? The high level commander generates setpoints with position, velocity and acceleration, but the PID controller does not use all the data, it also has an upper limit (1.0 m/s by default I think) for speed.
The Mellinger controller on the other hand is using all data in the setpoint and will try to satisfy all three, but as you mention, your set up does not have the correct initial conditions and it is likely that the Mellinger will crash in this situation. The PID controller is much more forgiving but also more sluggish.

Is there a limitation against using the commander framework, and the high-level commander framework, simultaneously?

Not really but it adds a bit of complexity. The idea is that setpoints that do not originate in the high level commander have higher priority than the HL commander setpoints, the thinking is that a user should be able to "take over" and fly manually with a game pad if needed. When a low level setpoit has been received, the firmware will not use the high level commander until it is explicitly enabled again by calling commander.send_notify_setpoint_stop() in the python lib (or commanderRelaxPriority() in the firmware)

As a side note: the questions in this discussion are both related to trajectories, but are otherwise not not that similar. It would probably have been better to use two discussions instead, please see the Discussion rules

[natesimon]

Thank you @krichardsson ! I will make two discussion posts next time.

It is unfortunate that yaw is not relative. Since we are using an FPV camera, we would like to ensure the Crazyflie is facing "forward" (relative to the trajectory), multiple trajectories in a row.

We can get around this by specifying the trajectory using velocity control (send_hover_setpoint); the downside is that:

1. without position feedback, the trajectory is less precise,
2. send_hover_setpoint is not asynchronous, like the high level commander.

[krichardsson]

This functionality was implemented by the Crazyswarm team and I don't think they used yaw that much in their research, my guess is that they skipped yaw to make it simpler. Maybe it is not too complex to extend the functionality to also include the yaw? If you take a look at this, please consider contributing.