Definition of a path 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 | createOrientationConstraint |
Create orientation 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 | createPositionConstraint |
Create position constraint between two joints. | |
def | resetConstraints |
Reset Constraints. | |
def | setNumericalConstraints |
Set numerical constraints in ConfigProjector. | |
def | applyConstraints |
Apply constraints. | |
def | addPassiveDofs |
Create a vector of passive dofs. | |
def | generateValidConfig |
Generate a configuration satisfying the constraints. | |
def | lockJoint |
Lock joint with given joint configuration. | |
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 | configAtParam |
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 |
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.
def hpp::corbaserver::problem_solver::ProblemSolver::__init__ | ( | self, | |
robot | |||
) |
def hpp::corbaserver::problem_solver::ProblemSolver::addGoalConfig | ( | self, | |
dofArray | |||
) |
Add goal configuration to specified problem.
dofArray | Array of degrees of freedom |
Error. |
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::applyConstraints | ( | self, | |
q | |||
) |
def hpp::corbaserver::problem_solver::ProblemSolver::clearRoadmap | ( | self | ) |
Clear the roadmap.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::configAtParam | ( | 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 |
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::createOrientationConstraint | ( | self, | |
constraintName, | |||
joint1Name, | |||
joint2Name, | |||
p, | |||
mask | |||
) |
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 or joint2 is "", the corresponding joint is replaced by the global frame. constraints are stored in ProblemSolver object |
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::directPath | ( | self, | |
startConfig, | |||
endConfig | |||
) |
def hpp::corbaserver::problem_solver::ProblemSolver::edge | ( | self, | |
edgeId | |||
) |
Edge at given rank.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::generateValidConfig | ( | self, | |
maxIter | |||
) |
def hpp::corbaserver::problem_solver::ProblemSolver::getGoalConfigs | ( | self | ) |
Get goal configurations of specified problem.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::getInitialConfig | ( | self | ) |
Get initial configuration of specified problem.
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::getObstaclePosition | ( | self, | |
objectName | |||
) |
Get the position of an obstacle.
objectName | name of the polyhedron. |
cfg | Position of the obstacle. |
Error. |
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::getWaypoints | ( | self, | |
pathId | |||
) |
def hpp::corbaserver::problem_solver::ProblemSolver::interruptPathPlanning | ( | self | ) |
Interrupt path planning activity.
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::lockJoint | ( | self, | |
jointName, | |||
value | |||
) |
Lock joint with given joint configuration.
jointName | name of the joint |
value | value of the joint configuration |
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::nodes | ( | self | ) |
Get nodes of the roadmap.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::nodesConnectedComponent | ( | self, | |
ccId | |||
) |
Nodes of a connected component.
connectedComponentId | index of connected component in roadmap |
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::numberConnectedComponents | ( | self | ) |
Number of connected components.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::numberEdges | ( | self | ) |
Number of edges.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::numberPaths | ( | self | ) |
Get Number of paths.
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::optimizePath | ( | self, | |
inPathId | |||
) |
Optimize a given path.
inPathId | Id of the path in this problem. |
Error. |
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::pathLength | ( | self, | |
inPathId | |||
) |
Get length of path.
inPathId | rank of the path in the problem |
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::resetConstraints | ( | self | ) |
Reset Constraints.
Reset all constraints, including numerical constraints and locked joints
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::resetGoalConfigs | ( | self | ) |
Reset goal configurations.
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::selectPathProjector | ( | self, | |
pathProjectorType, | |||
tolerance | |||
) |
Select path projector method.
Name | of the path projector method, either "Discretized" "Progressive", "Dichotomy", or any type added by core::ProblemSolver::addPathProjectorType, |
tolerance | maximal acceptable penetration. |
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::setErrorThreshold | ( | self, | |
threshold | |||
) |
error threshold in numerical constraint resolution
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::setInitialConfig | ( | self, | |
dofArray | |||
) |
Set initial configuration of specified problem.
dofArray | Array of degrees of freedom |
Error. |
References client.
def hpp::corbaserver::problem_solver::ProblemSolver::setMaxIterations | ( | self, | |
iterations | |||
) |
Set the maximal number of iterations.
References client.
def hpp::corbaserver::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 client.
def hpp::corbaserver::problem_solver::ProblemSolver::solve | ( | self | ) |
Solve the problem of corresponding ChppPlanner object.
References client.
Referenced by addGoalConfig(), addPartialCom(), addPassiveDofs(), applyConstraints(), clearRoadmap(), hpp::corbaserver::robot::Robot::collisionTest(), configAtParam(), createComBeetweenFeet(), createOrientationConstraint(), createPositionConstraint(), createRelativeComConstraint(), directPath(), hpp::corbaserver::robot::Robot::distancesToCollision(), edge(), generateValidConfig(), hpp::corbaserver::robot::Robot::getAllJointNames(), hpp::corbaserver::robot::Robot::getCenterOfMass(), hpp::corbaserver::robot::Robot::getConfigSize(), hpp::corbaserver::robot::Robot::getCurrentConfig(), getGoalConfigs(), getInitialConfig(), hpp::corbaserver::robot::Robot::getJacobianCenterOfMass(), hpp::corbaserver::robot::Robot::getJointConfigSize(), hpp::corbaserver::robot::Robot::getJointInnerObjects(), hpp::corbaserver::robot::Robot::getJointNames(), hpp::corbaserver::robot::Robot::getJointNumberDof(), hpp::corbaserver::robot::Robot::getJointOuterObjects(), hpp::corbaserver::robot::Robot::getJointPosition(), hpp::corbaserver::robot::Robot::getLinkName(), hpp::corbaserver::robot::Robot::getLinkPosition(), hpp::corbaserver::robot::Robot::getMass(), hpp::corbaserver::robot::Robot::getNumberDof(), hpp::corbaserver::robot::Robot::getObjectPosition(), getObstacleNames(), getObstaclePosition(), hpp::corbaserver::robot::Robot::getRootJointPosition(), getWaypoints(), interruptPathPlanning(), hpp::corbaserver::robot::Robot::isConfigValid(), hpp::corbaserver::robot::Robot::loadModel(), hpp::corbaserver::robot::HumanoidRobot::loadModel(), loadObstacleFromUrdf(), lockJoint(), moveObstacle(), nodes(), nodesConnectedComponent(), numberConnectedComponents(), numberEdges(), numberPaths(), optimizePath(), removeObstacleFromJoint(), hpp::corbaserver::robot::Robot::removeObstacleFromJoint(), resetConstraints(), resetGoalConfigs(), selectPathOptimizer(), selectPathPlanner(), selectPathProjector(), selectPathValidation(), hpp::corbaserver::robot::Robot::setCurrentConfig(), setErrorThreshold(), setInitialConfig(), hpp::corbaserver::robot::Robot::setJointBounds(), hpp::corbaserver::robot::Robot::setJointPosition(), setMaxIterations(), setNumericalConstraints(), hpp::corbaserver::robot::Robot::setRootJointPosition(), hpp::corbaserver::robot::Robot::shootRandomConfig(), and solve().