hpp-rbprm  4.12.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 {
35 
39 class RbPrmInterpolation;
40 typedef std::shared_ptr<RbPrmInterpolation> RbPrmInterpolationPtr_t;
41 
43  public:
51  static RbPrmInterpolationPtr_t create(const RbPrmFullBodyPtr_t robot, const State& start, const State& end,
52  const core::PathVectorConstPtr_t path = core::PathVectorConstPtr_t(),
53  const bool testReachability = true, const bool quasiStatic = false);
54 
55  public:
57 
68  const std::map<std::string, std::vector<std::string> >& affFilters,
69  const double timeStep = 0.01, const double robustnessTreshold = 0.,
70  const bool filterStates = false);
71 
85  const std::map<std::string, std::vector<std::string> >& affFilters,
86  const T_Configuration& configs, const double robustnessTreshold = 0.,
87  const pinocchio::value_type timeStep = 1.,
88  const pinocchio::value_type initValue = 0., const bool filterStates = false);
89 
90  core::Configuration_t configPosition(core::ConfigurationIn_t previous, const core::PathVectorConstPtr_t path,
91  double i);
92 
100  rbprm::T_StateFrame addGoalConfigRec(const rbprm::T_StateFrame& states, const std::vector<std::string> variations);
101 
102  public:
103  const core::PathVectorConstPtr_t path_;
104  const State start_;
105  const State end_;
106  bool testReachability_; // decide if we use the reachability criterion during interpolation
107  bool quasiStatic_; // decide if we use the criterion only in quasi-static
108 
109  private:
110  RbPrmFullBodyPtr_t robot_;
111 
112  T_StateFrame FilterStates(const T_StateFrame& originStates, const bool deep);
113  T_StateFrame FilterStatesRec(const T_StateFrame& originStates);
114  T_StateFrame tryReplaceStates(const T_StateFrame& originStates);
115  void tryReplaceStates(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
116  T_StateFrame trySkipStates(const T_StateFrame& originStates);
117  void trySkipStates(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
118  void FilterRepositioning(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
119  T_StateFrame FilterRepositioning(const T_StateFrame& originStates);
120  void FilterBreakCreate(const CIT_StateFrame& from, const CIT_StateFrame to, T_StateFrame& res);
121  T_StateFrame FilterBreakCreate(const T_StateFrame& originStates);
122  StateFrame findBestRepositionState(T_StateFrame candidates, std::vector<std::string> limbsNames);
123  bool testReachability(const State& s0, const State& s1);
124 
125  protected:
126  RbPrmInterpolation(const core::PathVectorConstPtr_t path, const RbPrmFullBodyPtr_t robot, const State& start,
127  const State& end, const bool testReachability = true, const bool quasiStatic = false);
128 
132  void init(const RbPrmInterpolationWkPtr_t& weakPtr);
133 
134  private:
135  RbPrmInterpolationWkPtr_t weakPtr_;
136 }; // class RbPrmLimb
137 
145 
146 } // namespace interpolation
147 } // namespace rbprm
148 } // namespace hpp
149 
150 #endif // HPP_RBPRM_PATH_INTERPOLATION_HH
Definition: rbprm-path-interpolation.hh:42
bool testReachability_
Definition: rbprm-path-interpolation.hh:106
static RbPrmInterpolationPtr_t create(const RbPrmFullBodyPtr_t robot, const State &start, const State &end, const core::PathVectorConstPtr_t path=core::PathVectorConstPtr_t(), const bool testReachability=true, const bool quasiStatic=false)
const State start_
Definition: rbprm-path-interpolation.hh:104
void init(const RbPrmInterpolationWkPtr_t &weakPtr)
Initialization.
rbprm::T_StateFrame Interpolate(const affMap_t &affordances, const std::map< std::string, std::vector< std::string > > &affFilters, const double timeStep=0.01, const double robustnessTreshold=0., const bool filterStates=false)
rbprm::T_StateFrame addGoalConfig(const rbprm::T_StateFrame &states)
addGoalConfig add goal configuration (end_state) at the end of a states list. Modify the last state (...
const State end_
Definition: rbprm-path-interpolation.hh:105
core::Configuration_t configPosition(core::ConfigurationIn_t previous, const core::PathVectorConstPtr_t path, double i)
rbprm::T_StateFrame addGoalConfigRec(const rbprm::T_StateFrame &states, const std::vector< std::string > variations)
const core::PathVectorConstPtr_t path_
Definition: rbprm-path-interpolation.hh:103
rbprm::T_StateFrame Interpolate(const affMap_t &affordances, const std::map< std::string, std::vector< std::string > > &affFilters, const T_Configuration &configs, const double robustnessTreshold=0., const pinocchio::value_type timeStep=1., const pinocchio::value_type initValue=0., const bool filterStates=false)
bool quasiStatic_
Definition: rbprm-path-interpolation.hh:107
RbPrmInterpolation(const core::PathVectorConstPtr_t path, const RbPrmFullBodyPtr_t robot, const State &start, const State &end, const bool testReachability=true, const bool quasiStatic=false)
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
HPP_PREDEF_CLASS(ComTrajectory)
std::shared_ptr< RbPrmInterpolation > RbPrmInterpolationPtr_t
Definition: rbprm-path-interpolation.hh:39
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
std::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
std::vector< StateFrame > T_StateFrame
Definition: rbprm-state.hh:35
T_Configuration::const_iterator CIT_Configuration
Definition: rbprm-path-interpolation.hh:32
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:36
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:34
std::vector< pinocchio::vector_t, Eigen::aligned_allocator< pinocchio::vector_t > > T_Configuration
Definition: rbprm-path-interpolation.hh:31
Definition: algorithm.hh:27
Definition: rbprm-state.hh:40