PlanToEndEffectorPose crashes if called before ActiveDOFs are manually set.

[psigen]

Open herbpy console and do the following:

In [5]: robot.GetActiveManipulator()
Out[5]: RaveGetEnvironment(1).GetRobot('herb').GetManipulator('left')

In [6]: robot.PlanToEndEffectorPose(robot.left_arm.GetEndEffectorTransform())
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Sequence(SnapPlanner, VectorFieldPlanner, Trajopt)".
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
---------------------------------------------------------------------------
openrave_exception                        Traceback (most recent call last)
/home/pkv/ws/src/deps/planning/herbpy/scripts/console.py in <module>()
----> 1 robot.PlanToEndEffectorPose(robot.left_arm.GetEndEffectorTransform())

/home/pkv/ws/src/deps/planning/prpy/src/prpy/base/robot.pyc in wrapper_method(*args, **kw_args)
    108             @functools.wraps(delegate_method)
    109             def wrapper_method(*args, **kw_args):
--> 110                 return self._PlanWrapper(delegate_method, args, kw_args)
    111 
    112             return wrapper_method

/home/pkv/ws/src/deps/planning/prpy/src/prpy/base/robot.pyc in _PlanWrapper(self, planning_method, args, kw_args)
    605         from ..util import Timer
    606         with Timer() as timer:
--> 607             result = planning_method(self, *args, **kw_args)
    608         SetTrajectoryTags(result, {Tags.PLAN_TIME: timer.get_duration()}, append=True)
    609 

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in meta_wrapper(*args, **kw_args)
    236                                                    args, kw_args))
    237             else:
--> 238                 return self.plan(method_name, args, kw_args)
    239 
    240         # Grab docstrings from the delegate planners.

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in plan(self, method, args, kw_args)
    407 
    408                 try:
--> 409                     return plan_fn(*args, **kw_args)
    410                 except UnsupportedPlanningError:
    411                     continue

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in meta_wrapper(*args, **kw_args)
    236                                                    args, kw_args))
    237             else:
--> 238                 return self.plan(method_name, args, kw_args)
    239 
    240         # Grab docstrings from the delegate planners.

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in plan(self, method, args, kw_args)
    300 
    301                     with Timer() as timer:
--> 302                         output = planner_method(*args, **kw_args)
    303 
    304                     logger.info('Sequence - Planning succeeded after %.3f'

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in meta_wrapper(*args, **kw_args)
    236                                                    args, kw_args))
    237             else:
--> 238                 return self.plan(method_name, args, kw_args)
    239 
    240         # Grab docstrings from the delegate planners.

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in plan(self, method, args, kw_args)
    300 
    301                     with Timer() as timer:
--> 302                         output = planner_method(*args, **kw_args)
    303 
    304                     logger.info('Sequence - Planning succeeded after %.3f'

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in __call__(self, instance, robot, *args, **kw_args)
    151                 return wrap_future(executor.submit(call_planner))
    152             else:
--> 153                 return call_planner()
    154 
    155     def __get__(self, instance, instancetype):

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/base.pyc in call_planner()
    131                 try:
    132                     planner_traj = self.func(instance, cloned_robot,
--> 133                                              *args, **kw_args)
    134 
    135                     # Tag the trajectory with the planner and planning method

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/snap.pyc in PlanToEndEffectorPose(self, robot, goal_pose, **kw_args)
    120             raise PlanningError('There is no IK solution at the goal pose.')
    121 
--> 122         return self._Snap(robot, ik_solution, **kw_args)
    123 
    124     def _Snap(self, robot, goal, **kw_args):

/home/pkv/ws/src/deps/planning/prpy/src/prpy/planning/snap.pyc in _Snap(self, robot, goal, **kw_args)
    138         params.SetGoalConfig(goal)
    139         check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed,
--> 140             options = 15, returnconfigurations = True)
    141 
    142         # If valid, above function returns (0,None, None, 0)

openrave_exception: openrave (InvalidArguments): [virtual void OpenRAVE::RobotBase::SetActiveDOFValues(const std::vector<double>&, uint32_t):607] (int)values.size() >= GetActiveDOF(), (eval 7 >= 26) not enough values 7<26

[DavidB-CMU]

@psigen I noticed the same behaviour. It tells you that "(eval 7 >= 26) not enough values 7<26" So I interpreted this as "you can't plan the end effector pose for 26 DOF" which makes sense to me.

What would you like to happen?

[psigen]

Well, the thing is: you absolutely can plan the end effector pose for 26 DOF. PlanToEndEffector is simply enforcing a constraint on the final end-effector transform of the active manipulator, and any number of DOFs can be used to form a plan that satisfies that constraint.

In this case, the active end effector is left_arm, so the planning operation is free to do whatever it wants with whatever DOFs are active to move the left arm end-effector transform to the desired final pose. I believe that PlanToEndEffectorPose implicitly assumes that it should take the active manipulator, use that manipulator's DOFs, and plan in that space.

But that's just the semantics argument. The even more salient issue here is that this is failing in SnapPlanner in a way that kills all the other planners (it's not a PlanningError, it's an openrave_exception).

[DavidB-CMU]

@psigen Ok I see. If PlanToEndEffector pose can work without setting the active DOF, perhaps a Unit Test should also be added for that?

[psigen]

That's a good idea. I think this is the intended behavior, we can confirm with @mkoval.