Fixing speed commands with motors - #171
Conversation
The clamp+low-pass moderation stack was silently discounting velocity_max by ~85% at steady state (low-pass runs after the velocity clamp). Replace it with an opt-in reactive trapezoidal profile (accel_max/velocity_max, re-planned every tick) in joint_command_core, off by default pending bench test. Also wire up deg/s->ERPM conversion for can_node's POSITION_VELOCITY path using CubeMars AK-series pole-pair count (21) and gear ratio (9:1), gated to only the motor models that value has been verified for. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
|
Saw that you implemented the trapezoidal velocity ramp which is nice, but did you consider the following issue that can possibly be raised and how are you planning to deal with it? 
only leave velocity ramp as an option for testing pure joint control will be fine too for now though or we can also simply make it a pure joint control without any ramp actually |
| static constexpr double kAkSeriesPolePairs = 21.0; | ||
| static constexpr double kAkSeriesGearRatio = 9.0; |
There was a problem hiding this comment.
is it always gonna this number for all our cubemars motors btw?
If it could change in the future for other cubemars motors can note that down too
There was a problem hiding this comment.
just commented next to them. for the current motors were using though it should be constant unless someone changes them in the firmware
| return alpha * previous + (1.0 - alpha) * target; | ||
| } | ||
|
|
||
| // Accelerate/cruise/decelerate ramp onto a (possibly moving) target, no overshoot. |
There was a problem hiding this comment.
Still think that comment like this may not be that necessary, feel free to disagree though
Generally convention in comments is that as few comment as possible will be nice, if the code is self-readable, it's better to not put comment
can double check all comments as a whole
There was a problem hiding this comment.
just removed it. i double checked comments, everything else seems essential. lmk if u disagree though
| @@ -5,6 +5,8 @@ | |||
| #include <map> | |||
| #include <stdexcept> | |||
There was a problem hiding this comment.
Maybe need fix CI clang issue
can rebase too, maybe some clang issue is caused by other people's change that are fixed now
|
nit: can have better pr title that are more descriptive |
yeah i had already initally planned to keep trapezoidal ramp for pure joint control. you can even see in the yaml file that its intially off. to solve the IK deployment i just disabled the low pass filter, since it doesnt really do anything if we set joint velocity limits. i think keeping trapezoidal limit as backup is probably a better option though for pure joint control |
A bunch of fixes were made as we had a couple of issues in our interfacing stack to the real arm. Here is a list:
Problem 1 — Velocity limit silently discounted ~85%
The moderation pipeline clamped velocity, then ran a low-pass filter (α=0.85) after the clamp — re-smoothing an already-limited value every tick, cutting real steady-state speed far below the configured velocity_max (e.g. 40°/s configured, ~6°/s actual).
Fix: Added a reactive trapezoidal velocity ramp (accelerate → cruise → decelerate onto target, re-planned every tick) that replaces the clamp+low-pass combo when enabled via the new enable_trapezoidal_limit flag. Off by default pending bench test.
Problem 2 — POSITION_VELOCITY sent in wrong units
can_node passed velocity/acceleration straight through in deg/s onto CAN signals that are wire-encoded in ERPM, so this control mode would command the wrong motor speed entirely.
Fix: Added degPerSecToErpm(), converting using the AK-series motor's pole-pair count (21) and 9:1 gear ratio. Gated to only run for motors with a validated profile, refuses otherwise instead of guessing.