Ask Your Question
0

Moveit Pose "unable to sample any valid states"

asked 2017-02-03 15:06:57 -0600

jbeck28 gravatar image

updated 2017-02-03 15:40:35 -0600

gvdhoorn gravatar image

I've been trying to get a fanuc lrmate200id to work go to a pose goal in Rviz through the move_group_interface. If I set a simple position goal using move_group.setPositionTarget(x,y,z,"tool0"), moveit is able to plan a path. However, if I use either setOrientationTarget, or setPoseTarget, moveit is never able to figure out a path. The block of code I've borrowed/modified is below:

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 = "manipulator";
  moveit::planning_interface::MoveGroup move_group(PLANNING_GROUP);

 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

const robot_state::JointModelGroup *joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("odom_combined");
visual_tools.deleteAllMarkers();


 ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

visual_tools.loadRemoteControl();


  ROS_INFO("Reference frame: %s", move_group.getPlanningFrame().c_str());
  ROS_INFO("Reference frame: %s", move_group.getEndEffectorLink().c_str());

move_group.setPlanningTime(60*5);
move_group.setGoalTolerance(.001);

const float theta = 10*3.1416/180.0;
double x1 = .5, x2 = 0, x3 = 0;
geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1;
  target_pose1.position.x = x1*sin(theta/2);
  target_pose1.position.y = x2*sin(theta/2);
  target_pose1.position.z = x3*sin(theta/2);

  //move_group.setPositionTarget(x1,x2,x3,"tool0");
  //move_group.setRPYTarget(0,0,0,"tool0");
  //move_group.setOrientationTarget(x1,x2,x3,1,"tool0");
  move_group.setPoseTarget(target_pose1,"tool0");
moveit::planning_interface::MoveGroup::Plan my_plan;
  bool success = move_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);


  ros::shutdown();  
  return 0;
}

any ideas on how to proceed would be most appreciated!

thanks, Josh

edit retag flag offensive close merge delete

Comments

Not to sound repetitive, but are the pose and orientation targets reachable? Can try set random joint state target and get the pose of the end effector link from that. Another option is to try changing the motion planner being used.

JoshMarino gravatar imageJoshMarino ( 2017-02-04 01:44:40 -0600 )edit

I've seen trying a different motion planner come up on online. However, I'm not sure what my options are. Is there somewhere I can read about what motion planner options I have?

jbeck28 gravatar imagejbeck28 ( 2017-02-07 11:46:38 -0600 )edit

setOrientationTarget() accepts raw quaternion values, but you try to pass position coordinates instead. When managing positions and orientations I highly recommend you to use tf (or even tf2) package for you calculations. Then convert it to target message format via tf::convertXXX() methods family.

dvolosnykh gravatar imagedvolosnykh ( 2017-04-12 05:12:17 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2017-02-17 08:55:53 -0600

FábioBarbosa gravatar image

Hi! i don't known if it helps but here my way of planning:

First, i do a planning by joints, in other words i do the "Planning to a joint-space goal" from the move_group_interface_tutorial.cpp

Second i do a reading of eef position with this code:

  geometry_msgs::PoseStamped robot_pose;
  robot_pose = group.getCurrentPose();

  geometry_msgs::Pose current_position;
  current_position = robot_pose.pose;

  /*Retrive position and orientation */
  geometry_msgs::Point exact_pose = current_position.position;
  geometry_msgs::Quaternion exact_orientation = current_position.orientation;

  ROS_INFO("Reference frame : %s",group.getPlanningFrame().c_str());
  ROS_INFO("Reference frame : %s",group.getEndEffectorLink().c_str());

  std::cout<<"Robot position : "<<exact_pose.x<<"\t"<<exact_pose.y<<"\t"<<exact_pose.z<<std::endl;
  std::cout<<"Robot Orientation : "<<exact_orientation.x<<"\t"<<exact_orientation.y<<"\t"<<exact_orientation.z<<"\t"<<exact_orientation.w<<std::endl;

Third with the coordinates that i read on the step 2, i do the planning by coordinates, in other words, the "Planning to a Pose goal" from move_group_interface_tutorial.cpp

And thats about it, that way my coordinates are always valid

Hope it helped

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-02-03 15:06:57 -0600

Seen: 1,621 times

Last updated: Feb 17 '17