hpp-rbprm  4.15.1
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/core/path-vector.hh>
23 #include <hpp/pinocchio/device.hh>
24 #include <hpp/rbprm/config.hh>
26 #include <vector>
27 
28 namespace hpp {
29 namespace rbprm {
30 typedef std::vector<pinocchio::vector_t,
31  Eigen::aligned_allocator<pinocchio::vector_t> >
33 typedef T_Configuration::const_iterator CIT_Configuration;
34 namespace interpolation {
35 HPP_PREDEF_CLASS(RbPrmInterpolation);
36 
41 typedef shared_ptr<RbPrmInterpolation> RbPrmInterpolationPtr_t;
42 
44  public:
52  static RbPrmInterpolationPtr_t create(
53  const RbPrmFullBodyPtr_t robot, const State& start, const State& end,
54  const core::PathVectorConstPtr_t path = core::PathVectorConstPtr_t(),
55  const bool testReachability = true, const bool quasiStatic = false);
56 
57  public:
59 
70  rbprm::T_StateFrame Interpolate(
71  const affMap_t& affordances,
72  const std::map<std::string, std::vector<std::string> >& affFilters,
73  const double timeStep = 0.01, const double robustnessTreshold = 0.,
74  const bool filterStates = false);
75 
89  rbprm::T_StateFrame Interpolate(
90  const affMap_t& affordances,
91  const std::map<std::string, std::vector<std::string> >& affFilters,
92  const T_Configuration& configs, const double robustnessTreshold = 0.,
93  const pinocchio::value_type timeStep = 1.,
94  const pinocchio::value_type initValue = 0.,
95  const bool filterStates = false);
96 
97  core::Configuration_t configPosition(core::ConfigurationIn_t previous,
98  const core::PathVectorConstPtr_t path,
99  double i);
100 
108  rbprm::T_StateFrame addGoalConfig(const rbprm::T_StateFrame& states);
109  rbprm::T_StateFrame addGoalConfigRec(
110  const rbprm::T_StateFrame& states,
111  const std::vector<std::string> variations);
112 
113  public:
114  const core::PathVectorConstPtr_t path_;
115  const State start_;
116  const State end_;
117  bool testReachability_; // decide if we use the reachability criterion during
118  // interpolation
119  bool quasiStatic_; // decide if we use the criterion only in quasi-static
120 
121  private:
122  RbPrmFullBodyPtr_t robot_;
123 
124  T_StateFrame FilterStates(const T_StateFrame& originStates, const bool deep);
125  T_StateFrame FilterStatesRec(const T_StateFrame& originStates);
126  T_StateFrame tryReplaceStates(const T_StateFrame& originStates);
127  void tryReplaceStates(const CIT_StateFrame& from, const CIT_StateFrame to,
128  T_StateFrame& res);
129  T_StateFrame trySkipStates(const T_StateFrame& originStates);
130  void trySkipStates(const CIT_StateFrame& from, const CIT_StateFrame to,
131  T_StateFrame& res);
132  void FilterRepositioning(const CIT_StateFrame& from, const CIT_StateFrame to,
133  T_StateFrame& res);
134  T_StateFrame FilterRepositioning(const T_StateFrame& originStates);
135  void FilterBreakCreate(const CIT_StateFrame& from, const CIT_StateFrame to,
136  T_StateFrame& res);
137  T_StateFrame FilterBreakCreate(const T_StateFrame& originStates);
138  StateFrame findBestRepositionState(T_StateFrame candidates,
139  std::vector<std::string> limbsNames);
140  bool testReachability(const State& s0, const State& s1);
141 
142  protected:
143  RbPrmInterpolation(const core::PathVectorConstPtr_t path,
144  const RbPrmFullBodyPtr_t robot, const State& start,
145  const State& end, const bool testReachability = true,
146  const bool quasiStatic = false);
147 
151  void init(const RbPrmInterpolationWkPtr_t& weakPtr);
152 
153  private:
154  RbPrmInterpolationWkPtr_t weakPtr_;
155 }; // class RbPrmLimb
156 
164 
165 } // namespace interpolation
166 } // namespace rbprm
167 } // namespace hpp
168 
169 #endif // HPP_RBPRM_PATH_INTERPOLATION_HH
const core::PathVectorConstPtr_t path_
Definition: rbprm-path-interpolation.hh:114
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
bool quasiStatic_
Definition: rbprm-path-interpolation.hh:119
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:35
Definition: algorithm.hh:26
std::vector< StateFrame > T_StateFrame
Definition: rbprm-state.hh:34
std::vector< pinocchio::vector_t, Eigen::aligned_allocator< pinocchio::vector_t > > T_Configuration
Definition: rbprm-path-interpolation.hh:32
Definition: rbprm-path-interpolation.hh:43
const State end_
Definition: rbprm-path-interpolation.hh:116
shared_ptr< RbPrmInterpolation > RbPrmInterpolationPtr_t
Definition: rbprm-path-interpolation.hh:40
const State start_
Definition: rbprm-path-interpolation.hh:115
bool testReachability_
Definition: rbprm-path-interpolation.hh:117
T_Configuration::const_iterator CIT_Configuration
Definition: rbprm-path-interpolation.hh:33
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:33
HPP_PREDEF_CLASS(ComTrajectory)
Definition: rbprm-state.hh:40
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11