Commit e7494ff1 by Pierre Fernbach

### Merge remote-tracking branch 'jchemin/Topic/EditDoc' into devel

parents 00c41527 b30b8293
 ... ... @@ -21,7 +21,7 @@ #include #include namespace curves { // REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels /// \brief An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference : Boulic et Al. 2003) template void PseudoInverse(_Matrix_Type_& pinvmat) { Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); ... ...
 ... ... @@ -31,7 +31,7 @@ inline unsigned int bin(const unsigned int n, const unsigned int k) { } /// \class Bernstein. /// \brief Computes a Bernstein polynome. /// \brief Computes a Bernstein polynomial. /// template struct Bern { ... ... @@ -40,6 +40,9 @@ struct Bern { ~Bern() {} /// \brief Evaluation of Bernstein polynomial at value u. /// \param u : value between 0 and 1. /// \return Evaluation corresponding at value u. Numeric operator()(const Numeric u) const { if (!(u >= 0. && u <= 1.)) { throw std::invalid_argument("u needs to be betwen 0 and 1."); ... ... @@ -47,11 +50,17 @@ struct Bern { return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i); } /// \brief Check if actual Bernstein polynomial and other are approximately equal. /// \param other : the other Bernstein polynomial to check. /// \return true if the two Bernstein polynomials are approximately equals. virtual bool operator==(const Bern& other) const { return curves::isApprox(m_minus_i, other.m_minus_i) && curves::isApprox(i_, other.i_) && curves::isApprox(bin_m_i_, other.bin_m_i_); } /// \brief Check if actual Bernstein polynomial and other are different. /// \param other : the other Bernstein polynomial to check. /// \return true if the two Bernstein polynomials are different. virtual bool operator!=(const Bern& other) const { return !(*this == other); } /* Attributes */ ... ...
 ... ... @@ -54,7 +54,8 @@ struct bezier_curve : public curve_abc { /// Given the first and last point of a control points set, create the bezier curve. /// \param PointsBegin : an iterator pointing to the first element of a control point container. /// \param PointsEnd : an iterator pointing to the last element of a control point container. /// \param T : upper bound of time which is between \f\$[0;T]\f\$ (default \f\$[0;1]\f\$). /// \param T_min : lower bound of time, curve will be defined for time in [T_min, T_max]. /// \param T_max : upper bound of time, curve will be defined for time in [T_min, T_max]. /// \param mult_T : ... (default value is 1.0). /// template ... ... @@ -66,7 +67,8 @@ struct bezier_curve : public curve_abc { mult_T_(mult_T), size_(std::distance(PointsBegin, PointsEnd)), degree_(size_ - 1), bernstein_(curves::makeBernstein((unsigned int)degree_)) { bernstein_(curves::makeBernstein((unsigned int)degree_)) { if (bernstein_.size() != size_) { throw std::invalid_argument("Invalid size of polynomial"); } ... ... @@ -81,12 +83,15 @@ struct bezier_curve : public curve_abc { } } /// \brief Constructor /// \brief Constructor with constraints. /// This constructor will add 4 points (2 after the first one, 2 before the last one) /// to ensure that velocity and acceleration constraints are respected. /// \param PointsBegin : an iterator pointing to the first element of a control point container. /// \param PointsEnd : an iterator pointing to the last element of a control point container. /// \param constraints : constraints applying on start / end velocities and acceleration. /// \param T_min : lower bound of time, curve will be defined for time in [T_min, T_max]. /// \param T_max : upper bound of time, curve will be defined for time in [T_min, T_max]. /// \param mult_T : ... (default value is 1.0). /// template bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min = 0., ... ... @@ -97,7 +102,8 @@ struct bezier_curve : public curve_abc { mult_T_(mult_T), size_(std::distance(PointsBegin, PointsEnd) + 4), degree_(size_ - 1), bernstein_(curves::makeBernstein((unsigned int)degree_)) { bernstein_(curves::makeBernstein((unsigned int)degree_)) { if (Safe && (size_ < 1 || T_max_ <= T_min_)) { throw std::invalid_argument("can't create bezier min bound is higher than max bound"); } ... ... @@ -146,7 +152,7 @@ struct bezier_curve : public curve_abc { * isEquivalent * @param other the other curve to check * @param prec the precision treshold, default Eigen::NumTraits::dummy_precision() * @return true is the two curves are approximately equals * @return true if the two curves are approximately equals */ bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits::dummy_precision()) const { bool equal = curves::isApprox(T_min_, other.min()) && curves::isApprox(T_max_, other.max()) && ... ... @@ -337,7 +343,6 @@ struct bezier_curve : public curve_abc { /// \brief Split the bezier curve in 2 at time t. /// \param t : list of points. /// \param u : unNormalized time. /// \return pair containing the first element of both bezier curve obtained. /// std::pair split(const Numeric t) const { ... ... @@ -411,6 +416,10 @@ struct bezier_curve : public curve_abc { } private: /// \brief Ensure constraints of bezier curve. /// Add 4 points (2 after the first one, 2 before the last one) to biezer curve /// to ensure that velocity and acceleration constraints are respected. /// template t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) { t_point_t res; ... ...
 ... ... @@ -52,8 +52,8 @@ struct cubic_hermite_spline : public curve_abc { cubic_hermite_spline() : dim_(0), T_min_(0), T_max_(0) {} /// \brief Constructor. /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container. /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container. /// \param PairsBegin : an iterator pointing to the first element of a pair(position, derivative) container. /// \param PairsEnd : an iterator pointing to the last element of a pair(position, derivative) container. /// \param time_control_points : vector containing time for each waypoint. /// template ... ... @@ -215,7 +215,6 @@ struct cubic_hermite_spline : public curve_abc { /// std::size_t numIntervals() const { return size() - 1; } private: /// \brief Get index of the interval (subspline) corresponding to time t for the interpolation. /// \param t : time where to look for interval. ... ... @@ -266,6 +265,8 @@ struct cubic_hermite_spline : public curve_abc { return bezier_t(control_points.begin(), control_points.end(), t0, t1); } /// \brief Check if control points list is not empty and dimension of point superior to zero. /// void check_conditions() const { if (control_points_.size() == 0) { throw std::runtime_error( ... ...
 ... ... @@ -72,7 +72,7 @@ struct curve_abc : std::unary_function, public serialization::Seria * @param other the other curve to check * @param order the order up to which the derivatives of the curves are checked for equality * @param prec the precision treshold, default Eigen::NumTraits::dummy_precision() * @return true is the two curves are approximately equals * @return true if the two curves are approximately equal */ bool isEquivalent(const curve_t* other, const Numeric prec = Eigen::NumTraits::dummy_precision(), const size_t order = 5) const { ... ... @@ -90,7 +90,7 @@ struct curve_abc : std::unary_function, public serialization::Seria } t += inc; } // check if the derivatives are equals // check if the derivatives are equal for (size_t n = 1; n <= order; ++n) { t = min(); while (t <= max()) { ... ... @@ -104,12 +104,12 @@ struct curve_abc : std::unary_function, public serialization::Seria } /** * @brief isApprox check if other and *this are approximately equals given a precision treshold * Only two curves of the same class can be approximately equals, * @brief isApprox check if other and *this are approximately equal given a precision treshold * Only two curves of the same class can be approximately equal, * for comparison between different type of curves see isEquivalent. * @param other the other curve to check * @param prec the precision treshold, default Eigen::NumTraits::dummy_precision() * @return true is the two curves are approximately equals * @return true if the two curves are approximately equal */ virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits::dummy_precision()) const = 0; ... ...
 ... ... @@ -119,8 +119,14 @@ class rotation_spline : public curve_abc_quat_t { return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end()); } /// \brief Get dimension of curve. /// \return dimension of curve. virtual std::size_t 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. virtual time_t min() const { return min_; } /// \brief Get the maximum time for which the curve is defined. /// \return \f\$t_{max}\f\$, upper bound of time range. virtual time_t max() const { return max_; } /// \brief Get the degree of the curve. /// \return \f\$degree\f\$, the degree of the curve. ... ...
 ... ... @@ -32,7 +32,10 @@ struct linear_variable : public serialization::Serializable { 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 evaluation /// \brief Linear evaluation for vector x. /// \param val : vector to evaluate the linear variable. /// \return Evaluation of linear variable for vector x. /// vector_x_t operator()(const Eigen::Ref& val) const { if (isZero()) return c(); if (Safe && B().cols() != val.rows()) ... ... @@ -40,6 +43,10 @@ struct linear_variable : public serialization::Serializable { return B() * val + c(); } /// \brief Add another linear variable. /// \param w1 : linear variable to add. /// \return Linear variable after operation. /// linear_variable_t& operator+=(const linear_variable_t& w1) { if (w1.isZero()) return *this; if (isZero()) { ... ... @@ -51,6 +58,11 @@ struct linear_variable : public serialization::Serializable { this->c_ += w1.c_; return *this; } /// \brief Substract another linear variable. /// \param w1 : linear variable to substract. /// \return Linear variable after operation. /// linear_variable_t& operator-=(const linear_variable_t& w1) { if (w1.isZero()) return *this; if (isZero()) { ... ... @@ -62,25 +74,48 @@ struct linear_variable : public serialization::Serializable { this->c_ -= w1.c_; return *this; } /// \brief Divide by a constant : p_i / d = B_i*x/d + c_i/d. /// \param d : constant. /// \return Linear variable after operation. /// linear_variable_t& operator/=(const double d) { B_ /= d; c_ /= d; return *this; } /// \brief Multiply by a constant : p_i / d = B_i*x*d + c_i*d. /// \param d : constant. /// \return Linear variable after operation. /// linear_variable_t& operator*=(const double d) { B_ *= d; c_ *= d; return *this; } /// \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::Identity(dim, dim), vector_x_t::Zero(dim)); } /// \brief Get dimension of linear variable. /// \return Dimension of linear variable. /// std::size_t size() const { return zero ? 0 : std::max(B_.cols(), c_.size()); } /// \brief Get norm of linear variable (Norm of B plus norm of C). /// \return Norm of linear variable. Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); } /// \brief Check if actual linear variable and other are approximately equal given a precision treshold. /// Only two curves of the same class can be approximately equal, /// \param prec : the precision treshold, default Eigen::NumTraits::dummy_precision() /// \return true if the two linear variables are approximately equal. bool isApprox(const linear_variable_t& other, const double prec = Eigen::NumTraits::dummy_precision()) const { return (*this - other).norm() < prec; ... ...
 ... ... @@ -215,10 +215,20 @@ struct piecewise_curve : public curve_abc= num_curves()) { throw std::length_error( ... ... @@ -227,6 +237,9 @@ struct piecewise_curve : public curve_abc piecewise_curve_t convert_piecewise_curve_to_bezier() { check_if_not_empty(); ... ... @@ -242,6 +255,10 @@ struct piecewise_curve : public curve_abc piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() { check_if_not_empty(); ... ... @@ -257,6 +274,9 @@ struct piecewise_curve : public curve_abc piecewise_curve_t convert_piecewise_curve_to_polynomial() { check_if_not_empty(); ... ... @@ -272,6 +292,11 @@ struct piecewise_curve : public curve_abc static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_time_t time_points) { if (Safe & !(points.size() > 1)) { ... ... @@ -295,6 +320,12 @@ struct piecewise_curve : public curve_abc static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, ... ... @@ -326,6 +357,13 @@ struct piecewise_curve : public curve_abc static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!