Refactor velocity limit enforcement and add a unit test #2260
Conversation
Codecov Report
@@ Coverage Diff @@
## master #2260 +/- ##
==========================================
+ Coverage 57.85% 57.93% +0.08%
==========================================
Files 327 327
Lines 25644 25639 -5
==========================================
+ Hits 14834 14851 +17
+ Misses 10810 10788 -22
Continue to review full report at Codecov.
|
| @@ -50,6 +50,7 @@ | |||
| #include <moveit_servo/make_shared_from_pool.h> | |||
|
|
|||
| static const std::string LOGNAME = "servo_cpp_interface_test"; | |||
| static constexpr double LARGEST_ALLOWABLE_PANDA_VEL = 2.8710; // to test joint velocity limit enforcement | |||
There was a problem hiding this comment.
Where is this passed to servo so it knows what the vel limit is?
There was a problem hiding this comment.
Servo gets it from:
joint_model_group_->getActiveJointModels();
const std::vector<moveit_msgs::JointLimits> limits = joint->getVariableBoundsMsg();
I guess I could make a getter method to access that more easily. Worth it?
There was a problem hiding this comment.
Sounds like scope creep, but that would be a nice contribution for a new MoveIt dev!
|
|
||
| // Store the scaling factor if it's the smallest yet | ||
| if (scaling_factor < velocity_limit_scaling_factor) | ||
| velocity_limit_scaling_factor = scaling_factor; |
There was a problem hiding this comment.
Maybe I misunderstood but it looks to me like the function first looks for the smallest velocity limit scaling factor (which can end up being very small) and then, couple lines down, applies that lowest velocity limit to all the joints. What if there is a use case where someone wants, let's say limit the maximum velocity of the largest joint while keeping the other joints moving much faster. Wouldn't this approach then slow the whole robot down since one of the joints is limited to very low velocity?
There was a problem hiding this comment.
Yeah, the whole robot will slow down.
But, we want the robot to keep moving in the right direction. IMO that trumps speed. Here's a video of the bad way.
|
Kinetic is causing your CI failure, if you rebase this on master it will get my change to stop testing on kinetic. |
AndyZe
changed the title
AndyZe
changed the title
|
@AndyZe I'm fine with merging this as this version seems to be safer than the old implementation. In the future we should still check for acceleration values, though. |
* Rework velocity limit enforcement, add unit test * Test every command out of Servo * Minor variable renaming * Clang format & clang tidy * Delete unused acceleration variable
* Rework velocity limit enforcement, add unit test * Test every command out of Servo * Minor variable renaming * Clang format & clang tidy * Delete unused acceleration variable
* Rework velocity limit enforcement, add unit test * Test every command out of Servo * Minor variable renaming * Clang format & clang tidy * Delete unused acceleration variable
The way I was doing acceleration & velocity limit enforcement before had some flaws. It was only applied to one joint at a time, so it could affect the direction of motion of the robot. It was causing some serious direction swings.
This simplifies the whole limit checking, only enforcing velocity limits.
Also renamed the functions from enforceSRDFAccelVelLimits -> enforceVelLimits. I think this function name was misleading because limits actually get stored in joint_limits.yaml or the URDF, not the SRDF.