MoveIt Code - RRTConnect: Unable to sample any valid states for goal tree

[Anthony Kolodzinski]

Hi All, 

New to this forum. I have a small project in which I am trying to use MoveIt with a 2dof planar robot. I have generated the URDF with three links. Link one attaches to the base through joint 1, link two to link one via joint 2, and I have a point link, link 3, which is the end effector and connects to joint 2 through a fixed joint 3. All are part of the group: arm_group. 

I have successfully used the setup assistant to generate the necessary configuration files.

My goal is to just be able to specify x and y, and have the robot navigate its end effector to that point. Through my research I have read that I can set the line

position_only_ik: True

in the kinematics.yaml file, and orientations will be ignored. 

I have done this, and when I spin up the demo I can successfully specify a point and have the robot navigate to it: 

However, when I try to specify and run a path in code I get an error - RRTConnect: Unable to sample any valid states for goal tree

Please see my code below:

  ros::init(argc, argv, "plan_node");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();


  moveit::planning_interface::MoveGroup group("arm_group");
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  ros::Publisher display_publisher = nh.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;
  //geometry_msgs::Pose target_pose1;                                                                                                                  
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());


  ROS_INFO_STREAM("#" << group.getCurrentPose());
  group.setPositionTarget(group.getCurrentPose().pose.position.x, group.getCurrentPose().pose.position.y, group.getCurrentPose().pose.position.z, "link3");
  //group.setRandomTarget();                                                                                                                            
  group.setGoalTolerance(0.1);
  moveit::planning_interface::MoveGroup::Plan my_plan;
  bool 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);

This is essentially copied from the tutorial. It does not matter what I put in the setPositionTarget arguments (here it is the starting position). That error will always be thrown. Through my previous research I have heard of issues with KDL and arms with <6dof. However, I do not see how that explains my ability to choose arbitrary points and run IKs from the GUI. Also, when I try to install openrave to try and add an ikfast plugin I go through the following: 

sudo add-apt-repository ppa:openrave/release
sudo apt-get update
sudo apt-get install openrave0.8-dp-ikfast

but apt returns a package not found error. 

Any thoughts or help would be greatly appreciated. Thanks. 

[Minerva Vargas]

I'm having the same problem you did, did you mannaged to solve it?