linear_variable.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_LINEAR_VARIABLE
10 #define _CLASS_LINEAR_VARIABLE
11 
12 #include <math.h>
13 
14 #include <Eigen/Core>
15 #include <stdexcept>
16 #include <vector>
17 
18 #include "MathDefs.h"
19 #include "bezier_curve.h"
20 #include "curve_abc.h"
23 
24 namespace ndcurves {
25 template <typename Numeric = double, bool Safe = true>
27  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
28  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
29  typedef Eigen::Matrix<Numeric, 3, 1> vector_3_t;
30  typedef Eigen::Matrix<Numeric, 3, 3> matrix_3_t;
32 
34  : B_(matrix_x_t::Identity(0, 0)),
35  c_(vector_x_t::Zero(0)),
36  zero(true) {} // variable
38  : B_(matrix_x_t::Zero(c.size(), c.size())),
39  c_(c),
40  zero(false) {} // constant
42  : B_(B), c_(c), zero(false) {} // mixed
44  : B_(other.B()),
45  c_(other.c()),
46  zero(other.isZero()) {} // copy constructor
47 
49 
54  vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
55  if (isZero()) return c();
56  if (Safe && B().cols() != val.rows())
57  throw std::length_error(
58  "Cannot evaluate linear variable, variable value does not have the "
59  "correct dimension");
60  return B() * val + c();
61  }
62 
68  if (w1.isZero()) return *this;
69  if (isZero()) {
70  this->B_ = w1.B_;
71  this->c_ = w1.c_;
72  zero = w1.isZero();
73  } else {
74  if (Safe && B().rows() != w1.B().rows())
75  throw std::length_error(
76  "Cannot add linear variables, variables do not have the same "
77  "dimension");
78  else if (B().cols() >
79  w1.B().cols()) { // new variables added left for primitive
80  B_.block(0, B().cols() - w1.B().cols(), B().rows(), w1.B().cols()) +=
81  w1.B();
82  c_.tail(w1.c().rows()) += w1.c();
83  } else if (B().cols() <
84  w1.B().cols()) { // new variables added left for primitive
85  linear_variable_t opp = w1 + (*this);
86  this->B_ = opp.B_;
87  this->c_ = opp.c_;
88  } else {
89  this->B_ += w1.B_;
90  this->c_ += w1.c_;
91  }
92  }
93  return *this;
94  }
95 
101  if (w1.isZero()) return *this;
102  if (isZero()) {
103  this->B_ = -w1.B_;
104  this->c_ = -w1.c_;
105  zero = w1.isZero();
106  } else {
107  if (Safe && B().rows() != w1.B().rows())
108  throw std::length_error(
109  "Cannot add linear variables, variables do not have the same "
110  "dimension");
111  else if (B().cols() >
112  w1.B().cols()) { // new variables added left for primitive
113  B_.block(0, B().cols() - w1.B().cols(), B().rows(), w1.B().cols()) -=
114  w1.B();
115  c_.tail(w1.c().rows()) -= w1.c();
116  } else if (B().cols() <
117  w1.B().cols()) { // new variables added left for primitive
118  linear_variable_t opp = -w1 + (*this);
119  this->B_ = opp.B_;
120  this->c_ = opp.c_;
121  } else {
122  this->B_ -= w1.B_;
123  this->c_ -= w1.c_;
124  }
125  }
126  return *this;
127  }
128 
133  linear_variable_t& operator/=(const double d) {
134  B_ /= d;
135  c_ /= d;
136  return *this;
137  }
138 
143  linear_variable_t& operator*=(const double d) {
144  B_ *= d;
145  c_ *= d;
146  return *this;
147  }
148 
159  if (B().rows() != 3)
160  throw std::invalid_argument(
161  "Can't perform cross product on linear variables with dimensions != "
162  "3 ");
163  if (B().cols() != 3)
164  throw std::invalid_argument(
165  "Can't perform cross product on linear variables more than one "
166  "unknown ");
167  if (isZero() || other.isZero()) return linear_variable_t::Zero(3);
168  if ((B().squaredNorm() - B().diagonal().squaredNorm() > MARGIN) ||
169  (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > MARGIN))
170  throw std::invalid_argument(
171  "Can't perform cross product on linear variables if B is not "
172  "diagonal ");
173  // (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2
174  typename linear_variable_t::matrix_3_t newB =
176  typename linear_variable_t::vector_3_t>(-other.c()) *
177  B() +
179  typename linear_variable_t::vector_3_t>(c()) *
180  other.B();
181  typename linear_variable_t::vector_3_t newC =
182  ndcurves::cross(c(), other.c());
183  return linear_variable_t(newB, newC);
184  }
185 
190  static linear_variable_t Zero(size_t dim = 0) {
191  return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim));
192  }
193 
198  static linear_variable_t X(size_t dim = 0) {
199  return linear_variable_t(matrix_x_t::Identity(dim, dim),
200  vector_x_t::Zero(dim));
201  }
202 
206  std::size_t size() const { return zero ? 0 : std::max(B_.rows(), c_.size()); }
207 
210  Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
211 
217  bool isApprox(
218  const linear_variable_t& other,
219  const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
220  return (*this - other).norm() < prec;
221  }
222 
223  const matrix_x_t& B() const { return B_; }
224  const vector_x_t& c() const { return c_; }
225  bool isZero() const { return zero; }
226 
227  // Serialization of the class
229 
230  template <class Archive>
231  void serialize(Archive& ar, const unsigned int version) {
232  if (version) {
233  // Do something depending on version ?
234  }
235  ar& boost::serialization::make_nvp("B_", B_);
236  ar& boost::serialization::make_nvp("c_", c_);
237  ar& boost::serialization::make_nvp("zero", zero);
238  }
239 
240  private:
241  matrix_x_t B_;
242  vector_x_t c_;
243  bool zero;
244 };
245 
246 template <typename N, bool S>
248  const linear_variable<N, S>& w2) {
249  linear_variable<N, S> res(w1.B(), w1.c());
250  return res += w2;
251 }
252 
253 template <typename N, bool S>
255  const linear_variable<N, S>& w2) {
256  linear_variable<N, S> res(w1.B(), w1.c());
257  return res -= w2;
258 }
259 
260 template <typename N, bool S>
262  return linear_variable<N, S>(-w1.B(), -w1.c());
263 }
264 
265 template <typename N, bool S>
267  const linear_variable<N, S>& w) {
268  linear_variable<N, S> res(w.B(), w.c());
269  return res *= k;
270 }
271 
272 template <typename N, bool S>
274  const double k) {
275  linear_variable<N, S> res(w.B(), w.c());
276  return res *= k;
277 }
278 
279 template <typename N, bool S>
281  const double k) {
282  linear_variable<N, S> res(w.B(), w.c());
283  return res /= k;
284 }
285 
286 template <typename BezierFixed, typename BezierLinear, typename X>
287 BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
288  typename BezierFixed::t_point_t fixed_wps;
289  for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin();
290  cit != bIn.waypoints().end(); ++cit)
291  fixed_wps.push_back(cit->operator()(x));
292  return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_,
293  bIn.T_max_);
294 }
295 
296 template <typename N, bool S>
297 std::ostream& operator<<(std::ostream& os, const linear_variable<N, S>& l) {
298  return os << "linear_variable: \n \t B:\n"
299  << l.B() << "\t c: \n"
300  << l.c().transpose();
301 }
302 
303 } // namespace ndcurves
304 
306  SINGLE_ARG(typename Numeric, bool Safe),
308 #endif //_CLASS_LINEAR_VARIABLE
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
interface for a Curve of arbitrary dimension.
double Numeric
Definition: effector_spline.h:26
Definition: bernstein.h:20
Eigen::Vector3d cross(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Definition: cross_implementation.h:14
std::ostream & operator<<(std::ostream &os, const linear_variable< N, S > &l)
Definition: linear_variable.h:297
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:805
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:755
Matrix3 skew(const Point &x)
Definition: MathDefs.h:42
BezierFixed evaluateLinear(const BezierLinear &bIn, const X x)
Definition: linear_variable.h:287
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:812
bezier_curve< T, N, S, P > operator+(const bezier_curve< T, N, S, P > &p1, const bezier_curve< T, N, S, P > &p2)
Definition: bezier_curve.h:748
Definition: linear_variable.h:26
linear_variable< Numeric > linear_variable_t
Definition: linear_variable.h:31
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:133
vector_x_t operator()(const Eigen::Ref< const vector_x_t > &val) const
Linear evaluation for vector x.
Definition: linear_variable.h:54
Eigen::Matrix< Numeric, 3, 3 > matrix_3_t
Definition: linear_variable.h:30
Eigen::Matrix< Numeric, 3, 1 > vector_3_t
Definition: linear_variable.h:29
~linear_variable()
Definition: linear_variable.h:48
const vector_x_t & c() const
Definition: linear_variable.h:224
linear_variable(const vector_x_t &c)
Definition: linear_variable.h:37
linear_variable()
Definition: linear_variable.h:33
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:217
std::size_t size() const
Get dimension of linear variable.
Definition: linear_variable.h:206
Numeric norm() const
Get norm of linear variable (Norm of B plus norm of C).
Definition: linear_variable.h:210
void serialize(Archive &ar, const unsigned int version)
Definition: linear_variable.h:231
const matrix_x_t & B() const
Definition: linear_variable.h:223
linear_variable(const linear_variable_t &other)
Definition: linear_variable.h:43
linear_variable(const matrix_x_t &B, const vector_x_t &c)
Definition: linear_variable.h:41
static linear_variable_t X(size_t dim=0)
Get a linear variable equal to the variable.
Definition: linear_variable.h:198
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: linear_variable.h:28
friend class boost::serialization::access
Definition: linear_variable.h:228
linear_variable_t cross(const linear_variable_t &other) const
Compute the cross product of the current linear_variable and the other. This method of course only ma...
Definition: linear_variable.h:158
linear_variable_t & operator-=(const linear_variable_t &w1)
Substract another linear variable.
Definition: linear_variable.h:100
static linear_variable_t Zero(size_t dim=0)
Get a linear variable equal to zero.
Definition: linear_variable.h:190
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: linear_variable.h:27
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:143
bool isZero() const
Definition: linear_variable.h:225
linear_variable_t & operator+=(const linear_variable_t &w1)
Add another linear variable.
Definition: linear_variable.h:67
Definition: archive.hpp:41