ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

[Gazebo] child link "base_link" not found (World -> UR5) [closed]

asked 2021-11-16 09:47:45 -0500

3473f gravatar image

I am working on a Gazebo simulation of a UR5 with a Robotiq 2f gripper (ROS Melodic + Ubuntu 18.04) and created my xacro file by loading the ur5 description, creating a world link and attaching the world link to the base_link of the robot as follows:

<?xml version="1.0" ?>
<robot name="ur5_2f85" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur5/default_kinematics.yaml"/>

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

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

<joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!-- GRIPPER COUPLING -->
<xacro:include filename="$(find ur5_description)/urdf/common/2f_coupling.urdf.xacro"/>
<xacro:coupling_robotiq_2f prefix="" parent="tool0">
    <origin xyz="0 0 -0.0045" rpy="0 0 -1.5708"/>
</xacro:coupling_robotiq_2f>

<!-- GRIPPER -->
<xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro"/>
<xacro:robotiq_85_gripper prefix="" parent="coupling_gripper_fix">
    <origin xyz="0 0 0" rpy="0 -1.5708 0"/>
</xacro:robotiq_85_gripper>

<link name="tcp"/>

<joint name="tcp_to_gripper" type="fixed">
    <parent link="tool0"/>
    <child link="tcp"/>
    <origin xyz="0 0 0.19" rpy="0 0 0"/>
</joint>

<!-- GAZEBO -->
<gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/</robotNamespace>
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
        </plugin>
</gazebo>

</robot>

I then used moveit to create custom configuration and whenever I try to launch the gazebo simulation (gazebo.launch) I get the following error:

Failed to build tree: parent link [tool0] of joint [tcp_to_gripper] not found.  This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [tcp_to_gripper] from your urdf file, or add "<link name="tool0" />" to your urdf file.

I tried removing the tcp link and rerunning the configuration, but again I end up with another error connecting to the robot itself:

Failed to build tree: child link [base_link] of joint [world_to_robot] not found.

In order to debug this problem, I checked my xacro file with the check_urdf command and the result looks fine

 name is: ur5_2f85
---------- Successfully Parsed XML ---------------
root Link: world has 1 child(ren)
    child(1):  base_link
        child(1):  base
        child(2):  shoulder_link
            child(1):  upper_arm_link
                child(1):  forearm_link
                    child(1):  wrist_1_link
                        child(1):  wrist_2_link
                            child(1):  wrist_3_link
                                child(1):  ee_link
                                child(2):  tool0
                                    child(1):  2f_coupling
                                        child(1):  coupling_gripper_fix
                                            child(1):  gripper_base_link
                                                child(1):  gripper_finger1_inner_knuckle_link
                                                    child(1):  gripper_finger1_finger_tip_link
                                                child(2):  gripper_finger1_knuckle_link
                                                    child(1):  gripper_finger1_finger_link
                                                child(3):  gripper_finger2_inner_knuckle_link
                                                    child(1):  gripper_finger2_finger_tip_link
                                                child(4):  gripper_finger2_knuckle_link
                                                    child(1):  gripper_finger2_finger_link
                                    child(2):  tcp

Finally, I ran rviz and the robot is correctly visualized and fully controllable

image description

Any leads on how to ... (more)

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by 3473f
close date 2021-11-17 08:34:00.202293

Comments

From what you shared one observation your links are missing physical properties so Gazebo as physical simulator will err.

Take a look at the following example that integrated Gazebo: https://github.com/lihuang3/ur5_ROS-G...

osilva gravatar image osilva  ( 2021-11-16 19:41:56 -0500 )edit

I had a look through the source code and it seems he is calling ur_upload.launch to spawn the robot which in turn is calling ur5_joint_limited_robot.urdf.xacro where the world joint is exactly attached like mine.

3473f gravatar image 3473f  ( 2021-11-17 02:23:25 -0500 )edit

What about the gripper?

osilva gravatar image osilva  ( 2021-11-17 04:26:47 -0500 )edit

Doesn't seem to be the problem. I stripped down my model and only kept this part

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

which ended up with the following error

Error: No link elements found in urdf file

so it seems that (for whatever reason) the macro is not being instantiated.

3473f gravatar image 3473f  ( 2021-11-17 05:20:42 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-11-17 05:30:46 -0500

3473f gravatar image

So I figured out the problem in case anyone is facing the same issue. Inside the launch file genereated by moveit I had to change the following line

<param name="robot_description" textfile="$(arg urdf_path)" />

into

<param name="robot_description" command="$(find xacro)/xacro --inorder $(find ur5_description)/urdf/workstation/ur5_workstation.urdf.xacro" />
edit flag offensive delete link more

Comments

Glad you found the answer. You can accept your answer by clicking on check mark

osilva gravatar image osilva  ( 2021-11-17 06:00:34 -0500 )edit

Thank you for your help! you definitely pushed me to disect my xacro file a bit more. :) I cannot accept my own answer at the moment since I have less than 10 points.

3473f gravatar image 3473f  ( 2021-11-17 08:33:19 -0500 )edit
1

I added points. Thank you again for documenting the answer.

osilva gravatar image osilva  ( 2021-11-17 09:00:39 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-11-16 09:46:27 -0500

Seen: 573 times

Last updated: Nov 17 '21