Cannot replicate this moveit_config after following the moveit tutorial

[BolunDai0216]

Hi,

I was trying to replicate this repo by following the moveit setup assistance tutorial. However, after launching demo.launch, when I run

rosrun moveit_tutorials move_group_python_interface_tutorial.py

I get the following error:

[ INFO] [1665712527.059983625]: rviz version 1.14.19
[ INFO] [1665712527.060113875]: compiled against Qt version 5.12.8
[ INFO] [1665712527.060137084]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1665712527.066628243]: Forcing OpenGl version 0.
[ INFO] [1665712527.217702737]: Stereo is NOT SUPPORTED
[ INFO] [1665712527.217748528]: OpenGL device: virgl (Apple M1 Max (Compat))
[ INFO] [1665712527.217761904]: OpenGl version: 3.1 (GLSL 1.4).
[ INFO] [1665712530.419976200]: Loading robot model 'panda'...
[ INFO] [1665712530.486287127]: Starting planning scene monitor
[ INFO] [1665712530.487593842]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1665712530.712158048]: Constructing new MoveGroup connection for group 'panda_arm' in namespace ''
[ INFO] [1665712531.770384153]: Ready to take commands for planning group panda_arm.
[ INFO] [1665712554.401739469]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1665712554.402760516]: Planning attempt 1 of at most 1
[ INFO] [1665712554.404013647]: Found a contact between 'panda_rightfinger' (type 'Robot link') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1665712554.404038314]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1665712554.404058731]: Start state appears to be in collision with respect to group panda_arm
[ WARN] [1665712554.429660027]: Unable to find a valid state nearby the start state (using jiggle fraction of 0.050000 and 100 sampling attempts). Passing the original planning request to the planner.
[ INFO] [1665712554.431216826]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ WARN] [1665712554.431613036]: panda_arm/panda_arm: Skipping invalid start state (invalid state)
[ERROR] [1665712554.431685495]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[ INFO] [1665712554.431729037]: No solution found after 0.000186 seconds
[ WARN] [1665712554.441238127]: Goal sampling thread never did any work.
[ INFO] [1665712554.441400294]: Unable to solve the planning problem
[ INFO] [1665712554.442807718]: Received event 'stop'

Would it be possible to help me understand the issue?

Thanks in advance!

[rhaschke]

If I understand correctly, you re-created panda_moveit_config from scratch following the MSA tutorial and only using this newly created config, you observe the failure? (I just tested the released panda_moveit_config, version 0.8.1, w/o any issues). Could you please provide the generated config/*.srdf file? Probably the pre-defined robot poses are wrong, leading to a collision in the initial pose. Note that the MSA tutorial is exemplary here only and does not define all robot poses required for the remaining tutorials.

[BolunDai0216]

If I understand correctly, you re-created panda_moveit_config from scratch following the MSA tutorial and only using this newly created config, you observe the failure? (I just tested the released panda_moveit_config, version 0.8.1, w/o any issues). Could you please provide the generated config/*.srdf file? Probably the pre-defined robot poses are wrong, leading to a collision in the initial pose. Note that the MSA tutorial is exemplary here only and does not define all robot poses required for the remaining tutorials.

Yes, I recreated it from scratch following the MSA tutorial. The generatedpanda.srdf file is:

<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="panda">
    <!--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="panda_hand">
        <link name="panda_hand"/>
        <link name="panda_leftfinger"/>
        <link name="panda_rightfinger"/>
    </group>
    <group name="panda_arm">
        <chain base_link="panda_link0" tip_link="panda_link8"/>
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="ready" group="panda_arm">
        <joint name="panda_joint1" value="0"/>
        <joint name="panda_joint2" value="-0.7853"/>
        <joint name="panda_joint3" value="0"/>
        <joint name="panda_joint4" value="-2.3561"/>
        <joint name="panda_joint5" value="0"/>
        <joint name="panda_joint6" value="1.5707"/>
        <joint name="panda_joint7" value="0.7853"/>
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="panda_hand" parent_link="panda_link8" group="panda_hand"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0"/>
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="panda_hand" link2="panda_hand_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent"/>
    <disable_collisions link1="panda_hand" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_hand" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_hand" link2="panda_link5_sc" reason="Default"/>
    <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_hand" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_hand" link2="panda_link7" reason="Adjacent"/>
    <disable_collisions link1="panda_hand" link2="panda_link7_sc" reason="Default"/>
    <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_leftfinger" reason="Default"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link5" reason="Default"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link5_sc" reason="Default"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link7" reason="Default"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_link7_sc" reason="Default"/>
    <disable_collisions link1="panda_hand_sc" link2="panda_rightfinger" reason="Default"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never"/>
    <disable_collisions link1="panda_leftfinger" link2="panda_link7_sc" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link0_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
    <disable_collisions link1="panda_link0" link2="panda_link1_sc" reason="Default"/>
    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link2_sc" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link3_sc" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link0" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link1" reason="Default"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link1_sc" reason="Default"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link2" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link2_sc" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link3_sc" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link0_sc" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link1" link2="panda_link1_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
    <disable_collisions link1="panda_link1" link2="panda_link2_sc" reason="Default"/>
    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_link1" link2="panda_link3_sc" reason="Default"/>
    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link1" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link2" reason="Default"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link2_sc" reason="Default"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link3_sc" reason="Default"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link1_sc" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link2" link2="panda_link2_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
    <disable_collisions link1="panda_link2" link2="panda_link3_sc" reason="Default"/>
    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link2" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link2_sc" link2="panda_link3" reason="Never"/>
    <disable_collisions link1="panda_link2_sc" link2="panda_link3_sc" reason="Default"/>
    <disable_collisions link1="panda_link2_sc" link2="panda_link4" reason="Never"/>
    <disable_collisions link1="panda_link2_sc" link2="panda_link4_sc" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_link3_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
    <disable_collisions link1="panda_link3" link2="panda_link4_sc" reason="Always"/>
    <disable_collisions link1="panda_link3" link2="panda_link5" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_link5_sc" reason="Always"/>
    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_link7" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_link7_sc" reason="Never"/>
    <disable_collisions link1="panda_link3" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link3_sc" link2="panda_link4" reason="Always"/>
    <disable_collisions link1="panda_link3_sc" link2="panda_link4_sc" reason="Always"/>
    <disable_collisions link1="panda_link3_sc" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_link3_sc" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_link3_sc" link2="panda_link7" reason="Never"/>
    <disable_collisions link1="panda_link4" link2="panda_link4_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
    <disable_collisions link1="panda_link4" link2="panda_link5_sc" reason="Default"/>
    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_link4" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
    <disable_collisions link1="panda_link4" link2="panda_link7_sc" reason="Never"/>
    <disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link5" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link5_sc" reason="Default"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link6" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link6_sc" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link7" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_link7_sc" reason="Never"/>
    <disable_collisions link1="panda_link4_sc" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link5" link2="panda_link5_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
    <disable_collisions link1="panda_link5" link2="panda_link6_sc" reason="Always"/>
    <disable_collisions link1="panda_link5" link2="panda_link7_sc" reason="Always"/>
    <disable_collisions link1="panda_link5_sc" link2="panda_link6_sc" reason="Always"/>
    <disable_collisions link1="panda_link5_sc" link2="panda_link7_sc" reason="Default"/>
    <disable_collisions link1="panda_link5_sc" link2="panda_rightfinger" reason="Default"/>
    <disable_collisions link1="panda_link6" link2="panda_link6_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
    <disable_collisions link1="panda_link6" link2="panda_link7_sc" reason="Always"/>
    <disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link6_sc" link2="panda_link7" reason="Always"/>
    <disable_collisions link1="panda_link6_sc" link2="panda_link7_sc" reason="Always"/>
    <disable_collisions link1="panda_link6_sc" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link7" link2="panda_link7_sc" reason="Adjacent"/>
    <disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never"/>
    <disable_collisions link1="panda_link7_sc" link2="panda_rightfinger" reason="Never"/>
</robot>

Another issue that I found is the automatically generated Self-Collision Matrix is different from the Self-Collision Matrix of the released version of panda_moveit_config. Is this because the released version is using an older version of the URDF file? I noticed the URDF relative path in .setup_assistant is different from the newest version of franka_description.

[BolunDai0216]

Did you also do some post-processing to the config files after they are generated by MSA? If that is the case, would it be possible to also point out some of the necessary changes that need to be made?

[rhaschke]

The self-collision matrix looks perfectly fine. Sure, the definitions in the .srdf are different, but the resulting matrices are identical:	panda_moveit_config release 0.8.1	your panda_moveit_config generated from scratch

Yes, the released .srdf config was manually tweaked to handle the very special .urdf description of the Panda robot (defining both fine and coarse collision models). This is nothing to be handled in the basic MSA tutorial.

The actual culprit for your failure was due to a missing initial configuration for the finger joints: Looks like the zero configuration creates a collision between both fingers as indicated in the Status tab of the MotionPlanning plugin in rviz. Opening the fingers beforehand, resolved that problem.

I noticed the URDF relative path in .setup_assistant is different from the newest version of franka_description.

That was already fixed in 584e88375500e215f1881513e69f5366f0291fd4, but not yet released.

[BolunDai0216]

The self-collision matrix looks perfectly fine. Sure, the definitions in the .srdf are different, but the resulting matrices are identical:

panda_moveit_config release 0.8.1 your panda_moveit_config generated from scratch Yes, the released .srdf config was manually tweaked to handle the very special .urdf description of the Panda robot (defining both fine and coarse collision models). This is nothing to be handled in the basic MSA tutorial.

The actual culprit for your failure was due to a missing initial configuration for the finger joints: Looks like the zero configuration creates a collision between both fingers as indicated in the Status tab of the MotionPlanning plugin in rviz. Opening the fingers beforehand, resolved that problem.

I noticed the URDF relative path in .setup_assistant is different from the newest version of franka_description.

That was already fixed in 584e883, but not yet released.

The collision matrix I am getting looks like this:

I am using the URDF file from franka_description: https://github.com/frankaemika/franka_ros/blob/develop/franka_description/robots/panda/panda.urdf.xacro, and giving the additional command: hand:=true

[rhaschke]

Hm, looks like I didn't load your panda.srdf. In any case, the Panda robot requires a very special AllowedCollisionMatrix, which cannot be found by the algorithm used by MSA. So, please continue the tutorials with the released panda_moveit_config.

[BolunDai0216]

Hm, looks like I didn't load your panda.srdf. In any case, the Panda robot requires a very special AllowedCollisionMatrix, which cannot be found by the algorithm used by MSA. So, please continue the tutorials with the released panda_moveit_config.

I see. Actually, I am trying to create a moveit_config package for Franka Research 3 (FR3).

Do you know if one exists on GitHub?

I see the only difference in the URDF file between panda and FR3 is the joint limits. Do you think I can simply change the joint_limits.yamlfile and make it work for FR3?

[rhaschke]

I am trying to create a moveit_config package for Franka Research 3 (FR3). Do you know if one exists on GitHub?

I don't think so.

I see the only difference in the URDF file between panda and FR3 is the joint limits. Do you think I can simply change the joint_limits.yaml file and make it work for FR3?

Yes, to switch panda_moveit_config to FR3, you probably just need to load the correct .urdf and adapt joint limits.

[CamiloMGS]

The same thing is happening to me, I am not able to replicate any of the tutorials on the site, I am using ROS Melodic. Before, everything was working perfectly fine, but the upgrade broke all my work.

[rhaschke]

@CamiloMGS, please describe your issue in more detail. What did you upgraded? What does not work anymore? Please provide error logs.

[CamiloMGS]

Thanks for your quick response, Starting with the "roslaunch panda_moveit_config demo.launch" demo I am getting these errors.

[rhaschke]

Running roslaunch panda_moveit_config demo.launch with the system-installed packages works just fine for me. If you adapted the panda_moveit_config following the tutorials, I refer to: https://github.com/ros-planning/panda_moveit_config/issues/125#issuecomment-1278551429 and https://github.com/ros-planning/panda_moveit_config/issues/125#issuecomment-1279345087 (both in this thread!) Please read the comments in a thread before you add more questions to it.

[CamiloMGS]

@rhaschke Can you tell me your current setup, please?

[rhaschke]

I tested with MoveIt 1.0.11 and panda_moveit_config 0.7.8 installed from Bionic/Melodic .deb packages.

[CamiloMGS]

Now when I launch the command for the Pick and Place toturial I get these errors.

[rhaschke]

There is a collision between the to-be-grasped object and panda_hand_sc, a coarse collision model around the hand. This coarse collision model was only recently introduced by Franka and unfortunately it doesn't play with the old pick+place code. One needs to explicitly disable collisions between hand and object. Please try https://github.com/ros-planning/moveit_tutorials/pull/741 and report back.

By the way, I suggest to update to ROS Noetic. MoveIt Melodic doesn't receive much of maintenance these days anymore.