iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
StateDef.H
Go to the documentation of this file.
1 /*! @file
2  @author Laurent Itti
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 #ifndef INCLUDE_NRT_PROBABILISTIC_TYPES_STATEDEF_H
37 #define INCLUDE_NRT_PROBABILISTIC_TYPES_STATEDEF_H
38 
39 
40 #include <nrt/Eigen/Eigen.H>
41 #include <iostream>
42 
43 // Include some definitions of no interest to the user
45 
46 namespace nrt
47 {
48  //! A Simple class to represent possibly complex and nested state information with dual access
49  /*! The goal of this class is not to store data but rather to provide easy, intuitive, hierarchical named access to
50  data stored in an Eigen vector. The StateDef class thus does not provide storage but only convenience access. This
51  class assumes that you have defined some datatypes whose internal data members are reference to double, such as:
52  @code
53  struct Pos3D
54  {
55  Pos3D(double storage[]) : x(storage[0]), y(storage[1]), z(storage[2]) { }
56 
57  double & x;
58  double & y;
59  double & z;
60 
61  static size_t const size = 3;
62  };
63  @endcode
64 
65  @code
66  struct Vel3D
67  {
68  Vel3D(double storage[]) : vx(storage[0]), vy(storage[1]), vz(storage[2]) { }
69 
70  double & vx;
71  double & vy;
72  double & vz;
73 
74  static size_t const size = 3;
75  };
76  @endcode
77 
78  The constructor from double[] and the size static data member are required. Once you have this, you can use StateDef
79  to build hierarchical structures of these datatypes and to access their leaf data elements naturally:
80 
81  @code
82  typedef StateDef<Pos3D, Vel3D> myStateDef;
83  myStateDef::datatype data; // allocate some Eigen vector to actually store the data
84  myStateDef state(data); // attach a state to it, which will allow easy named access to data elements
85 
86  state.get<Pos3D>().x = 1.0;
87  state.get<Vel3D>().vz = 6.0;
88  @endcode
89 
90  And of course the data remains accessible as an Eigen vector for vector/matrix operations on it. You can compose
91  State elements, for example:
92  @code
93  typedef StateDef<Vel3D, Pos3D> MachineState;
94  typedef StateDef<MachineState, Pos3D, Vel3D> FactoryState;
95  typedef StateDef<Pos3D, FactoryState, Vel3D> ComplicatedState;
96 
97  FactoryState::datatype factorydata;
98  FactoryState s(factorydata);
99 
100  std::cout << "FactoryState size = " << FactoryState::size << std::endl;
101 
102  ComplicatedState::datatype csdata;
103  ComplicatedState cs(csdata);
104 
105  std::cout << "ComplicatedState size = " << ComplicatedState::size << std::endl;
106 
107  cs.get<Vel3D>().vy = 123.0;
108  cs.get<FactoryState>().get<Pos3D>().x = 456.0;
109  cs.get<FactoryState>().get<MachineState>().get<Pos3D>().z = 789.0;
110 
111  size_t idx = cs.indexof(cs.get<FactoryState>().get<MachineState>().get<Vel3D>().vy);
112  csdata[idx] = 42.0; // same effect as cs[idx] = 42.0;
113 
114  std::cout << "ComplicatedState: " << cs << std::endl;
115  @endcode */
116  template <typename ...Elements>
117  class StateDef : public StateAccessor<Elements...>
118  {
119  public:
120  //! The datatype this particular StateDef instance should be attached to
121  typedef typename Eigen::Matrix<double, StateAccessor<Elements...>::size, 1> datatype;
122 
123  //! Construct by attaching to an already allocated Eigen vector
124  StateDef(datatype & data);
125 
126  //! Construct by attaching to an already allocated raw C vector
127  /*! Users should normally not use this constructor but it is used by nested StateDef elements. */
128  StateDef(double data[]);
129 
130  //! Get the index of a particular leaf element
131  /*! Leaf elements are of type double &, and can be retrieved using the get() functions of StateAccessor. */
132  size_t const indexof(double & addr);
133 
134  //! Get an element by index, non-const version
135  double & operator[](size_t const idx);
136 
137  //! Get an element by index, const version
138  double const & operator[](size_t const idx) const;
139 
140  private:
141  double * baseaddress;
142  };
143 
144 
145  //! Helper of StateDef which provides access to elements by their type
146  template <typename Head, typename ...Tail>
147  class StateAccessor<Head, Tail...> : public StateAccessorCore<Head>, public StateAccessor<Tail...>
148  {
149  public:
150  //! Constructor, used by State, should not be of general interest but left public for now
151  StateAccessor(double data[]);
152 
153  //! The compile-time-known size of the state, in number of double-type elements (not in bytes)
154  static size_t const size = Head::size + StateAccessor<Tail...>::size;
155 
156  //! Get an element by type name
157  /*! Beware that you should not have repeated elements of the same type in a State, otherwise this will always
158  return the first match. Use nested State structures or different types. Example:
159  @code
160  typedef State<Pos3D, Vel3D> myState;
161  myState::datatype data; // allocate some Eigen vector to actually store the data
162  myState state(data); // attach a State to it, which will allow easy named access to data elements
163 
164  state.get<Pos3D>().x = 1.0;
165  state.get<Vel3D>().vz = 6.0;
166  @endcode */
167  template <typename Elem>
168  typename std::enable_if<std::is_same<Elem, Head>::value, Elem &>::type
169  get();
170 
171  //! Get an element by type name (SFINAE recursion function when Elem is not the Head)
172  template <typename Elem>
173  typename std::enable_if<!std::is_same<Elem, Head>::value, Elem &>::type
174  get();
175 
176  //! Get an element by type name, const version
177  template <typename Elem>
178  typename std::enable_if<std::is_same<Elem, Head>::value, Elem const &>::type
179  get() const;
180 
181  //! Get an element by type name, const version (SFINAE recursion function when Elem is not the Head)
182  template <typename Elem>
183  typename std::enable_if<!std::is_same<Elem, Head>::value, Elem const &>::type
184  get() const;
185 
186  //! Get the index in our Eigen vector of a particular leaf double& element
187  template <typename Elem>
188  typename std::enable_if<std::is_same<Elem, Head>::value, size_t const>::type
189  index() const;
190 
191  //! Get the index in our Eigen vector of a particular leaf double& element (SFINAE)
192  template <typename Elem>
193  typename std::enable_if<!std::is_same<Elem, Head>::value, size_t const>::type
194  index() const;
195 
196  //! Recursively print our structure and contents
197  void print(std::ostream & os) const;
198  };
199 
200 } // namespace nrt
201 
202 //! Recursively print out a State's structure and data to an ostream
203 template <typename ...Elements>
204 std::ostream & operator<<(std::ostream & os, nrt::StateDef<Elements...> const & state);
205 
206 // Include some implementation details of no interest to the user
208 
209 
210 #endif // INCLUDE_NRT_PROBABILISTIC_TYPES_STATEDEF_H