Definition of a manipulation planning problem. More...
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 |
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.
def manipulation::problem_solver::ProblemSolver::__init__ | ( | self, | |
robot | |||
) |
def manipulation::problem_solver::ProblemSolver::addGoalConfig | ( | self, | |
dofArray | |||
) |
Add goal configuration to specified problem.
dofArray | Array of degrees of freedom |
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.
name | of the partial com |
jointNames | list of joint name of each tree ROOT to consider. |
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.
name | name of the vector in the ProblemSolver map. |
dofNames | list 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.
q | initial configuration |
Error | if 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 | ) |
Clear the roadmap.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::configAtDistance | ( | self, | |
inPathId, | |||
atDistance | |||
) |
Get the robot's config at param on the a path.
inPathId | rank of the path in the problem |
atDistance | : the user parameter choice |
def manipulation::problem_solver::ProblemSolver::createComBeetweenFeet | ( | self, | |
constraintName, | |||
comName, | |||
jointLName, | |||
jointRName, | |||
pointL, | |||
pointR, | |||
jointRefName, | |||
mask | |||
) |
Create ComBeetweenFeet constraint between two joints.
constraintName | name of the constraint created, |
comName | name of CenterOfMassComputation |
jointLName | name of first joint |
jointRName | name of second joint |
pointL | point in local frame of jointL. |
pointR | point in local frame of jointR. |
jointRefName | name of second joint |
mask | Select 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.
constraintName | name of the resulting constraint, |
q0 | configuration 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.
graspName | key in the map of numerical constraints, |
jointName | name of the joint that grasps "robot/joint", |
handleName | name of the handle grasped "object/handle", |
handlePositioninJoint | position 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.
lockedDofName | key of the constraint in the map |
jointName | name of the joint |
value | value of the joint configuration |
compType | Comparison 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
constraintName | name of the constraint created, |
joint1Name | name of first joint |
joint2Name | name of second joint |
p | quaternion representing the desired orientation of joint2 in the frame of joint1. |
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 |
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.
constraintName | name of the constraint created, |
joint1Name | name of first joint |
joint2Name | name of second joint |
point1 | point in local frame of joint1, |
point2 | point in local frame of joint2. |
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 |
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.
preGraspName | key in the map of numerical constraints, |
jointName | name of the joint that grasps "robot/joint", |
handleName | name of the handle grasped "object/handle", |
handlePositioninJoint | position 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.
constraintName | name of the constraint created, |
comName | name of CenterOfMassComputation |
jointName | name of joint |
point | point in local frame of joint. |
mask | Select 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.
constraintName | name of the resulting constraint, |
q0 | configuration that satisfies the constraints, |
comName | name of a partial COM, |
type | Type of static stability constraints (Default value: ProblemSolver.SLIDING) |
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.
startConfig,endConfig,: | the configurations to link. |
Error | if 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 | |||
) |
Edge at given rank.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::generateValidConfig | ( | self, | |
maxIter | |||
) |
Generate a configuration satisfying the constraints.
maxIter | maximum number of tries, |
Error | if 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.
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.
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.
collision | whether to return obstacle for collision, |
distance | whether to return obstacles for distance computation |
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.
objectName | name of the polyhedron. |
cfg | Position of the obstacle. |
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.
pathId | rank 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.
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.
package | Name of the package containing the model, |
filename | name of the urdf file in the package (without suffix .urdf) |
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.
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.
jointName | name of the joint |
value | value of the locked degree of freedom, |
rankInConfiguration | rank of the locked dof in the joint configuration vector |
rankInVelocity | rank 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.
freeflyerBname | base name of the joint (It will be completed by '_xyz' and '_SO3'), |
lockJointBname | base name of the LockedJoint constraints (It will be completed by '_xyz' and '_SO3'), |
values | config of the locked joints (7 float) |
compType | Comparison 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.
jointName | name of the joint |
value | value 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.
planar | base name of the joint (It will be completed by '_xy' and '_rz'), |
lockJointBname | base name of the LockedJoint constraints (It will be completed by '_xy' and '_rz'), |
values | config of the locked joints (4 float) |
compType | Comparison 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.
objectName | name of the polyhedron. |
cfg | the configuration of the obstacle. |
Error. |
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::nodes | ( | self | ) |
Get nodes of the roadmap.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::nodesConnectedComponent | ( | self, | |
ccId | |||
) |
Nodes of a connected component.
connectedComponentId | index of connected component in roadmap |
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::numberConnectedComponents | ( | self | ) |
Number of connected components.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::numberEdges | ( | self | ) |
Number of edges.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::numberPaths | ( | self | ) |
Get Number of paths.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::optimizePath | ( | self, | |
inPathId | |||
) |
Optimize a given path.
inPathId | Id of the path in this problem. |
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.
inPathId | rank of the path in the problem |
def manipulation::problem_solver::ProblemSolver::removeObstacleFromJoint | ( | self, | |
objectName, | |||
jointName, | |||
collision, | |||
distance | |||
) |
Remove an obstacle from outer objects of a joint body.
objectName | name of the object to remove, |
jointName | name of the joint owning the body, |
collision | whether collision with object should be computed, |
distance | whether distance to object should be computed. |
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 | ) |
Reset goal configurations.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::selectPathOptimizer | ( | self, | |
pathOptimizerType | |||
) |
Select path optimizer type.
Name | of 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.
Name | of 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.
Name | of the path projector method, either "None" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathProjectorType, |
tolerance | maximal 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.
Name | of the path validation method, either "Discretized" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathValidationType, |
tolerance | maximal 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.
dofArray | Array of degrees of freedom |
Error. |
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::setMaxIterations | ( | self, | |
iterations | |||
) |
Set the maximal number of iterations.
References hpp::corbaserver::problem_solver::ProblemSolver::client, hpp::corbaserver::robot::Robot::client, and client.
def manipulation::problem_solver::ProblemSolver::setNumericalConstraints | ( | self, | |
name, | |||
names | |||
) |
Set numerical constraints in ConfigProjector.
name | name of the resulting numerical constraint obtained by stacking elementary numerical constraints, |
names | list 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.
manipulation::problem_solver::ProblemSolver::ALIGNED_COM = hpp.corbaserver.wholebody_step.Problem.ALIGNED_COM [static] |
Referenced by createStaticStabilityConstraints().
Referenced by balanceConstraints(), and createStaticStabilityConstraints().
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().
Referenced by createComplementStaticStabilityConstraints(), and createStaticStabilityConstraints().