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

edit

Hi,

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.