moveit - nearest reachable pose for arm gripper

edit

Hi,

This is a bit of a tough one :) Technically, the functionality you need is there, and can be exposed, but it is tricky. The issue is that when planning with OMPL, the assumption is that a specification of a goal (conjunction of constraints) can be sampled -- i.e., there is a procedure that generates states that satisfy the goal constraints. If a state that is also valid satisfies the goal constraints, then a potential goal is found. This nicely maps to the notion of a GoalSampleableRegion in OMPL (http://ompl.kavrakilab.org/classompl_1_1base_1_1GoalSampleableRegion.html). With this notion, it is possible to use bi-directional search planners, which are often more efficient.

What you would need to do to obtain the behaviour you want, is use single-tree planners from OMPL (e.g., EST, RRT) that allow forward search when only a distance to goal is available (the GoalRegion class in OMPL). So you would have to either extend the ompl_interface package to allow such things, or bypass it and call planners such as ones mentioned here to get to your pose. The planner will fail, since there is no solution, but will report an approximate solution to the state that was closest to the goal (whatever was found given the time spent). I think extending ompl_interface would be best here.

An alternative implementation would be to use the genetic algorithms in OMPL to find a valid state that is closest to the pose you would like. And valid here could simply mean kinematically feasible. Once a state is obtained using this search, the regular ompl_interface can be used to plan a motion. This would probably be simpler to implement.

Depending on your application, you might be able to try something like the carrot_planner.

As far as I know, this doesn't exist in MoveIt, but it shouldn't be too hard to implement something manually. Not as an official MoveIt planner, but as a wrapper around calls to the existing MoveIt plan() interface. This is a pretty naive approach, but may satisfy some simple just-out-of-reach cases.