[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,24 +20,25 @@
 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,
+        '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

src/hpp/corbaserver/rbprm/fewstepsplanner.py

@@ -16,46 +16,61 @@
 # 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:
         filt = 0
 
-  #discretize path
+    # discretize path
     totalLength = ps.pathLength(pathId)
-  configsPlan = []; step = 0.
-  configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId,0.))
-  z = [0. for _ in range(configSize) ]
+    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[:]]
+        configsPlan += [ps.configAtParam(pathId, step) + z[:]]
         step += stepsize
-  configsPlan += [ps.configAtParam(pathId,totalLength)+ z[:]]
+    configsPlan += [ps.configAtParam(pathId, totalLength) + z[:]]
 
-  configs = fullBody.clientRbprm.rbprm.interpolateConfigs(configsPlan, robustnessTreshold, filt, testReachability, quasiStatic, erasePreviousStates)
+    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
+    return [State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs))], configs
 
 
 def _guidePath(problemSolver, fromPos, toPos):
     ps = problemSolver
-  ps.setInitialConfig (fromPos)
+    ps.setInitialConfig(fromPos)
     ps.resetGoalConfigs()
     ps.addGoalConfig(toPos)
-  ps.solve ()
+    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 ):
+    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
@@ -69,47 +84,65 @@ class FewStepPlanner(object):
         self.client.problem.selectProblem(self.planContext)
 
     def setFullBodyContext(self):
-    self.client.problem.selectProblem(self.fullBodyContext ) 
+        self.client.problem.selectProblem(self.fullBodyContext)
 
-  def setCurrentContext(self,context):
+    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):
+    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)
+    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"
+                print("can't display path, no path player given")
             else:
                 self.pathPlayer(pId)
-    self.client.problem.movePathToProblem(pId,self.fullBodyContext, names)
+        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)    
+    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
+        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)
-    
-       
-              
+            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,18 +19,19 @@
 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
 #  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):
+class Builder(Robot):
+    # # Constructor
+    def __init__(self, load=True):
         self.tf_root = "base_link"
-      self.clientRbprm = RbprmClient ()
+        self.clientRbprm = RbprmClient()
         self.load = load
 
-  ## Virtual function to load the robot model.
+    # # 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.
@@ -40,15 +41,18 @@ class Builder (Robot):
     # \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)):
+    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)
+                self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom,
+                                                         urdfSuffix, srdfSuffix)
         else:
-      self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms, urdfSuffix, srdfSuffix)
+            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.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
+                                                      srdfSuffix)
         self.client.robot.meshPackageName = meshPackageName
         self.meshPackageName = meshPackageName
         self.packageName = packageName
@@ -56,61 +60,61 @@ class Builder (Robot):
         self.urdfSuffix = urdfSuffix
         self.srdfSuffix = srdfSuffix
 
-
-
-  ## Init RbprmShooter
+    # # Init RbprmShooter
     #
-  def initshooter (self):
-    return self.clientRbprm.rbprm.initshooter ()
+    def initshooter(self):
+        return self.clientRbprm.rbprm.initshooter()
 
-  ## Sets limits on robot orientation, described according to Euler's ZYX rotation order
+    # # 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)
+    def boundSO3(self, bounds):
+        return self.clientRbprm.rbprm.boundSO3(bounds)
 
-  ## Specifies a preferred affordance for a given rom.
+    # # 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)
+    def setAffordanceFilter(self, rom, affordances):
+        return self.clientRbprm.rbprm.setAffordanceFilter(rom, affordances)
 
-  ## Specifies a rom constraint for the planner.
+    # # 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)
+    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):
+    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
+    # # 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)
+    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
+    # # 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)
+    # \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)
+            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,22 +16,21 @@
 # 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
 #  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
         else:
@@ -41,16 +40,16 @@ class State (object):
         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):
@@ -61,10 +60,10 @@ class State (object):
     # \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
+        if (self.isIntermediate):
+            return self.cl.isLimbInContactIntermediary(limbname, self.sId) > 0
         else:
-            return self.cl.isLimbInContact(limbname, self.sId) >0
+            return self.cl.isLimbInContact(limbname, self.sId) > 0
 
     #
     # \param state1 current state considered
@@ -73,95 +72,102 @@ class State (object):
     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())
+        return self.cl.getEffectorPosition(limbName, self.q())
 
-    ## 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 getContactPosAndNormals(self):
-        if(self.isIntermediate):  
+        if (self.isIntermediate):
             rawdata = self.cl.computeContactPointsAtState(self.sId, 1)
         else:
             rawdata = self.cl.computeContactPointsAtState(self.sId, 0)
-        return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
+        return [[b[i:i + 3] for i in range(0, len(b), 6)]
+                for b in rawdata], [[b[i + 3:i + 6] for i in range(0, len(b), 6)] for b in rawdata]
 
-    ## 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 getContactPosAndNormalsForLimb(self, limbName):
-        assert self.isLimbInContact(limbName), "in getContactPosAndNormals: limb " + limbName +  "is not in contact at  state" + str(stateId) 
-        if(self.isIntermediate):  
-            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,1, limbName)
+        assert self.isLimbInContact(
+            limbName), "in getContactPosAndNormals: limb " + limbName + "is not in contact at  state" + str(stateId)
+        if (self.isIntermediate):
+            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId, 1, limbName)
         else:
-            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,0, limbName) 
-        return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
+            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId, 0, limbName)
+        return [[b[i:i + 3] for i in range(0, len(b), 6)]
+                for b in rawdata], [[b[i + 3:i + 6] for i in range(0, len(b), 6)] for b in rawdata]
 
-    def getCenterOfContactForLimb(self,limbName):
-        assert self.isLimbInContact(limbName), "in getContactPosAndNormals: limb " + limbName +  "is not in contact at  state" + str(stateId)
-        return self.cl.computeCenterOfContactAtStateForLimb(self.sId,self.isIntermediate, limbName)
+    def getCenterOfContactForLimb(self, limbName):
+        assert self.isLimbInContact(
+            limbName), "in getContactPosAndNormals: limb " + limbName + "is not in contact at  state" + str(stateId)
+        return self.cl.computeCenterOfContactAtStateForLimb(self.sId, self.isIntermediate, limbName)
 
-    ## Get position of center of mass
-    def getCenterOfMass (self):
+    # # Get position of center of mass
+    def getCenterOfMass(self):
         q0 = self.fullBody.client.robot.getCurrentConfig()
         self.fullBody.client.robot.setCurrentConfig(self.q())
         c = self.fullBody.client.robot.getCenterOfMass()
         self.fullBody.client.robot.setCurrentConfig(q0)
         return c
 
-    
-    ## 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 getContactCone(self, friction):
-        if (self.H_h == None or self.__last_used_friction != friction):
+        if self.H_h is None or self.__last_used_friction != friction:
             self.__last_used_friction = friction
-            if(self.isIntermediate):  
-                rawdata = self.cl.getContactIntermediateCone(self.sId,friction)
+            if (self.isIntermediate):
+                rawdata = self.cl.getContactIntermediateCone(self.sId, friction)
             else:
-                rawdata = self.cl.getContactCone(self.sId,friction) 
+                rawdata = self.cl.getContactCone(self.sId, friction)
             self.H_h = array(rawdata)
-        return self.H_h[:,:-1], self.H_h[:, -1]
+        return self.H_h[:, :-1], self.H_h[:, -1]
 
-    def projectToCOM(self, targetCom, maxNumSample = 0, toNewState=False):
+    def projectToCOM(self, targetCom, maxNumSample=0, toNewState=False):
         if toNewState:
             return self.fullBody.projectToCom(self.sId, targetCom)
         else:
-            return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample)     > 0
+            return self.fullBody.projectStateToCOM(self.sId, targetCom, maxNumSample) > 0
 
-    ## Project a state into a given root position and orientation
+    # # Project a state into a given root position and orientation
     #
     # \param stateId target state
     # \param root the root configuration (size 7)
     # \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
     # \return Whether the projection has been successful or not
-    def projectToRoot(self,targetRoot, offset = [0., 0., 0.]):
-        return self.fullBody.projectStateToRoot(self.sId,targetRoot, offset) > 0
-        
-    def getComConstraint(self, limbsCOMConstraints, exceptList = []):
-        return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
+    def projectToRoot(self, targetRoot, offset=[0., 0., 0.]):
+        return self.fullBody.projectStateToRoot(self.sId, targetRoot, offset) > 0
 
-    def contactsVariations(self,state):
-        return self.fullBody.getContactsVariations(self.sId,state.sId)
+    def getComConstraint(self, limbsCOMConstraints, exceptList=[]):
+        return get_com_constraint(self.fullBody,
+                                  self.sId,
+                                  self.cl.getConfigAtState(self.sId),
+                                  limbsCOMConstraints,
+                                  interm=self.isIntermediate,
+                                  exceptList=exceptList)
 
+    def contactsVariations(self, state):
+        return self.fullBody.getContactsVariations(self.sId, state.sId)
 
     def isValid(self):
         return self.fullBody.isConfigValid(self.cl.getConfigAtState(self.sId))
 
-    def isBalanced(self,robustness = 0):
-        return self.fullBody.isStateBalanced(self.sId,robustness)
+    def isBalanced(self, robustness=0):
+        return self.fullBody.isStateBalanced(self.sId, robustness)
 
     def robustness(self):
         return self.fullBody.clientRbprm.rbprm.isStateBalanced(self.sId)
@@ -169,63 +175,67 @@ class State (object):
 
 class StateHelper(object):
 
-    ## tries to add a new contact to the state
-    ## if the limb is already in contact, replace the
-    ## previous contact. Only considers kinematic limitations.
-    ## collision and equilibrium are NOT considered.
+    # # tries to add a new contact to the state
+    # # if the limb is already in contact, replace the
+    # # previous contact. Only considers kinematic limitations.
+    # # collision and equilibrium are NOT considered.
     # \param state State considered
     # \param limbName name of the considered limb to create contact with
     # \param p 3d position of the point
     # \param n 3d normal of the contact location center
     # \param max_num_sample number of times it will try to randomly sample a configuration before failing
-    # \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
-    # \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
-    #                This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
+    # \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to
+    # be constant.