Ask Your Question
0

Planning fails for End-effector Pose

asked 2019-01-23 00:53:13 -0500

pranavb104 gravatar image

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");
  }
edit retag flag offensive close merge delete

Comments

You seem to be setting a 6D goal for a 3D robot. That most likely won't work without enabling approximate IK solutions.

  1. Trying a different IK solver (Trac_ik)

Trac IK doesn't really work with anything < 6 dof afaik (similar to KDL).

gvdhoorn gravatar imagegvdhoorn ( 2019-01-23 02:36:59 -0500 )edit

So do I have to make my own IKFast solver?

pranavb104 gravatar imagepranavb104 ( 2019-01-23 02:41:12 -0500 )edit

You could try that, although I don't know what the successrate of that would be.

gvdhoorn gravatar imagegvdhoorn ( 2019-01-23 02:44:46 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-23 04:09:30 -0500

pranavb104 gravatar image

So I have fixed it by using

move_group.setApproximateJointValueTarget(target_pose1, "link3"); (instead of move_group.setPoseTarget(target_pose1))

edit flag offensive delete link more

Comments

A bit more of a work-around, wouldn't you agree?

gvdhoorn gravatar imagegvdhoorn ( 2019-01-23 04:24:05 -0500 )edit

yes I agree...do I add it as a comment then? I'm sorry im a bit new to the ros q&a platform

pranavb104 gravatar imagepranavb104 ( 2019-01-23 04:48:36 -0500 )edit

No, this is fine.

gvdhoorn gravatar imagegvdhoorn ( 2019-01-23 05:09:35 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-23 00:53:13 -0500

Seen: 148 times

Last updated: Jan 23