To define and solve a path planning problem. More...
import "/local/robotpkg/var/tmp/robotpkg/path/hpp-corbaserver/work/hpp-corbaserver-3.1.0/idl/hpp/corbaserver/problem.idl";
Public Member Functions | |
Initial and goal configurations | |
void | setInitialConfig (in floatSeq dofArray) raises (Error) |
Set initial configuration of specified problem. | |
floatSeq | getInitialConfig () raises (Error) |
Get initial configuration of specified problem. | |
void | addGoalConfig (in floatSeq dofArray) raises (Error) |
Add goal configuration to specified problem. | |
floatSeqSeq | getGoalConfigs () raises (Error) |
Get goal configurations of specified problem. | |
void | resetGoalConfigs () raises (Error) |
Reset goal configurations. | |
Constraints | |
boolean | applyConstraints (in floatSeq input, out floatSeq output, out double residualError) raises (Error) |
Apply constaints to a configuration. | |
boolean | generateValidConfig (in unsigned short maxIter, out floatSeq output, out double residualError) raises (Error) |
Generate random configuration and apply constaints to a configuration. | |
void | createOrientationConstraint (in string constraintName, in string joint1Name, in string joint2Name, in Quaternion_ p, in boolSeq mask) raises (Error) |
Create orientation constraint between two joints. | |
void | createComBeetweenFeet (in string constraintName, in string comName, in string jointLName, in string jointRName, in floatSeq pointL, in floatSeq pointR, in string jointRefName, in boolSeq mask) raises (Error) |
Create ComBeetweenFeet constraint between two joints. | |
void | createRelativeComConstraint (in string constraintName, in string comName, in string jointLName, in floatSeq point, in boolSeq mask) raises (Error) |
Create RelativeCom constraint between two joints. | |
void | createStaticStabilityGravityConstraint (in string constraintName, in string jointName, in floatSeqSeq pts, in intSeqSeq objectTriangles, in intSeqSeq floorTriangles) raises (Error) |
void | createPositionConstraint (in string constraintName, in string joint1Name, in string joint2Name, in floatSeq point1, in floatSeq point2, in boolSeq mask) raises (Error) |
Create position constraint between two joints. | |
void | resetConstraints () raises (Error) |
Reset constraints. | |
void | addPassiveDofs (in string constraintName, in Names_t jointNames) raises (Error) |
Add vector of passive dofs into the ProblemSolver local map. | |
void | setConstantRightHandSide (in string constraintName, in boolean constant) raises (Error) |
(Dis-)Allow to modify right hand side of a numerical constraint | |
boolean | getConstantRightHandSide (in string constraintName) raises (Error) |
Get whether right hand side of a numerical constraint is constant. | |
void | setNumericalConstraints (in string constraintName, in Names_t constraintNames) raises (Error) |
Set numerical constraints by names in ConfigProjector. | |
void | lockJoint (in string jointName, in floatSeq value) raises (Error) |
Lock joint with given joint configuration. | |
void | setErrorThreshold (in double threshold) raises (Error) |
Set error threshold in numerical constraint resolution. | |
void | setMaxIterations (in unsigned short iterations) raises (Error) |
Set maximal number of iterations in numerical constraint resolution. | |
Solve problem and get paths | |
void | selectPathPlanner (in string pathPlannerType) raises (Error) |
Select path planner type. | |
void | selectPathOptimizer (in string pathOptimizerType) raises (Error) |
Select path optimizer type. | |
void | selectPathValidation (in string pathValidationType, in double tolerance) raises (Error) |
Select path validation method. | |
void | selectPathProjector (in string pathProjectorType, in double tolerance) raises (Error) |
Select path projector method. | |
boolean | prepareSolveStepByStep () raises (Error) |
Prepare the solver for a step by step planning. | |
boolean | executeOneStep () raises (Error) |
Execute one step of the planner. | |
void | finishSolveStepByStep () raises (Error) |
Finish the step-by-step planning. | |
double | solve () raises (Error) |
Solve the problem. | |
void | directPath (in floatSeq startConfig, in floatSeq endConfig) raises (Error) |
Make direct connection between two configurations. | |
short | numberPaths () raises (Error) |
Get Number of paths. | |
void | optimizePath (in unsigned short inPathId) raises (Error) |
Optimize a given path. | |
double | pathLength (in unsigned short inPathId) raises (Error) |
Get length of path. | |
floatSeq | configAtParam (in unsigned short inPathId, in double atDistance) raises (Error) |
Get the robot config at param on the a path. | |
floatSeqSeq | getWaypoints (in unsigned short pathId) raises (Error) |
Get way points of a path. | |
Interruption of a path planning request | |
void | interruptPathPlanning () raises (Error) |
Interrupt path planning activity. | |
exploring the roadmap | |
floatSeqSeq | nodes () raises (Error) |
Nodes of the roadmap. | |
long | numberEdges () raises (Error) |
Number of edges. | |
void | edge (in unsigned long edgeId, out floatSeq q1, out floatSeq q2) raises (Error) |
Edge at given rank. | |
long | numberConnectedComponents () |
Number of connected components. | |
floatSeqSeq | nodesConnectedComponent (in unsigned long connectedComponentId) raises (Error) |
Nodes of a connected component. | |
void | clearRoadmap () raises (Error) |
Clear the roadmap. | |
void | resetRoadmap () raises (Error) |
Reset the roadmap This should be done when joints bounds are modified because the KDTree must be resized. |
To define and solve a path planning problem.
void hpp::corbaserver::Problem::addGoalConfig | ( | in floatSeq | dofArray | ) | raises (Error) |
Add goal configuration to specified problem.
dofArray | Array of degrees of freedom |
Error. |
void hpp::corbaserver::Problem::addPassiveDofs | ( | in string | constraintName, |
in Names_t | jointNames | ||
) | raises (Error) |
Add vector of passive dofs into the ProblemSolver local map.
constraintName | Name of the vector of passive dofs, |
jointNames | a sequence of joint names. |
boolean hpp::corbaserver::Problem::applyConstraints | ( | in floatSeq | input, |
out floatSeq | output, | ||
out double | residualError | ||
) | raises (Error) |
Apply constaints to a configuration.
constraints are stored in ProblemSolver object
input | input configuration, |
output | output configuration, |
error | norm of the residual error. |
void hpp::corbaserver::Problem::clearRoadmap | ( | ) | raises (Error) |
Clear the roadmap.
floatSeq hpp::corbaserver::Problem::configAtParam | ( | in unsigned short | inPathId, |
in double | atDistance | ||
) | raises (Error) |
Get the robot config at param on the a path.
inPathId | rank of the path in the problem |
atDistance | : the user parameter choice |
void hpp::corbaserver::Problem::createComBeetweenFeet | ( | in string | constraintName, |
in string | comName, | ||
in string | jointLName, | ||
in string | jointRName, | ||
in floatSeq | pointL, | ||
in floatSeq | pointR, | ||
in string | jointRefName, | ||
in boolSeq | mask | ||
) | raises (Error) |
Create ComBeetweenFeet constraint between two joints.
constraintName | name of the constraint created, |
comName | name of a partial com computation object in problem solver map. Set "" for a full COM computation. |
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 global frame is used. Constraints are stored in ProblemSolver object |
void hpp::corbaserver::Problem::createOrientationConstraint | ( | in string | constraintName, |
in string | joint1Name, | ||
in string | joint2Name, | ||
in Quaternion_ | p, | ||
in boolSeq | mask | ||
) | raises (Error) |
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 |
void hpp::corbaserver::Problem::createPositionConstraint | ( | in string | constraintName, |
in string | joint1Name, | ||
in string | joint2Name, | ||
in floatSeq | point1, | ||
in floatSeq | point2, | ||
in boolSeq | mask | ||
) | raises (Error) |
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 |
void hpp::corbaserver::Problem::createRelativeComConstraint | ( | in string | constraintName, |
in string | comName, | ||
in string | jointLName, | ||
in floatSeq | point, | ||
in boolSeq | mask | ||
) | raises (Error) |
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 |
void hpp::corbaserver::Problem::createStaticStabilityGravityConstraint | ( | in string | constraintName, |
in string | jointName, | ||
in floatSeqSeq | pts, | ||
in intSeqSeq | objectTriangles, | ||
in intSeqSeq | floorTriangles | ||
) | raises (Error) |
void hpp::corbaserver::Problem::directPath | ( | in floatSeq | startConfig, |
in floatSeq | endConfig | ||
) | raises (Error) |
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 |
void hpp::corbaserver::Problem::edge | ( | in unsigned long | edgeId, |
out floatSeq | q1, | ||
out floatSeq | q2 | ||
) | raises (Error) |
Edge at given rank.
boolean hpp::corbaserver::Problem::executeOneStep | ( | ) | raises (Error) |
Execute one step of the planner.
void hpp::corbaserver::Problem::finishSolveStepByStep | ( | ) | raises (Error) |
Finish the step-by-step planning.
The path optimizer is not called
boolean hpp::corbaserver::Problem::generateValidConfig | ( | in unsigned short | maxIter, |
out floatSeq | output, | ||
out double | residualError | ||
) | raises (Error) |
Generate random configuration and apply constaints to a configuration.
constraints are stored in ProblemSolver object The class hpp::core::BasicConfigurationShooter is used for random generation of configuration.
maxIter | maximum number of tries, |
output | output configuration, |
error | norm of the residual error. |
boolean hpp::corbaserver::Problem::getConstantRightHandSide | ( | in string | constraintName | ) | raises (Error) |
Get whether right hand side of a numerical constraint is constant.
constraintName | Name of the numerical constraint, |
floatSeqSeq hpp::corbaserver::Problem::getGoalConfigs | ( | ) | raises (Error) |
Get goal configurations of specified problem.
floatSeq hpp::corbaserver::Problem::getInitialConfig | ( | ) | raises (Error) |
Get initial configuration of specified problem.
floatSeqSeq hpp::corbaserver::Problem::getWaypoints | ( | in unsigned short | pathId | ) | raises (Error) |
Get way points of a path.
pathId | rank of the path in the problem |
void hpp::corbaserver::Problem::interruptPathPlanning | ( | ) | raises (Error) |
Interrupt path planning activity.
void hpp::corbaserver::Problem::lockJoint | ( | in string | jointName, |
in floatSeq | value | ||
) | raises (Error) |
Lock joint with given joint configuration.
jointName | name of the joint |
value | value of the joint configuration |
floatSeqSeq hpp::corbaserver::Problem::nodes | ( | ) | raises (Error) |
Nodes of the roadmap.
floatSeqSeq hpp::corbaserver::Problem::nodesConnectedComponent | ( | in unsigned long | connectedComponentId | ) | raises (Error) |
Nodes of a connected component.
connectedComponentId | index of connected component in roadmap |
long hpp::corbaserver::Problem::numberConnectedComponents | ( | ) |
Number of connected components.
long hpp::corbaserver::Problem::numberEdges | ( | ) | raises (Error) |
Number of edges.
short hpp::corbaserver::Problem::numberPaths | ( | ) | raises (Error) |
Get Number of paths.
void hpp::corbaserver::Problem::optimizePath | ( | in unsigned short | inPathId | ) | raises (Error) |
Optimize a given path.
inPathId | Id of the path in this problem. |
Error. |
double hpp::corbaserver::Problem::pathLength | ( | in unsigned short | inPathId | ) | raises (Error) |
Get length of path.
inPathId | rank of the path in the problem |
boolean hpp::corbaserver::Problem::prepareSolveStepByStep | ( | ) | raises (Error) |
Prepare the solver for a step by step planning.
and try to make direct connections (call PathPlanner::tryDirectPath)
void hpp::corbaserver::Problem::resetConstraints | ( | ) | raises (Error) |
Reset constraints.
void hpp::corbaserver::Problem::resetGoalConfigs | ( | ) | raises (Error) |
Reset goal configurations.
void hpp::corbaserver::Problem::resetRoadmap | ( | ) | raises (Error) |
Reset the roadmap This should be done when joints bounds are modified because the KDTree must be resized.
void hpp::corbaserver::Problem::selectPathOptimizer | ( | in string | pathOptimizerType | ) | raises (Error) |
Select path optimizer type.
Name | of the path optimizer type, either "RandomShortcut" or any type added by core::ProblemSolver::addPathOptimizerType |
void hpp::corbaserver::Problem::selectPathPlanner | ( | in string | pathPlannerType | ) | raises (Error) |
Select path planner type.
Name | of the path planner type, either "DiffusingPlanner", "VisibilityPrmPlanner", or any type added by method core::ProblemSolver::addPathPlannerType |
void hpp::corbaserver::Problem::selectPathProjector | ( | in string | pathProjectorType, |
in double | tolerance | ||
) | raises (Error) |
Select path projector method.
Name | of the path projector method, either "None" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathProjectorType, |
tolerance |
void hpp::corbaserver::Problem::selectPathValidation | ( | in string | pathValidationType, |
in double | tolerance | ||
) | raises (Error) |
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. |
void hpp::corbaserver::Problem::setConstantRightHandSide | ( | in string | constraintName, |
in boolean | constant | ||
) | raises (Error) |
(Dis-)Allow to modify right hand side of a numerical constraint
constraintName | Name of the numerical constraint, |
constant | whether right hand side is constant |
Constraints should have been added in the ProblemSolver local map, but not inserted in the config projector.
void hpp::corbaserver::Problem::setErrorThreshold | ( | in double | threshold | ) | raises (Error) |
Set error threshold in numerical constraint resolution.
void hpp::corbaserver::Problem::setInitialConfig | ( | in floatSeq | dofArray | ) | raises (Error) |
Set initial configuration of specified problem.
dofArray | Array of degrees of freedom |
Error. |
void hpp::corbaserver::Problem::setMaxIterations | ( | in unsigned short | iterations | ) | raises (Error) |
Set maximal number of iterations in numerical constraint resolution.
void hpp::corbaserver::Problem::setNumericalConstraints | ( | in string | constraintName, |
in Names_t | constraintNames | ||
) | raises (Error) |
Set numerical constraints by names in ConfigProjector.
constraintName | Name of the set of numerical constraint, |
constraintNames | names of the constraints to apply, |
Constraints should have been added in the ProblemSolver local map.
double hpp::corbaserver::Problem::solve | ( | ) | raises (Error) |
Solve the problem.
Error. |