Merge pull request #276 from personalrobotics/bugfix/planning_pep8

Added PEP8 fixes and lint corrections for `prpy.planning`.

Author: mkoval

src/prpy/planning/cbirrt.py

@@ -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
@@ -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):
@@ -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],
@@ -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,
@@ -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:
@@ -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
@@ -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:
@@ -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:
@@ -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(),
@@ -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.
@@ -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'):

src/prpy/planning/ompl.py

@@ -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__)
 
@@ -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)
@@ -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
@@ -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)
 
@@ -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)
 
@@ -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)
@@ -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

src/prpy/planning/openrave.py

@@ -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():
@@ -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