Ask Your Question

Different starting positions in Rviz and Gazebo

asked 2016-06-07 03:56:36 -0600

ipa-hsd gravatar image

updated 2016-06-07 08:46:48 -0600

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)/ $(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

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 ...
edit retag flag offensive close merge delete


The weird behavior is probably due to the tf receiving joint_states from multiple conflicting sources.

Laurens Verhulst gravatar image Laurens Verhulst  ( 2016-06-08 02:30:42 -0600 )edit

@Laurens Verhulst seems to be the case. With rostopic echo /joint_states I get 2 different sequence id's in the header.

ipa-hsd gravatar image ipa-hsd  ( 2016-06-08 04:24:00 -0600 )edit

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.

Laurens Verhulst gravatar image Laurens Verhulst  ( 2016-06-08 04:47:25 -0600 )edit

@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.

ipa-hsd gravatar image ipa-hsd  ( 2016-06-08 05:00:24 -0600 )edit

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.

Laurens Verhulst gravatar image Laurens Verhulst  ( 2016-06-08 06:09:48 -0600 )edit

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?

ipa-hsd gravatar image ipa-hsd  ( 2016-06-09 04:02:19 -0600 )edit

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.

Laurens Verhulst gravatar image Laurens Verhulst  ( 2016-06-09 04:26:44 -0600 )edit

OK thanks! Then I'll close the issue.

ipa-hsd gravatar image ipa-hsd  ( 2016-06-09 04:33:41 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2016-06-07 04:19:23 -0600

Laurens Verhulst gravatar image

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.

edit flag offensive delete link more


How can I solve the gravity issue in that case?

ipa-hsd gravatar image ipa-hsd  ( 2016-06-07 07:56:17 -0600 )edit

you can start gazebo with paused physics by providing the argument <arg name="paused" default="false"/> 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.

Laurens Verhulst gravatar image Laurens Verhulst  ( 2016-06-08 02:28:18 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2016-06-07 03:56:36 -0600

Seen: 2,195 times

Last updated: Jun 07 '16