hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
rbprm-path-interpolation.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_RBPRM_PATH_INTERPOLATION_HH
20 # define HPP_RBPRM_PATH_INTERPOLATION_HH
21 
22 # include <hpp/rbprm/config.hh>
24 # include <hpp/core/path-vector.hh>
25 # include <hpp/pinocchio/device.hh>
26 
27 # include <vector>
28 
29 namespace hpp {
30  namespace rbprm {
31  typedef std::vector<pinocchio::vector_t,Eigen::aligned_allocator<pinocchio::vector_t> > T_Configuration;
32  typedef T_Configuration::const_iterator CIT_Configuration;
33  namespace interpolation {
34  HPP_PREDEF_CLASS(RbPrmInterpolation);
35 
40  typedef boost::shared_ptr <RbPrmInterpolation> RbPrmInterpolationPtr_t;
41 
43  {
44  public:
52  static RbPrmInterpolationPtr_t create (const RbPrmFullBodyPtr_t robot, const State& start, const State& end,
53  const core::PathVectorConstPtr_t path = core::PathVectorConstPtr_t(), const bool testReachability = true, const bool quasiStatic = false);
54 
55  public:
57 
67  rbprm::T_StateFrame Interpolate(const affMap_t& affordances, const std::map<std::string, std::vector<std::string> >& affFilters,
68  const double timeStep = 0.01, const double robustnessTreshold=0.,
69  const bool filterStates = false);
70 
82  rbprm::T_StateFrame Interpolate(const affMap_t& affordances, const std::map<std::string, std::vector<std::string> >& affFilters,
83  const T_Configuration& configs, const double robustnessTreshold=0.,
84  const pinocchio::value_type timeStep = 1., const pinocchio::value_type initValue = 0.,
85  const bool filterStates = false);
86 
87  core::Configuration_t configPosition(core::ConfigurationIn_t previous, const core::PathVectorConstPtr_t path, double i);
88 
89 
95  rbprm::T_StateFrame addGoalConfig(const rbprm::T_StateFrame& states);
96  rbprm::T_StateFrame addGoalConfigRec(const rbprm::T_StateFrame& states,const std::vector<std::string> variations);
97 
98 
99  public:
100  const core::PathVectorConstPtr_t path_;
101  const State start_;
102  const State end_;
103  bool testReachability_; // decide if we use the reachability criterion during interpolation
104  bool quasiStatic_; // decide if we use the criterion only in quasi-static
105 
106 
107  private:
108  RbPrmFullBodyPtr_t robot_;
109 
110  T_StateFrame FilterStates(const T_StateFrame& originStates, const bool deep);
111  T_StateFrame FilterStatesRec( const T_StateFrame& originStates);
112  T_StateFrame tryReplaceStates( const T_StateFrame& originStates);
113  void tryReplaceStates(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
114  T_StateFrame trySkipStates( const T_StateFrame& originStates);
115  void trySkipStates(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
116  void FilterRepositioning(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
117  T_StateFrame FilterRepositioning(const T_StateFrame& originStates);
118  void FilterBreakCreate(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
119  T_StateFrame FilterBreakCreate(const T_StateFrame& originStates);
120  StateFrame findBestRepositionState(T_StateFrame candidates,std::vector<std::string> limbsNames);
121  bool testReachability(const State& s0, const State& s1);
122 
123 
124  protected:
125  RbPrmInterpolation (const core::PathVectorConstPtr_t path, const RbPrmFullBodyPtr_t robot,const State& start, const State& end, const bool testReachability = true, const bool quasiStatic = false);
126 
130  void init (const RbPrmInterpolationWkPtr_t& weakPtr);
131 
132  private:
133  RbPrmInterpolationWkPtr_t weakPtr_;
134  }; // class RbPrmLimb
135 
143 
144  } // namespace interpolation
145  } // namespace rbprm
146 } // namespace hpp
147 
148 #endif // HPP_RBPRM_PATH_INTERPOLATION_HH
rbprm-fullbody.hh
hpp::rbprm::interpolation::RbPrmInterpolation::quasiStatic_
bool quasiStatic_
Definition: rbprm-path-interpolation.hh:104
hpp::rbprm::interpolation::RbPrmInterpolation::path_
const core::PathVectorConstPtr_t path_
Definition: rbprm-path-interpolation.hh:100
hpp::rbprm::CIT_Configuration
T_Configuration::const_iterator CIT_Configuration
Definition: rbprm-path-interpolation.hh:32
hpp::rbprm::interpolation::RbPrmInterpolation::start_
const State start_
Definition: rbprm-path-interpolation.hh:101
hpp::rbprm::T_StateFrame
std::vector< StateFrame > T_StateFrame
Definition: rbprm-state.hh:35
hpp::rbprm::interpolation::RbPrmInterpolation
Definition: rbprm-path-interpolation.hh:42
hpp::rbprm::StateFrame
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:34
hpp::rbprm::CIT_StateFrame
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:36
hpp::rbprm::interpolation::RbPrmInterpolation::end_
const State end_
Definition: rbprm-path-interpolation.hh:102
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t
boost::shared_ptr< RbPrmInterpolation > RbPrmInterpolationPtr_t
Definition: rbprm-path-interpolation.hh:39
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::RbPrmInterpolation::testReachability_
bool testReachability_
Definition: rbprm-path-interpolation.hh:103
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::affMap_t
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
config.hh
hpp::rbprm::interpolation::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(ComTrajectory)
hpp::rbprm::T_Configuration
std::vector< pinocchio::vector_t, Eigen::aligned_allocator< pinocchio::vector_t > > T_Configuration
Definition: rbprm-path-interpolation.hh:31
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64