linear_variable.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_LINEAR_VARIABLE
10 #define _CLASS_LINEAR_VARIABLE
11 
12 #include "curve_abc.h"
13 #include "bezier_curve.h"
14 #include "serialization/archive.hpp"
15 #include "serialization/eigen-matrix.hpp"
16 
17 #include "MathDefs.h"
18 
19 #include <math.h>
20 #include <vector>
21 #include <Eigen/Core>
22 #include <stdexcept>
23 
24 namespace curves {
25 template <typename Numeric = double, bool Safe = true>
26 struct linear_variable : public serialization::Serializable {
27  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
28  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
30 
31  linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable
32  linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
33  linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
34 
39  vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
40  if (isZero()) return c();
41  if (Safe && B().cols() != val.rows())
42  throw std::length_error("Cannot evaluate linear variable, variable value does not have the correct dimension");
43  return B() * val + c();
44  }
45 
50  linear_variable_t& operator+=(const linear_variable_t& w1) {
51  if (w1.isZero()) return *this;
52  if (isZero()) {
53  this->B_ = w1.B_;
54  zero = w1.isZero();
55  } else {
56  this->B_ += w1.B_;
57  }
58  this->c_ += w1.c_;
59  return *this;
60  }
61 
66  linear_variable_t& operator-=(const linear_variable_t& w1) {
67  if (w1.isZero()) return *this;
68  if (isZero()) {
69  this->B_ = -w1.B_;
70  zero = w1.isZero();
71  } else {
72  this->B_ -= w1.B_;
73  }
74  this->c_ -= w1.c_;
75  return *this;
76  }
77 
82  linear_variable_t& operator/=(const double d) {
83  B_ /= d;
84  c_ /= d;
85  return *this;
86  }
87 
92  linear_variable_t& operator*=(const double d) {
93  B_ *= d;
94  c_ *= d;
95  return *this;
96  }
97 
102  static linear_variable_t Zero(size_t dim = 0) {
103  return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
104  }
105 
109  std::size_t size() const { return zero ? 0 : std::max(B_.cols(), c_.size()); }
110 
113  Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
114 
119  bool isApprox(const linear_variable_t& other,
120  const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
121  return (*this - other).norm() < prec;
122  }
123 
124  const matrix_x_t& B() const { return B_; }
125  const vector_x_t& c() const { return c_; }
126  bool isZero() const { return zero; }
127 
128  // Serialization of the class
130 
131  template <class Archive>
132  void serialize(Archive& ar, const unsigned int version) {
133  if (version) {
134  // Do something depending on version ?
135  }
136  ar& boost::serialization::make_nvp("B_", B_);
137  ar& boost::serialization::make_nvp("c_", c_);
138  ar& boost::serialization::make_nvp("zero", zero);
139  }
140 
141  private:
142  matrix_x_t B_;
143  vector_x_t c_;
144  bool zero;
145 };
146 
147 template <typename N, bool S>
149  linear_variable<N, S> res(w1.B(), w1.c());
150  return res += w2;
151 }
152 
153 template <typename N, bool S>
155  linear_variable<N, S> res(w1.B(), w1.c());
156  return res -= w2;
157 }
158 
159 template <typename N, bool S>
161  linear_variable<N, S> res(w.B(), w.c());
162  return res *= k;
163 }
164 
165 template <typename N, bool S>
167  linear_variable<N, S> res(w.B(), w.c());
168  return res *= k;
169 }
170 
171 template <typename N, bool S>
173  linear_variable<N, S> res(w.B(), w.c());
174  return res /= k;
175 }
176 
177 template <typename BezierFixed, typename BezierLinear, typename X>
178 BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
179  typename BezierFixed::t_point_t fixed_wps;
180  for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
181  fixed_wps.push_back(cit->operator()(x));
182  return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
183 }
184 
185 } // namespace curves
186 #endif //_CLASS_LINEAR_VARIABLE
std::size_t size() const
Get dimension of linear variable.
Definition: linear_variable.h:109
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: linear_variable.h:28
BezierFixed evaluateLinear(const BezierLinear &bIn, const X x)
Definition: linear_variable.h:178
linear_variable< Numeric > linear_variable_t
Definition: linear_variable.h:29
double Numeric
Definition: effector_spline.h:26
Numeric norm() const
Get norm of linear variable (Norm of B plus norm of C).
Definition: linear_variable.h:113
linear_variable_t & operator-=(const linear_variable_t &w1)
Substract another linear variable.
Definition: linear_variable.h:66
bool isApprox(const linear_variable_t &other, const double prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Check if actual linear variable and other are approximately equal given a precision treshold...
Definition: linear_variable.h:119
const matrix_x_t & B() const
Definition: linear_variable.h:124
static linear_variable_t Zero(size_t dim=0)
Get a linear variable equal to zero.
Definition: linear_variable.h:102
bool isZero() const
Definition: linear_variable.h:126
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: linear_variable.h:27
linear_variable< N, S > operator/(const linear_variable< N, S > &w, const double k)
Definition: linear_variable.h:172
linear_variable< N, S > operator-(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:154
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
linear_variable()
Definition: linear_variable.h:31
const vector_x_t & c() const
Definition: linear_variable.h:125
friend class boost::serialization::access
Definition: linear_variable.h:129
linear_variable_t & operator/=(const double d)
Divide by a constant : p_i / d = B_i*x/d + c_i/d.
Definition: linear_variable.h:82
linear_variable(const matrix_x_t &B, const vector_x_t &c)
Definition: linear_variable.h:33
void serialize(Archive &ar, const unsigned int version)
Definition: linear_variable.h:132
linear_variable< N, S > operator*(const double k, const linear_variable< N, S > &w)
Definition: linear_variable.h:160
vector_x_t operator()(const Eigen::Ref< const vector_x_t > &val) const
Linear evaluation for vector x.
Definition: linear_variable.h:39
linear_variable_t & operator*=(const double d)
Multiply by a constant : p_i / d = B_i*x*d + c_i*d.
Definition: linear_variable.h:92
Definition: fwd.h:55
linear_variable_t & operator+=(const linear_variable_t &w1)
Add another linear variable.
Definition: linear_variable.h:50
linear_variable< N, S > operator+(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:148
linear_variable(const vector_x_t &c)
Definition: linear_variable.h:32