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>
38 namespace interpolation {
45 template <
class Path_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
47 typedef boost::shared_ptr<core::pathProjector::Progressive> ProgressivePtr_t;
48 typedef core::pathProjector::Progressive Progressive;
57 const ConstraintFactory_T& constraintFactory, core::ProblemPtr_t referenceProblem,
58 core::PathPtr_t refPath,
const pinocchio::value_type error_treshold = 1e-3)
59 : fullbody_(fullbody),
60 fullBodyDevice_(fullbody->device_->clone()),
61 rootProblem_(core::Problem::create(fullBodyDevice_)),
63 shooterFactory_(shooterFactory),
64 constraintFactory_(constraintFactory) {
66 fullBodyDevice_->setDimensionExtraConfigSpace(fullBodyDevice_->extraConfigSpace().dimension() + 1);
67 proj_ = core::ConfigProjector::create(rootProblem_->robot(),
"proj", error_treshold, 1000);
68 rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
70 rootProblem_->steeringMethod(steeringMethod_);
71 ProgressivePtr_t pProj = Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
78 void SetConfigShooter(
const State& from,
const State& to);
79 void InitConstraints();
80 void SetContactConstraints(
const State& from,
const State& to);
81 core::PathVectorPtr_t Run(
const State& from,
const State& to,
const size_t maxIterations = 0);
89 core::ConfigProjectorPtr_t
proj_;
123 template <
class Helper_T,
class ShooterFactory_T,
typename Constra
intFactory_T,
typename StateConstIterator>
125 RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,
const ShooterFactory_T& shooterFactory,
126 const ConstraintFactory_T& constraintFactory,
const StateConstIterator& startState,
127 const StateConstIterator& endState,
const std::size_t numOptimizations = 10,
const bool keepExtraDof =
false,
128 const pinocchio::value_type error_treshold = 0.001,
const size_t maxIterations = 0);
156 template <
class Helper_T,
class ShooterFactory_T,
typename Constra
intFactory_T>
158 RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,
const ShooterFactory_T& shooterFactory,
159 const ConstraintFactory_T& constraintFactory,
const core::PathPtr_t refPath,
const CIT_StateFrame& startState,
160 const CIT_StateFrame& endState,
const std::size_t numOptimizations = 10,
const bool keepExtraDof =
false,
161 const pinocchio::value_type error_treshold = 0.001,
const size_t maxIterations = 0);
172 #include <hpp/rbprm/interpolation/time-constraint-helper.inl>
173 #endif // HPP_TIME_CONSTRAINT_HELPER_HH