hpp-rbprm
4.15.1
Implementation of RB-PRM planner using hpp.
|
Classes | |
struct | ComRRTShooterFactory |
struct | EffectorRRTShooterFactory |
struct | SetComRRTConstraints |
class | ComTrajectory |
struct | VecRightSide |
struct | funEvaluator |
struct | LimbRRTShooterFactory |
struct | SetLimbRRTConstraints |
class | PolynomTrajectory |
class | RbPrmInterpolation |
struct | SetEffectorRRTConstraints |
struct | EndEffectorPath |
class | TimeConstraintHelper |
class | TimeConstraintPathValidation |
class | TimeConstraintPath |
class | TimeConstraintShooter |
class | TimeConstraintSteering |
struct | RightHandSideFunctor |
Time varying right hand side of constraint. More... | |
struct | TimeDependant |
Typedefs | |
typedef TimeConstraintHelper< TimeConstraintPath, ComRRTShooterFactory, SetComRRTConstraints > | ComRRTHelper |
typedef shared_ptr< ComTrajectory > | ComTrajectoryPtr_t |
typedef constraints::PointCom | PointCom |
typedef constraints::CalculusBaseAbstract< PointCom::ValueType_t, PointCom::JacobianType_t > | s_t |
typedef constraints::SymbolicFunction< s_t > | PointComFunction |
typedef constraints::SymbolicFunction< s_t >::Ptr_t | PointComFunctionPtr_t |
typedef TimeConstraintHelper< TimeConstraintPath, LimbRRTShooterFactory, SetLimbRRTConstraints > | LimbRRTHelper |
typedef shared_ptr< PolynomTrajectory > | PolynomTrajectoryPtr_t |
typedef ndcurves::curve_abc< core::value_type, core::value_type, true, Eigen::Vector3d > | Polynom |
typedef shared_ptr< Polynom > | PolynomPtr_t |
typedef shared_ptr< RbPrmInterpolation > | RbPrmInterpolationPtr_t |
typedef TimeConstraintHelper< TimeConstraintPath, EffectorRRTShooterFactory, SetEffectorRRTConstraints > | EffectorRRTHelper |
typedef ndcurves::exact_cubic< double, double, true, Eigen::Matrix< value_type, 3, 1 > > | exact_cubic_t |
typedef ndcurves::curve_constraints< Eigen::Matrix< value_type, 3, 1 > > | curve_constraint_t |
typedef shared_ptr< exact_cubic_t > | exact_cubic_Ptr |
typedef shared_ptr< TimeConstraintPathValidation > | TimeConstraintPathValidationPtr_t |
typedef shared_ptr< TimeConstraintPath > | TimeConstraintPathPtr_t |
typedef shared_ptr< TimeConstraintShooter > | TimeConstraintShooterPtr_t |
typedef shared_ptr< const RightHandSideFunctor > | RightHandSideFunctorPtr_t |
typedef std::vector< TimeDependant > | T_TimeDependant |
typedef T_TimeDependant::const_iterator | CIT_TimeDependant |
Functions | |
core::PathPtr_t | comRRT (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof=false) |
core::PathPtr_t | comRRT (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof=false) |
core::PathPtr_t | comRRTFromPath (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const PathPtr_t guidePath, const CIT_StateFrame &startState, const CIT_StateFrame &endState, const std::size_t numOptimizations) |
core::Configuration_t | projectOnCom (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const State &model, const fcl::Vec3f &targetCom, bool &success) |
HPP_PREDEF_CLASS (ComTrajectory) | |
template<class Helper_T > | |
void | CreateContactConstraints (const State &from, const State &to) |
template<class Helper_T , typename Reference > | |
void | CreateEffectorConstraint (Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Vec3f &initTarget=fcl::Vec3f()) |
template<class Helper_T , typename Reference > | |
void | Create6DEffectorConstraint (Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f()) |
template<class Helper_T , typename Reference > | |
void | CreateOrientationConstraint (Helper_T &helper, const Reference &ref, const pinocchio::Frame effector, const pinocchio::DevicePtr_t endEffectorDevice, const fcl::Transform3f &initTarget=fcl::Transform3f()) |
template<class Helper_T , typename Reference > | |
void | CreateComConstraint (Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f()) |
template<class Helper_T , typename Reference > | |
void | CreatePosturalTaskConstraint (Helper_T &helper, const Reference &ref) |
constraints::PositionPtr_t | createPositionMethod (pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame) |
constraints::OrientationPtr_t | createOrientationMethod (pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame) |
void | addContactConstraints (rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State &state, const std::vector< std::string > active) |
template<class Helper_T > | |
void | CreateContactConstraints (Helper_T &helper, const State &from, const State &to) |
std::string | getEffectorLimb (const State &startState, const State &nextState) |
fcl::Vec3f | getNormal (const std::string &effector, const State &state, bool &found) |
pinocchio::Frame | getEffector (RbPrmFullBodyPtr_t fullbody, const State &startState, const State &nextState) |
DevicePtr_t | createFreeFlyerDevice () |
core::PathPtr_t | limbRRT (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const rbprm::CIT_State &startState, const rbprm::CIT_State &endState, const std::size_t numOptimizations, const std::size_t maxIteration=0) |
core::PathPtr_t | limbRRTFromPath (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const PathPtr_t refPath, const CIT_StateFrame &startState, const CIT_StateFrame &endState, const std::size_t numOptimizations) |
HPP_PREDEF_CLASS (PolynomTrajectory) | |
HPP_PREDEF_CLASS (RbPrmInterpolation) | |
core::PathPtr_t | effectorRRT (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof) |
core::PathPtr_t | effectorRRT (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof, const std::vector< std::string > &constrainedJointPos=std::vector< std::string >(), const std::vector< std::string > &constrainedLockedJoints=std::vector< std::string >()) |
core::PathPtr_t | generateEndEffectorBezier (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState) |
PathPtr_t | effectorRRTFromPath (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const PathPtr_t fullBodyComPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof, const PathPtr_t refPath, const std::vector< std::string > &constrainedJointPos, const std::vector< std::string > &constrainedLockedJoints) |
effectorRRTFromPath Call comRRT to compute a whole body path between two states, then compute an end-effector's trajectory with a bezier curve that fit the initial path found by the rrt, and recompute the whole body trajectory that follow the end effector constraint More... | |
core::PathPtr_t | effectorRRTFromPath (RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof, const PathPtr_t refFullBodyPath, const std::vector< std::string > &constrainedJointPos=std::vector< std::string >(), const std::vector< std::string > &constrainedLockedJoints=std::vector< std::string >()) |
effectorRRTFromPath Call comRRT to compute a whole body path between two states, then compute an end-effector's trajectory with a bezier curve that fit the initial path found by the rrt, and recompute the whole body trajectory that follow the end effector constraint More... | |
std::vector< core::PathVectorPtr_t > | fitBeziersToPath (RbPrmFullBodyPtr_t fullbody, const pinocchio::Frame &effector, const value_type comPathLength, const PathPtr_t fullBodyComPath, const State &startState, const State &nextState) |
fitBeziersToPath generate a vector of pathVector : each pathVector containt BezierPath, computed with varying value of weight-rrt More... | |
template<class Helper_T , class ShooterFactory_T , typename ConstraintFactory_T , typename StateConstIterator > | |
core::PathPtr_t HPP_RBPRM_DLLAPI | interpolateStates (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, const StateConstIterator &startState, const StateConstIterator &endState, const std::size_t numOptimizations=10, const bool keepExtraDof=false, const pinocchio::value_type error_treshold=0.001, const size_t maxIterations=0) |
template<class Helper_T , class ShooterFactory_T , typename ConstraintFactory_T > | |
core::PathPtr_t HPP_RBPRM_DLLAPI | interpolateStatesFromPath (RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, const core::PathPtr_t refPath, const CIT_StateFrame &startState, const CIT_StateFrame &endState, const std::size_t numOptimizations=10, const bool keepExtraDof=false, const pinocchio::value_type error_treshold=0.001, const size_t maxIterations=0) |
HPP_PREDEF_CLASS (TimeConstraintPathValidation) | |
HPP_PREDEF_CLASS (TimeConstraintPath) | |
HPP_PREDEF_CLASS (TimeConstraintShooter) | |
template<class Helper_T > | |
void | SetPathValidation (Helper_T &helper) |
template<class Helper_T > | |
core::ConfigurationPtr_t | TimeConfigFromDevice (const Helper_T &helper, const State &state, const double time) |
void | UpdateConstraints (core::ConfigurationOut_t configuration, const T_TimeDependant &tds, const std::size_t pathDofRank) |
typedef T_TimeDependant::const_iterator hpp::rbprm::interpolation::CIT_TimeDependant |
typedef TimeConstraintHelper<TimeConstraintPath, ComRRTShooterFactory, SetComRRTConstraints> hpp::rbprm::interpolation::ComRRTHelper |
typedef shared_ptr<ComTrajectory> hpp::rbprm::interpolation::ComTrajectoryPtr_t |
typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> > hpp::rbprm::interpolation::curve_constraint_t |
typedef TimeConstraintHelper<TimeConstraintPath, EffectorRRTShooterFactory, SetEffectorRRTConstraints> hpp::rbprm::interpolation::EffectorRRTHelper |
typedef shared_ptr<exact_cubic_t> hpp::rbprm::interpolation::exact_cubic_Ptr |
typedef ndcurves::exact_cubic<double, double, true, Eigen::Matrix<value_type, 3, 1> > hpp::rbprm::interpolation::exact_cubic_t |
typedef TimeConstraintHelper<TimeConstraintPath, LimbRRTShooterFactory, SetLimbRRTConstraints> hpp::rbprm::interpolation::LimbRRTHelper |
typedef constraints::PointCom hpp::rbprm::interpolation::PointCom |
typedef constraints::SymbolicFunction<s_t> hpp::rbprm::interpolation::PointComFunction |
typedef constraints::SymbolicFunction<s_t>::Ptr_t hpp::rbprm::interpolation::PointComFunctionPtr_t |
typedef ndcurves::curve_abc<core::value_type, core::value_type, true, Eigen::Vector3d> hpp::rbprm::interpolation::Polynom |
typedef shared_ptr<Polynom> hpp::rbprm::interpolation::PolynomPtr_t |
typedef shared_ptr<PolynomTrajectory> hpp::rbprm::interpolation::PolynomTrajectoryPtr_t |
typedef shared_ptr<RbPrmInterpolation> hpp::rbprm::interpolation::RbPrmInterpolationPtr_t |
typedef shared_ptr<const RightHandSideFunctor> hpp::rbprm::interpolation::RightHandSideFunctorPtr_t |
typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t> hpp::rbprm::interpolation::s_t |
typedef std::vector<TimeDependant> hpp::rbprm::interpolation::T_TimeDependant |
typedef shared_ptr<TimeConstraintPath> hpp::rbprm::interpolation::TimeConstraintPathPtr_t |
typedef shared_ptr<TimeConstraintPathValidation> hpp::rbprm::interpolation::TimeConstraintPathValidationPtr_t |
typedef shared_ptr<TimeConstraintShooter> hpp::rbprm::interpolation::TimeConstraintShooterPtr_t |
void hpp::rbprm::interpolation::addContactConstraints | ( | rbprm::RbPrmFullBodyPtr_t | fullBody, |
pinocchio::DevicePtr_t | device, | ||
core::ConfigProjectorPtr_t | projector, | ||
const State & | state, | ||
const std::vector< std::string > | active | ||
) |
core::PathPtr_t hpp::rbprm::interpolation::comRRT | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof = false |
||
) |
core::PathPtr_t hpp::rbprm::interpolation::comRRT | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof = false |
||
) |
core::PathPtr_t hpp::rbprm::interpolation::comRRTFromPath | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const PathPtr_t | guidePath, | ||
const CIT_StateFrame & | startState, | ||
const CIT_StateFrame & | endState, | ||
const std::size_t | numOptimizations | ||
) |
void hpp::rbprm::interpolation::Create6DEffectorConstraint | ( | Helper_T & | helper, |
const Reference & | ref, | ||
const pinocchio::Frame | effectorJoint, | ||
const fcl::Transform3f & | initTarget = fcl::Transform3f() |
||
) |
void hpp::rbprm::interpolation::CreateComConstraint | ( | Helper_T & | helper, |
const Reference & | ref, | ||
const fcl::Vec3f & | initTarget = fcl::Vec3f() |
||
) |
void hpp::rbprm::interpolation::CreateContactConstraints | ( | const State & | from, |
const State & | to | ||
) |
void hpp::rbprm::interpolation::CreateContactConstraints | ( | Helper_T & | helper, |
const State & | from, | ||
const State & | to | ||
) |
void hpp::rbprm::interpolation::CreateEffectorConstraint | ( | Helper_T & | helper, |
const Reference & | ref, | ||
const pinocchio::Frame | effectorJoint, | ||
const fcl::Vec3f & | initTarget = fcl::Vec3f() |
||
) |
DevicePtr_t hpp::rbprm::interpolation::createFreeFlyerDevice | ( | ) |
void hpp::rbprm::interpolation::CreateOrientationConstraint | ( | Helper_T & | helper, |
const Reference & | ref, | ||
const pinocchio::Frame | effector, | ||
const pinocchio::DevicePtr_t | endEffectorDevice, | ||
const fcl::Transform3f & | initTarget = fcl::Transform3f() |
||
) |
|
inline |
|
inline |
void hpp::rbprm::interpolation::CreatePosturalTaskConstraint | ( | Helper_T & | helper, |
const Reference & | ref | ||
) |
core::PathPtr_t hpp::rbprm::interpolation::effectorRRT | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof | ||
) |
core::PathPtr_t hpp::rbprm::interpolation::effectorRRT | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof, | ||
const std::vector< std::string > & | constrainedJointPos = std::vector< std::string >() , |
||
const std::vector< std::string > & | constrainedLockedJoints = std::vector< std::string >() |
||
) |
PathPtr_t hpp::rbprm::interpolation::effectorRRTFromPath | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const PathPtr_t | fullBodyComPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof, | ||
const PathPtr_t | refPath, | ||
const std::vector< std::string > & | constrainedJointPos, | ||
const std::vector< std::string > & | constrainedLockedJoints | ||
) |
effectorRRTFromPath Call comRRT to compute a whole body path between two states, then compute an end-effector's trajectory with a bezier curve that fit the initial path found by the rrt, and recompute the whole body trajectory that follow the end effector constraint
fullbody | |
referenceProblem | |
comPath | reference path for the center of mass |
fullBodyComPath | fullBody path previously computed |
startState | |
nextState | |
numOptimizations | |
keepExtraDof | if false, remove the additionnal extraDoF introduced by comRRT |
pathId | the Id of the returned path in the problem-solver. Usef to match with the end-effector path indice stored in fullBody |
refFullBodyPath | |
constrainedJointPos | |
constrainedLockedJoints |
core::PathPtr_t hpp::rbprm::interpolation::effectorRRTFromPath | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState, | ||
const std::size_t | numOptimizations, | ||
const bool | keepExtraDof, | ||
const PathPtr_t | refFullBodyPath, | ||
const std::vector< std::string > & | constrainedJointPos = std::vector< std::string >() , |
||
const std::vector< std::string > & | constrainedLockedJoints = std::vector< std::string >() |
||
) |
effectorRRTFromPath Call comRRT to compute a whole body path between two states, then compute an end-effector's trajectory with a bezier curve that fit the initial path found by the rrt, and recompute the whole body trajectory that follow the end effector constraint
fullbody | |
referenceProblem | |
comPath | reference path for the center of mass |
startState | |
nextState | |
numOptimizations | |
keepExtraDof | if false, remove the additionnal extraDoF introduced by comRRT |
pathId | the Id of the returned path in the problem-solver. Usef to match with the end-effector path indice stored in fullBody |
refFullBodyPath | |
constrainedJointPos | |
constrainedLockedJoints |
std::vector<core::PathVectorPtr_t> hpp::rbprm::interpolation::fitBeziersToPath | ( | RbPrmFullBodyPtr_t | fullbody, |
const pinocchio::Frame & | effector, | ||
const value_type | comPathLength, | ||
const PathPtr_t | fullBodyComPath, | ||
const State & | startState, | ||
const State & | nextState | ||
) |
fitBeziersToPath generate a vector of pathVector : each pathVector containt BezierPath, computed with varying value of weight-rrt
fullbody | |
effector | |
comPathLength | |
fullBodyComPath | |
startState | |
nextState |
core::PathPtr_t hpp::rbprm::interpolation::generateEndEffectorBezier | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemSolverPtr_t | problemSolver, | ||
const PathPtr_t | comPath, | ||
const State & | startState, | ||
const State & | nextState | ||
) |
pinocchio::Frame hpp::rbprm::interpolation::getEffector | ( | RbPrmFullBodyPtr_t | fullbody, |
const State & | startState, | ||
const State & | nextState | ||
) |
std::string hpp::rbprm::interpolation::getEffectorLimb | ( | const State & | startState, |
const State & | nextState | ||
) |
fcl::Vec3f hpp::rbprm::interpolation::getNormal | ( | const std::string & | effector, |
const State & | state, | ||
bool & | found | ||
) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | ComTrajectory | ) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | PolynomTrajectory | ) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | RbPrmInterpolation | ) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | TimeConstraintPath | ) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | TimeConstraintPathValidation | ) |
hpp::rbprm::interpolation::HPP_PREDEF_CLASS | ( | TimeConstraintShooter | ) |
core::PathPtr_t HPP_RBPRM_DLLAPI hpp::rbprm::interpolation::interpolateStates | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const ShooterFactory_T & | shooterFactory, | ||
const ConstraintFactory_T & | constraintFactory, | ||
const StateConstIterator & | startState, | ||
const StateConstIterator & | endState, | ||
const std::size_t | numOptimizations = 10 , |
||
const bool | keepExtraDof = false , |
||
const pinocchio::value_type | error_treshold = 0.001 , |
||
const size_t | maxIterations = 0 |
||
) |
Runs the LimbRRT to create a kinematic, continuous, collision free path between an ordered State contrainer (Between each consecutive state, only one effector position differs between the states). Equilibrium is not verified along the path. To achieve this, an oriented RRT is run for the transitioning limb. The root path between two State is given by the problem steering method defined in the helper parameter. An extra DOF is used to sample a root position along the normalized path as well as the limb configuration. To avoid going back and forth, two configurations can thus only be connected if the first configuration has a DOF value lower than the second one. The LimbRRT algorithm is a modification of the original algorithm introduced in Qiu et al. "A Hierarchical Framework for Realizing Dynamically-stable Motions of Humanoid Robot in Obstacle-cluttered Environments" If OpenMP is activated, the interpolation between the states is run in parallel WARNING: At the moment, no more than 100 states can be interpolated simultaneously TODO: include parametrization of shortcut algorithm
helper | holds the problem parameters and the considered device An extra DOF is added to the cloned device, as required by the algorithm. The method assumes that the steering method associated with the helper's rootProblem_ produces a collision free path all parts of the Device different that the transitioning limb. Here the steering method of the reference problem will be used to generate a root path between each consecutive state with the assumption that the path is valid. |
iterator | to the initial State |
to | iterator to the final State |
numOptimizations | Number of iterations of the shortcut algorithm to apply between each states |
maxIterations | : the maximal number of iterations allowed for the path-planner to solve the problem, 0 = no limitations |
core::PathPtr_t HPP_RBPRM_DLLAPI hpp::rbprm::interpolation::interpolateStatesFromPath | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const ShooterFactory_T & | shooterFactory, | ||
const ConstraintFactory_T & | constraintFactory, | ||
const core::PathPtr_t | refPath, | ||
const CIT_StateFrame & | startState, | ||
const CIT_StateFrame & | endState, | ||
const std::size_t | numOptimizations = 10 , |
||
const bool | keepExtraDof = false , |
||
const pinocchio::value_type | error_treshold = 0.001 , |
||
const size_t | maxIterations = 0 |
||
) |
Runs the LimbRRT to create a kinematic, continuous, collision free path between an ordered State contrainer (Between each consecutive state, only one effector position differs between the states). Equilibrium is not verified along the path. To achieve this, an oriented RRT is run for the transitioning limb. The root path between two State is given by the problem steering method defined in the helper parameter. An extra DOF is used to sample a root position along the normalized path as well as the limb configuration. To avoid going back and forth, two configurations can thus only be connected if the first configuration has a DOF value lower than the second one. The LimbRRT algorithm is a modification of the original algorithm introduced in Qiu et al. "A Hierarchical Framework for Realizing Dynamically-stable Motions of Humanoid Robot in Obstacle-cluttered Environments" If OpenMP is activated, the interpolation between the states is run in parallel WARNING: At the moment, no more than 100 states can be interpolated simultaneously TODO: include parametrization of shortcut algorithm
helper | holds the problem parameters and the considered device An extra DOF is added to the cloned device, as required by the algorithm. The method assumes that the steering method associated with the helper's rootProblem_ produces a collision free path all parts of the Device different that the transitioning limb. |
path | reference path for the root |
iterator | to the initial State with its associated keyFrame in the path |
to | iterator to the final State with its associated keyFrame in the path |
numOptimizations | Number of iterations of the shortcut algorithm to apply between each states |
maxIterations | : the maximal number of iterations allowed for the path-planner to solve the problem, 0 = no limitations |
core::PathPtr_t hpp::rbprm::interpolation::limbRRT | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const rbprm::CIT_State & | startState, | ||
const rbprm::CIT_State & | endState, | ||
const std::size_t | numOptimizations, | ||
const std::size_t | maxIteration = 0 |
||
) |
core::PathPtr_t hpp::rbprm::interpolation::limbRRTFromPath | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const PathPtr_t | refPath, | ||
const CIT_StateFrame & | startState, | ||
const CIT_StateFrame & | endState, | ||
const std::size_t | numOptimizations | ||
) |
core::Configuration_t hpp::rbprm::interpolation::projectOnCom | ( | RbPrmFullBodyPtr_t | fullbody, |
core::ProblemPtr_t | referenceProblem, | ||
const State & | model, | ||
const fcl::Vec3f & | targetCom, | ||
bool & | success | ||
) |
void hpp::rbprm::interpolation::SetPathValidation | ( | Helper_T & | helper | ) |
core::ConfigurationPtr_t hpp::rbprm::interpolation::TimeConfigFromDevice | ( | const Helper_T & | helper, |
const State & | state, | ||
const double | time | ||
) |
|
inline |