hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
rbprm-state.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_STATE_HH
20 #define HPP_RBPRM_STATE_HH
21 
22 #include <algorithm>
23 #include <hpp/pinocchio/device.hh>
24 #include <hpp/rbprm/config.hh>
25 #include <hpp/rbprm/rbprm-limb.hh>
26 #include <queue>
27 
28 namespace hpp {
29 namespace rbprm {
30 struct State;
31 typedef std::vector<State> T_State;
32 typedef T_State::const_iterator CIT_State;
33 typedef std::pair<pinocchio::value_type, rbprm::State> StateFrame;
34 typedef std::vector<StateFrame> T_StateFrame;
35 typedef T_StateFrame::const_iterator CIT_StateFrame;
36 
41  State() : nbContacts(0), stable(false) {}
42  State(const State& other);
43  ~State() {}
44 
45  State& operator=(const State& other);
46 
51  bool RemoveContact(const std::string& contactId);
52 
58  std::string RemoveFirstContact();
59 
64  std::vector<std::string> contactVariations(const State& previous) const;
65 
70  std::vector<std::string> freeVariations(
71  const State& previous,
72  const std::vector<std::string>& allEffectors) const;
73 
79  std::vector<std::string> allVariations(
80  const State& previous,
81  const std::vector<std::string>& allEffectors) const;
82 
87  std::vector<std::string> fixedContacts(const State& previous) const;
88 
93  void contactCreations(const State& previous,
94  std::vector<std::string>& outList) const;
95 
100  std::vector<std::string> contactCreations(const State& previous) const;
101 
106  std::vector<std::string> contactBreaks(const State& previous) const;
107 
112  void contactBreaks(const State& previous,
113  std::vector<std::string>& outList) const;
114 
115  void print() const;
116  void print(std::stringstream& ss) const;
117  void print(std::stringstream& ss, const State& previous) const;
118  void printInternal(std::stringstream& ss) const;
119 
120  hpp::pinocchio::Configuration_t configuration_;
121  std::map<std::string, bool> contacts_;
122  std::map<std::string, fcl::Vec3f> contactNormals_;
123  std::map<std::string, fcl::Vec3f> contactPositions_;
124  std::map<std::string, fcl::Matrix3f> contactRotation_;
125  std::queue<std::string> contactOrder_;
126  std::size_t nbContacts;
127  bool stable;
128  double robustness;
129 }; // struct State
132 HPP_RBPRM_DLLAPI pinocchio::value_type effectorDistance(const State& from,
133  const State& to);
134 
140 template <typename Iter>
141 HPP_RBPRM_DLLAPI std::vector<std::string> freeEffectors(const State& state,
142  Iter start, Iter end) {
143  std::vector<std::string> res;
144  for (Iter it = start; it != end; ++it) {
145  const std::string& eff = *it;
146  std::map<std::string, bool>::const_iterator cit = state.contacts_.find(eff);
147  if (cit == state.contacts_.end() || !cit->second) {
148  res.push_back(eff);
149  }
150  }
151  return res;
152 }
153 } // namespace rbprm
154 } // namespace hpp
155 
156 #endif // HPP_RBPRM_STATE_HH
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
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
double robustness
Definition: rbprm-state.hh:128
~State()
Definition: rbprm-state.hh:43
std::queue< std::string > contactOrder_
Definition: rbprm-state.hh:125
HPP_RBPRM_DLLAPI pinocchio::value_type effectorDistance(const State &from, const State &to)
T_State::const_iterator CIT_State
Definition: rbprm-state.hh:32
State()
Definition: rbprm-state.hh:41
std::map< std::string, bool > contacts_
Definition: rbprm-state.hh:121
HPP_RBPRM_DLLAPI std::vector< std::string > freeEffectors(const State &state, Iter start, Iter end)
Definition: rbprm-state.hh:141
std::vector< State > T_State
Definition: rbprm-state.hh:30
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: rbprm-state.hh:123
bool stable
Definition: rbprm-state.hh:127
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:33
std::size_t nbContacts
Definition: rbprm-state.hh:126
Definition: rbprm-state.hh:40
std::map< std::string, fcl::Vec3f > contactNormals_
Definition: rbprm-state.hh:122
hpp::pinocchio::Configuration_t configuration_
Definition: rbprm-state.hh:120
std::map< std::string, fcl::Matrix3f > contactRotation_
Definition: rbprm-state.hh:124