implemented arm constraint formula #268
Conversation
sarahfredrickson
force-pushed
the
improve-arm-constraint
branch
from
59fdf3c to
ecd9384
Compare
There was a problem hiding this comment.
Looks good, we just need some documentation. 2 things:
- Victor will merge his PR soon, so be prepared to resolve those merge conflicts. Basically just move your code into the
normalizeEEPosWithinRadiusmethod (I forget the exact name) - You'll need to edit Victor's testcase for our updated logic. Basically it should just be changing the constants in the testcase
There was a problem hiding this comment.
Took a look, I understand what the bug you were bumping into was. First, the math had a minor typo, so some parenthesis were missing.
Secondly, it wasn't a bug that you introduced necessarily. Your code was working properly, and normalizing the length to 9.5, but due to floating point rounding errors it was just over 9.5. So when get_setpoint() was called, it tries to normalize it again, but eePos is the same as setpoint, so the numerator becomes 0, so the whole thing becomes nan.
I'd say we could fix this by just adding a very small epsilon to the radius when we're checking if we should normalize. So, in normalizeEEWithinRadius(), change if (eePos.norm() > radius) to if (eePos.norm() > radius + 1e-4), so it will only normalize if we exceed the radius by any perceptible amount.
Note that after fixing this you'll have to fix your testcase, since the correct answer isn't (9.5,0) anymore.
|
Great job, this looks good! The code looks fine, let's do some hardware testing before merge. Firmware should get ironed out very soon, which will enable this testing. Let's look into sim testing as well |
src/control/PlanarArmController.h
Outdated
| @@ -41,7 +41,7 @@ class PlanarArmController { | |||
| // NOTE: currJointPos could extend beyond the safetyFactor, so safety factor | |||
| // normalization logic is performed. | |||
| set_setpoint(currJointPos); | |||
| CHECK_F(safetyFactor > 0.0 && safetyFactor < 1.0); | |||
| assert(safetyFactor > 0.0 && safetyFactor < 1.0); | |||
There was a problem hiding this comment.
Why the change from CHECK_F to assert? In general we should usually prefer CHECK_F, since loguru has some nicer features, and still runs the checks in release mode.
|
Setpoint no longer goes out of bounds and arm doesn't crumple on itself |
sarahfredrickson
requested review from
abhaybd
and removed request for
imisaacwu
The formatting for the "a" value looks weird but that's what the formatter told me to do. Also will probably have git issues bc I pulled changes to origin before pushing but still says I'm 4 commits behind master.