Commit 991a37b3 by Fernbach Pierre Committed by GitHub

Merge pull request #52 from stonneau/devel

`arithmetic operations on curves`
parents 48f7bd66 98647d94
 ... ... @@ -35,5 +35,20 @@ void PseudoInverse(_Matrix_Type_& pinvmat) { } pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose()); } template Matrix3 skew(const Point& x) { Matrix3 res = Matrix3::Zero(3,3); res(0, 1) = -x(2); res(0, 2) = x(1); res(1, 0) = x(2); res(1, 2) = -x(0); res(2, 0) = -x(1); res(2, 1) = x(0); return res; } static const double MARGIN(0.001); } // namespace curves #endif //_SPLINEMATH
 ... ... @@ -10,6 +10,7 @@ #define _CLASS_BEZIERCURVE #include "curve_abc.h" #include "cross_implementation.h" #include "bernstein.h" #include "curve_constraint.h" #include "piecewise_curve.h" ... ... @@ -215,7 +216,7 @@ struct bezier_curve : public curve_abc { if (order == 0) { return *this; } num_t new_degree = (num_t)(degree_ + 1); num_t new_degree_inv = 1. / ((num_t)(degree_ + 1)); t_point_t n_wp; point_t current_sum = point_t::Zero(dim_); // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant. ... ... @@ -223,12 +224,42 @@ struct bezier_curve : public curve_abc { n_wp.push_back(current_sum); for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) { current_sum += *pit; n_wp.push_back(current_sum / new_degree); n_wp.push_back(current_sum * new_degree_inv); } bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_)); return integ.compute_primitive(order - 1); } /// \brief Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent. /// Order elevation is required for addition / substraction and other comparison operations. /// \param order : number of order the curve must be updated /// \return An equivalent Bezier, with one more degree. bezier_curve_t elevate(const std::size_t order) const { t_point_t new_waypoints = control_points_, temp_waypoints; for (std::size_t i = 1; i<= order; ++i) { num_t new_degree_inv = 1. / ((num_t)(degree_ + i)); temp_waypoints.push_back(*new_waypoints.begin()); num_t idx_deg_inv = 0.; for (typename t_point_t::const_iterator pit = new_waypoints.begin()+1; pit != new_waypoints.end(); ++pit) { idx_deg_inv += new_degree_inv; temp_waypoints.push_back(idx_deg_inv * (*(pit-1)) + (1 - idx_deg_inv) * (*pit)); } temp_waypoints.push_back(*(new_waypoints.end()-1)); new_waypoints = temp_waypoints; temp_waypoints.clear(); } return bezier_curve_t (new_waypoints.begin(), new_waypoints.end(), T_min_, T_max_, mult_T_); } /// \brief Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent. /// Order elevation is required for addition / substraction and other comparison operations. /// \param order : number of order the curve must be updated void elevate_self(const std::size_t order) { if (order > 0) (*this) = elevate(order); } /// \brief Evaluate the derivative order N of curve at time t. /// If derivative is to be evaluated several times, it is /// rather recommended to compute derived curve using compute_derivate. ... ... @@ -415,6 +446,118 @@ struct bezier_curve : public curve_abc { return c_split.second.split(t2).first; } /// \brief Compute the cross product of the current bezier curve by another bezier curve. /// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. /// This method of course only makes sense for dimension 3 curves. /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined /// \param pOther other polynomial to compute the cross product with. /// \return a new polynomial defining the cross product between this and pother bezier_curve_t cross(const bezier_curve_t& g) const { //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html assert_operator_compatible(g); if (dim()!= 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 "); int m =(int)(degree()); int n =(int)(g.degree()); unsigned int mj, n_ij, mn_i; t_point_t new_waypoints; for(int i = 0; i<= m+n; ++i) { bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim()); for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){ mj = bin(m,j); n_ij = bin(n,i-j); mn_i = bin(m+n,i); num_t mul = num_t(mj*n_ij) / num_t(mn_i); current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j)); } new_waypoints.push_back(current_point); } return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_); } /// \brief Compute the cross product of the current bezier b by a point point. /// The cross product pXpoint of is defined such that /// forall t, bXpoint(t) = b(t) X point, with X designing the cross product. /// This method of course only makes sense for dimension 3 polynomials. /// \param point point to compute the cross product with. /// \return a new polynomial defining the cross product between this and point bezier_curve_t cross(const bezier_curve_t::point_t& point) const { //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html if (dim()!= 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 "); t_point_t new_waypoints; for(typename t_point_t::const_iterator cit = waypoints().begin(); cit != waypoints().end(); ++cit){ new_waypoints.push_back(curves::cross(*cit, point)); } return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_); } bezier_curve_t& operator+=(const bezier_curve_t& other) { assert_operator_compatible(other); bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier if(other.degree() > degree()){ elevate_self(other.degree() - degree()); } else if(other_elevated.degree() < degree()){ other_elevated.elevate_self(degree() - other_elevated.degree()); } typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin(); for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){ (*it)+=(*otherit); } return *this; } bezier_curve_t& operator-=(const bezier_curve_t& other) { assert_operator_compatible(other); bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); if(other.degree() > degree()){ elevate_self(other.degree() - degree()); } else if(other_elevated.degree() < degree()){ other_elevated.elevate_self(degree() - other_elevated.degree()); } typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin(); for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){ (*it)-=(*otherit); } return *this; } bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)+=point; } return *this; } bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)-=point; } return *this; } bezier_curve_t& operator/=(const double d) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)/=d; } return *this; } bezier_curve_t& operator*=(const double d) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)*=d; } return *this; } private: /// \brief Ensure constraints of bezier curve. /// Add 4 points (2 after the first one, 2 before the last one) to biezer curve ... ... @@ -453,6 +596,14 @@ struct bezier_curve : public curve_abc { "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?"); } } void assert_operator_compatible(const bezier_curve_t& other) const{ if ((fabs(min() - other.min()) > MARGIN) || (fabs(max() - other.max()) > MARGIN)){ throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges"); } } /*Operations*/ public: ... ... @@ -483,7 +634,6 @@ struct bezier_curve : public curve_abc { /*const*/ std::size_t degree_; /*const*/ std::vector > bernstein_; /*const*/ t_point_t control_points_; static const double MARGIN; /* Attributes */ static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) { ... ... @@ -512,8 +662,71 @@ struct bezier_curve : public curve_abc { } }; // End struct bezier_curve template const double bezier_curve::MARGIN(0.001); template bezier_curve operator+(const bezier_curve& p1, const bezier_curve& p2) { bezier_curve res(p1); return res+=p2; } template bezier_curve operator-(const bezier_curve& p1) { std::vector::point_t> ts; for (std::size_t i = 0; i <= p1.degree(); ++i){ ts.push_back(bezier_curve::point_t::Zero(p1.dim())); } bezier_curve res (ts.begin(),ts.end(),p1.min(),p1.max()); res-=p1; return res; } template bezier_curve operator-(const bezier_curve& p1, const bezier_curve& p2) { bezier_curve res(p1); return res-=p2; } template bezier_curve operator-(const bezier_curve& p1, const typename bezier_curve::point_t& point) { bezier_curve res(p1); return res-=point; } template bezier_curve operator-(const typename bezier_curve::point_t& point, const bezier_curve& p1) { bezier_curve res(-p1); return res+=point; } template bezier_curve operator+(const bezier_curve& p1, const typename bezier_curve::point_t& point) { bezier_curve res(p1); return res+=point; } template bezier_curve operator+(const typename bezier_curve::point_t& point, const bezier_curve& p1) { bezier_curve res(p1); return res+=point; } template bezier_curve operator/(const bezier_curve& p1, const double k) { bezier_curve res(p1); return res/=k; } template bezier_curve operator*(const bezier_curve& p1,const double k) { bezier_curve res(p1); return res*=k; } template bezier_curve operator*(const double k, const bezier_curve& p1) { bezier_curve res(p1); return res*=k; } } // namespace curves ... ...
 /** * \file cross_implementation.h * \brief class allowing to create a cubic hermite spline of any dimension. * \author Steve Tonneau * \date 09/2020 */ #ifndef _CLASS_CROSSIMP #define _CLASS_CROSSIMP #include "curves/fwd.h" namespace curves { inline Eigen::Vector3d cross(const Eigen::VectorXd& a, const Eigen::VectorXd& b){ Eigen::Vector3d c; c << a[1]*b[2] - a[2]*b[1], a[2]*b[0] - a[0]*b[2], a[0]*b[1] - a[1]*b[0] ; return c; } inline Eigen::Vector3d cross(const Eigen::Vector3d& a, const Eigen::Vector3d& b){ return a.cross(b); } inline Eigen::Vector3f cross(const Eigen::Vector3f& a, const Eigen::Vector3f& b){ return a.cross(b); } template linear_variable cross(const linear_variable& a, const linear_variable& b){ return a.cross(b); } } // namespace curves #endif //_CLASS_CROSSIMP
 ... ... @@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc { public: /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const { return dim_; }; std::size_t virtual dim() const { return dim_; } /// \brief Get the minimum time for which the curve is defined /// \return \f\$t_{min}\f\$, lower bound of time range. Time virtual min() const { return time_control_points_.front(); } ... ...
 ... ... @@ -87,6 +87,8 @@ typedef boost::shared_ptr curve_SE3_ptr_t; typedef polynomial polynomial_t; typedef exact_cubic exact_cubic_t; typedef bezier_curve bezier_t; typedef linear_variable linear_variable_t; typedef bezier_curve bezier_linear_variable_t; typedef constant_curve constant_t; typedef cubic_hermite_spline cubic_hermite_spline_t; typedef piecewise_curve piecewise_t; ... ...
 ... ... @@ -26,11 +26,14 @@ template struct linear_variable : public serialization::Serializable { typedef Eigen::Matrix vector_x_t; typedef Eigen::Matrix matrix_x_t; typedef Eigen::Matrix vector_3_t; typedef Eigen::Matrix matrix_3_t; typedef linear_variable linear_variable_t; linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed linear_variable(const linear_variable_t& other) : B_(other.B()), c_(other.c()), zero(other.isZero()) {} // copy constructor /// \brief Linear evaluation for vector x. /// \param val : vector to evaluate the linear variable. ... ... @@ -95,14 +98,45 @@ struct linear_variable : public serialization::Serializable { return *this; } /// \brief Compute the cross product of the current linear_variable and the other. /// This method of course only makes sense for dimension 3 curves and dimension 3 unknown, /// since otherwise the result is non-linear. /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined /// \param pOther other polynomial to compute the cross product with. /// \return a new polynomial defining the cross product between this and other linear_variable_t cross(const linear_variable_t& other) const { if (B().rows() !=3) throw std::invalid_argument("Can't perform cross product on linear variables with dimensions != 3 "); if (B().cols() !=3) throw std::invalid_argument("Can't perform cross product on linear variables more than one unknown "); if (isZero() || other.isZero()) return linear_variable_t::Zero(3); if ((B().squaredNorm() - B().diagonal().squaredNorm() > MARGIN ) || (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > MARGIN ) ) throw std::invalid_argument("Can't perform cross product on linear variables if B is not diagonal "); // (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2 typename linear_variable_t::matrix_3_t newB = skew(-other.c()) * B() + skew(c()) * other.B(); typename linear_variable_t::vector_3_t newC = curves::cross(c(),other.c()); return linear_variable_t(newB,newC); } /// \brief Get a linear variable equal to zero. /// \param dim : Dimension of linear variable. /// \return Linear variable equal to zero. /// static linear_variable_t Zero(size_t dim = 0) { return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim)); } /// \brief Get a linear variable equal to the variable /// \param dim : Dimension of linear variable. /// \return Linear variable equal to the variable. /// static linear_variable_t X(size_t dim = 0) { return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim)); } /// \brief Get dimension of linear variable. /// \return Dimension of linear variable. /// ... ... @@ -156,6 +190,11 @@ linear_variable operator-(const linear_variable& w1, const linear_va return res -= w2; } template linear_variable operator-(const linear_variable& w1) { return linear_variable (-w1.B(), -w1.c()); } template linear_variable operator*(const double k, const linear_variable& w) { linear_variable res(w.B(), w.c()); ... ... @@ -182,6 +221,11 @@ BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) { return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_); } template std::ostream &operator<<(std::ostream &os, const linear_variable& l) { return os << "linear_variable: \n \t B:\n"<< l.B() << "\t c: \n" << l.c().transpose(); } } // namespace curves DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Numeric, bool Safe), ... ...
 ... ... @@ -130,7 +130,7 @@ problem_data setup_control_points(const problem_definition } const std::size_t first_variable_idx = i; // variables for (; i + 4 < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_)); for (; i + 4 < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_)); // end constraints if (flag & END_POS) { if (flag & END_VEL) { ... ... @@ -147,7 +147,7 @@ problem_data setup_control_points(const problem_definition ++i; } else while (i < numControlPoints - 3) { variables_.push_back(var_t::Zero(pDef.dim_)); variables_.push_back(var_t::X(pDef.dim_)); ++i; } variables_.push_back(var_t(acc)); ... ... @@ -155,7 +155,7 @@ problem_data setup_control_points(const problem_definition ++i; } else while (i < numControlPoints - 2) { variables_.push_back(var_t::Zero(pDef.dim_)); variables_.push_back(var_t::X(pDef.dim_)); ++i; } variables_.push_back(var_t(vel)); ... ... @@ -163,7 +163,7 @@ problem_data setup_control_points(const problem_definition ++i; } else { while (i < numControlPoints - 1) { variables_.push_back(var_t::Zero(pDef.dim_)); variables_.push_back(var_t::X(pDef.dim_)); ++i; } } ... ... @@ -172,7 +172,7 @@ problem_data setup_control_points(const problem_definition ++i; } // add remaining variables (only if no end_pos constraints) for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_)); for (; i < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_)); if (numControlPoints <= numConstants) { throw std::runtime_error("numControlPoints < numConstants"); ... ...
 ... ... @@ -582,7 +582,6 @@ struct piecewise_curve : public curve_abc const double piecewise_curve::MARGIN(0.001); } // namespace curves DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, ... ...
 ... ... @@ -400,6 +400,107 @@ struct polynomial : public curve_abc { virtual std::size_t degree() const { return degree_; } /*Helpers*/ polynomial_t& operator+=(const polynomial_t& p1) { assert_operator_compatible(p1); if (p1.degree() > degree()) { polynomial_t::coeff_t res = p1.coeff(); res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_; coefficients_ = res; degree_ = p1.degree(); } else{ coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) += p1.coeff(); } return *this; } polynomial_t& operator-=(const polynomial_t& p1) { assert_operator_compatible(p1); if (p1.degree() > degree()) { polynomial_t::coeff_t res = -p1.coeff(); res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_; coefficients_ = res; degree_ = p1.degree(); } else{ coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) -= p1.coeff(); } return *this; } polynomial_t& operator+=(const polynomial_t::point_t& point) { coefficients_.col(0) += point; return *this; } polynomial_t& operator-=(const polynomial_t::point_t& point) { coefficients_.col(0) -= point; return *this; } polynomial_t& operator/=(const double d) { coefficients_ /= d; return *this; } polynomial_t& operator*=(const double d) { coefficients_ *= d; return *this; } /// \brief Compute the cross product of the current polynomial by another polynomial. /// The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. /// This method of course only makes sense for dimension 3 polynomials. /// \param pOther other polynomial to compute the cross product with. /// \return a new polynomial defining the cross product between this and pother polynomial_t cross(const polynomial_t& pOther) const { assert_operator_compatible(pOther); if (dim()!= 3) throw std::invalid_argument("Can't perform cross product on polynomials with dimensions != 3 "); std::size_t new_degree =degree() + pOther.degree(); coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+1); Eigen::Vector3d currentVec; Eigen::Vector3d currentVecCrossed; for(long i = 0; i< coefficients_.cols(); ++i){ currentVec = coefficients_.col(i); for(long j = 0; j< pOther.coeff().cols(); ++j){ currentVecCrossed = pOther.coeff().col(j); nCoeffs.col(i+j) += currentVec.cross(currentVecCrossed); } } // remove last degrees is they are equal to 0 long final_degree = new_degree; while(nCoeffs.col(final_degree).norm() <= curves::MARGIN && final_degree >0){ --final_degree; } return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max()); } /// \brief Compute the cross product of the current polynomial p by a point point. /// The cross product pXpoint of is defined such that