Fix velocity scaling factor calculations and support multi-DOF joints in Servo #2540
Conversation
sea-bass
force-pushed
the
fix-velocity-scaling
branch
from
2694d09
to
fafa495
Compare
Codecov ReportAll modified and coverable lines are covered by tests ✅
Additional details and impacted files@@ Coverage Diff @@
## main #2540 +/- ##
==========================================
- Coverage 50.52% 50.52% -0.00%
==========================================
Files 387 387
Lines 32064 32070 +6
==========================================
+ Hits 16198 16201 +3
- Misses 15866 15869 +3 ☔ View full report in Codecov by Sentry. |
sea-bass
changed the title
|
@marioprats I found another issue with multi-DOF joints as I was second-guessing something in a unit test... so the PR grew in scope. Mind re-reviewing? Thanks! |
sea-bass
force-pushed
the
fix-velocity-scaling
branch
from
c1d955e
to
7178a91
Compare
| @@ -163,7 +163,8 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request | |||
| else | |||
| { | |||
| // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing. | |||
| servo_->resetSmoothing(servo_->getCurrentRobotState()); | |||
| last_commanded_state_ = servo_->getCurrentRobotState(); | |||
There was a problem hiding this comment.
Ah good find. What's the guess here? It seems like it's possible that last_commanded_state_ might occasionally be plumbed through to smoothHalt? Just depending on timing. It's tough to replicate, but adding this change seems to #MakeItWork.
There was a problem hiding this comment.
yep, I think it was that. When the robot returned to unpaused, it seems there were some states in which not sending a command caused it to go into smoothHalt and leap over to a stale last_commanded_state_.
This PR fixes a safety bug we found with MoveIt Servo, in which the velocity scaling override could cause the robot commands sent to exceed the robot's joint velocity limits. This was discovered because of bad IK solutions "teleporting" the robot in mock hardware.
This correctly takes the minimum scaling factor of the joint safety limits (which MUST be always met) and the override scaling factor, if one is specified.
I also ended up fixing another issue in which Servo was only working for joints with 1DOF, because we were making assumptions about only extracting the first variable from a potentially multi-DOF joint. It's also why the scaling factor unit test was ignoring the first joint, which is a virtual joint in the robot model and not even part of the
panda_armjoint model group.@ibrahiminfinite FYI