Robotics StackExchange | Archived questions

Simple RRTConnect planning fails

Hello, I am trying to use the MoveIt package to generate the joint values for a pose/position target. Everyone time I try to plan a goal state the OMPL RRTConnect fails stating its unable to sample any valid states for the goal tree.

I know this is a common issue and the first question is always: " is the goal state truly reachable?" And I believe mine is.. the goal state I specify is only sliding forward with one joint. (Hopefully the code and picture will show this).

My process is in one terminal I run demo.launch (from a package I generated in setup_assistant) and in a second terminal I run my node.

Can anyone provide insight into what I may be doing wrong? Any advice would be greatly appreciated!!

Code:

#include <pluginlib/class_loader.h>
#include <ros/ros.h>

// MoveIt!
#include <moveit/move_group_interface/move_group_interface.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>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/String.h"
ros::Publisher paramPub;
ros::Publisher display_publisher;
ros::Publisher marker_pub;

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


  static const std::string PLANNING_GROUP = "group1";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("end");
  visual_tools.deleteAllMarkers();

  // Remote control is an introspection tool that allows users to step through a high level script
  // via buttons and keyboard shortcuts in RViz
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  visual_tools.trigger();

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

  move_group.setPoseReferenceFrame("end");

  // We can also print the name of the end-effector link for this group.
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:
  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  //std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
    //        std::ostream_iterator<std::string>(std::cout, ", "));


    std::cout << move_group.getEndEffectorLink() << std::endl;


  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.35;
  target_pose1.position.y = 0.0;
  target_pose1.position.z = 0.0;
  move_group.setPoseTarget(target_pose1, "end");


    move_group.move();

  //move_group.setPositionTarget(0.3, 0.0, 0.0, "end");  

  // 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::MoveGroupInterface::Plan my_plan;


  //bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);


  // Visualizing plans
  // ^^^^^^^^^^^^^^^^^
  // We can also visualize the plan as a line with markers in RViz.
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "target_pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  //visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  //visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // Moving to a pose goal
  // ^^^^^^^^^^^^^^^^^^^^^

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


  ros::shutdown();
  return 0;
}

And here is an image of the pose_goal in RViz:

image description

And here is an image showing that if I use the jointstatepublisher slider gui that I can reach the pose_goal:

image description

And here is my URDF:

<?xml version="1.0" encoding="UTF-8"?>
<robot name="test_robot">

   <link name="D" />
   <link name="tilt" />
   <link name="pan" />
   <link name="end" />
   <link name="baseLink">
      <!--<inertial>
     <origin xyz="0 0 0" rpy="0 0 0"/>
     <mass value="1"/>
     <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
   </inertial>-->
   </link>

   <joint name="D" type="prismatic">
      <parent link="baseLink" />
      <child link="D" />
      <limit effort="30" velocity="1.0" lower="0" upper="0.4500" />
      <axis xyz="1 0 0" />
      <origin xyz="0 0 0" rpy="0 0 0" />
   </joint>

   <joint name="thetaTilt" type="revolute">
      <parent link="D" />
      <child link="tilt" />
      <limit effort="30" velocity="1.0" lower="-.5" upper=".5" />
      <axis xyz="0 1 0" />
      <origin xyz="0 0 0.0635" rpy="0 0 0" />
   </joint>

    <joint name="phiPan" type="revolute">
      <parent link="tilt" />
      <child link="pan" />
      <limit effort="30" velocity="1.0" lower="-.9" upper=".9" />
      <axis xyz="0 0 1" />
      <origin xyz="0 0.0889 0" rpy="0 0 0" />
   </joint>

   <joint name="end" type="fixed">
      <parent link="pan" />
      <child link="end" />
      <origin xyz="0.5588 0 0" rpy="0 0 0" />
   </joint>

</robot>

And Here is the output from the terminal running demo.launch

[ INFO] [1568048392.751663999]: Loading robot model 'test_robot'...
[ WARN] [1568048392.751738764]: No geometry is associated to any robot links
[ WARN] [1568048392.751780066]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048392.785106805]: Starting scene monitor
[ INFO] [1568048392.787732976]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1568048392.811340857]: Constructing new MoveGroup connection for group 'group1' in namespace ''
[ INFO] [1568048393.750722561]: Ready to take commands for planning group group1.
[ INFO] [1568048393.750953306]: Looking around: no
[ INFO] [1568048393.751075878]: Replanning: no
[ INFO] [1568048399.956913044]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1568048399.958020452]: Planning attempt 1 of at most 1
[ INFO] [1568048399.961672096]: Planner configuration 'group1' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1568048399.963288246]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1568048404.971078802]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1568048404.971140463]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1568048404.971214635]: No solution found after 5.008364 seconds
[ INFO] [1568048404.991413882]: Unable to solve the planning problem

And here is the output from the terminal running m node:

[ INFO] [1568048398.145399968]: Loading robot model 'test_robot'...
[ WARN] [1568048398.146420670]: No geometry is associated to any robot links
[ WARN] [1568048398.146476044]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048398.237453827]: Loading robot model 'test_robot'...
[ WARN] [1568048398.238001746]: No geometry is associated to any robot links
[ WARN] [1568048398.238089516]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048399.348285005]: Ready to take commands for planning group group1.
[ INFO] [1568048399.669554589]: RemoteControl Ready.
[ INFO] [1568048399.948613767]: Planning frame: /baseLink
[ INFO] [1568048399.948914032]: End effector link: end
[ INFO] [1568048399.949158560]: Available Planning Groups:
end
[ INFO] [1568048404.991850290]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1568048404.991899784]: Visualizing plan 1 as trajectory line

Asked by PatFGarrett on 2019-09-09 12:02:13 UTC

Comments

You might want to explicitly set the other quaternion values in target_pose1. You are only explicitly setting w, which means that the other x, y, z values might be default initialized to some garbge. A mucked-up rotation could easily cause the goal to out of reach for your robot.

Asked by BryceWilley on 2019-09-11 09:50:06 UTC

@BryceWilley thank you for the response. I made the changes to ensure no random data is being set and still the same issue.

Asked by PatFGarrett on 2019-09-11 11:08:11 UTC

The only other missing piece here is what's in the robot's SRDF and in demo.launch. From the Could not identify parent group for end-effector 'end', it could be that your planning group is not set up correctly.

Asked by BryceWilley on 2019-09-13 08:13:06 UTC

Answers