default planning to named configurations on real ADA failure

[romesco]

Still running into issues when trying to plan something as simple as home configuration on real ADA. Not exactly sure what is motivating the failure, but it looks like the planner succeeds and when it goes to execute, there is a failure.

It is throwing an Attribute Error when it tries to call getDescription for a "JoinTrajectory" object in prpy

I am literally running test.py in default ADA and calling "planToNamedConfiguration('home',execute=True)"

Code is attached below.

robot.arm.PlanToNamedConfiguration('home',execute=True)
[environment-core.h:116 Environment] setting openrave home directory to /home/rscalise/.openrave
[odecollision.h:124 ODECollisionChecker] ode will be slow in multi-threaded environments
[environment-core.h:193 Init] using ode collision checker
[environment-core.h:848 SetCollisionChecker] setting ode collision checker
[qtcoinviewer.cpp:2928 UpdateFromModel] timeout of GetPublishedBodies
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[INFO] [prpy.planning.base:base.py:306]:plan: Sequence - Planning succeeded after 0.591 seconds with "SnapPlanner".
[environment-core.h:221 Destroy] destroy module
[INFO] [prpy.base.robot:robot.py:400]:do_execute: Post-processing path with 2 waypoints.
[environment-core.h:116 Environment] setting openrave home directory to /home/rscalise/.openrave
[odecollision.h:124 ODECollisionChecker] ode will be slow in multi-threaded environments
[environment-core.h:193 Init] using ode collision checker
[environment-core.h:848 SetCollisionChecker] setting ode collision checker
[qtcoinviewer.cpp:2928 UpdateFromModel] timeout of GetPublishedBodies
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[WARNING] [prpy.base.robot:robot.py:331]:do_postprocess: Post-processing smooth paths is not supported. Using the default post-processing logic; this may significantly change the geometric path.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "ParabolicSmoother".
[basemanipulation.cpp:117 main] BaseManipulation: using BiRRT planner
[taskmanipulation.cpp:179 main] using BiRRT planner
[parabolicsmoother.cpp:193 PlanPath] env=10, initial path size=1, duration=4.757549, pointtolerance=0.200000, multidof=0
[parabolicsmoother.cpp:387 PlanPath] after shortcutting 0 times: path waypoints=1, traj waypoints=4, traj time=4.757549s
[parabolicsmoother.cpp:395 PlanPath] path optimizing - computation time=0.001000s
[INFO] [prpy.planning.base:base.py:306]:plan: Sequence - Planning succeeded after 0.214 seconds with "ParabolicSmoother".
[environment-core.h:221 Destroy] destroy module
[INFO] [prpy.base.robot:robot.py:410]:do_execute: Post-processing took 0.416 seconds and produced a path with 4 waypoints and a duration of 4.758 seconds.
---------------------------------------------------------------------------
AttributeError                            Traceback (most recent call last)
/home/rscalise/storage/adaRun_ws/src/ada/adapy/scripts/test.py in <module>()
----> 1 robot.arm.PlanToNamedConfiguration('home',execute=True)

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/base/manipulator.pyc in wrapper_method(*args, **kwargs)
     71             @functools.wraps(delegate_method)
     72             def wrapper_method(*args, **kwargs):
---> 73                 return self._PlanWrapper(delegate_method, args, kwargs)
     74             return wrapper_method
     75 

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/base/manipulator.pyc in _PlanWrapper(self, planning_method, args, kw_args)
    183         # Optionally execute the trajectory.
    184         if kw_args.get('execute', False):
--> 185             return self.GetRobot().ExecutePath(traj, **kw_args)
    186         else:
    187             return traj

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/base/robot.pyc in ExecutePath(self, path, defer, executor, **kwargs)
    425             return wrap_future(executor.submit(do_execute))
    426         elif defer is False:
--> 427             return do_execute()
    428         else:
    429             raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/base/robot.pyc in do_execute()
    413             with Timer() as timer:
    414                 exec_traj = self.ExecuteTrajectory(traj, defer=False, **kwargs)
--> 415             SetTrajectoryTags(exec_traj, {Tags.EXECUTION_TIME: timer.get_duration()}, append=True)
    416             return exec_traj
    417 

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/util.pyc in SetTrajectoryTags(traj, tags, append)
    318 
    319     if append:
--> 320         all_tags = GetTrajectoryTags(traj)
    321         all_tags.update(tags)
    322     else:

/home/rscalise/storage/adaRun_ws/src/prpy/src/prpy/util.pyc in GetTrajectoryTags(traj)
    292     import json
    293 
--> 294     description = traj.GetDescription()
    295 
    296     if description == '':

AttributeError: 'JointTrajectory' object has no attribute 'GetDescription'

[psigen]

This has come up several times, but no one seems to have resolved it. The underlying issue is in this line: https://github.com/personalrobotics/ada/blob/master/adapy/src/adapy/adarobot.py#L205

The result of ExecuteTrajectory is supposed to be an OpenRAVE trajectory, but this code is returning a ROS JointTrajectory, which is totally different. Thus, the subsequent code gets messed up, because it can't do any of the usual postprocessing like setting Tags.

Someone needs to go in and convert the return of ExecuteTrajectory from ROS trajectories back to OpenRAVE trajectories. I'm not really sure how this code was originally intended to work in the first place.

[siddhss5]

@mkoval, comments on this? It looks like you were the last to touch this piece of code.

[mkoval]

The logic in that function is mostly correct. We correctly return a Future if defer=True and, otherwise, return the trajectory:

if defer:
    return traj_future
else:
    try:
        return traj_future.result(timeout)
    except TrajectoryExecutionFailed as e:
        logger.exception('Trajectory execution failed.')

The problem is that, like @psigen said, traj_future.result(timeout) returns a JointTrajectory message instead of an OpenRAVE Trajectory. Ideally, we would convert the result to an OpenRAVE Trajectory, restore the tag(s), and return it.

The quick fix, which is equivalent on ADA, is to simply return the OpenRAVE trajectory that we were passed:

if defer:
    return traj_future
else:
    try:
        traj_future.result(timeout)
        return traj
    except TrajectoryExecutionFailed as e:
        logger.exception('Trajectory execution failed.')

[sjavdani]

So this particular error was fixed #53 , however, I think we still aren't handling this properly when we error. If a trajectory throws an exception, we end up getting the same error about a traj description.