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,26 +20,27 @@ ...@@ -20,26 +20,27 @@
from hpp.corbaserver.client import Client as _Parent from hpp.corbaserver.client import Client as _Parent
from hpp_idl.hpp.corbaserver.rbprm import RbprmBuilder from hpp_idl.hpp.corbaserver.rbprm import RbprmBuilder
class Client (_Parent):
""" class Client(_Parent):
"""
Connect and create clients for hpp-rbprm library. Connect and create clients for hpp-rbprm library.
""" """
defaultClients = { 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. Initialize CORBA and create default clients.
:param url: URL in the IOR, corbaloc, corbalocs, and corbanames formats. :param url: URL in the IOR, corbaloc, corbalocs, and corbanames formats.
For a remote corba server, use For a remote corba server, use
url = "corbaloc:iiop:<host>:<port>/NameService" url = "corbaloc:iiop:<host>:<port>/NameService"
""" """
self._initOrb (url) self._initOrb(url)
self._makeClients ("rbprm", self.defaultClients, context) self._makeClients("rbprm", self.defaultClients, context)
# self.rbprmbuilder is created by self._makeClients # self.rbprmbuilder is created by self._makeClients
# The old code stored the object as self.rbprm # The old code stored the object as self.rbprm
# Make it backward compatible. # Make it backward compatible.
self.rbprm = self.rbprmbuilder self.rbprm = self.rbprmbuilder
...@@ -16,100 +16,133 @@ ...@@ -16,100 +16,133 @@
# hpp-manipulation-corba. If not, see # hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>. # <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 from hpp.corbaserver.rbprm.rbprmstate import State
def _interpolateState(ps, fullBody, stepsize, pathId, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = True): # from hpp.corbaserver.rbprm.tools.com_constraints import *
if(filterStates):
def _interpolateState(ps,
fullBody,
stepsize,
pathId,
robustnessTreshold=0,
filterStates=False,
testReachability=True,
quasiStatic=False,
erasePreviousStates=True):
if (filterStates):
filt = 1 filt = 1
else: else:
filt = 0 filt = 0
#discretize path # discretize path
totalLength = ps.pathLength(pathId) totalLength = ps.pathLength(pathId)
configsPlan = []; step = 0. configsPlan = []
configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId,0.)) step = 0.
z = [0. for _ in range(configSize) ] configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId, 0.))
while step < totalLength: z = [0. for _ in range(configSize)]
configsPlan += [ps.configAtParam(pathId,step) + z[:]] while step < totalLength:
step += stepsize configsPlan += [ps.configAtParam(pathId, step) + z[:]]
configsPlan += [ps.configAtParam(pathId,totalLength)+ 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) configs = fullBody.clientRbprm.rbprm.interpolateConfigs(configsPlan, robustnessTreshold, filt, testReachability,
return [ State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs)) ], configs 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): def _guidePath(problemSolver, fromPos, toPos):
ps = problemSolver ps = problemSolver
ps.setInitialConfig (fromPos) ps.setInitialConfig(fromPos)
ps.resetGoalConfigs() ps.resetGoalConfigs()
ps.addGoalConfig(toPos) ps.addGoalConfig(toPos)
ps.solve () ps.solve()
return ps.numberPaths() - 1 return ps.numberPaths() - 1
class FewStepPlanner(object): class FewStepPlanner(object):
def __init__ (self, client, problemSolver, rbprmBuilder, fullBody, planContext="rbprm_path", fullBodyContext="default", pathPlayer = None, viewer = None ): def __init__(self,
self.fullBody = fullBody client,
self.rbprmBuilder = rbprmBuilder problemSolver,
self.client = client rbprmBuilder,
self.planContext = planContext fullBody,
self.fullBodyContext = fullBodyContext planContext="rbprm_path",
self.problemSolver = problemSolver fullBodyContext="default",
self.pathPlayer = pathPlayer pathPlayer=None,
self.viewer = viewer viewer=None):
self.fullBody = fullBody
def setPlanningContext(self): self.rbprmBuilder = rbprmBuilder
self.client.problem.selectProblem(self.planContext) self.client = client
self.planContext = planContext
def setFullBodyContext(self): self.fullBodyContext = fullBodyContext
self.client.problem.selectProblem(self.fullBodyContext ) self.problemSolver = problemSolver
self.pathPlayer = pathPlayer
def setCurrentContext(self,context): self.viewer = viewer
return self.client.problem.selectProblem(context)
def setPlanningContext(self):
def currentContext(self): self.client.problem.selectProblem(self.planContext)
return self.client.problem.getSelected("problem")[0]
def setFullBodyContext(self):
def _actInContext(self, context,f,*args): self.client.problem.selectProblem(self.fullBodyContext)
oldContext = self.currentContext()
self.setCurrentContext(context) def setCurrentContext(self, context):
res = f(*args) return self.client.problem.selectProblem(context)
self.setCurrentContext(oldContext)
return res def currentContext(self):
return self.client.problem.getSelected("problem")[0]
def guidePath(self, fromPos, toPos, displayPath = False):
pId = self._actInContext(self.planContext,_guidePath,self.problemSolver, fromPos, toPos) def _actInContext(self, context, f, *args):
self.setPlanningContext() oldContext = self.currentContext()
names = self.rbprmBuilder.getAllJointNames()[1:] self.setCurrentContext(context)
if displayPath: res = f(*args)
if self.pathPlayer is None: self.setCurrentContext(oldContext)
print "can't display path, no path player given" return res
else:
self.pathPlayer(pId) def guidePath(self, fromPos, toPos, displayPath=False):
self.client.problem.movePathToProblem(pId,self.fullBodyContext, names) pId = self._actInContext(self.planContext, _guidePath, self.problemSolver, fromPos, toPos)
self.setFullBodyContext() self.setPlanningContext()
return pId names = self.rbprmBuilder.getAllJointNames()[1:]
if displayPath:
def interpolateStates(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = True): if self.pathPlayer is None:
return _interpolateState(self.problemSolver, self.fullBody, stepsize, pathId, robustnessTreshold, filterStates, testReachability, quasiStatic, erasePreviousStates) print("can't display path, no path player given")
else:
def goToQuasiStatic(self, currentState, toPos, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False, erasePreviousStates = False): self.pathPlayer(pId)
pId = self.guidePath(currentState.q()[:7], toPos, displayPath = displayGuidePath) self.client.problem.movePathToProblem(pId, self.fullBodyContext, names)
self.fullBody.setStartStateId(currentState.sId) self.setFullBodyContext()
targetState = currentState.q()[:]; targetState[:7] = toPos return pId
if goalLimbsInContact is None:
goalLimbsInContact = currentState.getLimbsInContact() def interpolateStates(self,
if goalNormals is None: stepsize,
goalNormals = [[0.,0.,1.] for _ in range(len(goalLimbsInContact))] pathId=1,
self.fullBody.setEndState(targetState, goalLimbsInContact,goalNormals) robustnessTreshold=0,
return self.interpolateStates(stepsize,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = erasePreviousStates) 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)
...@@ -19,98 +19,102 @@ ...@@ -19,98 +19,102 @@
from hpp.corbaserver.rbprm import Client as RbprmClient from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver.robot import Robot 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. # trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class Builder (Robot): class Builder(Robot):
## Constructor # # Constructor
def __init__ (self, load = True): def __init__(self, load=True):
self.tf_root = "base_link" self.tf_root = "base_link"
self.clientRbprm = RbprmClient () self.clientRbprm = RbprmClient()
self.load = load 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
# # 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 # # Init RbprmShooter
# #
def initshooter (self): def initshooter(self):
return self.clientRbprm.rbprm.initshooter () 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 # \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
def boundSO3 (self, bounds): def boundSO3(self, bounds):
return self.clientRbprm.rbprm.boundSO3 (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 # This constrains the planner to accept a rom configuration only if
# it collides with a surface the normal of which has these properties. # it collides with a surface the normal of which has these properties.
# #
# \param rom name of the rome, # \param rom name of the rome,
# \param affordances list of affordance names # \param affordances list of affordance names
def setAffordanceFilter (self, rom, affordances): def setAffordanceFilter(self, rom, affordances):
return self.clientRbprm.rbprm.setAffordanceFilter (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 # A configuration will be valid if and only if the considered rom collides
# with the environment. # with the environment.
# #
# \param romFilter array of roms indicated by name, which determine the constraint. # \param romFilter array of roms indicated by name, which determine the constraint.
def setFilter (self, romFilter): def setFilter(self, romFilter):
return self.clientRbprm.rbprm.setFilter (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 problem the problem associated with the path computed for the robot
# \param stepsize increment along the path # \param stepsize increment along the path
# \param pathId if of the considered path # \param pathId if of the considered path
# \param filename name of the output file where to save the output # \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 import hpp.gepetto.blender.exportmotion as em
em.exportPath(viewer, self.client.robot, problem, pathId, stepsize, filename) 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 # This reference will be used in the heuristic that choose the "best" contact surface
# and approximate the contact points in the kinodynamic planner # and approximate the contact points in the kinodynamic planner
# \param romName the name of the rom # \param romName the name of the rom
# \param ref the 3D reference position of the end effector, expressed in the root frame # \param ref the 3D reference position of the end effector, expressed in the root frame
def setReferenceEndEffector(self,romName,ref): def setReferenceEndEffector(self, romName, ref):
return self.clientRbprm.rbprm.setReferenceEndEffector(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 configuration the root configuration
# \param limbName name of the considered limb # \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 # \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id
def getContactSurfacesAtConfig(self,configuration,limbName): # is the 3 coordinate of each vertex
surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration,limbName) def getContactSurfacesAtConfig(self, configuration, limbName):
res = [] surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration, limbName)
for surface in surfaces: res = []
res += [[surface[i:i+3] for i in range(0, len(surface),3)]] # split list of coordinate every 3 points (3D points) for surface in surfaces:
return res 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,672 +18,728 @@ ...@@ -18,672 +18,728 @@
from hpp.corbaserver.rbprm import Client as RbprmClient from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver.robot import Robot from hpp.corbaserver.robot import Robot
from numpy import array, matrix
from hpp_spline import bezier 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 # A RbprmDevice robot is a set of two robots. One for the
# trunk of the robot, one for the range of motion # trunk of the robot, one for the range of motion
class FullBody (Robot): class FullBody(Robot):
## Constructor # # Constructor
def __init__ (self, load = True): def __init__(self, load=True):
self.tf_root = "base_link" self.tf_root = "base_link"
self.clientRbprm = RbprmClient () self.clientRbprm = RbprmClient()
self.load = load self.load = load
self.limbNames = [] 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 urdfName urdf description of the fullBody robot
# \param rootJointType type of root joint among ("freeflyer", "planar", # \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots # "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded # \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 packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package # \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package # \param srdfSuffix optional suffix for the srdf of the robot package
def loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix): def loadFullBodyModel(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
Robot.__init__(self,urdfName,rootJointType, False) Robot.__init__(self, urdfName, rootJointType, False)
self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix, self.client.problem.getSelected("problem")[0]) self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
self.client.robot.meshPackageName = meshPackageName srdfSuffix,
self.meshPackageName = meshPackageName self.client.problem.getSelected("problem")[0])
self.packageName = packageName self.client.robot.meshPackageName = meshPackageName
self.urdfName = urdfName self.meshPackageName = meshPackageName
self.urdfSuffix = urdfSuffix self.packageName = packageName
self.srdfSuffix = srdfSuffix self.urdfName = urdfName
self.octrees={} self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
# Virtual function to load the fullBody robot model. self.octrees = {}
#
def loadFullBodyModelFromActiveRobot (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix): # Virtual function to load the fullBody robot model.
Robot.__init__(self,urdfName,rootJointType, False) #
self.clientRbprm.rbprm.loadFullBodyRobotFromExistingRobot() def loadFullBodyModelFromActiveRobot(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix,
self.client.robot.meshPackageName = meshPackageName srdfSuffix):
self.meshPackageName = meshPackageName Robot.__init__(self, urdfName, rootJointType, False)
self.packageName = packageName self.clientRbprm.rbprm.loadFullBodyRobotFromExistingRobot()
self.urdfName = urdfName self.client.robot.meshPackageName = meshPackageName
self.urdfSuffix = urdfSuffix self.meshPackageName = meshPackageName
self.srdfSuffix = srdfSuffix self.packageName = packageName
self.octrees={} self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
## Add a limb to the model self.octrees = {}
#
# \param id: user defined id for the limb. Must be unique. # # Add a limb to the model
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot) #
# \param name: name of the joint corresponding to the root of the limb. # \param id: user defined id for the limb. Must be unique.
# \param effectorName name of the joint to be considered as the effector of the limb # The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
# \param offset position of the effector in joint coordinates relatively to the effector joint # \param name: name of the joint corresponding to the root of the limb.
# \param unit normal vector of the contact point, expressed in the effector joint coordinates # \param effectorName name of the joint to be considered as the effector of the limb
# \param x half width of the default support polygon of the effector # \param offset position of the effector in joint coordinates relatively to the effector joint
# \param y half height of the default support polygon of the effector