9 #ifndef _CLASS_LINEAR_VARIABLE 10 #define _CLASS_LINEAR_VARIABLE 14 #include "serialization/archive.hpp" 15 #include "serialization/eigen-matrix.hpp" 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;
37 : B_(other.
B()), c_(other.
c()), zero(other.
isZero()) {}
45 vector_x_t
operator()(
const Eigen::Ref<const vector_x_t>& val)
const {
47 if (Safe &&
B().cols() != val.rows())
48 throw std::length_error(
"Cannot evaluate linear variable, variable value does not have the correct dimension");
49 return B() * val +
c();
56 linear_variable_t&
operator+=(
const linear_variable_t& w1) {
57 if (w1.
isZero())
return *
this;
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()) {
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 }
else if (
B().cols() < w1.
B().cols()) {
69 linear_variable_t opp = w1 + (*this);
84 linear_variable_t&
operator-=(
const linear_variable_t& w1) {
85 if (w1.
isZero())
return *
this;
91 if (Safe &&
B().rows() != w1.
B().rows())
92 throw std::length_error(
"Cannot add linear variables, variables do not have the same dimension");
93 else if (
B().cols() > w1.
B().cols()) {
94 B_.block(0,
B().cols() - w1.
B().cols(),
B().rows(), w1.
B().cols()) -= w1.
B();
95 c_.tail(w1.
c().rows()) -= w1.
c();
96 }
else if (
B().cols() < w1.
B().cols()) {
97 linear_variable_t opp = -w1 + (*this);
134 linear_variable_t
cross(
const linear_variable_t& other)
const {
136 throw std::invalid_argument(
"Can't perform cross product on linear variables with dimensions != 3 ");
138 throw std::invalid_argument(
"Can't perform cross product on linear variables more than one unknown ");
140 if ((
B().squaredNorm() -
B().diagonal().squaredNorm() > MARGIN) ||
141 (other.
B().squaredNorm() - other.
B().diagonal().squaredNorm() > MARGIN))
142 throw std::invalid_argument(
"Can't perform cross product on linear variables if B is not diagonal ");
145 skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(-other.
c()) *
B() +
146 skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(
c()) * other.
B();
155 static linear_variable_t
Zero(
size_t dim = 0) {
163 static linear_variable_t
X(
size_t dim = 0) {
170 std::size_t
size()
const {
return zero ? 0 : std::max(B_.rows(), c_.size()); }
181 const double prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
182 return (*
this - other).norm() < prec;
185 const matrix_x_t&
B()
const {
return B_; }
186 const vector_x_t&
c()
const {
return c_; }
192 template <
class Archive>
193 void serialize(Archive& ar,
const unsigned int version) {
197 ar& boost::serialization::make_nvp(
"B_", B_);
198 ar& boost::serialization::make_nvp(
"c_", c_);
199 ar& boost::serialization::make_nvp(
"zero", zero);
208 template <
typename N,
bool S>
214 template <
typename N,
bool S>
220 template <
typename N,
bool S>
225 template <
typename N,
bool S>
231 template <
typename N,
bool S>
237 template <
typename N,
bool S>
243 template <
typename BezierFixed,
typename BezierLinear,
typename X>
245 typename BezierFixed::t_point_t fixed_wps;
246 for (
typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
247 fixed_wps.push_back(cit->operator()(x));
248 return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
251 template <
typename N,
bool S>
252 std::ostream& operator<<(std::ostream& os, const linear_variable<N, S>& l) {
253 return os <<
"linear_variable: \n \t B:\n" << l.B() <<
"\t c: \n" << l.c().transpose();
258 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Numeric,
bool Safe),
260 #endif //_CLASS_LINEAR_VARIABLE std::size_t size() const
Get dimension of linear variable.
Definition: linear_variable.h:170
Eigen::Matrix< Numeric, 3, 1 > vector_3_t
Definition: linear_variable.h:29
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
Eigen::Matrix< Numeric, 3, 3 > matrix_3_t
Definition: linear_variable.h:30
const matrix_x_t & B() const
Definition: linear_variable.h:185
Eigen::Vector3d cross(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Definition: cross_implementation.h:14
static linear_variable_t X(size_t dim=0)
Get a linear variable equal to the variable.
Definition: linear_variable.h:163
bool isZero() const
Definition: linear_variable.h:187
BezierFixed evaluateLinear(const BezierLinear &bIn, const X x)
Definition: linear_variable.h:244
interface for a Curve of arbitrary dimension.
linear_variable_t & operator+=(const linear_variable_t &w1)
Add another linear variable.
Definition: linear_variable.h:56
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:711
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:180
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:660
Numeric norm() const
Get norm of linear variable (Norm of B plus norm of C).
Definition: linear_variable.h:174
static linear_variable_t Zero(size_t dim=0)
Get a linear variable equal to zero.
Definition: linear_variable.h:155
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
const vector_x_t & c() const
Definition: linear_variable.h:186
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:717
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:122
void serialize(Archive &ar, const unsigned int version)
Definition: linear_variable.h:193
vector_x_t operator()(const Eigen::Ref< const vector_x_t > &val) const
Linear evaluation for vector x.
Definition: linear_variable.h:45
friend class boost::serialization::access
Definition: linear_variable.h:190
linear_variable_t & operator-=(const linear_variable_t &w1)
Substract another linear variable.
Definition: linear_variable.h:84
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:112
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:134
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: linear_variable.h:28
linear_variable< Numeric > linear_variable_t
Definition: linear_variable.h:31
~linear_variable()
Definition: linear_variable.h:39
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:666
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: linear_variable.h:27