Changed spline interpolation to use the last commanded joint velocity… #195
Conversation
… when calculating the next spline in the trajectory Also limited the acceleration to the maximum joint acceleration of the target accelerations calculated from the spline. But still added a constant acceleration when slowing down is active, as the acceleration in the trajectory no longer can be used. This will fix the issue with discontinuous motion when speed scaling is active
resources/external_control.urscript
Outdated
| end | ||
|
|
||
| # search for maximum | ||
| local i = 0 |
There was a problem hiding this comment.
To improve efficiency
| local i = 0 | |
| local i = 1 |
resources/external_control.urscript
Outdated
| # ensure we have something to iterate over | ||
| local length = get_list_length(list) | ||
| if length == 0: | ||
| popup("Getting the maximum of an empty list is impossible in list_max().", error = True, blocking = True) |
There was a problem hiding this comment.
Not sure that this is the best way to handle the case of receiving an empty list.
Maybe ignore the trajectory instead (with an error message) and let the driver continue to be ready to receive the next command.
resources/external_control.urscript
Outdated
| def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor): | ||
| local qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 | ||
| def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor, is_slowing_down=False): | ||
| last_spline_qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 |
There was a problem hiding this comment.
Try to find a better name, as it only is the last_spline_qd next time!
resources/external_control.urscript
Outdated
|
|
||
| if is_slowing_down: | ||
| last_spline_qd = last_spline_qd * scaling_factor | ||
| speedj(last_spline_qd, 15.0, timestep) |
There was a problem hiding this comment.
Avoid hardcoded duplicates constants. Use a variable instead
resources/external_control.urscript
Outdated
| last_spline_qd = last_spline_qd * scaling_factor | ||
| speedj(last_spline_qd, 15.0, timestep) | ||
| else: | ||
| max_qdd = list_max_norm(last_spline_qdd) |
There was a problem hiding this comment.
Suggest that we calculate the qdd argument for speedj more simple way; by substracting the last qd with the current calculated qd.
In practice it will not have any impact as we are doing it in very small intervals/steps
Codecov ReportAll modified and coverable lines are covered by tests ✅
Additional details and impacted files@@ Coverage Diff @@
## master #195 +/- ##
=======================================
Coverage 72.09% 72.09%
=======================================
Files 71 71
Lines 2652 2652
Branches 337 337
=======================================
Hits 1912 1912
Misses 554 554
Partials 186 186 ☔ View full report in Codecov by Sentry. |
Header urls in the xml files seems to be outdated!
| // Acceleration should only increase or be constant within one scaled timescale. | ||
| // It should not fluctuate to zero or overshoot | ||
| EXPECT_EQ(sign(last_change_acc[i]), sign(change_acc[i])) | ||
| << " acceleration change direction doing " |
There was a problem hiding this comment.
I think a debug message wasn't fully deleted here.
There was a problem hiding this comment.
This is intended. The debug message is only printed out if the EXPECT_EQ fails.
Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
| // Acceleration should only increase or be constant within one scaled timescale. | ||
| // It should not fluctuate to zero or overshoot | ||
| EXPECT_EQ(sign(last_change_acc[i]), sign(change_acc[i])) | ||
| << " acceleration change direction doing " |
There was a problem hiding this comment.
This is intended. The debug message is only printed out if the EXPECT_EQ fails.
… when calculating the next spline in the trajectory
Also limited the acceleration to the maximum joint acceleration of the target accelerations calculated from the spline. But still added a constant acceleration when slowing down is active, as the acceleration in the trajectory no longer can be used.
This will fix the issue with discontinuous motion when speed scaling is active
This will fix issue #194