hpp-rbprm  4.11.0
Implementation of RB-PRM planner using hpp.
time-constraint-helper.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Steve Tonneau (steve.tonneau@laas.fr)
4 //
5 // This file is part of hpp-rbprm.
6 // hpp-rbprm is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-rbprm is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_TIME_CONSTRAINT_HELPER_HH
20 #define HPP_TIME_CONSTRAINT_HELPER_HH
21 
22 #include <hpp/rbprm/config.hh>
24 #include <hpp/rbprm/rbprm-state.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>
32 
33 #include <vector>
34 #include <map>
35 
36 namespace hpp {
37 namespace rbprm {
38 namespace interpolation {
39 
45 template <class Path_T, class ShooterFactory_T, typename ConstraintFactory_T>
46 class HPP_CORE_DLLAPI TimeConstraintHelper {
47  typedef std::shared_ptr<core::pathProjector::Progressive> ProgressivePtr_t;
48  typedef core::pathProjector::Progressive Progressive;
49 
50  public:
56  TimeConstraintHelper(RbPrmFullBodyPtr_t fullbody, const ShooterFactory_T& shooterFactory,
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_)),
62  refPath_(refPath),
63  shooterFactory_(shooterFactory),
64  constraintFactory_(constraintFactory) {
65  // adding extra DOF for including time in sampling
66  fullBodyDevice_->setDimensionExtraConfigSpace(fullBodyDevice_->extraConfigSpace().dimension() + 1);
67  proj_ = core::ConfigProjector::create(rootProblem_->robot(), "proj", error_treshold, 1000);
68  rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
69  steeringMethod_ = TimeConstraintSteering<Path_T>::create(rootProblem_, fullBodyDevice_->configSize() - 1);
70  rootProblem_->steeringMethod(steeringMethod_);
71  ProgressivePtr_t pProj = Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
72  // rootProblem_->pathProjector(pProj);
73  }
74 
76 
77  void SetConstraints(const State& from, const State& to) { constraintFactory_(*this, from, to); }
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);
82 
83  public:
85  core::DevicePtr_t fullBodyDevice_;
86  core::ProblemPtr_t rootProblem_;
87  core::PathPlannerPtr_t planner_;
88  core::PathPtr_t refPath_;
89  core::ConfigProjectorPtr_t proj_;
90  std::shared_ptr<TimeConstraintSteering<Path_T> > steeringMethod_;
91  const ShooterFactory_T& shooterFactory_;
92  const ConstraintFactory_T& constraintFactory_;
93 };
94 
123 template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T, typename StateConstIterator>
124 core::PathPtr_t HPP_RBPRM_DLLAPI interpolateStates(
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);
129 
156 template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_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);
162 
163 /*typedef core::PathPtr_t (*interpolate_states)(rbprm::RbPrmFullBodyPtr_t, core::ProblemPtr_t,const rbprm::CIT_State&,
164  const rbprm::CIT_State&,const std::size_t);
165 
166 typedef core::PathPtr_t (*interpolate_states_from_path)(rbprm::RbPrmFullBodyPtr_t, core::ProblemPtr_t, const
167 core::PathPtr_t, const rbprm::CIT_StateFrame&,const rbprm::CIT_StateFrame&,const std::size_t);*/
168 } // namespace interpolation
169 } // namespace rbprm
170 } // namespace hpp
171 
172 #include <hpp/rbprm/interpolation/time-constraint-helper.inl>
173 #endif // HPP_TIME_CONSTRAINT_HELPER_HH
std::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:36
void SetConstraints(const State &from, const State &to)
Definition: time-constraint-helper.hh:77
Definition: algorithm.hh:27
~TimeConstraintHelper()
Definition: time-constraint-helper.hh:75
const ConstraintFactory_T & constraintFactory_
Definition: time-constraint-helper.hh:92
const ShooterFactory_T & shooterFactory_
Definition: time-constraint-helper.hh:91
static TimeConstraintSteeringPtr_t create(const core::ProblemPtr_t &problem, const std::size_t pathDofRank)
Create instance and return shared pointer.
Definition: time-constraint-steering.hh:48
std::shared_ptr< TimeConstraintSteering< Path_T > > steeringMethod_
Definition: time-constraint-helper.hh:90
core::DevicePtr_t fullBodyDevice_
Definition: time-constraint-helper.hh:85
RbPrmFullBodyPtr_t fullbody_
Definition: time-constraint-helper.hh:84
core::ConfigProjectorPtr_t proj_
Definition: time-constraint-helper.hh:89
core::PathPlannerPtr_t planner_
Definition: time-constraint-helper.hh:87
TimeConstraintHelper(RbPrmFullBodyPtr_t fullbody, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, core::ProblemPtr_t referenceProblem, core::PathPtr_t refPath, const pinocchio::value_type error_treshold=1e-3)
Definition: time-constraint-helper.hh:56
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)
core::PathPtr_t refPath_
Definition: time-constraint-helper.hh:88
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)
core::ProblemPtr_t rootProblem_
Definition: time-constraint-helper.hh:86
Definition: rbprm-state.hh:40
Definition: time-constraint-helper.hh:46