hpp-rbprm  4.10.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>
25 # include <hpp/rbprm/rbprm-device.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 
37 namespace hpp {
38  namespace rbprm {
39  namespace interpolation {
40 
46  template<class Path_T, class ShooterFactory_T, typename ConstraintFactory_T>
47  class HPP_CORE_DLLAPI TimeConstraintHelper
48  {
49  typedef boost::shared_ptr <core::pathProjector::Progressive> ProgressivePtr_t;
50  typedef core::pathProjector::Progressive Progressive;
51  public:
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)
63  : fullbody_(fullbody)
64  , fullBodyDevice_(fullbody->device_->clone())
65  , rootProblem_(core::Problem::create(fullBodyDevice_))
66  , refPath_(refPath)
67  , shooterFactory_(shooterFactory)
68  , constraintFactory_(constraintFactory)
69  {
70  // adding extra DOF for including time in sampling
71  fullBodyDevice_->setDimensionExtraConfigSpace(fullBodyDevice_->extraConfigSpace().dimension()+1);
72  proj_ = core::ConfigProjector::create(rootProblem_->robot(),"proj", error_treshold, 1000);
73  rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
74  steeringMethod_ = TimeConstraintSteering<Path_T>::create(rootProblem_,fullBodyDevice_->configSize()-1);
75  rootProblem_->steeringMethod(steeringMethod_);
76  ProgressivePtr_t pProj = Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
77  // rootProblem_->pathProjector(pProj);
78  }
79 
81 
82  void SetConstraints(const State& from, const State& to){constraintFactory_(*this, from, to);}
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);
87 
88  public:
90  core::DevicePtr_t fullBodyDevice_;
91  core::ProblemPtr_t rootProblem_;
92  core::PathPlannerPtr_t planner_;
93  core::PathPtr_t refPath_;
94  core::ConfigProjectorPtr_t proj_;
95  boost::shared_ptr<TimeConstraintSteering<Path_T> > steeringMethod_;
96  const ShooterFactory_T& shooterFactory_;
97  const ConstraintFactory_T& constraintFactory_;
98  };
99 
128  template<class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T, typename StateConstIterator>
129  core::PathPtr_t HPP_RBPRM_DLLAPI interpolateStates(RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,
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);
138 
165  template<class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T>
167  core::ProblemPtr_t referenceProblem,
168  const ShooterFactory_T& shooterFactory,
169  const ConstraintFactory_T& constraintFactory,
170  const core::PathPtr_t refPath,
171  const CIT_StateFrame& startState,
172  const CIT_StateFrame& endState,
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);
177 
178  /*typedef core::PathPtr_t (*interpolate_states)(rbprm::RbPrmFullBodyPtr_t, core::ProblemPtr_t,const rbprm::CIT_State&,
179  const rbprm::CIT_State&,const std::size_t);
180 
181  typedef core::PathPtr_t (*interpolate_states_from_path)(rbprm::RbPrmFullBodyPtr_t, core::ProblemPtr_t, const core::PathPtr_t,
182  const rbprm::CIT_StateFrame&,const rbprm::CIT_StateFrame&,const std::size_t);*/
183  } // namespace interpolation
184  } // namespace rbprm
185  } // namespace hpp
186 
187 #include <hpp/rbprm/interpolation/time-constraint-helper.inl>
188 #endif // HPP_TIME_CONSTRAINT_HELPER_HH
hpp::rbprm::interpolation::TimeConstraintHelper::planner_
core::PathPlannerPtr_t planner_
Definition: time-constraint-helper.hh:92
time-constraint-steering.hh
hpp::rbprm::interpolation::interpolateStatesFromPath
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)
rbprm-fullbody.hh
hpp::rbprm::interpolation::TimeConstraintHelper::fullbody_
RbPrmFullBodyPtr_t fullbody_
Definition: time-constraint-helper.hh:89
hpp::rbprm::interpolation::TimeConstraintHelper::TimeConstraintHelper
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:57
rbprm-state.hh
hpp::rbprm::interpolation::TimeConstraintHelper::proj_
core::ConfigProjectorPtr_t proj_
Definition: time-constraint-helper.hh:94
hpp::rbprm::interpolation::TimeConstraintHelper::refPath_
core::PathPtr_t refPath_
Definition: time-constraint-helper.hh:93
rbprm-device.hh
hpp::rbprm::interpolation::TimeConstraintHelper::constraintFactory_
const ConstraintFactory_T & constraintFactory_
Definition: time-constraint-helper.hh:97
hpp::rbprm::interpolation::TimeConstraintHelper::steeringMethod_
boost::shared_ptr< TimeConstraintSteering< Path_T > > steeringMethod_
Definition: time-constraint-helper.hh:95
hpp::rbprm::CIT_StateFrame
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:36
hpp::rbprm::interpolation::TimeConstraintHelper::shooterFactory_
const ShooterFactory_T & shooterFactory_
Definition: time-constraint-helper.hh:96
hpp::rbprm::interpolation::interpolateStates
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)
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::TimeConstraintHelper::fullBodyDevice_
core::DevicePtr_t fullBodyDevice_
Definition: time-constraint-helper.hh:90
hpp::rbprm::interpolation::TimeConstraintHelper::SetConstraints
void SetConstraints(const State &from, const State &to)
Definition: time-constraint-helper.hh:82
hpp::rbprm::State
Definition: rbprm-state.hh:40
hpp::rbprm::RbPrmFullBodyPtr_t
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
hpp::rbprm::interpolation::TimeConstraintHelper::~TimeConstraintHelper
~TimeConstraintHelper()
Definition: time-constraint-helper.hh:80
hpp::rbprm::interpolation::TimeConstraintSteering::create
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
hpp::rbprm::interpolation::TimeConstraintHelper::rootProblem_
core::ProblemPtr_t rootProblem_
Definition: time-constraint-helper.hh:91
config.hh
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
hpp::rbprm::interpolation::TimeConstraintHelper
Definition: time-constraint-helper.hh:47