19 #ifndef HPP_TIME_CONSTRAINT_HELPER_HH
20 # define HPP_TIME_CONSTRAINT_HELPER_HH
27 # include <hpp/core/path.hh>
28 # include <hpp/core/problem.hh>
29 # include <hpp/core/config-projector.hh>
30 # include <hpp/core/path-projector/progressive.hh>
31 # include <hpp/core/weighed-distance.hh>
39 namespace interpolation {
46 template<
class Path_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
49 typedef boost::shared_ptr <core::pathProjector::Progressive> ProgressivePtr_t;
50 typedef core::pathProjector::Progressive Progressive;
58 const ShooterFactory_T& shooterFactory,
59 const ConstraintFactory_T& constraintFactory,
60 core::ProblemPtr_t referenceProblem,
61 core::PathPtr_t refPath,
62 const pinocchio::value_type error_treshold = 1e-3)
64 , fullBodyDevice_(fullbody->device_->clone())
65 , rootProblem_(core::Problem::create(fullBodyDevice_))
67 , shooterFactory_(shooterFactory)
68 , constraintFactory_(constraintFactory)
71 fullBodyDevice_->setDimensionExtraConfigSpace(fullBodyDevice_->extraConfigSpace().dimension()+1);
72 proj_ = core::ConfigProjector::create(rootProblem_->robot(),
"proj", error_treshold, 1000);
73 rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
75 rootProblem_->steeringMethod(steeringMethod_);
76 ProgressivePtr_t pProj = Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
83 void SetConfigShooter(
const State& from,
const State& to);
84 void InitConstraints();
85 void SetContactConstraints(
const State& from,
const State& to);
86 core::PathVectorPtr_t Run(
const State& from,
const State& to,
const size_t maxIterations=0);
94 core::ConfigProjectorPtr_t
proj_;
128 template<
class Helper_T,
class ShooterFactory_T,
typename Constra
intFactory_T,
typename StateConstIterator>
130 const ShooterFactory_T& shooterFactory,
131 const ConstraintFactory_T& constraintFactory,
132 const StateConstIterator& startState,
133 const StateConstIterator& endState,
134 const std::size_t numOptimizations = 10,
135 const bool keepExtraDof =
false,
136 const pinocchio::value_type error_treshold = 0.001,
137 const size_t maxIterations = 0);
165 template<
class Helper_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
167 core::ProblemPtr_t referenceProblem,
168 const ShooterFactory_T& shooterFactory,
169 const ConstraintFactory_T& constraintFactory,
170 const core::PathPtr_t refPath,
173 const std::size_t numOptimizations = 10,
174 const bool keepExtraDof =
false,
175 const pinocchio::value_type error_treshold = 0.001,
176 const size_t maxIterations = 0);
187 #include <hpp/rbprm/interpolation/time-constraint-helper.inl>
188 #endif // HPP_TIME_CONSTRAINT_HELPER_HH