UR5 Robot does not move as it planned in Rviz

Hi all,

I tried to write some c++ code to control the UR5 robot simulated in Rviz by using universal_robot package. However, It seems that robot stops at the position where is not as it planned, please look into the gif at the end of the issue. How would I solve the problem? Thanks

I recorded all my steps for building this package and provided them here for someone could look into the details.

New a workspace

$ cd ~
$ mkdir -p ur5_moveit_ws/src
$ cd ur5_moveit_ws
$ catkin_make
$ source devel/setup.bash

Clone universal robot package

$ cd src
$ git clone -b kinetic-devel https://github.com/ros-industrial/universal_robot.git

Create our package

# under ur5_moveit_ws/src
$ catkin_create_pkg ur5_move moveit_ros_move_group moveit_planners_ompl moveit_ros_visualization joint_state_publisher robot_state_publisher xacro ur_description moveit_msgs moveit_core moveit_ros_planning_interface
$ mkdir src
$ mkdir launch

create ur5_move.launch file under ur5_moveit_ws/src/ur5_move/

<launch>
    <!-- If sim=false, then robot_ip is required -->
    <arg name="sim" default="true" />
    <arg name="robot_ip" unless="$(arg sim)" />
    <!-- By default, we are not in debug mode -->
    <arg name="debug" default="false" />

    <!-- Limited joint angles are used. Prevents complex robot configurations and makes the planner more efficient -->
    <arg name="limited" default="true" />

    <!-- Load UR5 URDF file - robot description file -->
    <include file="$(find ur5_moveit_config)/launch/planning_context.launch">
      <arg name="load_robot_description" value="true" />
      <arg name="limited" value="$(arg limited)" />
    </include>

    <!-- If sim mode, run the simulator -->
    <group if="$(arg sim)">
      <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
        <param name="/use_gui" value="true"/>
        <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
      </node>
    </group>

    <!-- If using real robot, initialise connection to the real robot -->
    <group unless="$(arg sim)">
      <include file="$(find ur5_moveit_config)/launch/ur5_bringup.launch">
        <arg name="robot_ip" value="$(arg robot_ip)" />
      </include>
    </group>

    <!-- Given the published joint states, publish tf for the robot links -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

    <!-- Launch the move group for motion planning -->
    <group if="$(arg sim)">
        <include file="$(find ur5_moveit_config)/launch/move_group.launch">
            <arg name="limited" value="$(arg limited)" />
            <arg name="allow_trajectory_execution" value="true"/>  
            <arg name="fake_execution" value="true"/>
            <arg name="info" value="true"/>
            <arg name="debug" value="$(arg debug)"/>
        </include>
    </group>

    <group unless="$(arg sim)">
        <include file="$(find ur5_moveit_config)/launch/move_group.launch">
            <arg name="limited" default="true" />
            <arg name="publish_monitored_planning_scene" value="true" />
        </include>
    </group>

    <!-- Launch the RViz visualizer -->
    <include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch">
        <arg name="config" value="true" />
    </include>

    <!-- Optionally, you can launch a database to record all the activities -->
    <!-- <include file="$(find ur5_moveit_config)/launch/default_warehouse_db.launch" /> -->

    <!-- Launch our own script -->
    <!-- <node name="move_group_interface_tutorial" pkg="ur5_move" type="ur5_move" respawn="false" output="screen"></node> -->
</launch>

make the package

$ cd ~/ur5_moveit_ws
$ catkin_make
$ source devel/setup.bash

try to launch
```
$ roslaunch ur5_move ur5_move.launch
```

I can see a UR5 Robot is put in Rviz, so far so good. untitled

add move_group_interface_tutorial.cpp under ur5_moveit_ws/src/ur5_move/

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;  
  ros::AsyncSpinner spinner(1);
  spinner.start();

  /* This sleep is ONLY to allow Rviz to come up */
  sleep(20.0);

  // BEGIN_TUTORIAL
  // 
  // Setup
  // ^^^^^
  // 
  // The :move_group_interface:`MoveGroup` class can be easily 
  // setup using just the name
  // of the group you would like to control and plan for.
  moveit::planning_interface::MoveGroup group("manipulator");

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to deal directly with the world.
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

  // (Optional) Create a publisher for visualizing plans in Rviz.
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the 
  // end-effector.
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, 0.0);
  target_pose1.position.x = 0.10;
  target_pose1.position.y = 0.25;
  target_pose1.position.z = 0.50;
  group.setPoseTarget(target_pose1);

  // Now, we call the planner to compute the plan
  // and visualize it.
  // Note that we are just planning, not asking move_group 
  // to actually move the robot.
  moveit::planning_interface::MoveGroup::Plan my_plan;
  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);

  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");    
  /* Sleep to give Rviz time to visualize the plan. */
  sleep(5.0);

  // Visualizing plans
  // ^^^^^^^^^^^^^^^^^
  // Now that we have a plan we can visualize it in Rviz.  This is not
  // necessary because the group.plan() call we made above did this
  // automatically.  But explicitly publishing plans is useful in cases that we
  // want to visualize a previously created plan.
  if (1)
  {
    ROS_INFO("Visualizing plan 1 (again)");    
    display_trajectory.trajectory_start = my_plan.start_state_;
    display_trajectory.trajectory.push_back(my_plan.trajectory_);
    display_publisher.publish(display_trajectory);
    /* Sleep to give Rviz time to visualize the plan. */
    sleep(5.0);
  }

  // Moving to a pose goal
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // Moving to a pose goal is similar to the step above
  // except we now use the move() function. Note that
  // the pose goal we had set earlier is still active 
  // and so the robot will try to move to that goal. We will
  // not use that function in this tutorial since it is 
  // a blocking function and requires a controller to be active
  // and report success on execution of a trajectory.

  /* Uncomment below line when working with a real robot*/
  //group.move();
  group.execute(my_plan);

// END_TUTORIAL

  ros::shutdown();  
  return 0;
}

modify CMakeLists.txt under ur5_moveit_ws/src/ur5_move/

# uncomment this line
add_compile_options(-std=c++11)

# add this line
add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)

# add this line
target_link_libraries(move_group_interface_tutorial                                                  
    ${catkin_LIBRARIES}                                                                                
)

make the package

$ cd ~/ur5_moveit_ws
$ catkin_make
$ source devel/setup.bash

add the node to ur5_move.launch launch file

<!-- Launch our own script -->
<node name="move_group_interface_tutorial" pkg="ur5_move" type="move_group_interface_tutorial" respawn="false" output="screen"></node>

launch and test move
```
$ roslaunch ur5_move ur5_move.launch
```

So here comes the problem: the robot stops at the position where is not as it planned. ur5_stuck

Is there something wrong during building the package? or would someone point out how I could fix this problem?

Thanks again!

ros-industrial / universal_robot

UR5 Robot does not move as it planned in Rviz #405