Starting state is in collision error when running new arm navigation stack

asked 2011-08-10 04:36:37 -0500

Pi Robot
4046 ●134 ●158 ●192 http://www.pirobot.org/

updated 2011-08-11 03:01:38 -0500

Hello,

I have run through the new arm planning wizard (arm_navigation version 1.0.2) and everything went smoothly to generate a set of configuration and launch files for my 5-dof Dynamixel arm.

Now I am trying to run the move_arm_joint_goal.cpp example found here. I had to substitute arm_navigation_msgs for move_arm_msgs but otherwise the sample code compiles and runs fine. The only trouble is that when I try specifying a simple joint goal (like all joint positions = 0 which has the arm pointing straight out from the robot), I always get the error message:

[ERROR] [1312993060.885707724]: Starting state is in collision, can't plan

and the arm does not move. It doesn't matter how I initially position the arm, the goal attempt always fails with the same error. My Dynamixel controller node publishes a valid set of joint positions on the /joint_states topic as I can verify in RViz.

UPDATE: After more testing, these are my consistent findings:

after running the wizard in easy mode, the planning visualizer cannot find IK solutions for any attempted positioning of the end effector.
When connected to the real robot and sending a move_arm_joint_goal with the arm initially straight out and slightly down, planning fails, reporting that the finger and thumb links are in collision with the base link even though both links are about 4 inches away from the base (including the collision envelopes around the links).
Moving the arm to any position further away from the base solves the problem: a planning solution is found and the arm moves perfectly.
Disabling collision checking between the base link and finger and thumb links also solves the problem but of course is not ideal since the thumb and finger might fairly often come into collision with the base during operations where the hand reaches for something on the ground

Thanks!
patrick

edit retag flag offensive close merge delete

add a comment

1

answered 2011-08-24 11:05:03 -0500

egiljones
2026 ●5 ●18 ●36

This particular problem was caused by a scaling error on COLLADA meshes in arm_navigation that has been rectified in the newest debs - update and things should be good (and check out the sweet new wizard logo).

edit flag offensive delete link

Comments

Cool--will give it a try this weekend.

Pi Robot ( 2011-08-24 13:27:59 -0500 )edit

Nice new wizard logo! Only one glitch--on my 1280x800 laptop screen, the wizard dialog box now runs off the bottom of the screen so that I do not have access to the buttons. I can hit Return at the right places to get the wizard to work, but I can't resize the window to allow me to see the buttons at the bottom.

Pi Robot ( 2011-08-25 08:38:28 -0500 )edit

Yeah, I just managed to beat Qt into allowing resizing all the way down yesterday - I'll release in the next couple days.

egiljones ( 2011-08-25 09:25:31 -0500 )edit

add a comment

0

answered 2011-08-10 20:30:16 -0500

Lorenzo Riano
1342 ●26 ●34 ●50 http://www.eecs.berkel...

I've had the same problem several times. I think it's due to floating point rounding errors. You can check that the current joint values are a bit off the limit specified in the URDF. Often moving the joints a little bit usually helps.

We usually save "safe" poses using the pr2_control_gui an we load them at startup. Please let me know if you need help with using the gui (which is at the moment undocumented but quite straightforward to use).

edit flag offensive delete link

Comments

Thanks Lorenzo. We're looking into my URDF model first to see if there might be some issue there.

Pi Robot ( 2011-08-12 01:15:28 -0500 )edit

add a comment

0

answered 2011-08-10 08:29:18 -0500

Matt Klingensmith
141 ●2 ●3 ●6

You might also want to enable debug logger levels for the ompl planner. This prints out which links are colliding when there is an error. This sounds like an issue with self-collisions with the arm to me. It's possible that one of your link pairs is usually in collision, but the wizard failed to disable collisions between the links.

edit flag offensive delete link

Comments

Changing logger levels is a good idea, but you'll need to do it for the move_<group_name> node - that's returning the collision before it actually invokes the planner at all. Specifically, you'll need to enable debugging for the collision_space namespace.

egiljones ( 2011-08-10 08:33:57 -0500 )edit

add a comment

0

answered 2011-08-10 05:14:51 -0500

egiljones
2026 ●5 ●18 ●36

Patrick,

Does the planning components visualizer work for you - can you solve ik and plan between two joint states? If that doesn't work then there's probably some allowed collision operation that is not being disabled that needs to be - did you use the "Easy" mode in the wizard? If that does work then something must be different about your setup when running with move arm - you are presumably running with your actual robot, right?

When it finds collisions with the start state move_arm should be publishing markers on the topic move_<group_name>_markers - it should produce little spheres in Rviz with the names of the colliding links. I'd check that against the list of disabled collisions in the config/<your_robot_name>_planning_description.yaml file.

edit flag offensive delete link

Comments

Thanks for the help! Yes, I am running this on the real robot. I tried the planning components visualizer and it seems that, no, it is not working--any attempt to drag the end effector results in an invalid pose. I can position joints individually but not the end effector. And yes, I did run the wizard in Easy mode so I'll take a look at Advanced Mode next. Thanks for the move_<group_name>_markers tip--it lit up an orange sphere right on the tip of the gripper link although there was nothing in collision with it. (There was an associated message indicating that the collision was with the base_link but it was several inches away.) My robot's arm is initially in the straight out and horizontal position (all joint angles = 0) so I wondered if there might be an extremum effect. Once I move the arm away from ...(more)

Pi Robot ( 2011-08-10 10:34:47 -0500 )edit

After more testing, this is my consistent finding: after running the wizard in easy mode, the planning visualizer cannot find IK solutions for any attempted positioning of the end effector. When connected to the real robot and sending a move_arm_joint_goal with the arm initially straight out and slightly down, planning fails reporting that the finger and thumb links are in collision with the base link even though both links are about 4 inches away from the base (including the collision envelopes around the links). Moving the arm to any position further away from the base solves the problem: a planning solution is found and the arm moves perfectly. Disabling collision checking between the base link and finger and thumb links also solves the problem but of course is not ideal since the thumb and finger might fairly often come into collision with the base during operations where the hand reaches ...(more)

Pi Robot ( 2011-08-11 02:05:42 -0500 )edit

P.S. I am happy to send along my URDF model if that would help.

Pi Robot ( 2011-08-11 02:35:50 -0500 )edit

I tried running the wizard with trunk of pi_description (linked from the urdf/Examples page) and it worked well, but I expect that you are using a different urdf as this one had no mention of any gripper links. Point me towards the new one and I'll take a look!

egiljones ( 2011-08-11 05:02:27 -0500 )edit

Yes, the planning wizard works perfectly with my pi_description URDF model. In the meantime, I'll email you the URDF for the robot I'm having trouble with to avoid cluttering the forum with a large attachment.

Pi Robot ( 2011-08-12 01:14:30 -0500 )edit

add a comment

Starting state is in collision error when running new arm navigation stack

4 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

Starting state is in collision error when running new arm navigation stack edit

4 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

Starting state is in collision error when running new arm navigation stack