ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Robot Arm "Violating" Joint Constraints prevents future movement/planning

asked 2011-07-21 02:58:18 -0500

John Hoare gravatar image

updated 2011-12-01 01:19:17 -0500

We have an arm that we're trying to control using the move_arm stack. We're having difficulties where the arm gets moved to the edge of our joint limits, but then gravity (or noise) pulls the arm slightly past the joint constraint. Once this happens, the arm can not move, because the start state violates the joint constraints. How do people normally deal with this problem?


I've tried to write some simple code which pushes the arm just out of the joint limit, but a problem occurs is that the arm is trying to go back into a position where it thinks it is in a limit, and can not actually get there. (For example, the solution from the IK solver is right at the end of the joint limit). So my follow-on question is this: Is there a way to treat the IK/planner to have slightly tighter joint limits then what exists in real life? This way the planner can plan to put a joint at its limit, but when we actually get there there is a bit of play so move_arm will at least successfully go to it?

Edit2: See my answer below for how I have solved this problem for our robot.

edit retag flag offensive close merge delete


Have you solved your problem yet? I have the same issue you do and found a workaround. I don't like the workaround, but until a solid fix comes out, I'll have to use it.
seth_g gravatar image seth_g  ( 2011-11-15 07:20:50 -0500 )edit
Not very well, however I haven't spent a lot of time dealing with it. I've implemented (like in the edit) something which "twitches" the arm out of a joint limit and then just tries to go to the next position. It works somewhat but its not an ideal solution.
John Hoare gravatar image John Hoare  ( 2011-11-17 07:42:38 -0500 )edit

FYI you can look at my answer to see how I solved this problem for our specific situation.

John Hoare gravatar image John Hoare  ( 2012-11-05 03:28:06 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2011-07-21 04:39:24 -0500

egiljones gravatar image


There are two approaches that might work for you. The first is what we're doing in the move_arm_head_monitor package in the arm_navigation_experimental directory. When used in concert with the move_arm_warehouse (the experimental version of move_arm) we call the monitor not only after planning but before planning. During this pre-planning stage among other things we check if the joints are just outside the joint limits. If so we generate a simple open loop trajectory that puts us back inside the limits and pass it to the controller - for the PR2 the required movement is pretty small. This method seems to work well in practice. You could probably do something similar in a script that you ran before calling the move arm request.

The other thing that occurs to me is something I haven't tried but should work in the unstable/electric version of arm_navigation. The new version supports setting a robot state that is not the current state in the MotionPlanRequest. You should be able to get the current state from the environment server, determine which joints are out of their limits using the functions in KinematicState, set values for those joints in the robot state in MotionPlanRequest, and pass that into MoveArm as part of the PlanningScene diff. The end result will be planning/execution from a slightly different start state, but if your controllers are robust to this minor discrepancy this approach should work. Definitely make sure that you aren't setting a diff that's wildly different than the current start state - the PR2 controllers at least will try very hard to achieve the first trajectory point from the current location and the planner will not have necessary ensured that the path to that point is collision-free.

If you go with the latter approach and something doesn't work let me know. I'll be writing up some documentation for the arm_navigation release that will talk about some of this new functionality but I haven't done so yet.

edit flag offensive delete link more


I've tried implementing your first suggestion, however I've run into some issues, please see my edit.
John Hoare gravatar image John Hoare  ( 2011-11-03 06:48:56 -0500 )edit

answered 2011-12-01 01:18:33 -0500

John Hoare gravatar image

I've implemented a third option, which is somewhat of a hack. I have created a joint state publisher which is aware of the pre-set limits for each joint, and will publish the limit value (minus some small amount) if it is past the limit. This works pretty well for us since whenever the joint is past a limit, it is by some miniscule amount and does not actually matter, other than it making move_arm complain.

edit flag offensive delete link more

Question Tools


Asked: 2011-07-21 02:58:18 -0500

Seen: 579 times

Last updated: Dec 01 '11