Unverified Commit 338503cc authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #13 from pFernbach/topic/remove_problemSolver

Remove rbprm.ProblemSolver python class
parents 275ad907 3f4f377d
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#reference pose for hyq
from hyq_ref_pose import hyq_ref
......@@ -10,7 +9,7 @@ from hyq_ref_pose import hyq_ref
import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
ins_dir = environ['DEVEL_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
......@@ -117,27 +116,9 @@ configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
pp = PathPlayer (fullBody.client, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = False, use_window = use_window,
verbose = verbose, draw = draw)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
import time
#DEMO METHODS
......@@ -234,7 +215,7 @@ d(0.01);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.client.basic.robot.getCenterOfMass
com = fullBody.getCenterOfMass()
s = None
def d1():
global s
......
......@@ -29,7 +29,7 @@ rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
......@@ -73,7 +73,7 @@ q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
rbprmBuilder.client.problem.optimizePath(i)
#~ pp(9)
......@@ -92,7 +92,7 @@ class Robot (Parent):
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.client = Client ()
self.load = load
#DEMO code to play root path and final contact plan
......
......@@ -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)
......@@ -17,302 +17,81 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
from hpp.corbaserver.robot import Robot
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
# trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class Builder (object):
## Constructor
def __init__ (self, load = True):
self.tf_root = "base_link"
self.rootJointType = dict()
self.client = CorbaClient ()
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):
if(isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
self.client.rbprm.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.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)
## Init RbprmShooter
#
def initshooter (self):
return self.client.rbprm.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)
## 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.client.rbprm.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.client.rbprm.rbprm.setFilter (romFilter)
## 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):
em.exportPath(viewer, self.client.basic.robot, problem, pathId, stepsize, filename)
## \name Degrees of freedom
# \{
## Get size of configuration
# \return size of configuration
def getConfigSize (self):
return self.client.basic.robot.getConfigSize ()
# Get size of velocity
# \return size of velocity
def getNumberDof (self):
return self.client.basic.robot.getNumberDof ()
## \}