Planning fails for End-effector Pose

edit

Hey all!

I'm trying to use the moveit group interface tutorial to move my custom robot (3-links). I'm having problems in planning motion when I give an End-effector pose. I keep getting RRTConnect: Unable to sample any valid states for goal tree.

I have tried the following solutions but none of them seem to work: 1. Trying a different IK solver (Trac_ik) 2. Setting longer planning times

I have triple-checked my goals for valid end-eff positions, but it still keeps failing . It however works flawlessly when I provide joint-space goals instead. Below is my code. If anyone could tell me where I am going wrong?

// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
  // are used interchangably.
  static const std::string PLANNING_GROUP = "Body";

  // The :move_group_interface:`MoveGroup` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
 /* const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);*/

  //-----------------------------
  //Getting Basic Information
  //-----------------------------

  // We can print the name of the reference frame for this robot.
  ROS_INFO_NAMED("moveff", "Reference frame: %s", move_group.getPlanningFrame().c_str());

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

  bool success; //Movement success variable

  // 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.position.x = 0.227560;
  target_pose1.position.y = -0.000002;
  target_pose1.position.z = 0.372389;
  target_pose1.orientation.x = -0.110141;
  target_pose1.orientation.y = 0.110147;
  target_pose1.orientation.z = -0.698475;
  target_pose1.orientation.w = 0.698476;

  move_group.setPoseTarget(target_pose1);

  move_group.setPlanningTime(30);
  // 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;

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

  if(success == true) {
   move_group.move();
   ROS_INFO("Movement Done");
  }