hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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>
26#include <queue>
27
28namespace hpp {
29namespace rbprm {
30struct State;
31typedef std::vector<State> T_State;
32typedef T_State::const_iterator CIT_State;
33typedef std::pair<pinocchio::value_type, rbprm::State> StateFrame;
34typedef std::vector<StateFrame> T_StateFrame;
35typedef 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;
129}; // struct State
132HPP_RBPRM_DLLAPI pinocchio::value_type effectorDistance(const State& from,
133 const State& to);
134
140template <typename Iter>
141HPP_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
HPP_RBPRM_DLLAPI std::vector< std::string > freeEffectors(const State &state, Iter start, Iter end)
Definition: rbprm-state.hh:141
T_State::const_iterator CIT_State
Definition: rbprm-state.hh:32
std::vector< StateFrame > T_StateFrame
Definition: rbprm-state.hh:34
std::vector< State > T_State
Definition: rbprm-state.hh:31
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:35
std::pair< pinocchio::value_type, rbprm::State > StateFrame
Definition: rbprm-state.hh:33
HPP_RBPRM_DLLAPI pinocchio::value_type effectorDistance(const State &from, const State &to)
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40
double robustness
Definition: rbprm-state.hh:128
hpp::pinocchio::Configuration_t configuration_
Definition: rbprm-state.hh:120
void print(std::stringstream &ss, const State &previous) const
std::vector< std::string > contactBreaks(const State &previous) const
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: rbprm-state.hh:123
std::map< std::string, fcl::Vec3f > contactNormals_
Definition: rbprm-state.hh:122
void print() const
void contactBreaks(const State &previous, std::vector< std::string > &outList) const
State & operator=(const State &other)
std::map< std::string, bool > contacts_
Definition: rbprm-state.hh:121
void contactCreations(const State &previous, std::vector< std::string > &outList) const
std::vector< std::string > allVariations(const State &previous, const std::vector< std::string > &allEffectors) const
bool stable
Definition: rbprm-state.hh:127
bool RemoveContact(const std::string &contactId)
std::vector< std::string > fixedContacts(const State &previous) const
void print(std::stringstream &ss) const
void printInternal(std::stringstream &ss) const
std::size_t nbContacts
Definition: rbprm-state.hh:126
State(const State &other)
std::vector< std::string > freeVariations(const State &previous, const std::vector< std::string > &allEffectors) const
std::vector< std::string > contactVariations(const State &previous) const
std::map< std::string, fcl::Matrix3f > contactRotation_
Definition: rbprm-state.hh:124
State()
Definition: rbprm-state.hh:41
std::string RemoveFirstContact()
std::vector< std::string > contactCreations(const State &previous) const
std::queue< std::string > contactOrder_
Definition: rbprm-state.hh:125
~State()
Definition: rbprm-state.hh:43