manipulation::problem_solver::ProblemSolver Class Reference

Definition of a manipulation planning problem. More...

List of all members.

Public Member Functions

def __init__
Initial and goal configurations
def setInitialConfig
 Set initial configuration of specified problem.
def getInitialConfig
 Get initial configuration of specified problem.
def addGoalConfig
 Add goal configuration to specified problem.
def getGoalConfigs
 Get goal configurations of specified problem.
def resetGoalConfigs
 Reset goal configurations.
Obstacles
def loadObstacleFromUrdf
 Load obstacle from urdf file.
def removeObstacleFromJoint
 Remove an obstacle from outer objects of a joint body.
def moveObstacle
 Move an obstacle to a given configuration.
def getObstaclePosition
 Get the position of an obstacle.
def getObstacleNames
 Get list of obstacles.
Constraints
def createGrasp
 Create a grasp constraint for the composite robot.
def createPreGrasp
 Create a pre-grasp constraint for the composite robot.
def createStaticStabilityConstraints
 Create static stability constraints.
def createComplementStaticStabilityConstraints
 Create complement of static stability constraints.
def balanceConstraints
 Return balance constraints created by method ProblemSolver.createStaticStabilityConstraints.
def createOrientationConstraint
 Reset Constraints.
def createPositionConstraint
 Create position constraint between two joints.
def createRelativeComConstraint
 Create RelativeCom constraint between two joints.
def createComBeetweenFeet
 Create ComBeetweenFeet constraint between two joints.
def addPartialCom
 Add an object to compute a partial COM of the robot.
def addPassiveDofs
 Create a vector of passive dofs.
def resetConstraints
 Reset Constraints.
def setNumericalConstraints
 Set numerical constraints in ConfigProjector.
def applyConstraints
 Apply constraints.
def generateValidConfig
 Generate a configuration satisfying the constraints.
def createLockedJoint
 Insert a new LockedDof constraint with given value in the hpp::manipulation::ProblemSolver map.
def lockFreeFlyerJoint
 Lock degree of freedom of a FreeFlyer joint.
def lockPlanarJoint
 Lock degree of freedom of a planar joint.
def lockDof
 Lock degree of freedom with given value.
def lockOneDofJoint
 Lock joint with one degree of freedom with given value.
def setErrorThreshold
 error threshold in numerical constraint resolution
def setMaxIterations
 Set the maximal number of iterations.
Solve problem and get paths
def selectPathPlanner
 Select path planner type.
def selectPathOptimizer
 Select path optimizer type.
def selectPathValidation
 Select path validation method.
def selectPathProjector
 Select path projector method.
def solve
 Solve the problem of corresponding ChppPlanner object.
def directPath
 Make direct connection between two configurations.
def numberPaths
 Get Number of paths.
def optimizePath
 Optimize a given path.
def pathLength
 Get length of path.
def configAtDistance
 Get the robot's config at param on the a path.
def getWaypoints
 Get way points of a path.
Interruption of a path planning request
def interruptPathPlanning
 Interrupt path planning activity.
exploring the roadmap
def nodes
 Get nodes of the roadmap.
def numberEdges
 Number of edges.
def edge
 Edge at given rank.
def numberConnectedComponents
 Number of connected components.
def nodesConnectedComponent
 Nodes of a connected component.
def clearRoadmap
 Clear the roadmap.

Public Attributes

 client
 robot
 balanceConstraints_

Static Public Attributes

 SLIDING = hpp.corbaserver.wholebody_step.Problem.SLIDING
 ALIGNED_COM = hpp.corbaserver.wholebody_step.Problem.ALIGNED_COM

Detailed Description

Definition of a manipulation planning problem.

This class wraps the Corba client to the server implemented by libhpp-manipulation-corba.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.


Constructor & Destructor Documentation

def manipulation::problem_solver::ProblemSolver::__init__ (   self,
  robot 
)

Member Function Documentation

def manipulation::problem_solver::ProblemSolver::addGoalConfig (   self,
  dofArray 
)

Add goal configuration to specified problem.

Parameters:
dofArrayArray of degrees of freedom
Exceptions:
Error.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::addPartialCom (   self,
  comName,
  jointNames 
)

Add an object to compute a partial COM of the robot.

Parameters:
nameof the partial com
jointNameslist 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.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::addPassiveDofs (   self,
  name,
  dofNames 
)

Create a vector of passive dofs.

Parameters:
namename of the vector in the ProblemSolver map.
dofNameslist of names of DOF that may

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::applyConstraints (   self,
  q 
)

Apply constraints.

Parameters:
qinitial configuration
Returns:
configuration projected in success,
Exceptions:
Errorif projection failed.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::balanceConstraints (   self)

Return balance constraints created by method ProblemSolver.createStaticStabilityConstraints.

References balanceConstraints_.

def manipulation::problem_solver::ProblemSolver::clearRoadmap (   self)
def manipulation::problem_solver::ProblemSolver::configAtDistance (   self,
  inPathId,
  atDistance 
)

Get the robot's config at param on the a path.

Parameters:
inPathIdrank of the path in the problem
atDistance: the user parameter choice
Returns:
dofseq : the config at param
def manipulation::problem_solver::ProblemSolver::createComBeetweenFeet (   self,
  constraintName,
  comName,
  jointLName,
  jointRName,
  pointL,
  pointR,
  jointRefName,
  mask 
)

Create ComBeetweenFeet constraint between two joints.

Parameters:
constraintNamename of the constraint created,
comNamename of CenterOfMassComputation
jointLNamename of first joint
jointRNamename of second joint
pointLpoint in local frame of jointL.
pointRpoint in local frame of jointR.
jointRefNamename of second joint
maskSelect axis to be constrained. If jointRef is "", the robot root joint is used. Constraints are stored in ProblemSolver object

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createComplementStaticStabilityConstraints (   self,
  constraintName,
  q0 
)

Create complement of static stability constraints.

Call corba request hpp::corbaserver::wholebody_step::Problem::addComplementStaticStabilityConstraints

The ankles are defined by members leftAnkle and rightAnkle of variable robot passed at construction of this object.

Parameters:
constraintNamename of the resulting constraint,
q0configuration that satisfies the constraints

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, client, hpp::corbaserver::problem_solver::ProblemSolver::robot, and robot.

def manipulation::problem_solver::ProblemSolver::createGrasp (   self,
  graspName,
  gripperName,
  handleName 
)

Create a grasp constraint for the composite robot.

constraint is stored in C++ hpp::core::ProblemSolver local map of numerical constraints.

See also:
hpp::core::ProblemSolver::addNumericalConstraint
Parameters:
graspNamekey in the map of numerical constraints,
jointNamename of the joint that grasps "robot/joint",
handleNamename of the handle grasped "object/handle",
handlePositioninJointposition of the handle in the joint frame.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createLockedJoint (   self,
  lockedDofName,
  jointName,
  value,
  compType = "EqualToZero" 
)

Insert a new LockedDof constraint with given value in the hpp::manipulation::ProblemSolver map.

Parameters:
lockedDofNamekey of the constraint in the map
jointNamename of the joint
valuevalue of the joint configuration
compTypeComparison type: "Equality" or "EqualToZero"

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

Referenced by lockFreeFlyerJoint(), and lockPlanarJoint().

def manipulation::problem_solver::ProblemSolver::createOrientationConstraint (   self,
  constraintName,
  joint1Name,
  joint2Name,
  p,
  mask 
)

Reset Constraints.

Create orientation constraint between two joints

Parameters:
constraintNamename of the constraint created,
joint1Namename of first joint
joint2Namename of second joint
pquaternion representing the desired orientation of joint2 in the frame of joint1.
maskSelect 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

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createPositionConstraint (   self,
  constraintName,
  joint1Name,
  joint2Name,
  point1,
  point2,
  mask 
)

Create position constraint between two joints.

Parameters:
constraintNamename of the constraint created,
joint1Namename of first joint
joint2Namename of second joint
point1point in local frame of joint1,
point2point in local frame of joint2.
maskSelect 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

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createPreGrasp (   self,
  preGraspName,
  gripperName,
  handleName 
)

Create a pre-grasp constraint for the composite robot.

constraint is stored in C++ hpp::core::ProblemSolver local map of numerical constraints.

See also:
hpp::core::ProblemSolver::addNumericalConstraint
Parameters:
preGraspNamekey in the map of numerical constraints,
jointNamename of the joint that grasps "robot/joint",
handleNamename of the handle grasped "object/handle",
handlePositioninJointposition of the handle in the joint frame.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createRelativeComConstraint (   self,
  constraintName,
  comName,
  jointLName,
  point,
  mask 
)

Create RelativeCom constraint between two joints.

Parameters:
constraintNamename of the constraint created,
comNamename of CenterOfMassComputation
jointNamename of joint
pointpoint in local frame of joint.
maskSelect axis to be constrained. If jointName is "", the robot root joint is used. Constraints are stored in ProblemSolver object

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::createStaticStabilityConstraints (   self,
  constraintName,
  q0,
  comName = "",
  type = None 
)

Create static stability constraints.

Call corba request hpp::corbaserver::wholebody_step::Problem::addStaticStabilityConstraints

The ankles are defined by members leftAnkle and rightAnkle of variable robot passed at construction of this object.

Parameters:
constraintNamename of the resulting constraint,
q0configuration that satisfies the constraints,
comNamename of a partial COM,
typeType of static stability constraints (Default value: ProblemSolver.SLIDING)
See also:
hpp::corbaserver::wholebody_step::Problem::StaticStabilityType

References hpp::corbaserver::wholebody_step::Problem::ALIGNED_COM, ALIGNED_COM, balanceConstraints_, hpp::corbaserver::robot::Robot::client, hpp::corbaserver::problem_solver::ProblemSolver::client, client, hpp::corbaserver::problem_solver::ProblemSolver::robot, robot, hpp::corbaserver::wholebody_step::Problem::SLIDING, and SLIDING.

def manipulation::problem_solver::ProblemSolver::directPath (   self,
  startConfig,
  endConfig 
)

Make direct connection between two configurations.

Parameters:
startConfig,endConfig,:the configurations to link.
Exceptions:
Errorif steering method fails to create a direct path of if direct path is not valid

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::edge (   self,
  edgeId 
)
def manipulation::problem_solver::ProblemSolver::generateValidConfig (   self,
  maxIter 
)

Generate a configuration satisfying the constraints.

Parameters:
maxItermaximum number of tries,
Returns:
configuration projected in success,
Exceptions:
Errorif projection failed.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::getGoalConfigs (   self)

Get goal configurations of specified problem.

Returns:
Array of degrees of freedom

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::getInitialConfig (   self)

Get initial configuration of specified problem.

Returns:
Array of degrees of freedom

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::getObstacleNames (   self,
  collision,
  distance 
)

Get list of obstacles.

Parameters:
collisionwhether to return obstacle for collision,
distancewhether to return obstacles for distance computation
Returns:
list of obstacles

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::getObstaclePosition (   self,
  objectName 
)

Get the position of an obstacle.

Parameters:
objectNamename of the polyhedron.
Return values:
cfgPosition of the obstacle.
Exceptions:
Error.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::getWaypoints (   self,
  pathId 
)

Get way points of a path.

Parameters:
pathIdrank of the path in the problem

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::interruptPathPlanning (   self)

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.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::loadObstacleFromUrdf (   self,
  package,
  filename,
  prefix 
)

Load obstacle from urdf file.

Parameters:
packageName of the package containing the model,
filenamename of the urdf file in the package (without suffix .urdf)
prefixprefix 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.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::lockDof (   self,
  jointName,
  value,
  rankInConfiguration,
  rankInVelocity 
)

Lock degree of freedom with given value.

Parameters:
jointNamename of the joint
valuevalue of the locked degree of freedom,
rankInConfigurationrank of the locked dof in the joint configuration vector
rankInVelocityrank of the locked dof in the joint velocity vector

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::lockFreeFlyerJoint (   self,
  freeflyerBname,
  lockJointBname,
  values = (0,0,
  compType = "EqualToZero" 
)

Lock degree of freedom of a FreeFlyer joint.

Parameters:
freeflyerBnamebase name of the joint (It will be completed by '_xyz' and '_SO3'),
lockJointBnamebase name of the LockedJoint constraints (It will be completed by '_xyz' and '_SO3'),
valuesconfig of the locked joints (7 float)
compTypeComparison type: "Equality" or "EqualToZero"

References hpp::corbaserver::manipulation::Problem::createLockedJoint(), and createLockedJoint().

def manipulation::problem_solver::ProblemSolver::lockOneDofJoint (   self,
  jointName,
  value 
)

Lock joint with one degree of freedom with given value.

Parameters:
jointNamename of the joint
valuevalue of the locked degree of freedom,

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::lockPlanarJoint (   self,
  planarBname,
  lockJointBname,
  values = (0,0,
  compType = "EqualToZero" 
)

Lock degree of freedom of a planar joint.

Parameters:
planarbase name of the joint (It will be completed by '_xy' and '_rz'),
lockJointBnamebase name of the LockedJoint constraints (It will be completed by '_xy' and '_rz'),
valuesconfig of the locked joints (4 float)
compTypeComparison type: "Equality" or "EqualToZero"

References hpp::corbaserver::manipulation::Problem::createLockedJoint(), and createLockedJoint().

def manipulation::problem_solver::ProblemSolver::moveObstacle (   self,
  objectName,
  cfg 
)

Move an obstacle to a given configuration.

Parameters:
objectNamename of the polyhedron.
cfgthe configuration of the obstacle.
Exceptions:
Error.
Note:
The obstacle is not added to local map impl::Obstacle::collisionListMap.
Build the collision entity of polyhedron for KCD.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::nodes (   self)
def manipulation::problem_solver::ProblemSolver::nodesConnectedComponent (   self,
  ccId 
)

Nodes of a connected component.

Parameters:
connectedComponentIdindex of connected component in roadmap
Returns:
list of nodes of the connected component.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::numberConnectedComponents (   self)
def manipulation::problem_solver::ProblemSolver::numberEdges (   self)
def manipulation::problem_solver::ProblemSolver::numberPaths (   self)
def manipulation::problem_solver::ProblemSolver::optimizePath (   self,
  inPathId 
)

Optimize a given path.

Parameters:
inPathIdId of the path in this problem.
Exceptions:
Error.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::pathLength (   self,
  inPathId 
)

Get length of path.

Parameters:
inPathIdrank of the path in the problem
Returns:
length of path if path exists.
def manipulation::problem_solver::ProblemSolver::removeObstacleFromJoint (   self,
  objectName,
  jointName,
  collision,
  distance 
)

Remove an obstacle from outer objects of a joint body.

Parameters:
objectNamename of the object to remove,
jointNamename of the joint owning the body,
collisionwhether collision with object should be computed,
distancewhether distance to object should be computed.
Exceptions:
Error.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::resetConstraints (   self)

Reset Constraints.

Reset all constraints, including numerical constraints and locked degrees of freedom.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::resetGoalConfigs (   self)
def manipulation::problem_solver::ProblemSolver::selectPathOptimizer (   self,
  pathOptimizerType 
)

Select path optimizer type.

Parameters:
Nameof the path optimizer type, either "RandomShortcut" or any type added by core::ProblemSolver::addPathOptimizerType

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::selectPathPlanner (   self,
  pathPlannerType 
)

Select path planner type.

Parameters:
Nameof the path planner type, either "DiffusingPlanner", "VisibilityPrmPlanner", or any type added by method core::ProblemSolver::addPathPlannerType

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::selectPathProjector (   self,
  pathProjectorType,
  tolerance 
)

Select path projector method.

Parameters:
Nameof the path projector method, either "None" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathProjectorType,
tolerancemaximal acceptable penetration.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::selectPathValidation (   self,
  pathValidationType,
  tolerance 
)

Select path validation method.

Parameters:
Nameof the path validation method, either "Discretized" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathValidationType,
tolerancemaximal acceptable penetration.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::setErrorThreshold (   self,
  threshold 
)

error threshold in numerical constraint resolution

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::setInitialConfig (   self,
  dofArray 
)

Set initial configuration of specified problem.

Parameters:
dofArrayArray of degrees of freedom
Exceptions:
Error.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::setMaxIterations (   self,
  iterations 
)
def manipulation::problem_solver::ProblemSolver::setNumericalConstraints (   self,
  name,
  names 
)

Set numerical constraints in ConfigProjector.

Parameters:
namename of the resulting numerical constraint obtained by stacking elementary numerical constraints,
nameslist of names of the numerical constraints as inserted by method hpp::core::ProblemSolver::addNumericalConstraint.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.

def manipulation::problem_solver::ProblemSolver::solve (   self)

Solve the problem of corresponding ChppPlanner object.

References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.


Member Data Documentation

Referenced by addGoalConfig(), addPartialCom(), addPassiveDofs(), applyConstraints(), clearRoadmap(), manipulation::robot::Robot::collisionTest(), createComBeetweenFeet(), createComplementStaticStabilityConstraints(), manipulation::constraint_graph::ConstraintGraph::createGrasp(), createGrasp(), createLockedJoint(), createOrientationConstraint(), createPositionConstraint(), manipulation::constraint_graph::ConstraintGraph::createPreGrasp(), createPreGrasp(), createRelativeComConstraint(), createStaticStabilityConstraints(), directPath(), manipulation::robot::Robot::distancesToCollision(), edge(), generateValidConfig(), manipulation::robot::Robot::getAllJointNames(), manipulation::robot::Robot::getCenterOfMass(), manipulation::robot::Robot::getConfigSize(), manipulation::robot::Robot::getCurrentConfig(), getGoalConfigs(), getInitialConfig(), manipulation::robot::Robot::getJacobianCenterOfMass(), manipulation::robot::Robot::getJointConfigSize(), manipulation::robot::Robot::getJointInnerObjects(), manipulation::robot::Robot::getJointNames(), manipulation::robot::Robot::getJointNumberDof(), manipulation::robot::Robot::getJointOuterObjects(), manipulation::robot::Robot::getJointPosition(), manipulation::robot::Robot::getLinkName(), manipulation::robot::Robot::getLinkPosition(), manipulation::robot::Robot::getMass(), manipulation::robot::Robot::getNumberDof(), manipulation::robot::Robot::getObjectPosition(), getObstacleNames(), getObstaclePosition(), getWaypoints(), manipulation::robot::Robot::insertObjectModel(), manipulation::robot::Robot::insertRobotModel(), interruptPathPlanning(), manipulation::robot::Robot::isConfigValid(), manipulation::robot::Robot::loadEnvironmentModel(), manipulation::robot::Robot::loadHumanoidModel(), manipulation::robot::Robot::loadModel(), manipulation::robot::HumanoidRobot::loadModel(), loadObstacleFromUrdf(), lockDof(), lockOneDofJoint(), moveObstacle(), nodes(), nodesConnectedComponent(), numberConnectedComponents(), numberEdges(), numberPaths(), optimizePath(), manipulation::robot::Robot::rebuildRanks(), removeObstacleFromJoint(), manipulation::robot::Robot::removeObstacleFromJoint(), resetConstraints(), resetGoalConfigs(), selectPathOptimizer(), selectPathPlanner(), selectPathProjector(), selectPathValidation(), manipulation::robot::Robot::setCurrentConfig(), setErrorThreshold(), setInitialConfig(), manipulation::robot::Robot::setJointBounds(), manipulation::robot::Robot::setJointPosition(), setMaxIterations(), setNumericalConstraints(), manipulation::robot::Robot::setTranslationBounds(), manipulation::robot::Robot::shootRandomConfig(), and solve().