Skip to content

Commit 6d4ba59

Browse files
committed
Added separate exception for trajectory precondition violations.
1 parent 372db4f commit 6d4ba59

File tree

2 files changed

+24
-5
lines changed

2 files changed

+24
-5
lines changed

src/prpy/base/robot.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -205,8 +205,8 @@ def PostProcessPath(self, path, defer=False, executor=None,
205205
curve through the waypoints (not implemented). If this curve is
206206
not collision free, then we fall back on...
207207
4. By default, we run a smoother that jointly times and smooths the
208-
path via self.smoother. This algorithm can change the geometric path to
209-
optimize runtime.
208+
path via self.smoother. This algorithm can change the geometric path
209+
to optimize runtime.
210210
211211
The behavior in (2) and (3) can be forced by passing constrained=True
212212
or smooth=True. By default, the case is inferred by the tag(s) attached
@@ -411,8 +411,8 @@ def do_execute():
411411
else:
412412
raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
413413

414-
415-
def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwargs):
414+
def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
415+
**kwargs):
416416
""" Executes a time trajectory on the robot.
417417
418418
This function directly executes a timed OpenRAVE trajectory on the
@@ -450,7 +450,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
450450
# Check that the current configuration of the robot matches the
451451
# initial configuration specified by the trajectory.
452452
if not util.IsAtTrajectoryStart(self, traj):
453-
raise exceptions.TrajectoryAborted(
453+
raise exceptions.TrajectoryNotExecutable(
454454
'Trajectory started from different configuration than robot.')
455455

456456
# If there was only one waypoint, at this point we are done!

src/prpy/exceptions.py

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,26 +3,44 @@ class PrPyException(Exception):
33
Generic PrPy exception.
44
"""
55

6+
7+
class TrajectoryNotExecutable(PrPyException):
8+
"""
9+
Trajectory could not begin execution.
10+
11+
This exception typically indicates that some precondition of trajectory
12+
execution was violated, such as the robot starting at a different
13+
configuration, the trajectory not being in the correct format.
14+
15+
This exception indicates that the trajectory was not even attempted due to
16+
one of these conditions.
17+
"""
18+
19+
620
class TrajectoryAborted(PrPyException):
721
"""
822
Trajectory was aborted.
923
"""
1024

25+
1126
class TrajectoryStalled(PrPyException):
1227
"""
1328
Trajectory stalled.
1429
"""
1530

31+
1632
class SynchronizationException(PrPyException):
1733
"""
1834
Controller synchronization failed.
1935
"""
2036

37+
2138
class SerializationException(PrPyException):
2239
"""
2340
Serialization failed.
2441
"""
2542

43+
2644
class UnsupportedTypeSerializationException(SerializationException):
2745
"""
2846
Serialization failed due to an unknown type.
@@ -35,6 +53,7 @@ def __init__(self, value):
3553
'Serializing type "{:s}.{:s}" is not supported.'.format(
3654
self.type.__module__, self.type.__name__))
3755

56+
3857
class UnsupportedTypeDeserializationException(SerializationException):
3958
"""
4059
Deserialization failed due to an unknown type.

0 commit comments

Comments
 (0)