def ExecuteTrajectory(self, traj, defer=False, timeout=None, switch=True,
unswitch=None, **kwargs):
""" Executes a time trajectory on the robot.
This function directly executes a timed OpenRAVE trajectory on the
robot. If you have a geometric path, such as those returned by a
geometric motion planner, you should first time the path using
PostProcessPath. Alternatively, you could use the ExecutePath helper
function to time and execute the path in one function call.
If timeout = None (the default), this function does not return until
execution has finished. Termination occurs if the trajectory is
successfully executed or if a fault occurs (in this case, an exception
will be raised). If timeout is a float (including timeout = 0), this
function will return None once the timeout has ellapsed, even if the
trajectory is still being executed.
NOTE: We suggest that you either use timeout=None or defer=True. If
trajectory execution times out, there is no way to tell whether
execution was successful or not. Other values of timeout are only
supported for legacy reasons.
This function returns the trajectory that was actually executed on the
robot, including controller error. If this is not available, the input
trajectory will be returned instead.
If switch = True, this function switches to the ros_control controllers
necessary to execute traj. If unswitch = True, it also undoes this
switch after the trajectory has finished executing. If unswitch is
unspecified, it defaults to the same value as switch.
@param traj: timed trajectory to execute
@type traj: openravepy.Trajectory
@param defer: execute asynchronously and return a trajectory Future
@type defer: bool
@param timeout: maximum time to wait for execution to finish
@type timeout: float or None
@param switch: switch to the controllers necessary to execute traj
@type switch: bool
@param unswitch: revert the controllers after executing traj
@type unswitch: bool or None
@return trajectory executed on the robot
@rtype openravepy.Trajectory or TrajectoryFuture
"""
from .util import or_to_ros_trajectory, pad_ros_trajectory
from rospy import Time
from prpy import exceptions
if self.simulated:
return Robot.ExecuteTrajectory(self, traj, defer=defer, timeout=timeout,
**kwargs)
# Check that the current configuration of the robot matches the
# initial configuration specified by the trajectory.
# if not prpy.util.IsAtTrajectoryStart(self, traj):
# raise exceptions.TrajectoryNotExecutable(
# 'Trajectory started from different configuration than robot.')
# If there was only one waypoint, at this point we are done!
if traj.GetNumWaypoints() == 1:
return traj
if unswitch is None:
unswitch = switch
traj_msg = or_to_ros_trajectory(self, traj)
# sometimes points are removed from message
# ensure this trajectory is useful to send before continuing
if len(traj_msg.points) < 1:
return traj
# The trajectory_controller expects the full set of DOFs to be
# specified in every trajectory. We pad the trajectory by adding
# constant values for any missing joints.
#print traj_msg
#print "\n\n\n\n\n\n"
#print traj.serialize()
#from IPython import embed
#embed()
with self.GetEnv():
pad_ros_trajectory(self, traj_msg, self._trajectory_joint_names)
if switch:
self._trajectory_switcher.switch()
traj_future = self._trajectory_client.execute(traj_msg)
if unswitch:
traj_future.add_done_callback(
lambda _: self._trajectory_switcher.unswitch
)
#for i in range(0,len(traj_msg.points)):
# print traj_msg.points[i].positions
# print '\n'
from trajectory_client import TrajectoryExecutionFailed
if defer:
return traj_future
else:
try:
traj_future.result(timeout)
return traj
except TrajectoryExecutionFailed as e:
logger.exception('Trajectory execution failed.')
