moveit Setup assistant finds duplicated links

asked 2021-03-08 12:53:49 -0500

Kappa95 gravatar image

updated 2021-03-19 06:41:03 -0500

Ranjit Kathiriya gravatar image

Hi Community, I'm practicing with xacro files and moveit package. I had created a new Xacro files which contains an UR5 with limited joints and a vacuum gripper attached to the tool0 link. When I'm creating the moveit package I'm finding this collision matrix:

collision-matrix

The elements are doubled both on the column and row sides. I think that this should not be ok, also because after the package is generated if i'm launching the moveit launcher for planning, every plan fails and in RViz each link is red coloured as it is in collision with itself. I'm following this guide for doing practice: ros-industrial

The Xacro file of the robot with everything attached:

<?xml version="1.0" ?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- world-->
  <link name="world"/>

  <!-- Robot pedestal-->
  <xacro:include filename="$(find main_support)/urdf/pedestal/pedestal.urdf.xacro"/>
  <xacro:pedestal_urdf pedestal_parent="world" pedestal_height="0.95">
    <origin xyz="0 0 0"/>
  </xacro:pedestal_urdf>

  <!--UR5-->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro"/>
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!--Import the vacuum gripper-->
  <xacro:include filename="$(find main_support)/urdf/vacuum_gripper/vacuum_gripper.urdf.xacro"/>
  <xacro:vacuum_gripper_urdf prefix="vacuum_gripper1_" joint_prefix="vacuum_gripper1_joint"/>


  <!--Joints-->
  <!--World to pedestal-->
  <joint name="robot_pedestal_to_world_interface" type="fixed">
    <parent link="world"/>
    <child link="pedestal_link"/>
    <origin xyz="0 0 0"/>
  </joint>

  <!--Robot to pedestal-->
  <joint name="robot_to_pedestal" type="fixed">
    <parent link="pedestal_link"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.95"/>
  </joint>

  <!--Gripper to robot-->
  <joint name="gripper1_to_robot" type="fixed">
    <parent link="tool0"/>
    <child link="vacuum_gripper1_base_link"/>
  </joint>

</robot>

When I'm launching the demo.launch I achieve this:

demo.launch

Note: I am using ROS Kinetic with Ubuntu 16.04

Thank you so much in advance for the help

edit retag flag offensive close merge delete