How to add moveit to the new URDF? Or revise URDF?

Ask
1

Sorry to trouble you again. Today, I try to use moveit to control the new robot. But I find that it seems that the link link_gripper_s3_body has collision with the link ‘link_gripper_finger_right’ at the start state and the moveit group can not initialize the planning problem. As shown below:

Found a contact between ‘link_gripper_s3_body’ (type ‘Robot link’) and ‘link_gripper_finger_right’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[ INFO] [1713143859.952075948, 420.239000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1713143859.952094551, 420.239000000]: Start state appears to be in collision with respect to group stretch_arm

Do you know how to revise “link_gripper_s3_body” to avoid the collision and keep the urdf following the real robot? I have upload my urdf here GitHub - cyb1212/urdf: The used robot. I am using “urdf/xacro/stretch_description_SE3_eoa_wrist_dw3_tool_sg3.xacro” and its genrated urdf “urdf/xacro/stretch.urdf”.

2

I have found the error in the moveit setting. I forget to revise the srdf file to disable some collisions. The revised one is shown in the attached file.
sg3_srdf.txt (38.6 KB)
Now the arm moves well. But the gripper still has some problems as shown:

[ WARN] [1713455386.102212327, 133.304000000]: Joint joint_gripper_finger_right is constrained to be above the maximum bounds. Assuming maximum bounds instead.
[ INFO] [1713455386.102430759, 133.304000000]: Planner configuration ‘stretch_gripper[RRTConnect]’ will use planner ‘geometric::RRTConnect’. Additional configuration parameters will be set when the planner is constructed.
[ERROR] [1713455386.102537626, 133.305000000]: OMPL encountered an error: The longest valid segment for state space stretch_gripper_JointModel must be positive.
Space settings:
ModelBasedStateSpace ‘stretch_gripper_JointModel’ at 0x7fe578015490

[ WARN] [1713455386.103354245, 133.305000000]: Fail: ABORTED: Catastrophic failure

The gripper does not move even using the robot urdf_spawner node (
“<node name=“urdf_spawner” pkg=“gazebo_ros” type=“spawn_model”
** args=” -urdf -x $(arg x) -y $(arg y) -z $(arg z) -Y $(arg yaw) -model robot -param robot_description -J joint_lift 0.4 -J joint_wrist_yaw 1.5 -J link_gripper_finger_left 0.3 -J link_gripper_finger_right 0.3" respawn=“false” output=“screen” />**").

3

The URDF of the gripper base is not correct and it causes this problem. Please update "link_SG3_gripper_boday.STL in the new URDF (GitHub - hello-robot/stretch_urdf: URDFs for the Stretch mobile manipulators from Hello Robot Inc.). No moving space for the gripper finger.

Screenshot from 2024-04-19 14-56-15
Screenshot from 2024-04-19 14-56-151724×1079 117 KB

4

I manually dig a hole for the URDF, which make the robot gripper collision-free. The controller and moveit planer still do not work. Shows the same error. Could you offer me a new moveit configuration node for the new robot model?

Screenshot from 2024-04-19 21-38-03
Screenshot from 2024-04-19 21-38-031025×581 68 KB

5

Hi @Yongbo, thanks for digging into this issue with self-collisions within the MoveIt2 SRDF. As you’ve mentioned, the solution to these issues is allowing self-collisions between neighboring links in the SRDF. For example, here are the allowed self-collisions we had with our ROS2 Galactic MoveIt2 support (now deprecated):

In your final image, it looks like you were able to use Blender to cut a hole in the gripper link of the URDF (nice!), but I’m wondering why this is necessary when you could allow self-collisions between the gripper link and the finger links.

When we switched to ROS 2 Humble, we deprecated our effort on supporting MoveIt2. If you’re interested in learning about the state of ROS 2 Galactic MoveIt2 codebase / ideas on how it might be revived, I’d be happy to meet with you and give you an overview of the codebase.