19 #ifndef HPP_TIME_CONSTRAINT_HELPER_HH
20 #define HPP_TIME_CONSTRAINT_HELPER_HH
22 #include <hpp/core/config-projector.hh>
23 #include <hpp/core/path-projector/progressive.hh>
24 #include <hpp/core/path.hh>
25 #include <hpp/core/problem.hh>
26 #include <hpp/core/weighed-distance.hh>
37 namespace interpolation {
44 template <
class Path_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
46 typedef shared_ptr<core::pathProjector::Progressive> ProgressivePtr_t;
47 typedef core::pathProjector::Progressive Progressive;
56 const ShooterFactory_T& shooterFactory,
57 const ConstraintFactory_T& constraintFactory,
58 core::ProblemPtr_t referenceProblem,
59 core::PathPtr_t refPath,
60 const pinocchio::value_type error_treshold = 1e-3)
61 : fullbody_(fullbody),
62 fullBodyDevice_(fullbody->device_->clone()),
63 rootProblem_(core::Problem::create(fullBodyDevice_)),
65 shooterFactory_(shooterFactory),
66 constraintFactory_(constraintFactory) {
68 fullBodyDevice_->setDimensionExtraConfigSpace(
69 fullBodyDevice_->extraConfigSpace().dimension() + 1);
70 proj_ = core::ConfigProjector::create(rootProblem_->robot(),
"proj",
71 error_treshold, 1000);
72 rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
74 rootProblem_, fullBodyDevice_->configSize() - 1);
75 rootProblem_->steeringMethod(steeringMethod_);
76 ProgressivePtr_t pProj =
77 Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
84 constraintFactory_(*
this, from, to);
86 void SetConfigShooter(
const State& from,
const State& to);
87 void InitConstraints();
88 void SetContactConstraints(
const State& from,
const State& to);
89 core::PathVectorPtr_t Run(
const State& from,
const State& to,
90 const size_t maxIterations = 0);
98 core::ConfigProjectorPtr_t
proj_;
134 template <
class Helper_T,
class ShooterFactory_T,
typename ConstraintFactory_T,
135 typename StateConstIterator>
138 const ShooterFactory_T& shooterFactory,
139 const ConstraintFactory_T& constraintFactory,
140 const StateConstIterator& startState,
const StateConstIterator& endState,
141 const std::size_t numOptimizations = 10,
const bool keepExtraDof =
false,
142 const pinocchio::value_type error_treshold = 0.001,
143 const size_t maxIterations = 0);
174 template <
class Helper_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
177 const ShooterFactory_T& shooterFactory,
178 const ConstraintFactory_T& constraintFactory,
const core::PathPtr_t refPath,
180 const std::size_t numOptimizations = 10,
const bool keepExtraDof =
false,
181 const pinocchio::value_type error_treshold = 0.001,
182 const size_t maxIterations = 0);
196 #include <hpp/rbprm/interpolation/time-constraint-helper.inl>
197 #endif // HPP_TIME_CONSTRAINT_HELPER_HH