Skip to content

GitLab

Commit 05e12ede authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] Format

parent 85b2ae88
Expand all Show whitespace changes
Inline Side-by-side
Showing with 2953 additions and 2547 deletions
.yapfignore 0 → 100644
View file @ 05e12ede
script
profile
affordance-tests
setup.cfg 0 → 100644
View file @ 05e12ede
[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
View file @ 05e12ede
from .client import Client
from .client import Client # noqa
src/hpp/corbaserver/rbprm/client.py
View file @ 05e12ede
......@@ -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
View file @ 05e12ede
......@@ -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
View file @ 05e12ede
......@@ -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/rbprmfullbody.py
View file @ 05e12ede
This diff is collapsed.
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @ 05e12ede
......@@ -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.