search

jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

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

Open Affonso-Gui opened 7 years ago

Affonso-Gui commented 7 years ago

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 commented 7 years ago

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 https://github.com/jsk-ros-pkg/jsk_robot/issues/722

Affonso-Gui commented 7 years ago

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 commented 7 years ago

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 (https://github.com/euslisp/jskeus/pull/232)

k-okada commented 7 years ago

euslisp level of collision check may slightly different between pr2 and fetch, see https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/232 for didscussion

k-okada commented 7 years ago

@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 commented 7 years ago

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 commented 7 years ago

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