humoto
triple_integrator.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Jan Michalczyk
4  @author Alexander Sherikov
5  @copyright 2014-2017 INRIA. Licensed under the Apache License, Version 2.0.
6  (see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7  @brief
8 */
9 
10 #pragma once
11 
12 namespace humoto
13 {
14  namespace rigidbody
15  {
16  /**
17  * @brief Triple integrator class
18  * Class supports arbitrary number of integrators in the system
19  *
20  * This class generates matrices of the following systems:
21  *
22  * 1) controlled by acceleration
23  * x(k+1) = A x(k) + B ddx(k+1)
24  * dddx(k) = D x(k) + E ddx(k+1)
25  *
26  * 2) controlled by velocity
27  * x(k+1) = A x(k) + B dx(k+1)
28  * dddx(k) = D x(k) + E dx(k+1)
29  *
30  * 3) controlled by position
31  * x(k+1) = A x(k) + B x(k+1)
32  * dddx(k) = D x(k) + E x(k+1)
33  *
34  * 4) controlled by jerk
35  * x(k+1) = A x(k) + B dddx(k+1)
36  */
37  class HUMOTO_LOCAL TripleIntegrator
38  {
39  public:
40  /**
41  * @brief Create A matrix of the model, suffix of the function
42  * name indicates the type of control [jerk, acc =
43  * acceleration, vel = velocity, pos = position].
44  *
45  * @tparam t_number_of_integrators number of integrators in the system
46  *
47  * @param[in] T sampling time
48  *
49  * @return A matrix
50  */
51  template <std::size_t t_number_of_integrators>
52  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
53  getAJerk(const double T)
54  {
55  etools::Matrix3 A3;
56  A3 << 1., T, T*T/2.,
57  0., 1., T,
58  0., 0., 1.;
59  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
60  }
61 
62  //======================================================================
63 
64  /// @copydoc getAJerk()
65  template <std::size_t t_number_of_integrators>
66  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
67  getAAcc(const double T)
68  {
69  etools::Matrix3 A3;
70  A3 << 1., T, T*T/3.,
71  0., 1., T/2.,
72  0., 0., 0.;
73  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
74  }
75 
76 
77  /// @copydoc getAJerk()
78  template <std::size_t t_number_of_integrators>
79  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
80  getAVel(const double T)
81  {
82  etools::Matrix3 A3;
83  A3 << 1., 2.*T/3., T*T/6.,
84  0., 0., 0.,
85  0., -2./T, -1.;
86  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
87  }
88 
89 
90  /// @copydoc getAJerk()
91  template <std::size_t t_number_of_integrators>
92  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
93  getAPos(const double T)
94  {
95  etools::Matrix3 A3;
96  A3 << 0., 0., 0.,
97  -3./T, -2., -T/2.,
98  -6./(T*T), -6./T, -2.;
99  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
100  }
101 
102  //======================================================================
103 
104  /**
105  * @brief Create A matrix of the model, suffix of the function
106  * name indicates the type of control [jerk, acc =
107  * acceleration, vel = velocity, pos = position].
108  *
109  * @tparam t_number_of_integrators number of integrators in the system
110  *
111  * @param[in] T sampling time
112  * @param[in] Ts subsampling time
113  *
114  * @return A matrix
115  */
116  template <std::size_t t_number_of_integrators>
117  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
118  getAsAcc(const double T, const double Ts)
119  {
120  etools::Matrix3 A3;
121  A3 <<
122  1 , Ts , - (Ts*Ts*Ts - 3*T*Ts*Ts)/(6*T) ,
123  0 , 1 , - (Ts*Ts - 2*T*Ts)/(2*T) ,
124  0 , 0 , - (Ts - T)/T ;
125  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
126  }
127 
128 
129  /// @copydoc getAsAcc()
130  template <std::size_t t_number_of_integrators>
131  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
132  getAsVel(const double T, const double Ts)
133  {
134  etools::Matrix3 A3;
135  A3 <<
136  1 , - (Ts*Ts*Ts - 3*T*T*Ts)/(3*T*T) , - (2*Ts*Ts*Ts - 3*T*Ts*Ts)/(6*T) ,
137  0 , - (Ts*Ts - T*T)/(T*T) , - (Ts*Ts - T*Ts)/T ,
138  0 , - (2*Ts)/(T*T) , - (2*Ts - T)/T ;
139  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
140  }
141 
142 
143  /// @copydoc getAsAcc()
144  template <std::size_t t_number_of_integrators>
145  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 3*t_number_of_integrators)
146  getAsPos(const double T, const double Ts)
147  {
148  etools::Matrix3 A3;
149  A3 <<
150  - (Ts*Ts*Ts - T*T*T)/(T*T*T) , - (Ts*Ts*Ts - T*T*Ts)/(T*T) , - (Ts*Ts*Ts - T*Ts*Ts)/(2*T) ,
151  - (3*Ts*Ts)/(T*T*T) , - (3*Ts*Ts - T*T)/(T*T) , - (3*Ts*Ts - 2*T*Ts)/(2*T) ,
152  - (6*Ts)/(T*T*T) , - (6*Ts)/(T*T) , - (3*Ts - T)/T ;
153  return(etools::makeBlockDiagonal(A3, t_number_of_integrators));
154  }
155 
156 
157  //======================================================================
158  //======================================================================
159  //======================================================================
160 
161 
162  /**
163  * @brief Create B matrix of the model, suffix of the function
164  * name indicates the type of control [jerk, acc =
165  * acceleration, vel = velocity, pos = position].
166  *
167  * @tparam t_number_of_integrators number of integrators in the system
168  *
169  * @param[in] T sampling time
170  * @return B3 matrix
171  */
172  template <std::size_t t_number_of_integrators>
173  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
174  getBJerk(const double T)
175  {
176  etools::Vector3 B3;
177  B3 << T*T*T/6.,
178  T*T/2.,
179  T;
180  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
181  }
182 
183  //======================================================================
184 
185  /// @copydoc getBJerk()
186  template <std::size_t t_number_of_integrators>
187  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
188  getBAcc(const double T)
189  {
190  etools::Vector3 B3;
191  B3 << T*T/6.,
192  T/2.,
193  1.;
194  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
195  }
196 
197 
198  /// @copydoc getBJerk()
199  template <std::size_t t_number_of_integrators>
200  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
201  getBVel(const double T)
202  {
203  etools::Vector3 B3;
204  B3 << T/3.,
205  1.,
206  2./T;
207  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
208  }
209 
210 
211  /// @copydoc getBJerk()
212  template <std::size_t t_number_of_integrators>
213  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
214  getBPos(const double T)
215  {
216  etools::Vector3 B3;
217  B3 << 1.,
218  3./T,
219  6./(T*T);
220  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
221  }
222 
223  //======================================================================
224 
225  /**
226  * @brief Create B matrix of the model, suffix of the function
227  * name indicates the type of control [jerk, acc =
228  * acceleration, vel = velocity, pos = position].
229  *
230  * @tparam t_number_of_integrators number of integrators in the system
231  *
232  * @param[in] T sampling time
233  * @param[in] Ts subsampling time
234  * @return B3 matrix
235  */
236  template <std::size_t t_number_of_integrators>
237  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
238  getBsAcc(const double T, const double Ts)
239  {
240  etools::Vector3 B3;
241  B3 <<
242  (Ts*Ts*Ts)/(6*T) ,
243  (Ts*Ts)/(2*T) ,
244  (Ts)/(T) ;
245  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
246  }
247 
248 
249  /// @copydoc getBsAcc()
250  template <std::size_t t_number_of_integrators>
251  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
252  getBsVel(const double T, const double Ts)
253  {
254  etools::Vector3 B3;
255  B3 <<
256  (Ts*Ts*Ts)/(3*T*T) ,
257  (Ts*Ts)/(T*T) ,
258  (2*Ts)/(T*T) ;
259  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
260  }
261 
262 
263  /// @copydoc getBsAcc()
264  template <std::size_t t_number_of_integrators>
265  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(3*t_number_of_integrators, 1*t_number_of_integrators)
266  getBsPos(const double T, const double Ts)
267  {
268  etools::Vector3 B3;
269  B3 <<
270  (Ts*Ts*Ts)/(T*T*T) ,
271  (3*Ts*Ts)/(T*T*T) ,
272  (6*Ts)/(T*T*T) ;
273  return(etools::makeBlockDiagonal(B3, t_number_of_integrators));
274  }
275 
276 
277  //======================================================================
278  //======================================================================
279  //======================================================================
280 
281 
282  /**
283  * @brief Create D matrix of the model, suffix of the function
284  * name indicates the type of control [acc = acceleration, vel
285  * = velocity, pos = position].
286  *
287  * @tparam t_number_of_integrators number of integrators in the system
288  *
289  * @param[in] T sampling time
290  * @return A matrix
291  */
292  template <std::size_t t_number_of_integrators>
293  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 3*t_number_of_integrators)
294  getDAcc(const double T)
295  {
296  etools::Matrix1x3 D3;
297  D3 << 0., 0., -1./T;
298  return(etools::makeBlockDiagonal(D3, t_number_of_integrators));
299  }
300 
301 
302  /// @copydoc getDAcc()
303  template <std::size_t t_number_of_integrators>
304  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 3*t_number_of_integrators)
305  getDVel(const double T)
306  {
307  etools::Matrix1x3 D3;
308  D3 << 0., -2./(T*T), -2./T;
309  return(etools::makeBlockDiagonal(D3, t_number_of_integrators));
310  }
311 
312 
313  /// @copydoc getDAcc()
314  template <std::size_t t_number_of_integrators>
315  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 3*t_number_of_integrators)
316  getDPos(const double T)
317  {
318  etools::Matrix1x3 D3;
319  D3 << -6./(T*T*T), -6./(T*T), -3./T;
320  return(etools::makeBlockDiagonal(D3, t_number_of_integrators));
321  }
322 
323  //======================================================================
324  //======================================================================
325  //======================================================================
326 
327 
328  /**
329  * @brief Create E matrix of the model, suffix of the function
330  * name indicates the type of control [acc = acceleration, vel
331  * = velocity, pos = position].
332  *
333  * @tparam t_number_of_integrators number of integrators in the system
334  *
335  * @param[in] T sampling time
336  * @return A matrix
337  */
338  template <std::size_t t_number_of_integrators>
339  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)
340  getEAcc(const double T)
341  {
342  double E3;
343  E3 = 1./T;
344  return(E3*EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)::Identity(t_number_of_integrators, t_number_of_integrators));
345  }
346 
347 
348  /// @copydoc getEAcc()
349  template <std::size_t t_number_of_integrators>
350  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)
351  getEVel(const double T)
352  {
353  double E3;
354  E3 = 2./(T*T);
355  return(E3*EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)::Identity(t_number_of_integrators, t_number_of_integrators));
356  }
357 
358 
359  /// @copydoc getEAcc()
360  template <std::size_t t_number_of_integrators>
361  static EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)
362  getEPos(const double T)
363  {
364  double E3;
365  E3 = 6./(T*T*T);
366  return(E3*EIGENTOOLS_CONSTANT_SIZE_MATRIX(1*t_number_of_integrators, 1*t_number_of_integrators)::Identity(t_number_of_integrators, t_number_of_integrators));
367  }
368  };
369  }
370 }
HUMOTO_LOCAL
#define HUMOTO_LOCAL
Definition: export_import.h:26
humoto::rigidbody::TripleIntegrator
Triple integrator class Class supports arbitrary number of integrators in the system.
Definition: triple_integrator.h:37
EIGENTOOLS_CONSTANT_SIZE_MATRIX
#define EIGENTOOLS_CONSTANT_SIZE_MATRIX(rows, cols)
Definition: eigentools.h:63
humoto
The root namespace of HuMoTo.
Definition: config.h:12
etools::Matrix1x3
EIGENTOOLS_CONSTANT_SIZE_MATRIX Matrix1x3
Definition: eigentools.h:87
etools::makeBlockDiagonal
Eigen::Matrix< t_Scalar, Eigen::Dynamic, Eigen::Dynamic > EIGENTOOLS_VISIBILITY_ATTRIBUTE makeBlockDiagonal(const std::vector< Eigen::Matrix< t_Scalar, t_rows, t_cols, t_flags > > &input_matrices)
Create a diagonal matrix consisting of the input matrices.
Definition: eigentools.h:368
etools::Matrix3
EIGENTOOLS_CONSTANT_SIZE_MATRIX Matrix3
Definition: eigentools.h:77
Generated on Fri Sep 29 2017 16:44:40 for humoto by   doxygen 1.8.13