Skip to content

Commit 9af922d

Browse files
committed
pbio/trajectory: Fix speed rounding on short maneuvers.
If t1=0, the speed skips to w1 right away. Even though w0 is not used in that case, it still makes sense to give it a well defined value. This simplifies debugging and is useful for validation in tests. See pybricks/support#786
1 parent 9b2329b commit 9af922d

File tree

1 file changed

+15
-0
lines changed

1 file changed

+15
-0
lines changed

lib/pbio/src/trajectory.c

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -331,6 +331,13 @@ static pbio_error_t pbio_trajectory_new_forward_time_command(pbio_trajectory_t *
331331
}
332332
}
333333

334+
// For numerically negligible speed differences (achieved in zero time),
335+
// set the initial speed equal to speed at t1. This ensures correct
336+
// behavior when evaluating the reference when time is 0.
337+
if (trj->t1 == 0) {
338+
trj->w0 = trj->w1;
339+
}
340+
334341
// With the difference between t1 and t2 known, we know t2.
335342
trj->t2 = trj->t1 + t2mt1;
336343

@@ -464,6 +471,14 @@ static pbio_error_t pbio_trajectory_new_forward_angle_command(pbio_trajectory_t
464471
// With the intermediate angles and speeds now known, we can calculate the
465472
// corresponding durations to match.
466473
trj->t1 = div_w_by_a(trj->w1 - trj->w0, trj->a0);
474+
475+
// For numerically negligible speed differences (achieved in zero time),
476+
// set the initial speed equal to speed at t1. This ensures correct
477+
// behavior when evaluating the reference when time is 0.
478+
if (trj->t1 == 0) {
479+
trj->w0 = trj->w1;
480+
}
481+
467482
trj->t2 = trj->t1 + t2mt1;
468483
trj->t3 = trj->t2 + div_w_by_a(trj->w3 - trj->w1, trj->a2);
469484

0 commit comments

Comments
 (0)