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 ndcurves {
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;
29  typedef Eigen::Matrix<Numeric, 3,1> vector_3_t;
30  typedef Eigen::Matrix<Numeric, 3,3> matrix_3_t;
32 
33  linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable
34  linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
35  linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
36  linear_variable(const linear_variable_t& other) : B_(other.B()), c_(other.c()), zero(other.isZero()) {} // copy constructor
37 
39 
44  vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
45  if (isZero()) return c();
46  if (Safe && B().cols() != val.rows())
47  throw std::length_error("Cannot evaluate linear variable, variable value does not have the correct dimension");
48  return B() * val + c();
49  }
50 
55  linear_variable_t& operator+=(const linear_variable_t& w1) {
56  if (w1.isZero())
57  return *this;
58  if (isZero()) {
59  this->B_ = w1.B_;
60  this->c_ = w1.c_;
61  zero = w1.isZero();
62  } else {
63  if (Safe && B().rows() != w1.B().rows())
64  throw std::length_error("Cannot add linear variables, variables do not have the same dimension");
65  else if (B().cols() > w1.B().cols()){ //new variables added left for primitive
66  B_.block(0,B().cols() - w1.B().cols(),B().rows(),w1.B().cols()) += w1.B();
67  c_.tail(w1.c().rows()) += w1.c();
68  }
69  else if (B().cols() < w1.B().cols()){ //new variables added left for primitive
70  linear_variable_t opp = w1 + (*this);
71  this->B_ = opp.B_;
72  this->c_ = opp.c_;
73  }
74  else{
75  this->B_ += w1.B_;
76  this->c_ += w1.c_;
77  }
78  }
79  return *this;
80  }
81 
86  linear_variable_t& operator-=(const linear_variable_t& w1) {
87  if (w1.isZero())
88  return *this;
89  if (isZero()) {
90  this->B_ = -w1.B_;
91  this->c_ = -w1.c_;
92  zero = w1.isZero();
93  } else {
94  if (Safe && B().rows() != w1.B().rows())
95  throw std::length_error("Cannot add linear variables, variables do not have the same dimension");
96  else if (B().cols() > w1.B().cols()){ //new variables added left for primitive
97  B_.block(0,B().cols() - w1.B().cols(),B().rows(),w1.B().cols()) -= w1.B();
98  c_.tail(w1.c().rows()) -= w1.c();
99  }
100  else if (B().cols() < w1.B().cols()){ //new variables added left for primitive
101  linear_variable_t opp = -w1 + (*this);
102  this->B_ = opp.B_;
103  this->c_ = opp.c_;
104  }
105  else{
106  this->B_ -= w1.B_;
107  this->c_ -= w1.c_;
108  }
109  }
110  return *this;
111  }
112 
117  linear_variable_t& operator/=(const double d) {
118  B_ /= d;
119  c_ /= d;
120  return *this;
121  }
122 
127  linear_variable_t& operator*=(const double d) {
128  B_ *= d;
129  c_ *= d;
130  return *this;
131  }
132 
139  linear_variable_t cross(const linear_variable_t& other) const {
140  if (B().rows() !=3)
141  throw std::invalid_argument("Can't perform cross product on linear variables with dimensions != 3 ");
142  if (B().cols() !=3)
143  throw std::invalid_argument("Can't perform cross product on linear variables more than one unknown ");
144  if (isZero() || other.isZero())
145  return linear_variable_t::Zero(3);
146  if ((B().squaredNorm() - B().diagonal().squaredNorm() > MARGIN ) || (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > MARGIN ) )
147  throw std::invalid_argument("Can't perform cross product on linear variables if B is not diagonal ");
148  // (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2
149  typename linear_variable_t::matrix_3_t newB = skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(-other.c()) * B() +
150  skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(c()) * other.B();
151  typename linear_variable_t::vector_3_t newC = ndcurves::cross(c(),other.c());
152  return linear_variable_t(newB,newC);
153  }
154 
159  static linear_variable_t Zero(size_t dim = 0) {
160  return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim));
161  }
162 
167  static linear_variable_t X(size_t dim = 0) {
168  return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
169  }
170 
171 
175  std::size_t size() const { return zero ? 0 : std::max(B_.rows(), c_.size()); }
176 
179  Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
180 
185  bool isApprox(const linear_variable_t& other,
186  const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
187  return (*this - other).norm() < prec;
188  }
189 
190  const matrix_x_t& B() const { return B_; }
191  const vector_x_t& c() const { return c_; }
192  bool isZero() const { return zero; }
193 
194  // Serialization of the class
196 
197  template <class Archive>
198  void serialize(Archive& ar, const unsigned int version) {
199  if (version) {
200  // Do something depending on version ?
201  }
202  ar& boost::serialization::make_nvp("B_", B_);
203  ar& boost::serialization::make_nvp("c_", c_);
204  ar& boost::serialization::make_nvp("zero", zero);
205  }
206 
207  private:
208  matrix_x_t B_;
209  vector_x_t c_;
210  bool zero;
211 };
212 
213 template <typename N, bool S>
215  linear_variable<N, S> res(w1.B(), w1.c());
216  return res += w2;
217 }
218 
219 template <typename N, bool S>
221  linear_variable<N, S> res(w1.B(), w1.c());
222  return res -= w2;
223 }
224 
225 template <typename N, bool S>
227  return linear_variable<N, S> (-w1.B(), -w1.c());
228 }
229 
230 template <typename N, bool S>
232  linear_variable<N, S> res(w.B(), w.c());
233  return res *= k;
234 }
235 
236 template <typename N, bool S>
238  linear_variable<N, S> res(w.B(), w.c());
239  return res *= k;
240 }
241 
242 template <typename N, bool S>
244  linear_variable<N, S> res(w.B(), w.c());
245  return res /= k;
246 }
247 
248 template <typename BezierFixed, typename BezierLinear, typename X>
249 BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
250  typename BezierFixed::t_point_t fixed_wps;
251  for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
252  fixed_wps.push_back(cit->operator()(x));
253  return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
254 }
255 
256 template <typename N, bool S>
257 std::ostream &operator<<(std::ostream &os, const linear_variable<N, S>& l) {
258  return os << "linear_variable: \n \t B:\n"<< l.B() << "\t c: \n" << l.c().transpose();
259 }
260 
261 } // namespace ndcurves
262 
263 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Numeric, bool Safe),
265 #endif //_CLASS_LINEAR_VARIABLE
std::size_t size() const
Get dimension of linear variable.
Definition: linear_variable.h:175
Definition: bernstein.h:20
linear_variable(const vector_x_t &c)
Definition: linear_variable.h:34
linear_variable(const matrix_x_t &B, const vector_x_t &c)
Definition: linear_variable.h:35
const matrix_x_t & B() const
Definition: linear_variable.h:190
Eigen::Vector3d cross(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Definition: cross_implementation.h:15
static linear_variable_t X(size_t dim=0)
Get a linear variable equal to the variable.
Definition: linear_variable.h:167
bool isZero() const
Definition: linear_variable.h:192
Eigen::Matrix< Numeric, 3, 1 > vector_3_t
Definition: linear_variable.h:29
BezierFixed evaluateLinear(const BezierLinear &bIn, const X x)
Definition: linear_variable.h:249
interface for a Curve of arbitrary dimension.
linear_variable_t & operator+=(const linear_variable_t &w1)
Add another linear variable.
Definition: linear_variable.h:55
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:677
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:185
Numeric norm() const
Get norm of linear variable (Norm of B plus norm of C).
Definition: linear_variable.h:179
static linear_variable_t Zero(size_t dim=0)
Get a linear variable equal to zero.
Definition: linear_variable.h:159
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
const vector_x_t & c() const
Definition: linear_variable.h:191
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:127
void serialize(Archive &ar, const unsigned int version)
Definition: linear_variable.h:198
vector_x_t operator()(const Eigen::Ref< const vector_x_t > &val) const
Linear evaluation for vector x.
Definition: linear_variable.h:44
friend class boost::serialization::access
Definition: linear_variable.h:195
Eigen::Matrix< Numeric, 3, 3 > matrix_3_t
Definition: linear_variable.h:30
linear_variable_t & operator-=(const linear_variable_t &w1)
Substract another linear variable.
Definition: linear_variable.h:86
Definition: fwd.h:55
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:117
linear_variable(const linear_variable_t &other)
Definition: linear_variable.h:36
linear_variable()
Definition: linear_variable.h:33
double Numeric
Definition: effector_spline.h:26
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:139
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: linear_variable.h:28
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:719
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:725
linear_variable< Numeric > linear_variable_t
Definition: linear_variable.h:31
~linear_variable()
Definition: linear_variable.h:38
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: linear_variable.h:27
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:671