|
23 | 23 | # You should have received a copy of the GNU General Public License |
24 | 24 | # along with this program. If not, see <https://www.gnu.org/licenses/>. |
25 | 25 | """ |
26 | | -Used for sending high level setpoints to the Crazyflie |
| 26 | +Used for sending high level setpoints to the Crazyflie. |
| 27 | +
|
| 28 | +The high level commander generates setpoints from within the firmware |
| 29 | +based on a predefined trajectory. This was merged as part of the |
| 30 | +Crazyswarm project of the USC ACT lab. The high level commander uses a |
| 31 | +planner to generate smooth trajectories based on actions like "take off", |
| 32 | +"go to" or "land" with 7th order polynomials. |
27 | 33 | """ |
28 | 34 | import math |
29 | 35 | import struct |
@@ -137,7 +143,24 @@ def stop(self, group_mask=ALL_GROUPS): |
137 | 143 | def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False, |
138 | 144 | group_mask=ALL_GROUPS): |
139 | 145 | """ |
140 | | - Go to an absolute or relative position |
| 146 | + Go to an absolute or relative position. |
| 147 | +
|
| 148 | + The path is designed to transition smoothly from the current |
| 149 | + state to the target position, gradually decelerating at the |
| 150 | + goal with minimal overshoot. When the system is at hover, the |
| 151 | + path will be a straight line, but if there is any initial |
| 152 | + velocity, the path will be a smooth curve. |
| 153 | +
|
| 154 | + The trajectory is derived by solving for a unique 7th-degree |
| 155 | + polynomial that satisfies the initial conditions of position, |
| 156 | + velocity, and acceleration, and ends at the goal with zero |
| 157 | + velocity and acceleration. Additionally, the jerk (derivative |
| 158 | + of acceleration) is constrained to be zero at both the starting |
| 159 | + and ending points. |
| 160 | +
|
| 161 | + Warning! Avoid overlapping go_to commands. When a command is sent to a |
| 162 | + Crazyflie when another one is currently executed, the generated polynomial |
| 163 | + can take unexpected routes and have high peaks. |
141 | 164 |
|
142 | 165 | :param x: X (m) |
143 | 166 | :param y: Y (m) |
|
0 commit comments