Skip to content

Commit

Permalink
Merge pull request #276 from personalrobotics/bugfix/planning_pep8
Browse files Browse the repository at this point in the history 
Added PEP8 fixes and lint corrections for `prpy.planning`.
  • Loading branch information
@mkoval
mkoval committed Mar 22, 2016
2 parents 1d77c53 + f4bb808 commit 62cec7a
Show file tree
Hide file tree
Showing 3 changed files with 80 additions and 74 deletions.
89 changes: 47 additions & 42 deletions src/prpy/planning/cbirrt.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
# - Neither the name of Carnegie Mellon University nor the names of its
# contributors may be used to endorse or promote products derived from this
# software without specific prior written permission.
#
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
Expand All @@ -28,11 +28,13 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import numpy, openravepy
import numpy
import openravepy
from ..util import SetTrajectoryTags
from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
PlanningMethod, Tags)
import prpy.kin, prpy.tsr
import prpy.kin
import prpy.tsr


class CBiRRTPlanner(BasePlanner):
Expand Down Expand Up @@ -112,21 +114,21 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
H_world_ee = manip.GetEndEffectorTransform()

# 'object frame w' is at ee, z pointed along direction to move
H_world_w = prpy.kin.H_from_op_diff(H_world_ee[0:3,3], direction)
H_world_w = prpy.kin.H_from_op_diff(H_world_ee[0:3, 3], direction)
H_w_ee = numpy.dot(prpy.kin.invert_H(H_world_w), H_world_ee)

# Serialize TSR string (goal)
Hw_end = numpy.eye(4)
Hw_end[2,3] = distance

goaltsr = prpy.tsr.tsr.TSR(T0_w = numpy.dot(H_world_w,Hw_end),
Tw_e = H_w_ee,
Bw = numpy.zeros((6,2)),
manip = robot.GetActiveManipulatorIndex())
goal_tsr_chain = prpy.tsr.tsr.TSRChain(sample_goal = True,
TSRs = [goaltsr])
Hw_end[2, 3] = distance

goaltsr = prpy.tsr.tsr.TSR(T0_w=numpy.dot(H_world_w, Hw_end),
Tw_e=H_w_ee,
Bw=numpy.zeros((6, 2)),
manip=robot.GetActiveManipulatorIndex())
goal_tsr_chain = prpy.tsr.tsr.TSRChain(sample_goal=True,
TSRs=[goaltsr])
# Serialize TSR string (whole-trajectory constraint)
Bw = numpy.zeros((6,2))
Bw = numpy.zeros((6, 2))
epsilon = 0.001
Bw = numpy.array([[-epsilon, epsilon],
[-epsilon, epsilon],
Expand All @@ -135,15 +137,16 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
[-epsilon, epsilon],
[-epsilon, epsilon]])

trajtsr = prpy.tsr.tsr.TSR(T0_w = H_world_w,
Tw_e = H_w_ee,
Bw = Bw,
manip = robot.GetActiveManipulatorIndex())
traj_tsr_chain = prpy.tsr.tsr.TSRChain(constrain=True, TSRs=[trajtsr])
traj_tsr = prpy.tsr.tsr.TSR(
T0_w=H_world_w, Tw_e=H_w_ee, Bw=Bw,
manip=robot.GetActiveManipulatorIndex())
traj_tsr_chain = prpy.tsr.tsr.TSRChain(constrain=True,
TSRs=[traj_tsr])

kw_args.setdefault('psample', 0.1)

return self.Plan(robot,
return self.Plan(
robot,
tsr_chains=[goal_tsr_chain, traj_tsr_chain],
# Smooth since this is a constrained trajectory.
smoothingitrs=smoothingitrs,
Expand All @@ -162,7 +165,6 @@ def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
@param smoothingitrs number of smoothing iterations to run
@return traj output path
"""
psample = None
is_constrained = False

for chain in tsr_chains:
Expand All @@ -176,7 +178,8 @@ def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
if not is_constrained:
smoothingitrs = 0

return self.Plan(robot,
return self.Plan(
robot,
smoothingitrs=smoothingitrs,
tsr_chains=tsr_chains,
**kw_args
Expand All @@ -187,41 +190,42 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
extra_args=None, **kw_args):
from openravepy import CollisionOptions, CollisionOptionsStateSaver

# TODO We may need this work-around because CBiRRT doesn't like it when
# an IK solver other than GeneralIK is loaded (e.g. nlopt_ik).
#self.ClearIkSolver(robot.GetActiveManipulator())
# TODO We may need this work-around because CBiRRT doesn't like it
# when an IK solver other than GeneralIK is loaded (e.g. nlopt_ik).
# self.ClearIkSolver(robot.GetActiveManipulator())

self.env.LoadProblem(self.problem, robot.GetName())

args = [ 'RunCBiRRT' ]
args = ['RunCBiRRT']

if extra_args is not None:
args += extra_args

if smoothingitrs is not None:
if smoothingitrs < 0:
raise ValueError('Invalid number of smoothing iterations. Value'
'must be non-negative; got {:d}.'.format(
smoothingitrs))
raise ValueError('Invalid number of smoothing iterations. '
'Value must be non-negative; got {:d}.'
.format(smoothingitrs))

args += [ 'smoothingitrs', str(smoothingitrs) ]
args += ['smoothingitrs', str(smoothingitrs)]

if timelimit is not None:
if not (timelimit > 0):
raise ValueError('Invalid value for "timelimit". Limit must be'
' non-negative; got {:f}.'.format(timelimit))

args += [ 'timelimit', str(timelimit) ]
args += ['timelimit', str(timelimit)]

if allowlimadj is not None:
args += [ 'allowlimadj', str(int(allowlimadj)) ]
args += ['allowlimadj', str(int(allowlimadj))]

if psample is not None:
if not (0 <= psample <= 1):
raise ValueError('Invalid value for "psample". Value must be in'
' the range [0, 1]; got {:f}.'.format(psample))
raise ValueError('Invalid value for "psample". Value must be '
'in the range [0, 1]; got {:f}.'
.format(psample))

args += [ 'psample', str(psample) ]
args += ['psample', str(psample)]

if jointstarts is not None:
for start_config in jointstarts:
Expand All @@ -233,7 +237,8 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
)
)

args += ['jointstarts'] + self.serialize_dof_values(start_config)
args += (['jointstarts'] +
self.serialize_dof_values(start_config))

if jointgoals is not None:
for goal_config in jointgoals:
Expand All @@ -254,7 +259,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
# FIXME: Why can't we write to anything other than cmovetraj.txt or
# /tmp/cmovetraj.txt with CBiRRT?
traj_path = 'cmovetraj.txt'
args += [ 'filename', traj_path ]
args += ['filename', traj_path]
args_str = ' '.join(args)

with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
Expand All @@ -267,12 +272,13 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
# Construct the output trajectory.
with open(traj_path, 'rb') as traj_file:
traj_xml = traj_file.read()
traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory')
traj = openravepy.RaveCreateTrajectory(
self.env, 'GenericTrajectory')
traj.deserialize(traj_xml)

# Tag the trajectory as constrained if a constraint TSR is present.
if (tsr_chains is not None
and any(tsr_chain.constrain for tsr_chain in tsr_chains)):
if (tsr_chains is not None and
any(tsr_chain.constrain for tsr_chain in tsr_chains)):
SetTrajectoryTags(traj, {Tags.CONSTRAINED: True}, append=True)

# Strip extraneous groups from the output trajectory.
Expand All @@ -289,11 +295,10 @@ def ClearIkSolver(self, manip):
if manip.GetIkSolver() is not None:
raise ValueError('Unable to clear IkSolver')


@staticmethod
def serialize_dof_values(dof_values):
return [ str(len(dof_values)),
' '.join([ str(x) for x in dof_values]) ]
return [str(len(dof_values)),
' '.join([str(x) for x in dof_values])]


def SerializeTransform12Col(tm, format='%.5f'):
Expand Down
57 changes: 28 additions & 29 deletions src/prpy/planning/ompl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,14 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import logging, numpy, openravepy, os, tempfile
import logging
import numpy
import openravepy
from ..util import CopyTrajectory, SetTrajectoryTags
from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
PlanningMethod, Tags)
from openravepy import PlannerStatus
from .cbirrt import SerializeTSRChain

logger = logging.getLogger(__name__)

Expand All @@ -49,7 +52,8 @@ def __init__(self, algorithm='RRTConnect'):

if self.planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner. Is or_ompl installed?'.format(planner_name))
'Unable to create "{:s}" planner. Is or_ompl installed?'
.format(planner_name))

def __str__(self):
return 'OMPL {0:s}'.format(self.algorithm)
Expand All @@ -69,20 +73,21 @@ def PlanToConfiguration(self, robot, goal, **kw_args):
def PlanToTSR(self, robot, tsrchains, **kw_args):
"""
Plan using the given set of TSR chains with OMPL.
@param robot
@param robot
@param tsrchains A list of tsrchains to use during planning
@param return traj
"""
return self._TSRPlan(robot, tsrchains, **kw_args)

def _Plan(self, robot, goal=None, timeout=30., shortcut_timeout=5.,
continue_planner=False, ompl_args=None,
continue_planner=False, ompl_args=None,
formatted_extra_params=None, **kw_args):
extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)

if ompl_args is not None:
for key,value in ompl_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key), v=str(value))
for key, value in ompl_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k=str(key), v=str(value))

if formatted_extra_params is not None:
extraParams += formatted_extra_params
Expand All @@ -95,42 +100,38 @@ def _Plan(self, robot, goal=None, timeout=30., shortcut_timeout=5.,

traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory')

# Plan. We manually lock and unlock the environment because the context
# manager is currently broken on cloned environments.
self.env.Lock()
try:
# Plan.
with self.env:
if (not continue_planner) or (not self.setup):
self.planner.InitPlan(robot, params)
self.setup = True

status = self.planner.PlanPath(traj, releasegil=True)
if status not in [ PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution ]:
raise PlanningError('Planner returned with status {0:s}.'.format(
str(status)))
finally:
self.env.Unlock()
if status not in [PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution]:
raise PlanningError('Planner returned with status {0:s}.'
.format(str(status)))

return traj

def _TSRPlan(self, robot, tsrchains, **kw_args):

extraParams = ''
for chain in tsrchains:
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k = 'tsr_chain', v=chain.serialize())

extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k='tsr_chain', v=SerializeTSRChain(chain))

return self._Plan(robot, formatted_extra_params=extraParams, **kw_args)


class RRTConnect(OMPLPlanner):
def __init__(self):
OMPLPlanner.__init__(self, algorithm='RRTConnect')

def _SetPlannerRange(robot, ompl_args=None):
def _SetPlannerRange(self, robot, ompl_args=None):
from copy import deepcopy

if ompl_args is None:
ompl_args = {}
ompl_args = dict()
else:
ompl_args = deepcopy(ompl_args)

Expand Down Expand Up @@ -167,20 +168,18 @@ def PlanToConfiguration(self, robot, goal, ompl_args=None, **kw_args):
@param goal desired configuration
@return traj
"""

ompl_args = self._SetPlannerRange(robot, ompl_args=ompl_args)
return self._Plan(robot, goal=goal, ompl_args=ompl_args, **kw_args)

@PlanningMethod
def PlanToTSR(self, robot, tsrchains, ompl_args=None, **kw_args):
"""
Plan using the given TSR chains with OMPL.
Plan using the given TSR chains with OMPL.
@param robot
@param tsrchains A list of TSRChain objects to respect during planning
@param ompl_args ompl RRTConnect specific parameters
@return traj
"""

ompl_args = self._SetPlannerRange(robot, ompl_args=ompl_args)
return self._TSRPlan(robot, tsrchains, ompl_args=ompl_args, **kw_args)

Expand All @@ -206,7 +205,7 @@ def ShortcutPath(self, robot, path, timeout=1., **kwargs):
output_path = CopyTrajectory(path, env=self.env)

extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)

params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetExtraParameters(extraParams)
Expand All @@ -216,10 +215,10 @@ def ShortcutPath(self, robot, path, timeout=1., **kwargs):
# issue that needs to be fixed in or_ompl.
self.planner.InitPlan(robot, params)
status = self.planner.PlanPath(output_path, releasegil=True)
if status not in [ PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution ]:
raise PlanningError('Simplifier returned with status {0:s}.'.format(
str(status)))
if status not in [PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution]:
raise PlanningError('Simplifier returned with status {0:s}.'
.format(str(status)))

SetTrajectoryTags(output_path, {Tags.SMOOTH: True}, append=True)
return output_path
8 changes: 5 additions & 3 deletions src/prpy/planning/openrave.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,12 @@ def _Plan(self, robot, goals, maxiter=500, continue_planner=False,
or_args=None, **kw_args):

# Get rid of default postprocessing
extraParams = '<_postprocessing planner=""><_nmaxiterations>0</_nmaxiterations></_postprocessing>'
extraParams = ('<_postprocessing planner="">'
'<_nmaxiterations>0</_nmaxiterations>'
'</_postprocessing>')
# Maximum planner iterations
extraParams += '<_nmaxiterations>{:d}</_nmaxiterations>'.format(maxiter)
extraParams += ('<_nmaxiterations>{:d}</_nmaxiterations>'
.format(maxiter))

if or_args is not None:
for key, value in or_args.iteritems():
Expand Down Expand Up @@ -122,7 +125,6 @@ def PlanToConfiguration(self, robot, goal, **kw_args):
@param goal the desired robot joint configuration
@return traj a trajectory from current configuration to specified goal
"""

return self._Plan(robot, goal, **kw_args)

@PlanningMethod
Expand Down

0 comments on commit 62cec7a

Please sign in to comment.