hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
hpp::rbprm::interpolation Namespace Reference

Classes

struct  ComRRTShooterFactory
 
class  ComTrajectory
 
struct  EffectorRRTShooterFactory
 
struct  EndEffectorPath
 
struct  funEvaluator
 
struct  LimbRRTShooterFactory
 
class  PolynomTrajectory
 
class  RbPrmInterpolation
 
struct  RightHandSideFunctor
 Time varying right hand side of constraint. More...
 
struct  SetComRRTConstraints
 
struct  SetEffectorRRTConstraints
 
struct  SetLimbRRTConstraints
 
class  TimeConstraintHelper
 
class  TimeConstraintPath
 
class  TimeConstraintPathValidation
 
class  TimeConstraintShooter
 
class  TimeConstraintSteering
 
struct  TimeDependant
 
struct  VecRightSide
 

Typedefs

typedef TimeConstraintHelper< TimeConstraintPath, ComRRTShooterFactory, SetComRRTConstraintsComRRTHelper
 
typedef shared_ptr< ComTrajectoryComTrajectoryPtr_t
 
typedef constraints::PointCom PointCom
 
typedef constraints::CalculusBaseAbstract< PointCom::ValueType_t, PointCom::JacobianType_t > s_t
 
typedef constraints::SymbolicFunction< s_tPointComFunction
 
typedef constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
 
typedef TimeConstraintHelper< TimeConstraintPath, LimbRRTShooterFactory, SetLimbRRTConstraintsLimbRRTHelper
 
typedef shared_ptr< PolynomTrajectoryPolynomTrajectoryPtr_t
 
typedef ndcurves::curve_abc< core::value_type, core::value_type, true, Eigen::Vector3d > Polynom
 
typedef shared_ptr< PolynomPolynomPtr_t
 
typedef shared_ptr< RbPrmInterpolationRbPrmInterpolationPtr_t
 
typedef TimeConstraintHelper< TimeConstraintPath, EffectorRRTShooterFactory, SetEffectorRRTConstraintsEffectorRRTHelper
 
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_texact_cubic_Ptr
 
typedef shared_ptr< TimeConstraintPathValidationTimeConstraintPathValidationPtr_t
 
typedef shared_ptr< TimeConstraintPathTimeConstraintPathPtr_t
 
typedef shared_ptr< TimeConstraintShooterTimeConstraintShooterPtr_t
 
typedef shared_ptr< const RightHandSideFunctorRightHandSideFunctorPtr_t
 
typedef std::vector< TimeDependantT_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 Documentation

◆ CIT_TimeDependant

typedef T_TimeDependant::const_iterator hpp::rbprm::interpolation::CIT_TimeDependant

◆ ComRRTHelper

◆ ComTrajectoryPtr_t

◆ curve_constraint_t

typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> > hpp::rbprm::interpolation::curve_constraint_t

◆ EffectorRRTHelper

◆ exact_cubic_Ptr

◆ exact_cubic_t

typedef ndcurves::exact_cubic<double, double, true, Eigen::Matrix<value_type, 3, 1> > hpp::rbprm::interpolation::exact_cubic_t

◆ LimbRRTHelper

◆ PointCom

typedef constraints::PointCom hpp::rbprm::interpolation::PointCom

◆ PointComFunction

typedef constraints::SymbolicFunction<s_t> hpp::rbprm::interpolation::PointComFunction

◆ PointComFunctionPtr_t

typedef constraints::SymbolicFunction<s_t>::Ptr_t hpp::rbprm::interpolation::PointComFunctionPtr_t

◆ Polynom

typedef ndcurves::curve_abc<core::value_type, core::value_type, true, Eigen::Vector3d> hpp::rbprm::interpolation::Polynom

◆ PolynomPtr_t

◆ PolynomTrajectoryPtr_t

◆ RbPrmInterpolationPtr_t

◆ RightHandSideFunctorPtr_t

◆ s_t

typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t> hpp::rbprm::interpolation::s_t

◆ T_TimeDependant

◆ TimeConstraintPathPtr_t

◆ TimeConstraintPathValidationPtr_t

◆ TimeConstraintShooterPtr_t

Function Documentation

◆ addContactConstraints()

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 
)

◆ comRRT() [1/2]

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 
)

◆ comRRT() [2/2]

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 
)

◆ comRRTFromPath()

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 
)

◆ Create6DEffectorConstraint()

template<class Helper_T , typename Reference >
void hpp::rbprm::interpolation::Create6DEffectorConstraint ( Helper_T &  helper,
const Reference &  ref,
const pinocchio::Frame  effectorJoint,
const fcl::Transform3f &  initTarget = fcl::Transform3f() 
)

◆ CreateComConstraint()

template<class Helper_T , typename Reference >
void hpp::rbprm::interpolation::CreateComConstraint ( Helper_T &  helper,
const Reference &  ref,
const fcl::Vec3f &  initTarget = fcl::Vec3f() 
)

◆ CreateContactConstraints() [1/2]

template<class Helper_T >
void hpp::rbprm::interpolation::CreateContactConstraints ( const State from,
const State to 
)

◆ CreateContactConstraints() [2/2]

template<class Helper_T >
void hpp::rbprm::interpolation::CreateContactConstraints ( Helper_T &  helper,
const State from,
const State to 
)

◆ CreateEffectorConstraint()

template<class Helper_T , typename Reference >
void hpp::rbprm::interpolation::CreateEffectorConstraint ( Helper_T &  helper,
const Reference &  ref,
const pinocchio::Frame  effectorJoint,
const fcl::Vec3f &  initTarget = fcl::Vec3f() 
)

◆ createFreeFlyerDevice()

DevicePtr_t hpp::rbprm::interpolation::createFreeFlyerDevice ( )

◆ CreateOrientationConstraint()

template<class Helper_T , typename Reference >
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() 
)

◆ createOrientationMethod()

constraints::OrientationPtr_t hpp::rbprm::interpolation::createOrientationMethod ( pinocchio::DevicePtr_t  device,
const fcl::Transform3f &  initTarget,
const pinocchio::Frame  effectorFrame 
)
inline

◆ createPositionMethod()

constraints::PositionPtr_t hpp::rbprm::interpolation::createPositionMethod ( pinocchio::DevicePtr_t  device,
const fcl::Vec3f &  initTarget,
const pinocchio::Frame  effectorFrame 
)
inline

◆ CreatePosturalTaskConstraint()

template<class Helper_T , typename Reference >
void hpp::rbprm::interpolation::CreatePosturalTaskConstraint ( Helper_T &  helper,
const Reference &  ref 
)

◆ effectorRRT() [1/2]

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 
)

◆ effectorRRT() [2/2]

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 >() 
)

◆ effectorRRTFromPath() [1/2]

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

Parameters
fullbody
referenceProblem
comPathreference path for the center of mass
fullBodyComPathfullBody path previously computed
startState
nextState
numOptimizations
keepExtraDofif false, remove the additionnal extraDoF introduced by comRRT
pathIdthe Id of the returned path in the problem-solver. Usef to match with the end-effector path indice stored in fullBody
refFullBodyPath
constrainedJointPos
constrainedLockedJoints
Returns
the fullBody path

◆ effectorRRTFromPath() [2/2]

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

Parameters
fullbody
referenceProblem
comPathreference path for the center of mass
startState
nextState
numOptimizations
keepExtraDofif false, remove the additionnal extraDoF introduced by comRRT
pathIdthe Id of the returned path in the problem-solver. Usef to match with the end-effector path indice stored in fullBody
refFullBodyPath
constrainedJointPos
constrainedLockedJoints
Returns
the fullBody path

◆ fitBeziersToPath()

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

Parameters
fullbody
effector
comPathLength
fullBodyComPath
startState
nextState
Returns

◆ generateEndEffectorBezier()

core::PathPtr_t hpp::rbprm::interpolation::generateEndEffectorBezier ( RbPrmFullBodyPtr_t  fullbody,
core::ProblemSolverPtr_t  problemSolver,
const PathPtr_t  comPath,
const State startState,
const State nextState 
)

◆ getEffector()

pinocchio::Frame hpp::rbprm::interpolation::getEffector ( RbPrmFullBodyPtr_t  fullbody,
const State startState,
const State nextState 
)

◆ getEffectorLimb()

std::string hpp::rbprm::interpolation::getEffectorLimb ( const State startState,
const State nextState 
)

◆ getNormal()

fcl::Vec3f hpp::rbprm::interpolation::getNormal ( const std::string &  effector,
const State state,
bool &  found 
)

◆ HPP_PREDEF_CLASS() [1/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( ComTrajectory  )

◆ HPP_PREDEF_CLASS() [2/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( PolynomTrajectory  )

◆ HPP_PREDEF_CLASS() [3/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( RbPrmInterpolation  )

◆ HPP_PREDEF_CLASS() [4/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( TimeConstraintPath  )

◆ HPP_PREDEF_CLASS() [5/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( TimeConstraintPathValidation  )

◆ HPP_PREDEF_CLASS() [6/6]

hpp::rbprm::interpolation::HPP_PREDEF_CLASS ( TimeConstraintShooter  )

◆ interpolateStates()

template<class Helper_T , class ShooterFactory_T , typename ConstraintFactory_T , typename StateConstIterator >
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

Parameters
helperholds 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.
iteratorto the initial State
toiterator to the final State
numOptimizationsNumber 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
Returns
the resulting path vector, concatenation of all the interpolation paths between the State

◆ interpolateStatesFromPath()

template<class Helper_T , class ShooterFactory_T , typename ConstraintFactory_T >
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

Parameters
helperholds 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.
pathreference path for the root
iteratorto the initial State with its associated keyFrame in the path
toiterator to the final State with its associated keyFrame in the path
numOptimizationsNumber 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
Returns
the resulting path vector, concatenation of all the interpolation paths between the State

◆ limbRRT()

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 
)

◆ limbRRTFromPath()

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 
)

◆ projectOnCom()

core::Configuration_t hpp::rbprm::interpolation::projectOnCom ( RbPrmFullBodyPtr_t  fullbody,
core::ProblemPtr_t  referenceProblem,
const State model,
const fcl::Vec3f &  targetCom,
bool &  success 
)

◆ SetPathValidation()

template<class Helper_T >
void hpp::rbprm::interpolation::SetPathValidation ( Helper_T &  helper)

◆ TimeConfigFromDevice()

template<class Helper_T >
core::ConfigurationPtr_t hpp::rbprm::interpolation::TimeConfigFromDevice ( const Helper_T &  helper,
const State state,
const double  time 
)

◆ UpdateConstraints()

void hpp::rbprm::interpolation::UpdateConstraints ( core::ConfigurationOut_t  configuration,
const T_TimeDependant tds,
const std::size_t  pathDofRank 
)
inline