Skip to content

Commit

Permalink
Merge pull request #302 from personalrobotics/bugfix/pep8_util
Browse files Browse the repository at this point in the history 
PEP8 and lint fixes.
  • Loading branch information
@mkoval
mkoval committed Apr 25, 2016
2 parents 028684d + 16e7bf6 commit ecbf890
Show file tree
Hide file tree
Showing 5 changed files with 392 additions and 339 deletions.
25 changes: 12 additions & 13 deletions src/prpy/planning/snap.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,20 +101,19 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
ikreturn=False, releasegil=True
)

if ik_solution is None:
# FindIKSolutions is slower than FindIKSolution,
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)
ik_param, ikfo.IgnoreSelfCollisions,
ikreturn=False, releasegil=True)

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

Expand All @@ -125,8 +124,6 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
def _Snap(self, robot, goal, **kw_args):
from prpy.util import CheckJointLimits
from prpy.util import GetLinearCollisionCheckPts
#from prpy.util import SampleTimeGenerator
from prpy.util import VanDerCorputSampleGenerator
from prpy.planning.exceptions import CollisionPlanningError
from prpy.planning.exceptions import SelfCollisionPlanningError

Expand Down Expand Up @@ -166,12 +163,14 @@ def _Snap(self, robot, goal, **kw_args):
#
# Sampling function:
# 'linear'
#linear = SampleTimeGenerator
# from prpy.util import SampleTimeGenerator
# linear = SampleTimeGenerator
# 'Van der Corput'
from prpy.util import VanDerCorputSampleGenerator
vdc = VanDerCorputSampleGenerator
checks = GetLinearCollisionCheckPts(robot, \
traj, \
norm_order=2, \

checks = GetLinearCollisionCheckPts(robot, traj,
norm_order=2,
sampling_func=vdc)

# Run constraint checks at DOF resolution:
Expand Down
4 changes: 2 additions & 2 deletions src/prpy/planning/tsr.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ def PlanToIK(self, robot, goal_pose, **kw_args):
@ClonedPlanningMethod
def PlanToTSR(self, robot, tsrchains, tsr_timeout=2.0,
num_attempts=3, chunk_size=1, ranker=None,
max_deviation=2*numpy.pi, **kw_args):
max_deviation=2 * numpy.pi, **kw_args):
"""
Plan to a desired TSR set using a-priori goal sampling. This planner
samples a fixed number of goals from the specified TSRs up-front, then
Expand Down Expand Up @@ -145,7 +145,7 @@ def PlanToTSR(self, robot, tsrchains, tsr_timeout=2.0,
# Group the IK solutions into sets of the specified size
# (plan for each set of IK solutions together).
ranked_ik_solution_sets = [
ranked_ik_solutions[i:i+chunk_size, :]
ranked_ik_solutions[i:i + chunk_size, :]
for i in range(0, ranked_ik_solutions.shape[0], chunk_size)
]

Expand Down
86 changes: 41 additions & 45 deletions src/prpy/planning/vectorfield.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,9 @@ def vf_geodesic():

# Go as fast as possible
vlimits = robot.GetDOFVelocityLimits(robot.GetActiveDOFIndices())
return min(abs(vlimits[i] / dqout[i]) if dqout[i] != 0. else 1. for i in xrange(vlimits.shape[0])) * dqout
return min(abs(vlimits[i] / dqout[i])
if dqout[i] != 0. else 1.
for i in xrange(vlimits.shape[0])) * dqout

def CloseEnough():
"""
Expand All @@ -119,8 +121,7 @@ def CloseEnough():
the integration will terminate.
"""
pose_error = util.GetGeodesicDistanceBetweenTransforms(
manip.GetEndEffectorTransform(),
goal_pose)
manip.GetEndEffectorTransform(), goal_pose)
if pose_error < pose_error_tol:
return Status.TERMINATE
return Status.CONTINUE
Expand Down Expand Up @@ -195,7 +196,7 @@ def TerminateMove():
Succeed if distance moved is larger than max_distance.
Cache and continue if distance moved is larger than distance.
"""
from .exceptions import ConstraintViolationPlanningError
from .exceptions import ConstraintViolationPlanningError

Tnow = manip.GetEndEffectorTransform()

Expand All @@ -208,7 +209,7 @@ def TerminateMove():
'Deviated from orientation constraint.')
distance_moved = numpy.dot(position_error, direction)
position_deviation = numpy.linalg.norm(position_error -
distance_moved*direction)
distance_moved * direction)
if position_deviation > position_tolerance:
raise ConstraintViolationPlanningError(
'Deviated from straight line constraint.')
Expand All @@ -224,8 +225,7 @@ def TerminateMove():
return Status.CONTINUE

return self.FollowVectorField(robot, vf_straightline, TerminateMove,
integration_interval,
timelimit,
integration_interval, timelimit,
**kw_args)

@ClonedPlanningMethod
Expand Down Expand Up @@ -285,13 +285,12 @@ def PlanWorkspacePath(self, robot, traj,
traj = util.ComputeGeodesicUnitTiming(traj, env=None, alpha=1.0)

# Set the default gains
if Kp_ff == None:
Kp_ff = 0.4*numpy.array([1.0,1.0,1.0,1.0,1.0,1.0])
if Kp_e == None:
Kp_e = 1.0*numpy.array([1.0,1.0,1.0,1.0,1.0,1.0])
if Kp_ff is None:
Kp_ff = 0.4 * numpy.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
if Kp_e is None:
Kp_e = 1.0 * numpy.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])

manip = robot.GetActiveManipulator()
Tstart = manip.GetEndEffectorTransform()

# Get the final end-effector pose
duration = traj.GetDuration()
Expand All @@ -305,25 +304,25 @@ def vf_path():

# Find where we are on the goal trajectory by finding
# the the closest point
(_,t,_) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_actual,
traj,
0.0005)
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_actual, traj, 0.0005)

# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])

# Get the next end-effector transform, using finite-differences
pose_ee_next = traj.Sample(t+t_step)[0:7]
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)

# Get the translation tangent to current position
tangent_vec = desired_T_ee_next[0:3,3] - desired_T_ee[0:3,3]
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]
# Get the translational error
position_error_vec = desired_T_ee[0:3,3] - T_ee_actual[0:3,3]
position_error_vec = desired_T_ee[0:3, 3] - T_ee_actual[0:3, 3]
# Get the translational error perpendicular to the path
tangent_trans_error = position_error_vec - \
numpy.dot(position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)

# The twist between the actual end-effector position and
Expand All @@ -345,11 +344,11 @@ def vf_path():
twist_parallel[3:6] = util.NormalizeVector(twist_parallel[3:6])

# Apply gains
twist = Kp_e*twist_perpendicular + Kp_ff*twist_parallel
twist = Kp_e * twist_perpendicular + Kp_ff * twist_parallel

# Calculate joint velocities using an optimized jacobian
dqout, _ = util.ComputeJointVelocityFromTwist(robot, twist,
joint_velocity_limits=numpy.PINF)
dqout, _ = util.ComputeJointVelocityFromTwist(
robot, twist, joint_velocity_limits=numpy.PINF)
return dqout

def TerminateMove():
Expand All @@ -366,19 +365,18 @@ def TerminateMove():

# Find where we are on the goal trajectory by finding
# the the closest point
(_,t,_) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_curr,
traj,
0.0005)
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_curr, traj, 0.0005)

# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])

# Get the position vector tangent to the trajectory,
# using finite-differences
pose_ee_next = traj.Sample(t+t_step)[0:7]
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)
tangent_vec = desired_T_ee_next[0:3,3] - desired_T_ee[0:3,3]
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]

# Calculate error between current end-effector pose
# and where we should be on the goal trajectory
Expand All @@ -388,8 +386,9 @@ def TerminateMove():

# Use only the translation error that is perpendicular
# to our current position
tangent_trans_error = position_error_vec - \
numpy.dot(position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)

position_error = tangent_trans_error
Expand All @@ -405,10 +404,10 @@ def TerminateMove():

# Check if we have reached the end of the goal trajectory
error_to_goal = util.GeodesicError(T_ee_curr, T_ee_goal)
goal_orientation_error = error_to_goal[3] # radians
goal_position_error = error_to_goal[0:3] # x,y,z
if ((numpy.fabs(goal_orientation_error) < angular_tolerance) and
(numpy.linalg.norm(goal_position_error) < position_tolerance)):
orientation_error = error_to_goal[3] # radians
position_error = error_to_goal[0:3] # x,y,z
if ((numpy.fabs(orientation_error) < angular_tolerance) and
(numpy.linalg.norm(position_error) < position_tolerance)):
return Status.CACHE_AND_TERMINATE

return Status.CONTINUE
Expand Down Expand Up @@ -437,10 +436,9 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
from .exceptions import (
CollisionPlanningError,
SelfCollisionPlanningError,
TimeoutPlanningError
)
from openravepy import CollisionReport, RaveCreateTrajectory
from ..util import ComputeJointVelocityFromTwist, GetCollisionCheckPts, ComputeUnitTiming
from ..util import GetCollisionCheckPts
import time
import scipy.integrate

Expand All @@ -456,9 +454,6 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
env = robot.GetEnv()
active_indices = robot.GetActiveDOFIndices()

# Get the robot's joint velocity limits
qdot_limit = robot.GetDOFVelocityLimits(active_indices)

# Create a new trajectory matching the current
# robot's joint configuration specification
cspec = robot.GetActiveConfigurationSpecification('linear')
Expand Down Expand Up @@ -534,7 +529,8 @@ def fn_callback(t, q):
# in GetCollisionCheckPts causes this to enter an infinite
# loop.
checks = GetCollisionCheckPts(robot, path,
include_start=False) #start_time=nonlocals['t_check'])
include_start=False)
# start_time=nonlocals['t_check'])

for t_check, q_check in checks:
fn_status_callback(t_check, q_check)
Expand All @@ -543,10 +539,10 @@ def fn_callback(t, q):
# DOF resolution the next time the integrator takes a step.
nonlocals['t_check'] = t_check

return 0 # Keep going.
return 0 # Keep going.
except PlanningError as e:
nonlocals['exception'] = e
return -1 # Stop.
return -1 # Stop.

# Integrate the vector field to get a configuration space path.
#
Expand All @@ -567,7 +563,7 @@ def fn_callback(t, q):
integrator.integrate(t=integration_time_interval)

t_cache = nonlocals['t_cache']
exception = nonlocals['exception']
exception = nonlocals['exception']

if t_cache is None:
raise exception or PlanningError('An unknown error has occurred.')
Expand Down
Loading

0 comments on commit ecbf890

Please sign in to comment.