[Python] Format

.yapfignore

+script
+profile
+affordance-tests

setup.cfg

+[flake8]
+max-line-length = 119
+exclude = cmake,script,profile,affordance-tests
+
+[yapf]
+column_limit = 119
+
+[isort]
+line_length = 119
+skip = cmake

src/hpp/corbaserver/rbprm/__init__.py

-from .client import Client
+from .client import Client  # noqa

src/hpp/corbaserver/rbprm/client.py

@@ -20,26 +20,27 @@
 from hpp.corbaserver.client import Client as _Parent
 from hpp_idl.hpp.corbaserver.rbprm import RbprmBuilder
 
-class Client (_Parent):
-  """
+
+class Client(_Parent):
+    """
   Connect and create clients for hpp-rbprm library.
   """
 
-  defaultClients = {
-          'rbprmbuilder'  : RbprmBuilder,
-          }
+    defaultClients = {
+        'rbprmbuilder': RbprmBuilder,
+    }
 
-  def __init__(self, url = None, context = "corbaserver"):
-    """
+    def __init__(self, url=None, context="corbaserver"):
+        """
     Initialize CORBA and create default clients.
     :param url: URL in the IOR, corbaloc, corbalocs, and corbanames formats.
                 For a remote corba server, use
                 url = "corbaloc:iiop:<host>:<port>/NameService"
     """
-    self._initOrb (url)
-    self._makeClients ("rbprm", self.defaultClients, context)
+        self._initOrb(url)
+        self._makeClients("rbprm", self.defaultClients, context)
 
-    # self.rbprmbuilder is created by self._makeClients
-    # The old code stored the object as self.rbprm
-    # Make it backward compatible.
-    self.rbprm = self.rbprmbuilder
+        # self.rbprmbuilder is created by self._makeClients
+        # The old code stored the object as self.rbprm
+        # Make it backward compatible.
+        self.rbprm = self.rbprmbuilder

src/hpp/corbaserver/rbprm/fewstepsplanner.py

@@ -16,100 +16,133 @@
 # hpp-manipulation-corba.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-from hpp.corbaserver.rbprm import Client as RbprmClient
-from hpp.corbaserver import Client as BasicClient
-#from hpp.corbaserver.rbprm.tools.com_constraints import *
-from numpy import array
-
-from hpp.corbaserver.rbprm import rbprmstate
 from hpp.corbaserver.rbprm.rbprmstate import State
 
-def _interpolateState(ps, fullBody, stepsize, pathId, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = True):
-  if(filterStates):
+# from hpp.corbaserver.rbprm.tools.com_constraints import *
+
+
+def _interpolateState(ps,
+                      fullBody,
+                      stepsize,
+                      pathId,
+                      robustnessTreshold=0,
+                      filterStates=False,
+                      testReachability=True,
+                      quasiStatic=False,
+                      erasePreviousStates=True):
+    if (filterStates):
         filt = 1
-  else:
+    else:
         filt = 0
-        
-  #discretize path
-  totalLength = ps.pathLength(pathId)
-  configsPlan = []; step = 0.
-  configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId,0.))
-  z = [0. for _ in range(configSize) ]
-  while step < totalLength:
-    configsPlan += [ps.configAtParam(pathId,step) + z[:]]
-    step += stepsize
-  configsPlan += [ps.configAtParam(pathId,totalLength)+ z[:]]
-    
-  configs = fullBody.clientRbprm.rbprm.interpolateConfigs(configsPlan, robustnessTreshold, filt, testReachability, quasiStatic, erasePreviousStates)
-  firstStateId = fullBody.clientRbprm.rbprm.getNumStates() - len(configs)
-  return [ State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs)) ], configs
-          
+
+    # discretize path
+    totalLength = ps.pathLength(pathId)
+    configsPlan = []
+    step = 0.
+    configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId, 0.))
+    z = [0. for _ in range(configSize)]
+    while step < totalLength:
+        configsPlan += [ps.configAtParam(pathId, step) + z[:]]
+        step += stepsize
+    configsPlan += [ps.configAtParam(pathId, totalLength) + z[:]]
+
+    configs = fullBody.clientRbprm.rbprm.interpolateConfigs(configsPlan, robustnessTreshold, filt, testReachability,
+                                                            quasiStatic, erasePreviousStates)
+    firstStateId = fullBody.clientRbprm.rbprm.getNumStates() - len(configs)
+    return [State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs))], configs
+
 
 def _guidePath(problemSolver, fromPos, toPos):
-  ps = problemSolver
-  ps.setInitialConfig (fromPos)
-  ps.resetGoalConfigs()
-  ps.addGoalConfig(toPos)
-  ps.solve ()
-  return ps.numberPaths() - 1
-  
+    ps = problemSolver
+    ps.setInitialConfig(fromPos)
+    ps.resetGoalConfigs()
+    ps.addGoalConfig(toPos)
+    ps.solve()
+    return ps.numberPaths() - 1
+
 
 class FewStepPlanner(object):
-  def __init__ (self, client, problemSolver, rbprmBuilder, fullBody, planContext="rbprm_path", fullBodyContext="default", pathPlayer = None, viewer = None ):
-    self.fullBody =  fullBody
-    self.rbprmBuilder =  rbprmBuilder
-    self.client   =  client
-    self.planContext   =  planContext
-    self.fullBodyContext   =  fullBodyContext
-    self.problemSolver   =  problemSolver
-    self.pathPlayer   =  pathPlayer
-    self.viewer   =  viewer
-    
-  def setPlanningContext(self):
-    self.client.problem.selectProblem(self.planContext) 
-    
-  def setFullBodyContext(self):
-    self.client.problem.selectProblem(self.fullBodyContext ) 
-    
-  def setCurrentContext(self,context):
-    return self.client.problem.selectProblem(context) 
-    
-  def currentContext(self):
-    return self.client.problem.getSelected("problem")[0]
-    
-  def _actInContext(self, context,f,*args):
-    oldContext = self.currentContext()
-    self.setCurrentContext(context)
-    res = f(*args)
-    self.setCurrentContext(oldContext)
-    return res
-    
-  def guidePath(self, fromPos, toPos, displayPath = False):
-    pId =  self._actInContext(self.planContext,_guidePath,self.problemSolver, fromPos, toPos)
-    self.setPlanningContext()
-    names =  self.rbprmBuilder.getAllJointNames()[1:]
-    if displayPath:
-      if self.pathPlayer is None:
-        print "can't display path, no path player given"
-      else:
-        self.pathPlayer(pId)        
-    self.client.problem.movePathToProblem(pId,self.fullBodyContext, names)
-    self.setFullBodyContext()
-    return pId
-    
-  def interpolateStates(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = True):
-    return _interpolateState(self.problemSolver, self.fullBody, stepsize, pathId, robustnessTreshold, filterStates, testReachability, quasiStatic, erasePreviousStates)
-    
-  def goToQuasiStatic(self, currentState, toPos, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False, erasePreviousStates = False):
-    pId = self.guidePath(currentState.q()[:7], toPos, displayPath = displayGuidePath)    
-    self.fullBody.setStartStateId(currentState.sId)
-    targetState = currentState.q()[:]; targetState[:7] = toPos
-    if goalLimbsInContact is None:
-      goalLimbsInContact = currentState.getLimbsInContact()
-    if goalNormals is None:
-      goalNormals = [[0.,0.,1.] for _ in range(len(goalLimbsInContact))]
-    self.fullBody.setEndState(targetState, goalLimbsInContact,goalNormals)
-    return self.interpolateStates(stepsize,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = erasePreviousStates)
-    
-       
-              
+    def __init__(self,
+                 client,
+                 problemSolver,
+                 rbprmBuilder,
+                 fullBody,
+                 planContext="rbprm_path",
+                 fullBodyContext="default",
+                 pathPlayer=None,
+                 viewer=None):
+        self.fullBody = fullBody
+        self.rbprmBuilder = rbprmBuilder
+        self.client = client
+        self.planContext = planContext
+        self.fullBodyContext = fullBodyContext
+        self.problemSolver = problemSolver
+        self.pathPlayer = pathPlayer
+        self.viewer = viewer
+
+    def setPlanningContext(self):
+        self.client.problem.selectProblem(self.planContext)
+
+    def setFullBodyContext(self):
+        self.client.problem.selectProblem(self.fullBodyContext)
+
+    def setCurrentContext(self, context):
+        return self.client.problem.selectProblem(context)
+
+    def currentContext(self):
+        return self.client.problem.getSelected("problem")[0]
+
+    def _actInContext(self, context, f, *args):
+        oldContext = self.currentContext()
+        self.setCurrentContext(context)
+        res = f(*args)
+        self.setCurrentContext(oldContext)
+        return res
+
+    def guidePath(self, fromPos, toPos, displayPath=False):
+        pId = self._actInContext(self.planContext, _guidePath, self.problemSolver, fromPos, toPos)
+        self.setPlanningContext()
+        names = self.rbprmBuilder.getAllJointNames()[1:]
+        if displayPath:
+            if self.pathPlayer is None:
+                print("can't display path, no path player given")
+            else:
+                self.pathPlayer(pId)
+        self.client.problem.movePathToProblem(pId, self.fullBodyContext, names)
+        self.setFullBodyContext()
+        return pId
+
+    def interpolateStates(self,
+                          stepsize,
+                          pathId=1,
+                          robustnessTreshold=0,
+                          filterStates=False,
+                          testReachability=True,
+                          quasiStatic=False,
+                          erasePreviousStates=True):
+        return _interpolateState(self.problemSolver, self.fullBody, stepsize, pathId, robustnessTreshold, filterStates,
+                                 testReachability, quasiStatic, erasePreviousStates)
+
+    def goToQuasiStatic(self,
+                        currentState,
+                        toPos,
+                        stepsize=0.002,
+                        goalLimbsInContact=None,
+                        goalNormals=None,
+                        displayGuidePath=False,
+                        erasePreviousStates=False):
+        pId = self.guidePath(currentState.q()[:7], toPos, displayPath=displayGuidePath)
+        self.fullBody.setStartStateId(currentState.sId)
+        targetState = currentState.q()[:]
+        targetState[:7] = toPos
+        if goalLimbsInContact is None:
+            goalLimbsInContact = currentState.getLimbsInContact()
+        if goalNormals is None:
+            goalNormals = [[0., 0., 1.] for _ in range(len(goalLimbsInContact))]
+        self.fullBody.setEndState(targetState, goalLimbsInContact, goalNormals)
+        return self.interpolateStates(stepsize,
+                                      pathId=pId,
+                                      robustnessTreshold=1,
+                                      filterStates=True,
+                                      quasiStatic=True,
+                                      erasePreviousStates=erasePreviousStates)

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -19,98 +19,102 @@
 from hpp.corbaserver.rbprm import Client as RbprmClient
 from hpp.corbaserver.robot import Robot
 
-## Load and handle a RbprmDevice robot for rbprm planning
+
+# # Load and handle a RbprmDevice robot for rbprm planning
 #
-#  A RbprmDevice robot is a dual representation of a robots. One robot describes the 
+#  A RbprmDevice robot is a dual representation of a robots. One robot describes the
 #  trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
-class Builder (Robot):
-  ## Constructor
-  def __init__ (self, load = True):
-      self.tf_root = "base_link"
-      self.clientRbprm = RbprmClient ()
-      self.load = load
-
-  ## Virtual function to load the robot model.
-  #
-  # \param urdfName urdf description of the robot trunk,
-  # \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
-  # \param rootJointType type of root joint among ("freeflyer", "planar",
-  #        "anchor"),
-  # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
-  # \param packageName name of the package from where the robot will be loaded
-  # \param urdfSuffix optional suffix for the urdf of the robot package
-  # \param srdfSuffix optional suffix for the srdf of the robot package
-  def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
-    Robot.__init__(self,urdfName,rootJointType, False)
-    if(isinstance(urdfNameroms, list)):
-      for urdfNamerom in urdfNameroms:
-        self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
-    else:
-      self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms, urdfSuffix, srdfSuffix)
-    self.clientRbprm.rbprm.initNewProblemSolver()
-    self.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
-    self.client.robot.meshPackageName = meshPackageName
-    self.meshPackageName = meshPackageName
-    self.packageName = packageName
-    self.urdfName = urdfName
-    self.urdfSuffix = urdfSuffix
-    self.srdfSuffix = srdfSuffix
-
+class Builder(Robot):
+    # # Constructor
+    def __init__(self, load=True):
+        self.tf_root = "base_link"
+        self.clientRbprm = RbprmClient()
+        self.load = load
 
+    # # Virtual function to load the robot model.
+    #
+    # \param urdfName urdf description of the robot trunk,
+    # \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
+    # \param rootJointType type of root joint among ("freeflyer", "planar",
+    #        "anchor"),
+    # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
+    # \param packageName name of the package from where the robot will be loaded
+    # \param urdfSuffix optional suffix for the urdf of the robot package
+    # \param srdfSuffix optional suffix for the srdf of the robot package
+    def loadModel(self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
+        Robot.__init__(self, urdfName, rootJointType, False)
+        if (isinstance(urdfNameroms, list)):
+            for urdfNamerom in urdfNameroms:
+                self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom,
+                                                         urdfSuffix, srdfSuffix)
+        else:
+            self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms,
+                                                     urdfSuffix, srdfSuffix)
+        self.clientRbprm.rbprm.initNewProblemSolver()
+        self.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
+                                                      srdfSuffix)
+        self.client.robot.meshPackageName = meshPackageName
+        self.meshPackageName = meshPackageName
+        self.packageName = packageName
+        self.urdfName = urdfName
+        self.urdfSuffix = urdfSuffix
+        self.srdfSuffix = srdfSuffix
 
-  ## Init RbprmShooter
-  #
-  def initshooter (self):
-    return self.clientRbprm.rbprm.initshooter ()
+    # # Init RbprmShooter
+    #
+    def initshooter(self):
+        return self.clientRbprm.rbprm.initshooter()
 
-  ## Sets limits on robot orientation, described according to Euler's ZYX rotation order
-  #
-  # \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
-  def boundSO3 (self,  bounds):
-    return self.clientRbprm.rbprm.boundSO3 (bounds)
+    # # Sets limits on robot orientation, described according to Euler's ZYX rotation order
+    #
+    # \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
+    def boundSO3(self, bounds):
+        return self.clientRbprm.rbprm.boundSO3(bounds)
 
-  ## Specifies a preferred affordance for a given rom.
-  # This constrains the planner to accept a rom configuration only if
-  # it collides with a surface the normal of which has these properties.
-  #
-  # \param rom name of the rome,
-  # \param affordances list of affordance names
-  def setAffordanceFilter (self, rom, affordances):
-    return self.clientRbprm.rbprm.setAffordanceFilter (rom, affordances)
+    # # Specifies a preferred affordance for a given rom.
+    # This constrains the planner to accept a rom configuration only if
+    # it collides with a surface the normal of which has these properties.
+    #
+    # \param rom name of the rome,
+    # \param affordances list of affordance names
+    def setAffordanceFilter(self, rom, affordances):
+        return self.clientRbprm.rbprm.setAffordanceFilter(rom, affordances)
 
-  ## Specifies a rom constraint for the planner.
-  # A configuration will be valid if and only if the considered rom collides
-  # with the environment.
-  #
-  # \param romFilter array of roms indicated by name, which determine the constraint.
-  def setFilter (self, romFilter):
-    return self.clientRbprm.rbprm.setFilter (romFilter)
+    # # Specifies a rom constraint for the planner.
+    # A configuration will be valid if and only if the considered rom collides
+    # with the environment.
+    #
+    # \param romFilter array of roms indicated by name, which determine the constraint.
+    def setFilter(self, romFilter):
+        return self.clientRbprm.rbprm.setFilter(romFilter)
 
-  ## Export a computed path for blender
+    # # Export a computed path for blender
     #
-  # \param problem the problem associated with the path computed for the robot
-  # \param stepsize increment along the path
-  # \param pathId if of the considered path
-  # \param filename name of the output file where to save the output
-  def exportPath (self, viewer, problem, pathId, stepsize, filename):
-    import hpp.gepetto.blender.exportmotion as em
-    em.exportPath(viewer, self.client.robot, problem, pathId, stepsize, filename)
+    # \param problem the problem associated with the path computed for the robot
+    # \param stepsize increment along the path
+    # \param pathId if of the considered path
+    # \param filename name of the output file where to save the output
+    def exportPath(self, viewer, problem, pathId, stepsize, filename):
+        import hpp.gepetto.blender.exportmotion as em
+        em.exportPath(viewer, self.client.robot, problem, pathId, stepsize, filename)
 
-  ## set a reference position of the end effector for the given ROM
-  # This reference will be used in the heuristic that choose the "best" contact surface
-  # and approximate the contact points in the kinodynamic planner
-  # \param romName the name of the rom
-  # \param ref the 3D reference position of the end effector, expressed in the root frame
-  def setReferenceEndEffector(self,romName,ref):
-    return self.clientRbprm.rbprm.setReferenceEndEffector(romName,ref)
+    # # set a reference position of the end effector for the given ROM
+    # This reference will be used in the heuristic that choose the "best" contact surface
+    # and approximate the contact points in the kinodynamic planner
+    # \param romName the name of the rom
+    # \param ref the 3D reference position of the end effector, expressed in the root frame
+    def setReferenceEndEffector(self, romName, ref):
+        return self.clientRbprm.rbprm.setReferenceEndEffector(romName, ref)
 
-  ## For a given limb, return all the intersections between the limb reachable workspace and a contact surface
-  # \param configuration the root configuration
-  # \param limbName name of the considered limb
-  # \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id is the 3 coordinate of each vertex
-  def getContactSurfacesAtConfig(self,configuration,limbName):
-    surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration,limbName)
-    res = []
-    for surface in surfaces:
-        res += [[surface[i:i+3] for i in range(0, len(surface),3)]] # split list of coordinate every 3 points (3D points)
-    return res
+    # # For a given limb, return all the intersections between the limb reachable workspace and a contact surface
+    # \param configuration the root configuration
+    # \param limbName name of the considered limb
+    # \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id
+    # is the 3 coordinate of each vertex
+    def getContactSurfacesAtConfig(self, configuration, limbName):
+        surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration, limbName)
+        res = []
+        for surface in surfaces:
+            res += [[surface[i:i + 3]
+                     for i in range(0, len(surface), 3)]]  # split list of coordinate every 3 points (3D points)
+        return res

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -16,152 +16,158 @@
 # hpp-manipulation-corba.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-from hpp.corbaserver.rbprm import Client as RbprmClient
-from hpp.corbaserver import Client as BasicClient
-#from hpp.corbaserver.rbprm.tools.com_constraints import *
+from hpp.corbaserver.rbprm.tools.com_constraints import get_com_constraint
 from numpy import array
 
 
-## Creates a state given an Id pointing to an existing c++ state
+# # Creates a state given an Id pointing to an existing c++ state
 #
-#  A RbprmDevice robot is a set of two robots. One for the 
+#  A RbprmDevice robot is a set of two robots. One for the
 #  trunk of the robot, one for the range of motion
-class State (object):
-    ## Constructor
-    def __init__ (self, fullBody, sId=-1, isIntermediate = False, q = None, limbsIncontact = []):
-        assert ((sId > -1 and len(limbsIncontact) == 0) or sId == -1), "state created from existing id can't specify limbs in contact"
+class State(object):
+    # # Constructor
+    def __init__(self, fullBody, sId=-1, isIntermediate=False, q=None, limbsIncontact=[]):
+        assert ((sId > -1 and len(limbsIncontact) == 0)
+                or sId == -1), "state created from existing id can't specify limbs in contact"
         self.cl = fullBody.clientRbprm.rbprm
-        if(sId == -1):
+        if (sId == -1):
             self.sId = self.cl.createState(q, limbsIncontact)
-            self.isIntermediate = False    
+            self.isIntermediate = False
         else:
             self.sId = sId
-            self.isIntermediate = isIntermediate    
+            self.isIntermediate = isIntermediate
         self.fullBody = fullBody
         self.H_h = None
         self.__last_used_friction = None
-            
-    ## assert for case where functions can't be used with intermediate state
+
+    # # assert for case where functions can't be used with intermediate state
     def _cni(self):
         assert not self.isIntermediate, "method can't be called with intermediate state"
-            
-    ## Get the state configuration
+
+    # # Get the state configuration
     def q(self):
         self._cni()
         return self.cl.getConfigAtState(self.sId)
-        
-    ## Set the state configuration
+
+    # # Set the state configuration
     # \param q configuration of the robot
     # \return whether or not the configuration was successfully set
     def setQ(self, q):
         self._cni()
-        return self.cl.setConfigAtState(self.sId, q)    > 0
-        
+        return self.cl.setConfigAtState(self.sId, q) > 0
+
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return whether the limb is in contact at this state
     def isLimbInContact(self, limbname):
-        if(self.isIntermediate):
-            return self.cl.isLimbInContactIntermediary(limbname, self.sId) >0
-        else:            
-            return self.cl.isLimbInContact(limbname, self.sId) >0
-                
-    # 
+        if (self.isIntermediate):
+            return self.cl.isLimbInContactIntermediary(limbname, self.sId) > 0
+        else:
+            return self.cl.isLimbInContact(limbname, self.sId) > 0
+
+    #
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return all limbs in contact at this state
     def getLimbsInContact(self):
         return [limbName for limbName in self.fullBody.limbNames if self.isLimbInContact(limbName)]
-        
-    ## Get the end effector position for a given configuration, assuming z normal
+
+    # # Get the end effector position for a given configuration, assuming z normal
     # \param limbName name of the limb from which to retrieve a sample
     # \param configuration configuration of the robot
     # \return world position of the limb end effector given the current robot configuration.
     # array of size 4, where each entry is the position of a corner of the contact surface
     def getEffectorPosition(self, limbName):
         self._cni()
-        return self.cl.getEffectorPosition(limbName,self.q())
-     
-    ## Get the end effector position for a given configuration, assuming z normal
+        return self.cl.getEffectorPosition(limbName, self.q())
+
+    # # Get the end effector position for a given configuration, assuming z normal
     # \param limbName name of the limb from which to retrieve a sample
     # \param configuration configuration of the robot
     # \return world position of the limb end effector given the current robot configuration.
     # array of size 4, where each entry is the position of a corner of the contact surface
     def getContactPosAndNormals(self):
-        if(self.isIntermediate):  
+        if (self.isIntermediate):
             rawdata = self.cl.computeContactPointsAtState(self.sId, 1)
-        else:            
-            rawdata = self.cl.computeContactPointsAtState(self.sId, 0)