Skip to content

GitLab

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

[Python] Format

parent 85b2ae88
Expand all Hide 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,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
View file @ 05e12ede
......@@ -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
View file @ 05e12ede
......@@ -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/rbprmfullbody.py
View file @ 05e12ede
This diff is collapsed.
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @ 05e12ede
......@@ -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):