hpp-rbprm  4.10.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/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  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 
67  rbprm::T_StateFrame Interpolate(const affMap_t& affordances,
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 
84  rbprm::T_StateFrame Interpolate(const affMap_t& affordances,
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 
99  rbprm::T_StateFrame addGoalConfig(const rbprm::T_StateFrame& states);
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
rbprm-fullbody.hh
hpp::rbprm::interpolation::RbPrmInterpolation::quasiStatic_
bool quasiStatic_
Definition: rbprm-path-interpolation.hh:107
hpp::rbprm::interpolation::RbPrmInterpolation::path_
const core::PathVectorConstPtr_t path_
Definition: rbprm-path-interpolation.hh:103
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:104
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:105
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t
boost::shared_ptr< RbPrmInterpolation > RbPrmInterpolationPtr_t
Definition: rbprm-path-interpolation.hh:39
hpp::rbprm::T_Configuration
std::vector< pinocchio::vector_t, Eigen::aligned_allocator< pinocchio::vector_t > > T_Configuration
Definition: rbprm-path-interpolation.hh:31
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::RbPrmInterpolation::testReachability_
bool testReachability_
Definition: rbprm-path-interpolation.hh:106
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_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64