Different starting positions in Rviz and Gazebo

edit

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 :

<launch>
    <!-- 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)"/>
  </include>

  <!-- 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" />
</launch>

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