Bug fix satisfies bounds

[bbrito]

Solution for the issue #40

[jrgnicho]

The problem you are trying to address may be related to an issue with the MoveGroupInterface in setting the start state to the current state #37. In summary, that is an issue that needs to be addressed in moveit. In order to make sure that it isn't a moveit issue could you briefly describe your test case (using rviz moveit, service calls, MoveGroup, etc)?

[bbrito]

Yes. I'm using the normal rviz interface with the motion planner to send a planning request. I changed the stomp_planner cpp to print the state that it gets in the request.
If I send a planning request when my robot is stopped, stomp gets the following initial state request:

[ INFO] [1494595802.853585269, 1822.656000000]: START STATE:joint_state:
header:
seq: 0
stamp: 0.000000000
frame_id: /base_footprint
name[]
name[0]: arm_shoulder_pan_joint
name[1]: arm_shoulder_lift_joint
name[2]: arm_elbow_joint
name[3]: arm_wrist_1_joint
name[4]: arm_wrist_2_joint
name[5]: arm_wrist_3_joint
name[6]: bl_caster_rotation_joint
name[7]: bl_caster_r_wheel_joint
name[8]: br_caster_rotation_joint
name[9]: br_caster_r_wheel_joint
name[10]: fl_caster_rotation_joint
name[11]: fl_caster_r_wheel_joint
name[12]: fr_caster_rotation_joint
name[13]: fr_caster_r_wheel_joint
name[14]: torso_pan_joint
name[15]: torso_tilt_joint
position[]
position[0]: 0.00395181
position[1]: -0.00324448
position[2]: -1.23173e-05
position[3]: 5.35801e-05
position[4]: -3.63409e-05
position[5]: 2.35438e-05
position[6]: -9.88932e-06
position[7]: 0.053995
position[8]: -8.32916e-06
position[9]: -0.0545914
position[10]: 9.85958e-06
position[11]: 0.704385
position[12]: 5.99431e-06
position[13]: 0.0616807
position[14]: -8.60211e-07
position[15]: 3.07665e-06
velocity[]
effort[]
multi_dof_joint_state:
header:
seq: 0
stamp: 0.000000000
frame_id: /base_footprint
joint_names[]
transforms[]
twist[]
wrench[]
attached_collision_objects[]
is_diff: 0

[ INFO] [1494595803.792179745, 1823.559000000]: STOMP found a valid solution with cost 18.026076 after 1 iterations
[ INFO] [1494595803.796327952, 1823.564000000]: STOMP found a valid path after 0.942868 seconds

Then If I move the robot around, using the teleop node for example it gives the following error:

000000]: START STATE:joint_state:
header:
seq: 0
stamp: 0.000000000
frame_id: /base_footprint
name[]
name[0]: arm_shoulder_pan_joint
name[1]: arm_shoulder_lift_joint
name[2]: arm_elbow_joint
name[3]: arm_wrist_1_joint
name[4]: arm_wrist_2_joint
name[5]: arm_wrist_3_joint
name[6]: bl_caster_rotation_joint
name[7]: bl_caster_r_wheel_joint
name[8]: br_caster_rotation_joint
name[9]: br_caster_r_wheel_joint
name[10]: fl_caster_rotation_joint
name[11]: fl_caster_r_wheel_joint
name[12]: fr_caster_rotation_joint
name[13]: fr_caster_r_wheel_joint
name[14]: torso_pan_joint
name[15]: torso_tilt_joint
position[]
position[0]: 0.003953
position[1]: -0.00324404
position[2]: 8.97255e-06
position[3]: 5.38706e-05
position[4]: -3.63415e-05
position[5]: 2.35435e-05
position[6]: -1.57084
position[7]: 62.3878
position[8]: -1.57084
position[9]: 61.9657
position[10]: -1.57081
position[11]: 64.1876
position[12]: -1.57081
position[13]: 62.9368
position[14]: -6.95107e-07
position[15]: 4.33287e-06
velocity[]
effort[]
multi_dof_joint_state:
header:
seq: 0
stamp: 0.000000000
frame_id: /base_footprint
joint_names[]
transforms[]
twist[]
wrench[]
attached_collision_objects[]
is_diff: 0

[ERROR] [1494598049.751569736, 4017.404000000]: STOMP Start joint pose is out of bounds
[ERROR] [1494598049.751585033, 4017.404000000]: STOMP failed to get the start and goal positions
[ WARN] [1494598049.756872455, 4017.410000000]: Fail: ABORTED: No motion plan found. No execution attempted.

My planning group are only the first six joints. If I check in moveit the active joint I got the list with the name of the first active joints. So something is wrong with the state->satisfiesBounds() function. This error happens because the joints from the wheels are also including in the planning and state validation which should not happen.

With the changes that I suggested it fixed the problem. I also tested it in the real robot and it worked.

[jrgnicho]

I'm somewhat on the fence about this but I agree with you on that the planner should only be responsible to verify the bounds of the joints that are in the designated planning group. In addition to the changed you already made, could you also add a check for the satisfiesBounds on the entire state and print a warning if it fails? This would at least inform the developer/user that something else could be wrong with the start state.

[Levi-Armstrong]

👍 ok to merge

[jrgnicho]

@ipa-bfb thanks for the contribution.