hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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
28namespace hpp {
29namespace rbprm {
30typedef std::vector<pinocchio::vector_t,
31 Eigen::aligned_allocator<pinocchio::vector_t> >
33typedef T_Configuration::const_iterator CIT_Configuration;
34namespace interpolation {
36
41typedef shared_ptr<RbPrmInterpolation> RbPrmInterpolationPtr_t;
42
44 public:
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
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
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
110 const rbprm::T_StateFrame& states,
111 const std::vector<std::string> variations);
112
113 public:
114 const core::PathVectorConstPtr_t path_;
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
Definition: rbprm-path-interpolation.hh:43
bool testReachability_
Definition: rbprm-path-interpolation.hh:117
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:115
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:116
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:114
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:119
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
shared_ptr< RbPrmInterpolation > RbPrmInterpolationPtr_t
Definition: rbprm-path-interpolation.hh:41
HPP_PREDEF_CLASS(ComTrajectory)
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
std::vector< StateFrame > T_StateFrame
Definition: rbprm-state.hh:34
T_Configuration::const_iterator CIT_Configuration
Definition: rbprm-path-interpolation.hh:33
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:35
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:33
std::vector< pinocchio::vector_t, Eigen::aligned_allocator< pinocchio::vector_t > > T_Configuration
Definition: rbprm-path-interpolation.hh:32
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40