Skip to content

Commit 2443565

Browse files
committed
Merge branch 'master' of https://github.com/personalrobotics/prpy into bugfix/cloning_race
2 parents 482716e + 035d6f3 commit 2443565

File tree

2 files changed

+81
-10
lines changed

2 files changed

+81
-10
lines changed

src/prpy/base/robot.py

Lines changed: 39 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,7 @@
2929
# POSSIBILITY OF SUCH DAMAGE.
3030

3131
import functools, logging, openravepy, numpy
32-
import prpy.util
33-
from .. import bind, named_config, planning, util
32+
from .. import bind, named_config, exceptions, util
3433
from ..clone import Clone, Cloned
3534
from ..tsr.tsrlibrary import TSRLibrary
3635
from ..planning.base import Sequence, Tags
@@ -281,7 +280,7 @@ def do_postprocess():
281280
# Extract active DOFs from teh trajectory and set them as active.
282281
dof_indices, _ = cspec.ExtractUsedIndices(self)
283282

284-
if prpy.util.HasAffineDOFs(cspec):
283+
if util.HasAffineDOFs(cspec):
285284
affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
286285

287286
# Bug in OpenRAVE ExtractUsedIndices function makes
@@ -422,7 +421,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
422421
will be raised). If timeout is a float (including timeout = 0), this
423422
function will return None once the timeout has ellapsed, even if the
424423
trajectory is still being executed.
425-
424+
426425
NOTE: We suggest that you either use timeout=None or defer=True. If
427426
trajectory execution times out, there is no way to tell whether
428427
execution was successful or not. Other values of timeout are only
@@ -438,18 +437,48 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
438437
@param period poll rate, in seconds, for checking trajectory status
439438
@return trajectory executed on the robot
440439
"""
440+
# Don't execute trajectories that don't have at least one waypoint.
441+
if traj.GetNumWaypoints() <= 0:
442+
raise ValueError('Trajectory must contain at least one waypoint.')
443+
444+
# Check that the current configuration of the robot matches the
445+
# initial configuration specified by the trajectory.
446+
if not util.IsAtTrajectoryStart(self, traj):
447+
raise exceptions.TrajectoryAborted(
448+
'Trajectory started from different configuration than robot.')
449+
450+
# If there was only one waypoint, at this point we are done!
451+
if traj.GetNumWaypoints() == 1:
452+
if defer is True:
453+
import trollius
454+
future = trollius.Future()
455+
future.set_result(traj)
456+
return future
457+
else:
458+
return traj
441459

442-
# TODO: Verify that the trajectory is timed.
443-
# TODO: Check if this trajectory contains the base.
460+
# Verify that the trajectory is timed by checking whether the first
461+
# waypoint has a valid deltatime value.
462+
cspec = traj.GetConfigurationSpecification()
463+
if cspec.ExtractDeltaTime(traj.GetWaypoint(0)) is None:
464+
raise ValueError('Trajectory cannot be executed, it is not timed.')
465+
466+
# Verify that the trajectory has non-zero duration.
467+
if traj.GetDuration() <= 0.0:
468+
import warnings
469+
warnings.warn('Executing zero-length trajectory. Please update the'
470+
' function that produced this trajectory to return a'
471+
' single-waypoint trajectory.', FutureWarning)
444472

473+
# TODO: Check if this trajectory contains the base.
445474
needs_base = util.HasAffineDOFs(traj.GetConfigurationSpecification())
446475

447476
self.GetController().SetPath(traj)
448477

449478
active_manipulators = self.GetTrajectoryManipulators(traj)
450479
active_controllers = [
451-
active_manipulator.controller \
452-
for active_manipulator in active_manipulators \
480+
active_manipulator.controller
481+
for active_manipulator in active_manipulators
453482
if hasattr(active_manipulator, 'controller')
454483
]
455484

@@ -485,8 +514,8 @@ def do_poll():
485514
util.WaitForControllers(active_controllers, timeout=timeout)
486515
return traj
487516
else:
488-
raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
489-
517+
raise ValueError('Received unexpected value "{:s}" for defer.'
518+
.format(str(defer)))
490519

491520
def ViolatesVelocityLimits(self, traj):
492521
"""

src/prpy/util.py

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -693,3 +693,45 @@ def FindCatkinResource(package, relative_path):
693693
else:
694694
raise IOError('Loading resource "{:s}" failed.'.format(
695695
relative_path))
696+
697+
698+
def IsAtTrajectoryStart(robot, trajectory):
699+
"""
700+
Check if robot's DOFs match the start configuration of a trajectory.
701+
702+
This function examines the current DOF values of the specified robot and
703+
compares these values to the first waypoint of the specified trajectory.
704+
If every DOF value specified in the trajectory differs by less than the
705+
DOF resolution of the specified joint/axis then it will return True.
706+
Otherwise, it returns False.
707+
708+
@param robot: the robot whose active DOFs will be checked
709+
@param trajectory: the trajectory whose start configuration will be checked
710+
@returns: True if the robot's active DOFs match the given trajectory
711+
False if one or more active DOFs differ by DOF resolution
712+
"""
713+
# Get used indices and starting configuration from trajectory.
714+
cspec = trajectory.GetConfigurationSpecification()
715+
dof_indices, _ = cspec.ExtractUsedIndices(robot)
716+
traj_values = cspec.ExtractJointValues(
717+
trajectory.GetWaypoint(0), robot, dof_indices)
718+
719+
# Get current configuration of robot for used indices.
720+
with robot.GetEnv():
721+
robot_values = robot.GetDOFValues(dof_indices)
722+
dof_resolutions = robot.GetDOFResolutions(dof_indices)
723+
724+
# Check deviation in each DOF, using OpenRAVE's SubtractValue function.
725+
dof_infos = zip(dof_indices, traj_values, robot_values, dof_resolutions)
726+
for dof_index, traj_value, robot_value, dof_resolution in dof_infos:
727+
# Look up the Joint and Axis of the DOF from the robot.
728+
joint = robot.GetJointFromDOFIndex(dof_index)
729+
axis = dof_index - joint.GetDOFIndex()
730+
731+
# If any joint deviates too much, return False.
732+
delta_value = abs(joint.SubtractValue(traj_value, robot_value, axis))
733+
if delta_value > dof_resolution:
734+
return False
735+
736+
# If all joints match, return True.
737+
return True

0 commit comments

Comments
 (0)