[fetch] Head movement might not be included in collision check

[Affonso-Gui]

I don't know if this is really a problem or not, but moving fetch's head when hand was nearby during gazebo simulation lead to a collision (without planning error).

Is fetch's :angle-vector planning suppose to cover only :rarm movements?

The collision looked like this capture.

Can be reproduced by sending the following code:

(send *ri* :angle-vector (send *fetch* :reset-pose))
(send *ri* :wait-interpolation)
(send *ri* :angle-vector #f(100.0 55.9892 86.9713 -76.9011 80.0 -116.076 54.6 -54.1273 -9.13166 44.5203))
(send *ri* :wait-interpolation)
(send *ri* :angle-vector #f(0.0 85.0 0.0 0.0 -90.0 0.0 -100.0 0.0 -30.0 0.0))
(send *ri* :wait-interpolation)
(send *ri* :angle-vector #f(0.0 85.0 0.0 0.0 -90.0 0.0 -100.0 0.0 50.0 0.0)) ;;collision

[knorth55]

Currently, head motion is not supported by MoveIt!
In fetch_moveit_config, there is a file called fetch.srdf, and groups are defined as below.
https://github.com/fetchrobotics/fetch_ros/blob/indigo-devel/fetch_moveit_config/config/fetch.srdf#L6-L35

<robot name="fetch">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm">
        <joint name="shoulder_pan_joint" />
        <joint name="shoulder_lift_joint" />
        <joint name="upperarm_roll_joint" />
        <joint name="elbow_flex_joint" />
        <joint name="forearm_roll_joint" />
        <joint name="wrist_flex_joint" />
        <joint name="wrist_roll_joint" />
    </group>
    <group name="arm_with_torso">
        <joint name="torso_lift_joint" />
        <joint name="shoulder_pan_joint" />
        <joint name="shoulder_lift_joint" />
        <joint name="upperarm_roll_joint" />
        <joint name="elbow_flex_joint" />
        <joint name="forearm_roll_joint" />
        <joint name="wrist_flex_joint" />
        <joint name="wrist_roll_joint" />
    </group>
    <group name="gripper">
        <link name="gripper_link" />
        <link name="l_gripper_finger_link" />
        <link name="r_gripper_finger_link" />
</group>

According to fetch_moveit_config, fetcheus defines moveit-environment as below.
https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/fetch-interface.l#L107-L126

(defclass fetch-moveit-environment
  :super moveit-environment)
(defmethod fetch-moveit-environment
  (:init (&key ((:robot rb) *fetch*) &rest args)
         (send-super* :init :robot rb :frame-id "base_link" args))
  (:default-configuration ()
   (list (list :rarm
               (cons :group-name "arm")
               (cons :target-link
                     (send self :search-link-from-name "wrist_roll_link"))
               (cons :joint-list (send robot :rarm :joint-list))
               )
         (list :rarm-torso
               (cons :group-name "arm_with_torso")
               (cons :target-link
                     (send self :search-link-from-name "wrist_roll_link"))
               (cons :joint-list (append
                                  (send robot :torso :joint-list)
                                  (send robot :rarm :joint-list)))
)

:angle-vector-motion-plan cannot find head group so that it passes to normal :angle-vector (same as :angle-vecor-raw) .
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus_moveit/euslisp/robot-moveit.l#L555-L567

Related issue is #722

[Affonso-Gui]

I see. Thank you for the information.

Doesn't seem to have a great demand, but are there any plans on making head motion planning to be supported by MoveIt! ? Would that be accomplished by simply adding the group description and setting the :default-configuration as above, or need some more work on it?

If I can I'll try to do some more testing around this.

[knorth55]

pr2_moveit_config have head group, but I think it doesn't work.
https://github.com/ros-planning/moveit_pr2/blob/indigo-devel/pr2_moveit_config/config/pr2.srdf
You can try it with two commands.

roslaunch pr2_gazebo pr2_empty_world.launch
# other terminal
roslaunch pr2_moveit_config demo.launch

If you set moveit_config properly, i think it works, and also fetch works, too.
However, you can do self-collision check on euslisp (euslisp/jskeus#232)

• :check-collision nil

45.irteusgl$ (send *pr2* :rarm :inverse-kinematics (make-coords :pos #f(0 200 800) :rpy #f(0 0 0)) :rotation-axis nil)
#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.9924 74.2283 -76.3779 -121.539 4.06685 -34.9326 180.0 0.0 0.0)

• :check-collision t

46.irteusgl$ (send *pr2* :rarm :inverse-kinematics (make-coords :pos #f(0 200 800) :rpy #f(0 0 0)) :rotation-axis nil :check-collision t)
;; inverse-kinematics failed.
;; dif-pos : #f(0.020012 -0.000192 0.004285)/(0.020467/10)
;; dif-rot : #f(0.0 0.0 0.0)/(0.0/0.087266)
;;  coords : #<coordinates #X7efb870  0.0 0.0 0.0 / 0.0 0.0 0.0>
;;  angles : (50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.9924 74.2283 -76.3779 -121.539 4.06685 -34.9326 180.0 0.0 0.0)
;;  collision : ((#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b8fb40 fr_caster_rotation_link  224.6 -224.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X653f4c0 fl_caster_rotation_link  224.6 224.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X694b7d0 br_caster_rotation_link  -224.6 -224.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X8055e60 bl_caster_rotation_link  -224.6 224.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X7cf3bd0 torso_lift_link  -50.0 0.0 840.675 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b8ff30 fr_caster_r_wheel_link  224.6 -273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.00.0> . #<bodyset-link #X6558008 fr_caster_l_wheel_link  224.6 -175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X694b3e0 fl_caster_r_wheel_link  224.6 175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X694b5d8 fl_caster_l_wheel_link  224.6 273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X694cda8 br_caster_r_wheel_link  -224.6 -273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X694cee0 br_caster_l_wheel_link  -224.6 -175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X8045b48 bl_caster_r_wheel_link  -224.6 175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X7f717d0 bl_caster_l_wheel_link  -224.6 273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X78246d8 base_footprint  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b8ff30 fr_caster_r_wheel_link  224.6 -273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6558008 fr_caster_l_wheel_link  224.6 -175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X694b3e0 fl_caster_r_wheel_link  224.6 175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X694b5d8 fl_caster_l_wheel_link  224.6 273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X694cda8 br_caster_r_wheel_link  -224.6 -273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X694cee0 br_caster_l_wheel_link  -224.6 -175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X8045b48 bl_caster_r_wheel_link  -224.6 175.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X7f717d0 bl_caster_l_wheel_link  -224.6 273.6 79.2 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X70207f0 r_shoulder_pan_link  -50.0 -188.0 840.675 / 0.416 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6f0a1a8 r_upper_arm_roll_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6b86ab0 r_upper_arm_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X6feb760 r_elbow_flex_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.336>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X7ed6780 base_link  0.0 0.0 51.0 / 0.0 0.0 0.0> . #<bodyset-link #X712f218 r_forearm_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X7ee2a48 base_bellow_link  -290.0 0.0 851.0 / 0.0 0.0 0.0> . #<bodyset-link #X7cf3bd0 torso_lift_link  -50.0 0.0 840.675 / 0.0 0.0 0.0>) (#<bodyset-link #X6b94c60 head_pan_link  -67.07 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b95d10 head_plate_frame  24.13 0.0 1286.625 / 0.0 0.0 0.0>) (#<bodyset-link #X6b94c60 head_pan_link  -67.07 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b96430 sensor_mount_link  24.13 0.0 1286.625 / 0.0 0.0 0.0>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0> . #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0> . #<bodyset-link #X712f218 r_forearm_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0>. #<bodyset-link #X713fa30 r_wrist_flex_link  49.481 147.35 635.116 / 2.325 -1.158 -0.581>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.0470.0 0.0> . #<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675/ 1.047 0.0 0.0> . #<bodyset-link #X6fb1bf0 r_gripper_palm_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0> . #<bodyset-link #X6f66888 r_gripper_l_finger_link  37.876 171.904 707.763 / 2.325 -1.158 2.561>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0> . #<bodyset-link #X7e6eff0 r_gripper_l_finger_tip_link  17.481 199.646 792.541 / 2.325 -1.158 2.561>) (#<bodyset-link #X70846a0 l_shoulder_pan_link  -50.0 188.0 840.675 / 1.047 0.0 0.0> . #<bodyset-link #X6ef4f60 l_upper_arm_link  4.263e-14 274.603 840.675 / 1.047 1.292 1.222>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b87308 head_mount_link  -113.87 0.0 1377.625 / 0.0 0.0 0.0>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b97f30 double_stereo_link  24.13 0.0 1286.625 / 0.0 0.0 0.0>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b98398 wide_stereo_link  59.22 30.582 1330.716 / -0.008 -0.012 -0.007>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b9d690 narrow_stereo_link  57.821 60.632 1330.738 / -0.01 -0.011 -0.01>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b99a30 wide_stereo_l_stereo_camera_frame  59.22 30.582 1330.716 / -0.008 -0.012 -0.007>) (#<bodyset-link#X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b9ac18 wide_stereo_r_stereo_camera_frame  58.487 -59.413 1331.379 / -0.008 -0.012 -0.007>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b82b20 narrow_stereo_l_stereo_camera_frame  57.821 60.632 1330.738 /-0.01 -0.011 -0.01>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b82c58 narrow_stereo_r_stereo_camera_frame  56.883-29.358 1331.639 / -0.01 -0.011 -0.01>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b88cd0 head_mount_prosilica_link  -160.327 12.5 1466.546 / 0.0 0.0 0.0>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b89528 head_mount_kinect_ir_link  -146.137 12.5 1514.078 / 0.0 0.0 0.0>) (#<bodyset-link #X6b959e0 head_tilt_link  0.93 0.0 1222.125 / 0.0 0.0 0.0> . #<bodyset-link #X6b8c210 head_mount_kinect_rgb_link  -164.504 -8.722 1492.191 / -0.01 -0.049 -0.011>) (#<bodyset-link #X6f882e8 r_shoulder_lift_link  41.485 -147.62 840.675 / 0.416 1.285 0.0> . #<bodyset-link #X6b86ab0 r_upper_arm_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337>) (#<bodyset-link #X6f0a1a8 r_upper_arm_roll_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> . #<bodyset-link #X6feb760 r_elbow_flex_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.336>) (#<bodyset-link #X6b86ab0 r_upper_arm_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> . #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X6b86ab0 r_upper_arm_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> . #<bodyset-link #X712f218 r_forearm_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268>) (#<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> . #<bodyset-link #X6fb1bf0 r_gripper_palm_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X712f218 r_forearm_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> . #<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X712f218 r_forearm_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> . #<bodyset-link #X6fb1bf0 r_gripper_palm_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f8fc90 r_gripper_r_finger_link  18.808 167.779 703.357 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f92ea0 r_gripper_motor_slider_link  3.228 196.563 789.248 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f75200 r_gripper_motor_accelerometer_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f66888 r_gripper_l_finger_link  37.876 171.904 707.763 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116/ 2.325 -1.158 2.561> . #<bodyset-link #X6f8fdc8 r_gripper_r_finger_tip_link  -11.026 193.48 785.956 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X7e6eff0 r_gripper_l_finger_tip_link  17.481 199.646 792.541 / 2.325 -1.158 2.561>) (#<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16>) (#<bodyset-link #X6fb1bf0 r_gripper_palm_link  49.481 147.35 635.116 / 2.325 -1.158 2.561> . #<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16>) (#<bodyset-link #X6f8fc90 r_gripper_r_finger_link  18.808 167.779 703.357 / 2.325 -1.158 2.561> . #<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16>) (#<bodyset-link #X6f92ea0 r_gripper_motor_slider_link  3.228 196.563 789.248 / 2.325 -1.158 2.561> . #<bodyset-link #X6f8fdc8 r_gripper_r_finger_tip_link  -11.026 193.48 785.956 / 2.325 -1.158 2.561>) (#<bodyset-link #X6f92ea0 r_gripper_motor_slider_link  3.228 196.563 789.248 / 2.325 -1.158 2.561> . #<bodyset-link #X7e6eff0 r_gripper_l_finger_tip_link  17.481 199.646 792.541 / 2.325 -1.158 2.561>) (#<bodyset-link #X6f66888 r_gripper_l_finger_link  37.876 171.904 707.763 / 2.325 -1.158 2.561> . #<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16>) (#<bodyset-link #X6f8fdc8 r_gripper_r_finger_tip_link  -11.026 193.48 785.956 / 2.325 -1.158 2.561> . #<bodyset-link #X7e6eff0 r_gripper_l_finger_tip_link  17.481 199.646 792.541 / 2.325 -1.158 2.561>) (#<bodyset-link #X6f8fdc8 r_gripper_r_finger_tip_link  -11.026 193.48 785.956 / 2.325 -1.158 2.561> . #<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16>) (#<bodyset-link #X6f03ec0 l_shoulder_lift_link  4.263e-14 274.603 840.675 / 1.047 1.292 3.331e-16> . #<bodyset-link #X6ef4f60 l_upper_arm_link  4.263e-14 274.603 840.675 / 1.047 1.292 1.222>) (#<bodyset-link #X6efdce8 l_upper_arm_roll_link  4.263e-14 274.603 840.675 / 1.0471.292 1.222> . #<bodyset-link #X6eed5b8 l_elbow_flex_link  55.127 370.086 456.17 / -0.345 -0.597 0.319>) (#<bodyset-link #X6ef4f60 l_upper_arm_link  4.263e-14 274.603840.675 / 1.047 1.292 1.222> . #<bodyset-link #X6ed8a90 l_forearm_roll_link  55.127 370.086 456.17 / -0.345 -0.597 0.668>) (#<bodyset-link #X6ef4f60 l_upper_arm_link4.263e-14 274.603 840.675 / 1.047 1.292 1.222> . #<bodyset-link #X6ecdae8 l_forearm_link  55.127 370.086 456.17 / -0.345 -0.597 0.668>) (#<bodyset-link #X6ed8a90 l_forearm_roll_link  55.127 370.086 456.17 / -0.345 -0.597 0.668> . #<bodyset-link #X6ec92d8 l_gripper_palm_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ecdae8 l_forearm_link  55.127 370.086 456.17 / -0.345 -0.597 0.668> . #<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ecdae8 l_forearm_link  55.127 370.086 456.17 / -0.345 -0.597 0.668> . #<bodyset-link #X6ec92d8 l_gripper_palm_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e7b960 l_gripper_r_finger_link  332.12253.586 704.203 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e416b8 l_gripper_motor_slider_link  365.77 203.092 773.243 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e42c90 l_gripper_motor_accelerometer_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e21698 l_gripper_l_finger_link  333.36 236.451 693.963 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e7d460 l_gripper_r_finger_tip_link  364.843 215.9 780.897 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6ec43d0 l_wrist_roll_link  304.937 280.31 636.66 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e11f08 l_gripper_l_finger_tip_link  366.697 190.284 765.589 / -0.904-0.947 -2.073>) (#<bodyset-link #X6e416b8 l_gripper_motor_slider_link  365.77 203.092 773.243 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e7d460 l_gripper_r_finger_tip_link  364.843 215.9 780.897 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6e416b8 l_gripper_motor_slider_link  365.77 203.092 773.243 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e11f08 l_gripper_l_finger_tip_link  366.697 190.284 765.589 / -0.904 -0.947 -2.073>) (#<bodyset-link #X6e7d460 l_gripper_r_finger_tip_link  364.843 215.9 780.897 / -0.904 -0.947 -2.073> . #<bodyset-link #X6e11f08 l_gripper_l_finger_tip_link  366.697 190.284 765.589 / -0.904 -0.947 -2.073>))
;;    args : ((#<coordinates #X90a5e98  0.0 200.0 800.0 / 0.0 0.0 0.0>) :move-target #<cascaded-coords #X7e65b48 :rarm-end-coords  0.006 199.99 799.983 / 2.325 -1.1582.561> :link-list (#<bodyset-link #X70207f0 r_shoulder_pan_link  -50.0 -188.0 840.675 / 0.416 0.0 0.0> #<bodyset-link #X6f882e8 r_shoulder_lift_link  41.485 -147.62 840.675 / 0.416 1.285 0.0> #<bodyset-link #X6f0a1a8 r_upper_arm_roll_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> #<bodyset-link #X6feb760 r_elbow_flex_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.336> #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> #<bodyset-link #X713fa30 r_wrist_flex_link  49.481 147.35 635.116 / 2.325 -1.158 -0.581> #<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list ((#<bodyset-link #X7cf3bd0 torso_lift_link  -50.0 0.0 840.675 / 0.0 0.0 0.0> 0.005)) :link-list (#<bodyset-link #X70207f0 r_shoulder_pan_link  -50.0 -188.0 840.675 / 0.416 0.0 0.0> #<bodyset-link #X6f882e8 r_shoulder_lift_link  41.485 -147.62 840.675 / 0.416 1.285 0.0> #<bodyset-link #X6f0a1a8 r_upper_arm_roll_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> #<bodyset-link #X6feb760 r_elbow_flex_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.336> #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> #<bodyset-link #X713fa30 r_wrist_flex_link  49.481 147.35 635.116 / 2.325 -1.158 -0.581> #<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) :move-target #<cascaded-coords #X7e65b48 :rarm-end-coords  0.006 199.99 799.983 / 2.325 -1.158 2.561> :move-target #<cascaded-coords #X7e65b48 :rarm-end-coords  0.006 199.99 799.983 / 2.325 -1.158 2.561> :collision-avoidance-link-pairnil :link-list (#<bodyset-link #X70207f0 r_shoulder_pan_link  -50.0 -188.0 840.675 / 0.416 0.0 0.0> #<bodyset-link #X6f882e8 r_shoulder_lift_link  41.485 -147.62 840.675 / 0.416 1.285 0.0> #<bodyset-link #X6f0a1a8 r_upper_arm_roll_link  41.485 -147.62 840.675 / 0.416 1.285 -1.337> #<bodyset-link #X6feb760 r_elbow_flex_link  144.614-102.101 456.888 / 1.935 -0.589 -0.336> #<bodyset-link #X7185c90 r_forearm_roll_link  144.614 -102.101 456.888 / 1.935 -0.589 -0.268> #<bodyset-link #X713fa30 r_wrist_flex_link  49.481 147.35 635.116 / 2.325 -1.158 -0.581> #<bodyset-link #X713fd60 r_wrist_roll_link  49.481 147.35 635.116 / 2.325 -1.158 2.561>) :rotation-axis nil :check-collision t)
;; command : (let ((r (instance pr2-sensor-robot :init))) (progn (send r :newcoords (make-coords :4x4 #2f((1.0 0.0 0.0 0.0) (0.0 1.0 0.0 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0)))) (mapc #'(lambda (j a) (send* j :joint-angle a nil)) (list (send r :torso_lift_joint) (send r :l_shoulder_pan_joint) (send r :l_shoulder_lift_joint) (send r :l_upper_arm_roll_joint) (send r :l_elbow_flex_joint) (send r :l_forearm_roll_joint) (send r :l_wrist_flex_joint) (send r :l_wrist_roll_joint) (send r :r_shoulder_pan_joint) (send r :r_shoulder_lift_joint) (send r :r_upper_arm_roll_joint) (send r :r_elbow_flex_joint) (send r :r_forearm_roll_joint) (send r :r_wrist_flex_joint) (send r :r_wrist_roll_joint) (send r :head_pan_joint) (send r :head_tilt_joint)) '(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.9924 74.2283 -76.3779 -121.539 4.06685 -34.9326 180.0 0.0 0.0)) (objects (list r))) (send* r :inverse-kinematics (list (list (make-coords :pos #f(0.0 200.0 800.0) :rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) :dump-command nil :debug-view t :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 -5.551115e-17 0.0 180.0) (5.551115e-17 1.0 0.0 2.842171e-14) (0.0 0.0 1.0 2.842171e-14) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :link-list (list (send r :link "r_shoulder_pan_link") (send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link")(send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link")) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list(list (list (send r :link "torso_lift_link") 0.005)) :link-list (list (send r :link "r_shoulder_pan_link") (send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link") (send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link")) :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 -5.551115e-17 0.0 180.0) (5.551115e-17 1.0 0.0 2.842171e-14) (0.0 0.0 1.0 2.842171e-14) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 -5.551115e-17 0.0 180.0) (5.551115e-17 1.0 0.0 2.842171e-14) (0.0 0.0 1.0 2.842171e-14) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :collision-avoidance-link-pair nil :link-list (list (send r :link "r_shoulder_pan_link")(send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link") (send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link")) :rotation-axis nil :check-collision t)))
;; dump debug command to /tmp/irtmodel-ik-19747/pr2-sensor-robot-2017-05-10-00-05-05-56-47-56-failure.l
;; (progn (load "/tmp/irtmodel-ik-19747/pr2-sensor-robot-2017-05-10-00-05-05-56-47-56-failure.l")(ik-setup)(ik-check))
nil

cc. @k-okada

[k-okada]

euslisp level of collision check may slightly different between pr2 and fetch, see jsk-ros-pkg/jsk_pr2eus#232 for didscussion

[k-okada]

@Affonso-Gui please try to add head element in https://github.com/fetchrobotics/fetch_ros/blob/indigo-devel/fetch_moveit_config/config/fetch.srdf and see if it handles collision between arm and head

[Affonso-Gui]

Added head group in move-it configuration file, with no success. When launching fetch_moveit_config demo.launch head group appears correctly on the groups tab, but the collision happens exactly the same way.

Also, even moving only the head, planner seem to use arm_with_torso group only.

generated 10 points for 3 sec using [arm_with_torso] group

Also tried to add similar blocks to controllers.yaml and fetch-interface's moveit-environment (separately), but both resulted on planning failing for every movement -- with or without the head.

[knorth55]

Can you tell me which branch you are working on and also paste all the output of Euslisp and demo.launch?