goal planning - unable to sample any valid states

[Rob Roberts]

Hello,

  This is my first post to the group, although I have been reading for quite a while.  I am building a robot arm based on high torque hobby servos and extra gearing, in preparation of buying some Dynamixel pros to build a real prototype (can't wait)

I have to say how impressed I am with the group, a real sense of camaraderie, it is a great time to be starting in robotics.  Ok, now to my problem....

I am having problems when trying to do goal planning.  It seems no matter what I do, I am getting the following message: "LBKPIECE1: Unable to sample any valid states for goal tree"

Below is some info:

1) I have built a custom URDF and setup the moveit Config, which ran successfully.

2) I have setup a launch file which runs successfully

3) My launch file opens the model in rviz, and using the moveit plugin I am able to update, plan and execute "random valid" position queries

4) I have tried both setPositionTarget() and setPoseTarget() and get the same message

5) I have added code to check for internal collision, there is no collision, but no valid states are found

6) when I create a pose based on getCurrectState() I get a message that the goal condition is already satisified

7) My end effector link is called "gripper_pole" and when I grab the random valid position coordinates generated from rviz planning and try to use them, i get the same "unable to sample any valid states" message

8) I am using the C++ moveit interface

Below is my code, any ideas what I am doing wrong?

int main(int argc, char **argv)

{

    ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);

    ros::AsyncSpinner spinner(1);

    spinner.start();

    move_group_interface::MoveGroup group("left_arm");

    geometry_msgs::PoseStamped pose = group.getRandomPose();

    pose.header.stamp = ros::Time::now();

    double x = pose.pose.position.x; 

    double y = pose.pose.position.y;

    double z = pose.pose.position.z;

    ROS_INFO("Move to : x=%f, y=%f, z=%f",x,y,z);

    group.setPositionTarget(x,y,z); 

    group.move();

    ros::waitForShutdown();

}

I have tried everything I can think of (which is honestly not very much, I am very new to robotics), and am banging my head against the wall on this one.  It is probably something super simple, or a concept I am not understanding, so any help you can provide would be greatly appreciated.

Thanks!

Rob

[Dinesh Madusanke]

Dear Rob,
I am having the same problem and I cannot plan for any valid position of link. Did you solved the problem ?

Thanks.

[Philippe Capdepuy]

Hello,

You may wanna try the following or a combination of these:

  - remove all objects except your arm, to make sure you don't collide with one of these

  - explicitely define a non-zero tolerance for your goal.

Good luck,

Philippe

--

Regards,

Dr. Philippe Capdepuy

Research Engineer

Génération Robots / HumaRobotics

Tel : +33 5 56 39 37 05

www.humarobotics.com

www.generationrobots.com

 

Découvrez Baxter, votre nouveau compagnon

pour la recherche et l’éducation

Baxter Research Robot

[Rob Roberts]

Philippe, thanks so much for the suggestion, I have been reviewing the logs and just prior to the error message  "LBKPIECE1: Unable to sample any valid states for goal tree", I am noticing a message (but not error) stating: " Collision checking is considered complete (collision was found and 0 contacts are stored)", which would lead me to believe there is a collision issue.

The strange thing is when I use RVIZ and click on a "random valid" end state in the planning tab, the arm moves in the simulator without error.

I will try stripping down the URDF to see if that helps.

I'll see if I can figure out how to add a non-zero tolerance and see if that helps.

Thanks!

Rob

[Benjamin Nilson]

Rob,

 

If your still having this issue or to anyone else having this issue, I had the same problem and found a couple solutions.

 

The first solution I found was to use setPositionTarget (double x, double y, double z, const std::string &end_effector_link="").

Your code posted above does not have the end effector link which might be why it’s not working?

 

geometry_msgs::Pose target_pose1;

target_pose1.position.x = 1.0;

target_pose1.position.y = -0.2;

target_pose1.position.z = 0.5;

group.setPoseTarget(target_pose1.position.x,  target_pose1.position.y, target_pose1.position.z, “tool0”);

group.setGoalTolerance(0.01);

 

The second solution I found was the orientation precision. If no orientation is given it uses the same orientation that the robot is in at the beginning which can cause it to fail. Also, if the given orientation does not have enough significant digits then it ignores it. To get setPoseTarget to work I had to specify the orientation to 10^-6.

 

Code excerpt:

geometry_msgs::Pose target_pose1;

target_pose1.orientation.x = -0.476214;

target_pose1.orientation.y = 0.485218;

target_pose1.orientation.z = -0.517232;

target_pose1.orientation.w = 0.519859;

target_pose1.position.x = 1.0;

target_pose1.position.y = -0.2;

target_pose1.position.z = 0.5;

group.setPoseTarget(target_pose1);

group.setGoalTolerance(0.01);

 

Using this code I was able to get the robot to move. I also implemented Philippe’s idea of goal tolerance. I hope this helps.

-Ben

[Jonathan Cacace]

Hi Guys!

This seems to be an old post, but I am having the same problems and I am not able to find a good solution.

I am trying to move a 7-dof arm using the "move group" tutorial with my own robot. If I use the pr2 robot everything go well while with my robot I got just failed.
 I have increased the IK tolerance and both with random states and reachable goal states.
The strange things is that using the Rviz plugins to control my robot failed never happen.

Anyone can suggest me other solutions?

[Qiang Qiu]

Hi Jonathan,

this problem is often caused by a invalid target pose, you may try some other poses.

[Jonathan Cacace]

Hi Qiang!

Thank you a lot for your reply.

Yes, my first test was to try several poses. Even random generated (that should be valid, I guess). In any case, I tried to put a very high tolerance, and try a lot of attempts ( 30 ) and now something  is happening. The eef reach more or less the desired position.

The thing that let me really confused is that I have a complete different behaviour between the Rviz plugin ( a perfect solution founded in 0.15 sec in mean ) and the move gropu API in C++, a very bad solution founded in 4.9 sec... I really don't understand why this big difference!

[Qiang Qiu]

The possible reason for the different performances, I guess, is that you didn't specify a planner id in your C++ program. the default planner LBKPIECEkConfigDefault costs more time than RRTConnectkConfigDefault planner. 

Besides, you can specify the planner as follow:

           group.setPlannerId("RRTConnectkConfigDefault");

[Jonathan Cacace]

Yes already know that... I think that is something related to the configuration of the scene. I will try other parameters of the move_group class!