Padding not used consistantly

[bit-pirate]

While working more with the planning scene, I noticed the link paddings and scales, but didn't remember to have set them anywhere.

Turns out those values are the defaults, since the standard moveit config does not set them. Hence, I suggest to add them somewhere in the launchers or configs.

For example, I did this:

In planning_context.launch:

  <group ns="robot_description_planning">    
    [...]
    <rosparam command="load" file="$(find korus_moveit_config)/config/paddings.yaml"/>
  </group>

And a new paddings.yaml in the config folder looking like this:

default_robot_padding: 0.0
default_robot_scale: 1.0
default_object_padding: 0.0
default_attached_padding: 0.0

This example contains the default values.

[isucan]

+1

[bit-pirate]

Maybe hold this change back a bit. Yesterday I experienced a strange situation, in which the planner returned a path we seemed valid to the planer, but turned invalid when doing path validation. Calling the planner multiple times (including changing the number of planning attempts) didn't solve this situation. Only after I set the paddings back to the defaults, I was getting valid solutions (was using paddings of 0.01). Here is the log:

[ INFO] [1381841739.360295034]: Planning attempt 1 of at most 1
[ INFO] [1381841739.365596102]: Planner configuration 'arm[BKPIECEkConfigDefault]' will use planner 'geometric::BKPIECE'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1381841739.365907664]: arm[BKPIECEkConfigDefault]: Attempting to use default projection.
[ INFO] [1381841739.366792892]: arm[BKPIECEkConfigDefault]: Starting with 1 states
[ INFO] [1381841739.391463717]: arm[BKPIECEkConfigDefault]: Created 8 (4 start + 4 goal) states in 8 cells (4 start (4 on boundary) + 4 goal (4 on boundary))
[ INFO] [1381841739.391527789]: Solution found in 0.025491 seconds
[ INFO] [1381841739.392470044]: Path simplification took 0.000884 seconds
[ERROR] [1381841739.399116490]: Computed path is not valid. Invalid states at index locations: [ 12 ] out of 24. Explanations follow in command line. Contacts are published on /korus/move_group/display_contacts
[ INFO] [1381841739.399484033]: Found a contact between '<octomap>' (type 'Object') and 'finger_right_knuckle_2_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1381841739.399526998]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1381841739.399916213]: Completed listing of explanations for invalid states.

I got the feeling the planner/state sampler is not using the paddings, but the following path validation does. I'll investigate this further when I find time. For now I will just leave the settings at their defaults.

[isucan]

This sounds like inconsistent use of parameters, indeed.

[awesomebytes]

Any work on this issue? Thanks :)

[k-okada]

Hi,

I look into this problem this weekend and here is what I understand. I'm not sure if this is correct or right direction, so I welcome your comments/corrections.

1. CollisionRobot vs CollisionRobotUnpadded

The system has two collision robot model, one is named "collision robot" and the other named "collision robot unpadded" (https://github.com/ros-planning/moveit_core/blob/hydro-devel/planning_scene/src/planning_scene.cpp#L306)

It seems that CollisionRobot is used for checking collision with world and CollisionRobotUnpadded is used for self collisions. (https://github.com/ros-planning/moveit_core/blob/hydro-devel/planning_scene/src/planning_scene.cpp#L538)

So the problem is that they only change padding value for CollisionRobot, and the padding parameter for CollisionRobotUnpadded remains 0, which is used for self collision checking (https://github.com/ros-planning/moveit_ros/blob/hydro-devel/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L181)

2 Padding for self collision

What we (at least I) requested at this issues is that we need safety-margin for self collision. Since the motion planner is likely to move very close to the (for example) base of the robot and that's is usually very dangerous for rigidly controlled robot. So I think many (non-PR2, non-Baxer) robot users wants to add 5-10mm safety margin.

Currently system has

    default_robot_padd_ = 0.0;
    default_robot_scale_ = 1.0;
    default_object_padd_ = 0.0;
    default_attached_padd_ = 0.0;

parameters, (https://github.com/ros-planning/moveit_ros/blob/hydro-devel/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L1077)

and default_robot_padd is used for padding parameter for CollisionRobot, which means collision between robot and world uses this value, but there are no parameters for CollisionRobotUnpadded.

1. A Solution

One solution is to add new parameter for CollisionRobotUnpadded, default_robot_unpadded_padd_ and defualt_robot_unpadded_scale_(54a3ab3) and set this values to CollisionRobotUnpadded (https://github.com/ros-planning/moveit_core/pull/139).

1. Other approaches

Another approach could be:

1. set same padding value for CollisionRobot and CollisionRobotUnpadded

2. use default_object_padd_ for CollsionRobot and default_robot_padd_ for CollisionRobotUnpadded or use default_object_padd_ + default_robot_padd_ for CollsionRobot .

[awesomebytes]

Hello everyone, I just tried the last version from source as of now and I was playing with adding padding to the hands of our robot. I see no difference. So I started putting really big paddings... like one meter! And it makes no difference at all. I tried setting a virtual obstacle to see if it's avoiding it (maybe this is just not working with self collision) and, indeed, it's avoiding it in the plan but ignoring the padding too.

My paddings.yaml looks like:

default_robot_padding: 0.0
default_robot_scale: 1.0
default_object_padding: 0.0
default_attached_padding: 0.0
default_robot_link_padding: {   "hand_left_index_1_link"    : &hand_padding 1.15,
                                "hand_left_index_2_link"    : *hand_padding,
                                "hand_left_index_3_link"    : *hand_padding,
                                "hand_left_middle_1_link"   : *hand_padding,
                                "hand_left_middle_2_link"   : *hand_padding,
                                "hand_left_middle_3_link"   : *hand_padding,
                                "hand_left_thumb_link"      : *hand_padding,
                                "hand_left_palm_link"       : *hand_padding,
                                "hand_right_index_1_link"   : *hand_padding,
                                "hand_right_index_2_link"   : *hand_padding,
                                "hand_right_index_3_link"   : *hand_padding,
                                "hand_right_middle_1_link"  : *hand_padding,
                                "hand_right_middle_2_link"  : *hand_padding,
                                "hand_right_middle_3_link"  : *hand_padding,
                                "hand_right_thumb_link"     : *hand_padding,
                                "hand_right_palm_link"      : *hand_padding }

And in Rviz it looks like:

I'm sorry if I'm not understanding something in this conversation, but I thought this padding was used in planning time. So when I add 1meter of padding I expected to have some "starting in collision" message.

Contact me (or @adolfo-rt) if you need any help on testing this, as we would really like to have this feature.

[davetcoleman]

Any progress on this? I think this paddings.yaml is nice - I'd especially like to be able to customize the amount of padding for point cloud filtering per link - does this help with that too?

I can add this configuration file to the Setup Assistant once its format and functionality is stable.

[ZdenekM]

Similar problem here - something with Computed path is not valid. Invalid states at index locations... so +1 for this issue - any progress / ideas / hints?

[skohlbr]

@awesomebytes Have you checked into which namespace you uploaded your padding data? Normally, robot_description_planning/default_robot_padding would be assumed to be the correct one, given other params are uploaded there, too. In https://github.com/ros-planning/moveit_ros/commit/85d78d4462d406697cfb0692bb41fddf1be4d1a9, @davetcoleman removed the leading slash from robot_description_planning, so from that commit on the params actually have to be uploaded to /move_group/robot_description_planning/default_robot_padding to be read correctly. Any insight on this is appreciated, this looks like a bug to me.

[skohlbr]

So the robot_description variable there does not have a leading slash in the first place per default, so the code by @davetcoleman does not modify it.