Commit daa7768e authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

remove rbprm.problemSolver class and update rbprmBuilder to be used with corbaserver.ProblemSolver

parent 275ad907
......@@ -104,7 +104,6 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmstate.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/state_alg.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
)
INSTALL(
......
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
## Definition of a path planning problem
#
# This class wraps the Corba client to the server implemented by
# libhpp-corbaserver.so
#
# Some method implemented by the server can be considered as private. The
# goal of this class is to hide them and to expose those that can be
# considered as public.
class ProblemSolver (object):
def __init__ (self, robot):
self.client = robot.client.basic
self.robot = robot
## \name Initial and goal configurations
# \{
## Set initial configuration of specified problem.
# \param dofArray Array of degrees of freedom
# \throw Error.
def setInitialConfig (self, dofArray):
return self.client.problem.setInitialConfig (dofArray)
## Get initial configuration of specified problem.
# \return Array of degrees of freedom
def getInitialConfig (self):
return self.client.problem.getInitialConfig ()
## Add goal configuration to specified problem.
# \param dofArray Array of degrees of freedom
# \throw Error.
def addGoalConfig (self, dofArray):
return self.client.problem.addGoalConfig (dofArray)
## Get goal configurations of specified problem.
# \return Array of degrees of freedom
def getGoalConfigs (self):
return self.client.problem.getGoalConfigs ()
## Reset goal configurations
def resetGoalConfigs (self):
return self.client.problem.resetGoalConfigs ()
## \}
## \name Obstacles
# \{
## Load obstacle from urdf file
# \param package Name of the package containing the model,
# \param filename name of the urdf file in the package
# (without suffix .urdf)
# \param prefix prefix added to object names in case the same file is
# loaded several times
#
# The ros url is built as follows:
# "package://${package}/urdf/${filename}.urdf"
#
# The kinematic structure of the urdf file is ignored. Only the geometric
# objects are loaded as obstacles.
def loadObstacleFromUrdf (self, package, filename, prefix):
return self.client.obstacle.loadObstacleModel (package, filename,
prefix)
## Remove an obstacle from outer objects of a joint body
#
# \param objectName name of the object to remove,
# \param jointName name of the joint owning the body,
# \param collision whether collision with object should be computed,
# \param distance whether distance to object should be computed.
# \throw Error.
def removeObstacleFromJoint (self, objectName, jointName, collision,
distance):
return self.client.obstacle.removeObstacleFromJoint \
(objectName, jointName, collision, distance)
## Move an obstacle to a given configuration.
# \param objectName name of the polyhedron.
# \param cfg the configuration of the obstacle.
# \throw Error.
#
# \note The obstacle is not added to local map
# impl::Obstacle::collisionListMap.
#
# \note Build the collision entity of polyhedron for KCD.
def moveObstacle (self, objectName, cfg):
return self.client.obstacle.moveObstacle (objectName, cfg)
## Get the position of an obstacle
#
# \param objectName name of the polyhedron.
# \retval cfg Position of the obstacle.
# \throw Error.
def getObstaclePosition (self, objectName):
return self.client.obstacle.getObstaclePosition (objectName)
## Get list of obstacles
#
# \param collision whether to return obstacle for collision,
# \param distance whether to return obstacles for distance computation
# \return list of obstacles
def getObstacleNames (self, collision, distance):
return self.client.obstacle.getObstacleNames (collision, distance)
##\}
## \name Constraints
# \{
## Create orientation constraint between two joints
#
# \param constraintName name of the constraint created,
# \param joint1Name name of first joint
# \param joint2Name name of second joint
# \param p quaternion representing the desired orientation
# of joint2 in the frame of joint1.
# \param mask Select which axis to be constrained.
# If joint1 or joint2 is "", the corresponding joint is replaced by
# the global frame.
# constraints are stored in ProblemSolver object
def createOrientationConstraint (self, constraintName, joint1Name,
joint2Name, p, mask):
return self.client.problem.createOrientationConstraint \
(constraintName, joint1Name, joint2Name, p, mask)
## Create RelativeCom constraint between two joints
#
# \param constraintName name of the constraint created,
# \param comName name of CenterOfMassComputation
# \param jointName name of joint
# \param point point in local frame of joint.
# \param mask Select axis to be constrained.
# If jointName is "", the robot root joint is used.
# Constraints are stored in ProblemSolver object
def createRelativeComConstraint (self, constraintName, comName, jointLName, point, mask):
return self.client.problem.createRelativeComConstraint \
(constraintName, comName, jointLName, point, mask)
## Create ComBeetweenFeet constraint between two joints
#
# \param constraintName name of the constraint created,
# \param comName name of CenterOfMassComputation
# \param jointLName name of first joint
# \param jointRName name of second joint
# \param pointL point in local frame of jointL.
# \param pointR point in local frame of jointR.
# \param jointRefName name of second joint
# \param mask Select axis to be constrained.
# If jointRef is "", the robot root joint is used.
# Constraints are stored in ProblemSolver object
def createComBeetweenFeet (self, constraintName, comName, jointLName, jointRName,
pointL, pointR, jointRefName, mask):
return self.client.problem.createComBeetweenFeet \
(constraintName, comName, jointLName, jointRName, pointL, pointR, jointRefName, mask)
## Add an object to compute a partial COM of the robot.
# \param name of the partial com
# \param jointNames list of joint name of each tree ROOT to consider.
# \note Joints are added recursively, it is not possible so far to add a
# joint without addind all its children.
def addPartialCom (self, comName, jointNames):
return self.client.robot.addPartialCom (comName, jointNames);
## Create position constraint between two joints
#
# \param constraintName name of the constraint created,
# \param joint1Name name of first joint
# \param joint2Name name of second joint
# \param point1 point in local frame of joint1,
# \param point2 point in local frame of joint2.
# \param mask Select which axis to be constrained.
# If joint1 of joint2 is "", the corresponding joint is replaced by
# the global frame.
# constraints are stored in ProblemSolver object
def createPositionConstraint (self, constraintName, joint1Name,
joint2Name, point1, point2, mask):
return self.client.problem.createPositionConstraint \
(constraintName, joint1Name, joint2Name, point1, point2, mask)
## Reset Constraints
#
# Reset all constraints, including numerical constraints and locked
# joints
def resetConstraints (self):
return self.client.problem.resetConstraints ()
## Set numerical constraints in ConfigProjector
#
# \param name name of the resulting numerical constraint obtained
# by stacking elementary numerical constraints,
# \param names list of names of the numerical constraints as
# inserted by method hpp::core::ProblemSolver::addNumericalConstraint.
def setNumericalConstraints (self, name, names):
return self.client.problem.setNumericalConstraints (name, names)
## Apply constraints
#
# \param q initial configuration
# \return configuration projected in success,
# \throw Error if projection failed.
def applyConstraints (self, q):
return self.client.problem.applyConstraints (q)
## Create a vector of passive dofs.
#
# \param name name of the vector in the ProblemSolver map.
# \param dofNames list of names of DOF that may
def addPassiveDofs (self, name, dofNames):
return self.client.problem.addPassiveDofs (name, dofNames)
## Generate a configuration satisfying the constraints
#
# \param maxIter maximum number of tries,
# \return configuration projected in success,
# \throw Error if projection failed.
def generateValidConfig (self, maxIter):
return self.client.problem.generateValidConfig (maxIter)
## Lock joint with given joint configuration
# \param jointName name of the joint
# \param value value of the joint configuration
def lockJoint (self, jointName, value):
return self.client.problem.lockJoint (jointName, value)
## error threshold in numerical constraint resolution
def setErrorThreshold (self, threshold):
return self.client.problem.setErrorThreshold (threshold)
## Set the maximal number of iterations
def setMaxIterations (self, iterations):
return self.client.problem.setMaxIterations (iterations)
## \}
## \name Solve problem and get paths
# \{
## Select path planner type
# \param Name of the path planner type, either "DiffusingPlanner",
# "VisibilityPrmPlanner", or any type added by method
# core::ProblemSolver::addPathPlannerType
def selectPathPlanner (self, pathPlannerType):
return self.client.problem.selectPathPlanner (pathPlannerType)
## Add a path optimizer
# \param Name of the path optimizer type, either "RandomShortcut" or
# any type added by core::ProblemSolver::addPathOptimizerType
def addPathOptimizer (self, pathOptimizerType):
return self.client.problem.addPathOptimizer (pathOptimizerType)
## Clear sequence of path optimizers
#
def clearPathOptimizers (self):
return self.client.problem.clearPathOptimizers ()
## Select path validation method
# \param Name of the path validation method, either "Discretized"
# "Progressive", "Dichotomy", or any type added by
# core::ProblemSolver::addPathValidationType,
# \param tolerance maximal acceptable penetration.
def selectPathValidation (self, pathValidationType, tolerance):
return self.client.problem.selectPathValidation (pathValidationType,
tolerance)
## Select path projector method
# \param Name of the path projector method, either "Discretized"
# "Progressive", "Dichotomy", or any type added by
# core::ProblemSolver::addPathProjectorType,
# \param tolerance maximal acceptable penetration.
def selectPathProjector (self, pathProjectorType, tolerance):
return self.client.problem.selectPathProjector (pathProjectorType,
tolerance)
## Solve the problem of corresponding ChppPlanner object
def solve (self):
return self.client.problem.solve ()
## Make direct connection between two configurations
# \param startConfig, endConfig: the configurations to link.
# \throw Error if steering method fails to create a direct path of if
# direct path is not valid
def directPath (self, startConfig, endConfig):
return self.client.problem.directPath (startConfig, endConfig)
## Get Number of paths
def numberPaths (self):
return self.client.problem.numberPaths ()
## Optimize a given path
# \param inPathId Id of the path in this problem.
# \throw Error.
def optimizePath(self, inPathId):
return self.client.problem.optimizePath (inPathId)
## Get length of path
# \param inPathId rank of the path in the problem
# \return length of path if path exists.
def pathLength(self, inPathId):
return self.client.problem.pathLength(inPathId)
## Get the robot's config at param on the a path
# \param inPathId rank of the path in the problem
# \param atDistance : the user parameter choice
# \return dofseq : the config at param
def configAtParam (self, inPathId, atDistance):
return self.client.problem.configAtParam (inPathId, atDistance)
## Get way points of a path
# \param pathId rank of the path in the problem
def getWaypoints (self, pathId):
return self.client.problem.getWaypoints (pathId)
## \name Interruption of a path planning request
# \{
## \brief Interrupt path planning activity
# \note this method is effective only when multi-thread policy is used
# by CORBA server.
# See constructor of class Server for details.
def interruptPathPlanning (self):
return self.client.problem.interruptPathPlanning ()
# \}
## \name exploring the roadmap
# \{
## Get nodes of the roadmap.
def nodes(self):
return self.client.problem.nodes ()
# the configuration of the node nodeId
def node(self,nodeId):
return self.client.problem.node(nodeId)
# the number of nodes in the roadmap
def numberNodes(self):
return self.client.problem.numberNodes ()
## Number of edges
def numberEdges (self):
return self.client.problem.numberEdges ()
## Edge at given rank
def edge (self, edgeId):
return self.client.problem.edge (edgeId)
## Number of connected components
def numberConnectedComponents (self):
return self.client.problem.numberConnectedComponents ()
## Nodes of a connected component
# \param connectedComponentId index of connected component in roadmap
# \return list of nodes of the connected component.
def nodesConnectedComponent (self, ccId):
return self.client.problem.nodesConnectedComponent (ccId)
## Clear the roadmap
def clearRoadmap (self):
return self.client.problem.clearRoadmap ()
## \}
## Select steering method type
# \param Name of the steering method type, either
# "SteeringMethodStraight" or any type added by method
# core::ProblemSolver::addSteeringMethodType
def selectSteeringMethod (self, steeringMethodType):
return self.client.problem.selectSteeringMethod (steeringMethodType)
## Select distance type
# \param Name of the distance type, either
# "WeighedDistance" or any type added by method
# core::ProblemSolver::addDistanceType
def selectDistance (self, distanceType):
return self.client.problem.selectDistance (distanceType)
......@@ -20,16 +20,6 @@ from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
import hpp.gepetto.blender.exportmotion as em
## Corba clients to the various servers
#
class CorbaClient:
"""
Container for corba clients to various interfaces.
"""
def __init__ (self):
self.basic = BasicClient ()
self.rbprm = RbprmClient ()
## Load and handle a RbprmDevice robot for rbprm planning
#
# A RbprmDevice robot is a dual representation of a robots. One robot describes the
......@@ -37,10 +27,11 @@ class CorbaClient:
class Builder (object):
## Constructor
def __init__ (self, load = True):
self.tf_root = "base_link"
self.rootJointType = dict()
self.client = CorbaClient ()
self.load = load
self.tf_root = "base_link"
self.rootJointType = dict()
self.client = BasicClient () # client of hpp.corbaserver.robot
self.clientRbprm = RbprmClient () # client of hpp.corbaserver.rbprm.rbprmBuilder
self.load = load
## Virtual function to load the robot model.
#
......@@ -55,18 +46,18 @@ class Builder (object):
def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
if(isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
else:
self.client.rbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms, urdfSuffix, srdfSuffix)
self.client.rbprm.rbprm.initNewProblemSolver()
self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, 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.name = urdfName
self.displayName = urdfName
self.tf_root = "base_link"
self.rootJointType = rootJointType
self.jointNames = self.client.basic.robot.getJointNames ()
self.allJointNames = self.client.basic.robot.getAllJointNames ()
self.client.basic.robot.meshPackageName = meshPackageName
self.jointNames = self.client.robot.getJointNames ()
self.allJointNames = self.client.robot.getAllJointNames ()
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.rankInConfiguration = dict ()
self.rankInVelocity = dict ()
......@@ -77,22 +68,22 @@ class Builder (object):
rankInConfiguration = rankInVelocity = 0
for j in self.jointNames:
self.rankInConfiguration [j] = rankInConfiguration
rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
rankInConfiguration += self.client.robot.getJointConfigSize (j)
self.rankInVelocity [j] = rankInVelocity
rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
rankInVelocity += self.client.robot.getJointNumberDof (j)
## Init RbprmShooter
#
def initshooter (self):
return self.client.rbprm.rbprm.initshooter ()
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.client.rbprm.rbprm.boundSO3 (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
......@@ -101,7 +92,7 @@ class Builder (object):
# \param rom name of the rome,
# \param affordances list of affordance names
def setAffordanceFilter (self, rom, affordances):
return self.client.rbprm.rbprm.setAffordanceFilter (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
......@@ -109,7 +100,7 @@ class Builder (object):
#
# \param romFilter array of roms indicated by name, which determine the constraint.
def setFilter (self, romFilter):
return self.client.rbprm.rbprm.setFilter (romFilter)
return self.clientRbprm.rbprm.setFilter (romFilter)
## Export a computed path for blender
#
......@@ -118,7 +109,7 @@ class Builder (object):
# \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):
em.exportPath(viewer, self.client.basic.robot, problem, pathId, stepsize, filename)
em.exportPath(viewer, self.client.robot, problem, pathId, stepsize, filename)
## \name Degrees of freedom
# \{
......@@ -126,12 +117,12 @@ class Builder (object):
## Get size of configuration
# \return size of configuration
def getConfigSize (self):
return self.client.basic.robot.getConfigSize ()
return self.client.robot.getConfigSize ()
# Get size of velocity
# \return size of velocity
def getNumberDof (self):
return self.client.basic.robot.getNumberDof ()
return self.client.robot.getNumberDof ()
## \}
## \name Joints
......@@ -139,41 +130,41 @@ class Builder (object):
## Get joint names in the same order as in the configuration.
def getJointNames (self):
return self.client.basic.robot.getJointNames ()
return self.client.robot.getJointNames ()
## Get joint names in the same order as in the configuration.
def getAllJointNames (self):
return self.client.basic.robot.getAllJointNames ()
return self.client.robot.getAllJointNames ()
## Get joint position.
def getJointPosition (self, jointName):
return self.client.basic.robot.getJointPosition (jointName)
return self.client.robot.getJointPosition (jointName)
## Set static position of joint in its parent frame
def setJointPosition (self, jointName, position):
return self.client.basic.robot.setJointPosition (jointName, position)
return self.client.robot.setJointPosition (jointName, position)
## Get joint number degrees of freedom.
def getJointNumberDof (self, jointName):
return self.client.basic.robot.getJointNumberDof (jointName)
return self.client.robot.getJointNumberDof (jointName)
## Get joint number config size.
def getJointConfigSize (self, jointName):
return self.client.basic.robot.getJointConfigSize (jointName)
return self.client.robot.getJointConfigSize (jointName)
## set bounds for the joint
def setJointBounds (self, jointName, inJointBound):
return self.client.basic.robot.setJointBounds (jointName, inJointBound)
return self.client.robot.setJointBounds (jointName, inJointBound)
## Set bounds on the translation part of the freeflyer joint.
#
# Valid only if the robot has a freeflyer joint.
def setTranslationBounds (self, xmin, xmax, ymin, ymax, zmin, zmax):
self.client.basic.robot.setJointBounds \
self.client.robot.setJointBounds \
(self.displayName + "base_joint_x", [xmin, xmax])
self.client.basic.robot.setJointBounds \
self.client.robot.setJointBounds \
(self.displayName + "base_joint_y", [ymin, ymax])
self.client.basic.robot.setJointBounds \
self.client.robot.setJointBounds \
(self.displayName + "base_joint_z", [zmin, zmax])
## Get link position in joint frame
......@@ -186,17 +177,17 @@ class Builder (object):
# \param jointName name of the joint
# \return position of the link in world frame.
def getLinkPosition (self, jointName):
return self.client.basic.robot.getLinkPosition (jointName)
return self.client.robot.getLinkPosition (jointName)
## Get link name
#
# \param jointName name of the joint,
# \return name of the link.
def getLinkName (self, jointName):
return self.client.basic.robot.getLinkName (jointName)
return self.client.robot.getLinkName (jointName)
def getLinkNames (self, jointName):
return self.client.basic.robot.getLinkNames (jointName)
return self.client.robot.getLinkNames (jointName)
## \}
## \name Access to current configuration
......@@ -206,18 +197,18 @@ class Builder (object):
#
# \param q configuration of the composite robot
def setCurrentConfig (self, q):
self.client.basic.robot.setCurrentConfig (q)
self.client.robot.setCurrentConfig (q)
## Get current configuration of composite robot