Add trapezoidal speed trajectory generator #527
Conversation
|
|
||
| if (instant <= m_ta) // first ramp | ||
| { | ||
| step = 0.5 * m_acceleration * std::pow(period, 2.0) * (2 * m_tick + 1) + m_v0 * period; |
There was a problem hiding this comment.
Nitpick: I think (not 100% sure to be honest) that std::pow(period, 2) should use the simple period*period while specifying the exponent as double will take another more expensive overload.
| } | ||
| else if (instant > m_tb) // second ramp | ||
| { | ||
| step = m_acceleration * (m_tf * period - 0.5 * std::pow(period, 2.0) * (2 * m_tick + 1)); |
There was a problem hiding this comment.
There was a problem hiding this comment.
Thanks a lot @PeterBowman ! This is a nice addition, thanks.
I add some minor comments inline, furthermore could you update the CHANGELOG? Thanks!
The code that initializes trajectory_type from config has been slightly simplified and now reports is value in a yDebug line.
I would prefer actually to get rid of most of the yDebug outputs, or at least put it under an option, but indeed as you did it know for sure is consistent with the rest of the code, so I think it is ok.
|
Can you also rebase on the top of the latest master? This should fix CI. |
PeterBowman
force-pushed
the
trap-trajectory
branch
from
f6adaf2
to
9c86631
Compare
PeterBowman
force-pushed
the
trap-trajectory
branch
from
9c86631
to
7a4ae98
Compare
Should be all done now, thanks! Edit: macOS-latest builds are still failing, but I'm not sure why. |
It is probably due to actions/runner-images#2322, actions/runner-images#2391 or other GitHub Actions/Homebrew regression, feel free to ignore. Probably eventually we should add conda-based macOS CI also here, for the robotology-superbuild has proved to be much more stable then the homebrew-one. |
|
Thanks @PeterBowman ! |
The
TrapezoidalSpeedTrajectoryGeneratorclass is added for use byGazeboYarpControlBoardDriver. It is meant to generate a trapezoidal profile capped at the provided reference speed and using fixed acceleration in both ramps. The signature ofTrajectoryGenerator::initTrajectoryhas been expanded to accomodate said reference acceleration value. This generator is also capable of appending new trajectories to ongoing joint motion, that is, whenever a setpoint is updated (e.g. by issuing apositionMovecommand while a previous one has not yet completed), last computed joint velocity is used to calculate the initial motion of the new trajectory.In order to configure a control board to use this new generator, set the
TRAJECTORY_GENERATION::trajectory_typeproperty totrapezoidal_speed.Additional changes:
GazeboYarpControlBoardDriverhas learned to fetch reference speed and acceleration data from initial configuration viaTRAJECTORY_GENERATION::refSpeedandTRAJECTORY_GENERATION::refAccelerationproperties, respectively. Existing code has been factored out and conveniently improved in a newsetTrajectoryReferencesmethod.0.01m/s² as max acceleration for prismatic joints.minimum_jerkif an unsupported value was passed toTRAJECTORY_GENERATION::trajectory_type; an error is reported instead.trajectory_typefrom config has been slightly simplified and now reports its value in ayDebugline.