moveit servo collision checker precheck

[yangtfu]

Is your feature request related to a problem? Please describe.
The collision checker now is checking wheather the robot is near collision. The threshold_distance type will slowdown the robot even if the robot is trying to get away from collison, the stop_distance type just halt the robot. Both type cannot get the robot away from collision fastly.

Describe the solution you'd like
Can we add the command direction into collision checking, if the robot is getting away, just maintain the normal speed, if the robot is getting nearer to collisio, halt the robot.

[AndyZe]

I would love a PR to do this!

As far as I know, there's no way to get the direction to nearest collision right now, so it's hard to know if you're moving toward or away from collision. If you could figure out how to do that, I could fill in the other pieces to make this happen.

[mechwiz]

one idea is to add a "servo_node/pause_collision_check" service call where if the user calls it, it pauses collision checking. the operator can quickly move out of the collision, and then re-engage with another service call

I can make an MR for that if you think that's worth implementing. Should be relatively straight forward based on servo's current architecture

[mechwiz]

Another idea is to keep track of the previous self_collision_distance and scene_collision_distance and the current ones and if they are increasing with respect to the previous value -> disengage velocity scaling due to being close to a collision

[AndyZe]

@mechwiz your Idea 1 sounds good to me. Seems straightforward.

Idea 2 sounds OK but I think there might be some issues in practical situations. Like, what if you servo into a corner and there is a collision wall on both sides of the end effector? If you back away from one wall then you are getting closer to the other one. So you wouldn't want to disengage velocity scaling in that situation.

[mechwiz]

In that corner scenario though the distance values returned wouldn't be increasing so velocity scaling would still apply. Maybe at first they would increase as you move away from the first wall, but then it would decrease again as you get closer to the second wall. But yes, in a corner situation due to this reasoning you woudn't be able to retreat quickly, but this solution will help for non-corner situations at least

[AndyZe]

That makes sense. I'm just worried about a sudden engage->disengage->engage cycle as the arm moves in the corner.

Well, either way. Idea 1 is low risk, idea 2 may or may not pan out in my opinion.

[mechwiz]

Yeah I hear that - could lead to jerky motion which is probably not desired. Idea 1 sounds like the best option then!

[zflat]

I have a comment that does not fix the issue but may help mitigate. I see that the test collision check rate in both https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/test/config/servo_settings.yaml and https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml are set to 5.0 Hz with the comment "Collision-checking can easily bog down a CPU if done too often".

In my situation, we had it set to 10.0 Hz and with the speeds that the robot was moving that was too slow to the effect where it was as if we did not have any collision checking at all. I noticed that trajectory messages are being sent out at around 20 Hz. When I increased the collision check rate to 30 Hz servo was able to detect and prevent collision with some of my "slower" motions and then increasing to 50 Hz detect and prevent collisions with my "faster" motions. (In this case the "slower" motions were cartisian and the "faster" where direct joint commands, not sure if that makes a difference).

So maybe at a minimum add to the collision_check_rate comment that too low of a check rate could prevent collision checking from working at higher speeds? And possibly have a way to look at robot speeds with respect to the collision check rate and issue warnings? Or even a suggested procedure for tuning that parameter?

The reason I think this comment is relevant to this issue is because with a better collision check rate, it may be possible to use lower collision proximity threshold values. And with lower collision proximity threshold values, the impact of being slowed down when moving away from collision will be diminished since the velocity scaling will only apply to a more narrow space.

[sea-bass]

I'm currently working on a way to dynamically disable collision checking as a workaround; however, the original request should be doable and something worth looking into.

The way this should work is roughly as follows:

• Collision checking libraries used by MoveIt should give you a vector between the two nearest collision point pairs
• This vector can be used in a few ways:
  • If the command is a twist, then we can directly do a dot product between the twist direction and the collision vector, and only allow motion if it's away from this vector (dot product < 0, for instance)
  • If the command is a joint velocity, then we can use the robot's Jacobian to approximate the Cartesian direction that a joint motion would lead to. And the same dot product rule as above can hold

Besides figuring out how to extract these distances in the servo collision checker, the other key difference is that the collison checker will now need knowledge of the reference command being passed into the servo calculations.

This should all be doable given the tools available, but it will take some work.

[AndyZe]

You still need to think carefully about what would happen if the end effector is near a corner.

Wall A being the closest collision object could quickly switch to Wall B being the closest collision object

[sea-bass]

Agreed. This will partially be recommendations to the user to ensure their servo speed + collision checking frequencies can mitigate this, but there might also be a way to basically do my proposed check for all collision pairs within some envelope so there are multiple "disallowed" directions based on the corners. Of course, this would still fail if the corner in question was all part of the same mesh.

[MarcoMagriDev]

Any update on this?

@sea-bass I really like your approach, have you investigate it further?

[sea-bass]

I personally haven't worked on this, but I know there is a WIP branch here that attempts it: kylc#2

If anyone wants to take a look at that as a reference, would be great!

[MarcoMagriDev]

I worked a bit on this feature here MarcoMagriDev#1 (starting from humble branch for other needs) and I got to a point in which things start working.

The current implementation is quite dirty, it would be great if someone could have a look at that and suggest improvements.
Then, if you think that it could be an interesting feature I can then work on a better integration, this time starting from the main branch