Rosanswers logo

I have made my own UR5 config pkg using Setup Assistant since I want to understand the whole process inside out. I am able to plan paths in Rviz. Initially I had problems loading the controllers (ERROR : Failed to load arm_controller) in Gazebo, but I solved the issue yesterday.

Now I am having issues with moving the arm. At init, the robot has different starting positions in Gazebo (starting position is random) and Rviz (all joints start at 0).

I can plan the path and visualize the plan in Rviz and see the robot moving in Gazebo as well (in Gazebo, the robot forces itself to come to 0 position before following the trajectory) and then I get the error :

Controller arm_controller failed with error code PATH_TOLERANCE_VIOLATED
Controller handle arm_controller reports status ABORTED

This is my launch file :

    <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>

  <!-- Convert xacro and put on parameter server -->
  <param name="robot_description" command="$(find xacro)/xacro.py $(find ur5_pnp)/urdf/ur5_pr2.urdf.xacro" />

  <!-- Spawn a robot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model ur5" />

  <!-- Load UR5 controllers -->
  <rosparam command="load" file="$(find ur5_pnp)/config/control_ur5.yaml" />
  <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller r_gripper_controller --shutdown-timeout 3"/>

  <!-- Fake Calibration -->
  <node pkg="rostopic" type="rostopic" name="fake_joint_calibration" args="pub calibrated std_msgs/Bool true" />

  <include file="$(find ur5_pr2_moveit_config)/launch/moveit_planning_execution.launch" />

  <!-- spawn a cube for pick and place -->
  <node name="spawn_cube" pkg="gazebo_ros" type="spawn_model" args="-file $(find ur5_pnp)/urdf/object.urdf -urdf -model cube" />

While the trajectory is being executed, rostopic echo /joint_states doesn't show updated joint values but always 0.

Edit 1:

As expected, I was not publishing the joint_states. I added joint_state_controller/JointStateController in my control_ur5.yaml and now the starting pose in Rviz is same as Gazebo. But now I see a weird behavior https://youtu.be/q9KEwDEx3-E

And if I try to plan a path in Rviz, I get the following list of errors ;

[ INFO] [1465305485.140122471, 25.980000000]: Planning attempt 1 of at most 1
[ INFO] [1465305485.140246091, 25.980000000]: Starting state is just outside bounds (joint 'ur5_arm_wrist_2_joint'). Assuming within bounds.
[ INFO] [1465305485.141022068, 25.980000000]: Found a contact between 'ur5_arm_forearm_link' (type 'Robot link') and 'r_gripper_r_finger_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1465305485.141057797, 25.980000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1465305485.141094384, 25.980000000]: Start state appears to be in collision with respect to group ur5_arm
[ WARN] [1465305485.245093353, 26.060000000]: 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.

[ WARN] [1465305485.248167133, 26.061000000]: ur5_arm[RRTConnectkConfigDefault]: Skipping invalid start state (invalid state)
[ERROR] [1465305485.248314594, 26.061000000]: ur5_arm[RRTConnectkConfigDefault]: Motion planning start tree could not be initialized!

[ INFO] [1465305485.257780607, 26.069000000]: Unable to solve the planning problem
[ INFO] [1465305485.286582422, 26.095000000]: ABORTED: No motion plan found. No execution attempted.
[ WARN] [1465305852.247270371, 368.000000000]: Unable to find a random collision free configuration after 100 attempts

Originally posted by ipa-hsd on ROS Answers with karma: 150 on 2016-06-07

Post score: 0

Original comments

Comment by Laurens Verhulst on 2016-06-08:
The weird behavior is probably due to the tf receiving joint_states from multiple conflicting sources.

Comment by ipa-hsd on 2016-06-08:
@Laurens Verhulst seems to be the case. With rostopic echo /joint_states I get 2 different sequence id's in the header.

Comment by Laurens Verhulst on 2016-06-08:
Ok, so the joint_states is not an issue anymore. As you said you constructed the moveit config yourself. I think the collision occurs because the sellf-collision matrix is not properly configured. Try modify it again with the setup assistent.

Comment by ipa-hsd on 2016-06-08:
@Laurens Verhulst but isn't having 2 different sequences a problem? 1 set of sequences shows the correct joint positions while the other set (which is published only intermittently) shows 0 joint positions. I will check with the self-collision matrix again.

Comment by Laurens Verhulst on 2016-06-08:
It's easy to find out where the second set is coming from via the rqt node graph. Then just eliminate this false publishing of joint positions.

Comment by ipa-hsd on 2016-06-09:
Yes that solved the problem. Now the only warning I'm getting is PATH_TOLERANCE_VIOLATED and Dropping first 1 trajectory point(s) out of 27, as they occur before the current time. I know this is not a critical issue, but is there a way to solve this?

Comment by Laurens Verhulst on 2016-06-09:
I am having the same issue. I think it's caused by a delay in communication causing the time stamps in the headers not to match. You can fix this by making a new timestamp for the message. I didn't do this yet either cause it's not a priority to me.

Comment by ipa-hsd on 2016-06-09:
OK thanks! Then I'll close the issue.


1 Answer 1


Rosanswers logo

The default position of the joints are defined in the URDF model. Using the same model for Rviz and Gazebo should result in the same joint positions when spawned. What might happen in Gazebo is that the arm undergoes gravity before the controllers are properly initiated causing the joints to move.

For the joint_states: Check whether the joints are properly published to the joint_states. This can also be an issue with the controller.

Originally posted by Laurens Verhulst with karma: 167 on 2016-06-07

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by ipa-hsd on 2016-06-07:
How can I solve the gravity issue in that case?

Comment by Laurens Verhulst on 2016-06-08:
you can start gazebo with paused physics by providing the argument then wait for the controllers to initialize and unpause the simulation manually. The controller should then keep the joints in the spawned state. Provided that the controller is capable.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.