Skip to content
Snippets Groups Projects
Commit 2d4f94bb authored by Steve Tonneau's avatar Steve Tonneau
Browse files

managed python bindings, can plan with a rbprmdevice

parent 428beda7
No related branches found
No related tags found
No related merge requests found
...@@ -119,6 +119,7 @@ INSTALL( ...@@ -119,6 +119,7 @@ INSTALL(
FILES FILES
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
) )
......
#!/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 ()
## \}
...@@ -42,18 +42,36 @@ class Builder (object): ...@@ -42,18 +42,36 @@ class Builder (object):
# \param load whether to actually load urdf files. Set to no if you only # \param load whether to actually load urdf files. Set to no if you only
# want to initialize a corba client to an already initialized # want to initialize a corba client to an already initialized
# problem. # problem.
def __init__ (self, trunkName, romName, rootJointType, load = True): def __init__ (self, load = True):
self.tf_root = "base_link" self.tf_root = "base_link"
self.rootJointType = dict() self.rootJointType = dict()
self.name = trunkName
self.displayName = trunkName
self.client = CorbaClient () self.client = CorbaClient ()
self.load = load self.load = load
self.loadModel (trunkName, romName, rootJointType)
## Virtual function to load the robot model ## Virtual function to load the robot model
def loadModel (self, trunkName, romName, rootJointType): def loadModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
self.client.rbprm.robot.create (trunkName, romName) self.client.rbprm.rbprm.loadRobotRomModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.client.rbprm.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.meshPackageName = meshPackageName
self.rankInConfiguration = dict ()
self.rankInVelocity = dict ()
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
rankInConfiguration = rankInVelocity = 0
for j in self.jointNames:
self.rankInConfiguration [j] = rankInConfiguration
rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
self.rankInVelocity [j] = rankInVelocity
rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
## \name Degrees of freedom ## \name Degrees of freedom
# \{ # \{
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment