Skip to content

Commit

Permalink
Modified SnapPlanner and GreedyIKPlanner to raise collision-specific …
Browse files Browse the repository at this point in the history
…errors. Added tests for collision-specific errors in PlantoConfiguration and PlanToEndEffectorOffset
  • Loading branch information
Gilwoo Lee committed Oct 19, 2015
1 parent 6eaabad commit 8ce67f7
Show file tree
Hide file tree
Showing 7 changed files with 145 additions and 12 deletions.
74 changes: 65 additions & 9 deletions src/prpy/planning/snap.py
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,10 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
@param goal_pose desired end-effector pose
@return traj
"""

from .exceptions import CollisionPlanningError,SelfCollisionPlanningError
from openravepy import CollisionReport

ikp = openravepy.IkParameterizationType
ikfo = openravepy.IkFilterOptions

Expand All @@ -92,13 +96,33 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
ik_param, ikfo.CheckEnvCollisions,
ikreturn=False, releasegil=True
)

if ik_solution is None:

if ik_solution is None:
# FindIKSolutions is slower than FindIKSolution,
# so call this only to identify and raise error when there is no solution
ik_solutions = manipulator.FindIKSolutions(
ik_param,
ikfo.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True
)

for q in ik_solutions:
robot.SetActiveDOFValues(q)
report = CollisionReport()
if self.env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)

raise PlanningError('There is no IK solution at the goal pose.')

return self._Snap(robot, ik_solution, **kw_args)

def _Snap(self, robot, goal, **kw_args):
from .exceptions import CollisionPlanningError,SelfCollisionPlanningError,JointLimitError
from openravepy import CollisionReport

Closed = openravepy.Interval.Closed

start = robot.GetActiveDOFValues()
Expand All @@ -110,13 +134,45 @@ def _Snap(self, robot, goal, **kw_args):
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(goal)
check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed)

# The function returns a bitmask of ConstraintFilterOptions flags,
# indicating which constraints are violated. We'll abort if any
# constraints are violated.
if check != 0:
raise PlanningError('Straight line trajectory is not valid.')
check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed,
options = 15, returnconfigurations = True)

# If valid, above function returns (0,None, None, 0)
# if not, it returns (1, invalid_config ,[], time_when_invalid),
# in which case we identify and raise appropriate error.
if check[0] != 0:
q = check[1]

# Check for joint limit violation
q_limit_min, q_limit_max = robot.GetActiveDOFLimits()

lower_position_violations = (q < q_limit_min)
if lower_position_violations.any():
index = lower_position_violations.nonzero()[0][0]
raise JointLimitError(robot,
dof_index=active_indices[index],
dof_value=q[index],
dof_limit=q_limit_min[index],
description='position')

upper_position_violations = (q> q_limit_max)
if upper_position_violations.any():
index = upper_position_violations.nonzero()[0][0]
raise JointLimitError(robot,
dof_index=active_indices[index],
dof_value=q[index],
dof_limit=q_limit_max[index],
description='position')

# Check for collision
robot.SetActiveDOFValues(q)
report = CollisionReport()
if self.env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)

raise PlanningError('Straight line trajectory is not valid.' )

# Create a two-point trajectory that starts at our current
# configuration and takes us to the goal.
Expand Down
37 changes: 34 additions & 3 deletions src/prpy/planning/workspace.py
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,14 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
@param timelimit timeout in seconds
@return qtraj configuration space path
"""
from .exceptions import TimeoutPlanningError
from .exceptions import (
TimeoutPlanningError,
CollisionPlanningError,
SelfCollisionPlanningError,
JointLimitError)
from openravepy import CollisionReport
from numpy import linalg as LA


with robot:
manip = robot.GetActiveManipulator()
Expand All @@ -171,17 +178,20 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,

# Smallest CSpace step at which to give up
min_step = min(robot.GetActiveDOFResolutions())/100.
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions

ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6


try:
collision_error = None
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
current_time = time.time()
if (timelimit is not None and
current_time - start_time > timelimit):
if collision_error is not None:
raise collision_error
raise TimeoutPlanningError(timelimit)

# Hypothesize new configuration as closest IK to current
Expand All @@ -193,12 +203,33 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
releasegil=True
)

# FindIKSolutions is slower than FindIKSolution,
# so call this only to identify error when there is no solution
if qnew is None:
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(traj.Sample(t+dt)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True
)

# update collision_error to contain collision info.
for q in ik_solutions:
robot.SetActiveDOFValues(q)
report = CollisionReport()
if self.env.CheckCollision(robot, report=report):
collision_error = CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
collision_error = SelfCollisionPlanningError.FromReport(report)

# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
step = abs(qnew - qcurr)
if (max(step) < min_step) and qtraj:
if collision_error is not None:
raise collision_error
raise PlanningError('Not making progress.')
infeasible_step = any(step > robot.GetActiveDOFResolutions())
if infeasible_step:
Expand Down
18 changes: 18 additions & 0 deletions tests/planning/methods/PlanToConfiguration.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,9 @@
from prpy.clone import CloneException
from prpy.planning.base import PlanningError
from prpy.planning.exceptions import (
CollisionPlanningError,
SelfCollisionPlanningError,
JointLimitError)

class PlanToConfigurationTest(object):
def test_PlanToConfiguration_StartInCollision_Throws(self):
Expand Down Expand Up @@ -80,3 +84,17 @@ def test_PlanToConfiguration_GoalIsFeasible_FindsSolution(self):
self.config_feasible_start, self.config_feasible_goal)


class PlanToConfigurationTestCollisionTest(object):
""" Expects collision-specific error"""

def test_PlanToConfiguration_GoalInCollision_Throws_Collision(self):
# Setup
with self.env:
self.robot.SetActiveDOFValues(self.config_feasible_start)

# Test/Assert
with self.assertRaises((CollisionPlanningError,
SelfCollisionPlanningError,
JointLimitError)):
self.planner.PlanToConfiguration(
self.robot, self.config_env_collision)
22 changes: 22 additions & 0 deletions tests/planning/methods/PlanToEndEffectorOffset.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
import numpy
from prpy.clone import CloneException
from prpy.planning.base import PlanningError
from prpy.planning.exceptions import (
CollisionPlanningError,
SelfCollisionPlanningError,
JointLimitError)


class PlanToEndEffectorOffsetTest(object):
Expand Down Expand Up @@ -106,3 +110,21 @@ def test_PlanToEndEffectorOffset_GoalInCollision_Throws(self):
self.planner.PlanToEndEffectorOffset(
self.robot, direction=numpy.array([0., 0., -1.]),
distance=0.1)

class PlanToEndEffectorOffsetCollisionTest(object):
""" Expects collision-specific error"""

def test_PlanToEndEffectorOffset_StartInCollision_Throws_CollisionError(self):
# Setup
with self.env:
self.robot.SetActiveDOFValues(self.config_feasible_goal)
goal_ik = self.manipulator.GetEndEffectorTransform()

self.robot.SetActiveDOFValues(self.config_env_collision)

# Test/Assert
with self.assertRaises((CollisionPlanningError,
SelfCollisionPlanningError,
JointLimitError)):
self.planner.PlanToEndEffectorOffset(
self.robot, direction=self.direction, distance=self.distance)
2 changes: 2 additions & 0 deletions tests/planning/test_GreedyIKPlanner.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,12 @@
from methods import PlanToEndEffectorOffsetTest
from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest
from planning_helpers import BasePlannerTest
from prpy.planning.workspace import GreedyIKPlanner
from unittest import TestCase


class GreedyIKPlannerTest(BasePlannerTest,
PlanToEndEffectorOffsetTest,
PlanToEndEffectorOffsetCollisionTest,
TestCase):
planner_factory = GreedyIKPlanner
2 changes: 2 additions & 0 deletions tests/planning/test_SnapPlanner.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
PlanToConfigurationTest,
PlanToConfigurationStraightLineTest,
)
from methods.PlanToConfiguration import PlanToConfigurationTestCollisionTest
from planning_helpers import BasePlannerTest
from prpy.planning.snap import SnapPlanner
from unittest import TestCase
Expand All @@ -10,6 +11,7 @@
class SnapPlannerTest(BasePlannerTest,
PlanToConfigurationTest,
PlanToConfigurationStraightLineTest,
PlanToConfigurationTestCollisionTest,
TestCase):
planner_factory = SnapPlanner

Expand Down
2 changes: 2 additions & 0 deletions tests/planning/test_VectorFieldPlanner.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,12 @@
from methods import PlanToEndEffectorOffsetTest
from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest
from planning_helpers import BasePlannerTest
from prpy.planning.vectorfield import VectorFieldPlanner
from unittest import TestCase


class VectorFieldPlannerTest(BasePlannerTest,
PlanToEndEffectorOffsetTest,
PlanToEndEffectorOffsetCollisionTest,
TestCase):
planner_factory = VectorFieldPlanner

0 comments on commit 8ce67f7

Please sign in to comment.