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

[Python] Format

parent 85b2ae88
script
profile
affordance-tests
[flake8]
max-line-length = 119
exclude = cmake,script,profile,affordance-tests
[yapf]
column_limit = 119
[isort]
line_length = 119
skip = cmake
from .client import Client
from .client import Client # noqa
......@@ -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
......
......@@ -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)
......@@ -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
......@@ -18,22 +18,23 @@
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver.robot import Robot
from numpy import array, matrix
from hpp_spline import bezier
from numpy import array, matrix
## Load and handle a RbprmFullbody robot for rbprm planning
# # Load and handle a RbprmFullbody robot for rbprm planning
#
# A RbprmDevice robot is a set of two robots. One for the
# trunk of the robot, one for the range of motion
class FullBody (Robot):
## Constructor
def __init__ (self, load = True):
class FullBody(Robot):
# # Constructor
def __init__(self, load=True):
self.tf_root = "base_link"
self.clientRbprm = RbprmClient ()
self.clientRbprm = RbprmClient()
self.load = load
self.limbNames = []
## Virtual function to load the fullBody robot model.
# # Virtual function to load the fullBody robot model.
#
# \param urdfName urdf description of the fullBody robot
# \param rootJointType type of root joint among ("freeflyer", "planar",
......@@ -42,21 +43,24 @@ class FullBody (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 loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
Robot.__init__(self,urdfName,rootJointType, False)
self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix, self.client.problem.getSelected("problem")[0])
def loadFullBodyModel(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
Robot.__init__(self, urdfName, rootJointType, False)
self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
srdfSuffix,
self.client.problem.getSelected("problem")[0])
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
self.octrees={}
self.octrees = {}
# Virtual function to load the fullBody robot model.
#
def loadFullBodyModelFromActiveRobot (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
Robot.__init__(self,urdfName,rootJointType, False)
def loadFullBodyModelFromActiveRobot(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix,
srdfSuffix):
Robot.__init__(self, urdfName, rootJointType, False)
self.clientRbprm.rbprm.loadFullBodyRobotFromExistingRobot()
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
......@@ -64,10 +68,9 @@ class FullBody (Robot):
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
self.octrees={}
self.octrees = {}
## Add a limb to the model
# # Add a limb to the model
#
# \param id: user defined id for the limb. Must be unique.
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
......@@ -82,13 +85,14 @@ class FullBody (Robot):
# \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
# \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
# structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for
# contact.
# This can be problematic in terms of performance. The default value is 3 cm.
def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples):
self.clientRbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)
self.limbNames += [limbId]
## Add a limb no used for contact generation to the model
# # Add a limb no used for contact generation to the model
#
# \param id: user defined id for the limb. Must be unique.
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
......@@ -100,7 +104,7 @@ class FullBody (Robot):
self.clientRbprm.rbprm.addNonContactingLimb(limbId, name, effectorname, samples)
self.limbNames += [limbId]
## Add a limb to the model
# # Add a limb to the model
#
# \param databasepath: path to the database describing the robot
# \param limbId: user defined id for the limb. Must be unique.
......@@ -108,20 +112,26 @@ class FullBody (Robot):
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param loadValues: whether values computed, other than the static ones, should be loaded in memory
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False, grasp =False):
def addLimbDatabase(self,
databasepath,
limbId,
heuristicName,
loadValues=True,
disableEffectorCollision=False,
grasp=False):
boolVal = 0.
boolValEff = 0.
graspv = 0.
if(loadValues):
if (loadValues):
boolVal = 1.
if(disableEffectorCollision):
if (disableEffectorCollision):
boolValEff = 1.
if(grasp):
if (grasp):
graspv = 1.
self.clientRbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff,graspv)
self.clientRbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal, boolValEff, graspv)
self.limbNames += [limbId]
## Add a limb to the model
# # Add a limb to the model
#
# \param id: user defined id for the limb. Must be unique.
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
......@@ -136,98 +146,120 @@ class FullBody (Robot):
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
# structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for
# contact.
# This can be problematic in terms of performance. The default value is 3 cm.
# \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
# \param limbOffset the offset between the limb joint and it's link
# \param kinematicConstraintsPath : path that point to the .obj file containing the kinematic constraints of the limb,
# \param kinematicConstraintsPath : path that point to the .obj file containing the kinematic constraints of the
# limb,
# if not set the default is "package://hpp-rbprm-corba/com_inequalities/"+name+"_com_constraints.obj"
# \param kinematicConstraintsMin : add an additionnal inequalities on the kinematicConstraints, of normal (0,0,1) and origin (0,0,kinematicConstraintsMin)
def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False,limbOffset=[0,0,0],kinematicConstraintsPath = "", kinematicConstraintsMin = 0.):
# \param kinematicConstraintsMin : add an additionnal inequalities on the kinematicConstraints, of normal (0,0,1)
# and origin (0,0,kinematicConstraintsMin)
def addLimb( # noqa
self,
limbId,
name,
effectorname,
offset,
normal,
x,
y,
samples,
heuristicName,
resolution,
contactType="_6_DOF",
disableEffectorCollision=False,
grasp=False,
limbOffset=[0, 0, 0],
kinematicConstraintsPath="",
kinematicConstraintsMin=0.):
boolValEff = 0.
if(disableEffectorCollision):
if (disableEffectorCollision):
boolValEff = 1.
graspv = 0.
if(grasp):
if (grasp):
graspv = 1.
self.clientRbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv,limbOffset,kinematicConstraintsPath,kinematicConstraintsMin)
self.clientRbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName,
resolution, contactType, boolValEff, graspv, limbOffset,
kinematicConstraintsPath, kinematicConstraintsMin)
self.limbNames += [limbId]
## Returns the configuration of a limb described by a sample
# # Returns the configuration of a limb described by a sample
#
# \param name name of the limb considered
# \param idsample identifiant of the sample considered
def getSample(self, name, idsample):
return self.clientRbprm.rbprm.getSampleConfig(name,idsample)
return self.clientRbprm.rbprm.getSampleConfig(name, idsample)
## Returns the end effector position of en effector,
# # Returns the end effector position of en effector,
# given the current root configuration of the robot and a limb sample
#
# \param name name of the limb considered
# \param idsample identifiant of the sample considered
def getSamplePosition(self, name, idsample):
return self.clientRbprm.rbprm.getSamplePosition(name,idsample)
return self.clientRbprm.rbprm.getSamplePosition(name, idsample)
## 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, configuration):
return self.clientRbprm.rbprm.getEffectorPosition(limbName,configuration)
return self.clientRbprm.rbprm.getEffectorPosition(limbName, configuration)
##compute the distance between all effectors replaced between two states
# #compute the distance between all effectors replaced between two states
# does not account for contact not present in both states
# \param state1 from state
# \param state2 destination state
# \return the value computed for the given sample and analytics
def getEffectorDistance(self, state1, state2):
return self.clientRbprm.rbprm.getEffectorDistance(state1,state2)
return self.clientRbprm.rbprm.getEffectorDistance(state1, state2)
## Generates a balanced contact configuration, considering the
# # Generates a balanced contact configuration, considering the
# given current configuration of the robot, and a direction of motion.
# Typically used to generate a start and / or goal configuration automatically for a planning problem.
#
# \param configuration the initial robot configuration
# \param direction a 3d vector specifying the desired direction of motion
# \return the configuration in contact
def generateContacts(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0):
def generateContacts(self, configuration, direction, acceleration=[0, 0, 0], robustnessThreshold=0):
return self.clientRbprm.rbprm.generateContacts(configuration, direction, acceleration, robustnessThreshold)