9 #ifndef _CLASS_QUADRATIC_VARIABLE
10 #define _CLASS_QUADRATIC_VARIABLE
24 template <
typename Numeric =
double>
26 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
matrix_x_t;
27 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1>
point_t;
32 b_ = point_t::Zero(1);
33 A_ = matrix_x_t::Zero(1, 1);
38 : c_(
c), b_(
b), A_(
A), zero(false) {
39 if (
A.cols() !=
b.rows() ||
A.cols() !=
A.rows()) {
40 throw std::invalid_argument(
"The dimensions of A and b are incorrect.");
57 throw std::runtime_error(
"Not initialized! (isZero)");
59 return val.transpose() *
A() * val +
b().transpose() * val +
c();
63 if (w1.
isZero())
return *
this;
77 if (w1.
isZero())
return *
this;
112 throw std::runtime_error(
"Not initialized! (isZero)");
118 throw std::runtime_error(
"Not initialized! (isZero)");
124 throw std::runtime_error(
"Not initialized! (isZero)");
130 return zero ? 0 : std::max(A_.cols(), (std::max(b_.cols(), c_.size())));
141 template <
typename N>
143 const Eigen::Ref<
const Eigen::Matrix<N, Eigen::Dynamic, 1> > vec) {
144 typedef typename Eigen::Matrix<N, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
145 return vec.asDiagonal();
146 matrix_t res(matrix_t::Zero(vec.rows(), vec.rows()));
147 for (
int i = 0; i < vec.rows(); ++i) res(i, i) = vec(i);
152 template <
typename N>
157 typedef typename quad_var_t::matrix_x_t matrix_x_t;
158 typedef typename quad_var_t::point_t point_t;
159 typedef typename lin_var_t::vector_x_t point_dim_t;
160 point_dim_t ones = point_dim_t::Ones(w1.
c().size());
161 point_t b1 = w1.
B().transpose() * ones, b2 = w2.
B().transpose() * ones;
162 matrix_x_t B1 = to_diagonal<N>(b1);
163 matrix_x_t B2 = to_diagonal<N>(b2);
165 matrix_x_t A = B1.transpose() * B2;
166 point_t b = w1.
c().transpose() * w2.
B() + w2.
c().transpose() * w1.
B();
167 N c = w1.
c().transpose() * w2.
c();
168 return quad_var_t(A, b, c);
171 template <
typename N>
178 template <
typename N>
185 template <
typename N>
192 template <
typename N>
199 template <
typename N>
interface for a Curve of arbitrary dimension.
storage for variable points of the form p_i = B_i x + c_i
double Numeric
Definition: effector_spline.h:26
Definition: bernstein.h:20
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
Eigen::Matrix< N, Eigen::Dynamic, Eigen::Dynamic > to_diagonal(const Eigen::Ref< const Eigen::Matrix< N, Eigen::Dynamic, 1 > > vec)
Transforms a vector into a diagonal matrix.
Definition: quadratic_variable.h:142
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
const vector_x_t & c() const
Definition: linear_variable.h:224
const matrix_x_t & B() const
Definition: linear_variable.h:223
Definition: quadratic_variable.h:25
quadratic_variable & operator-=(const quadratic_variable &w1)
Definition: quadratic_variable.h:76
const matrix_x_t & A() const
Definition: quadratic_variable.h:110
Numeric operator()(const Eigen::Ref< const point_t > &val) const
Definition: quadratic_variable.h:55
quadratic_variable & operator/=(const double d)
Definition: quadratic_variable.h:91
bool isZero() const
Definition: quadratic_variable.h:128
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > point_t
Definition: quadratic_variable.h:27
quadratic_variable()
Definition: quadratic_variable.h:30
std::size_t size() const
Definition: quadratic_variable.h:129
quadratic_variable< Numeric > quadratic_variable_t
Definition: quadratic_variable.h:28
quadratic_variable & operator+=(const quadratic_variable &w1)
Definition: quadratic_variable.h:62
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: quadratic_variable.h:26
const point_t & b() const
Definition: quadratic_variable.h:116
quadratic_variable(const matrix_x_t &A, const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:37
const Numeric c() const
Definition: quadratic_variable.h:122
quadratic_variable(const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:44
quadratic_variable & operator*=(const double d)
Definition: quadratic_variable.h:100
static quadratic_variable_t Zero(size_t=0)
Definition: quadratic_variable.h:50