Commit 13089d03 by JasonChmn Committed by Pierre Fernbach

### [Indentation] Reindentation of all the code

parent c55a3f93
 ... ... @@ -21,26 +21,22 @@ #include #include namespace curves{ //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels template void PseudoInverse(_Matrix_Type_& pinvmat) { //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels template void PseudoInverse(_Matrix_Type_& pinvmat) { Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); _Matrix_Type_ m_sigma = svd.singularValues(); double pinvtoler= 1.e-6; // choose your tolerance widely! _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows()); for (long i=0; i pinvtoler) { m_sigma_inv(i,i)=1.0/m_sigma(i); } if (m_sigma(i) > pinvtoler) { m_sigma_inv(i,i)=1.0/m_sigma(i); } } pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose()); } } } // namespace curves #endif //_SPLINEMATH
 ... ... @@ -22,71 +22,71 @@ namespace curves { /// \brief Computes a binomial coefficient. /// \param n : an unsigned integer. /// \param k : an unsigned integer. /// \return \f$\binom{n}{k}f$ /// inline unsigned int bin(const unsigned int n, const unsigned int k) { if(k > n) throw std::runtime_error("binomial coefficient higher than degree"); if(k == 0) return 1; if(k > n/2) return bin(n,n-k); /// \brief Computes a binomial coefficient . /// \param n : an unsigned integer. /// \param k : an unsigned integer. /// \return \f$\binom{n}{k}f$ /// inline unsigned int bin(const unsigned int n, const unsigned int k) { if(k > n) throw std::runtime_error("binomial coefficient higher than degree"); if(k == 0) return 1; if(k > n/2) return bin(n,n-k); return n * bin(n-1,k-1) / k; } /// \class Bernstein. /// \brief Computes a Bernstein polynome. /// template struct Bern : public serialization::Serializable< Bern > { Bern(){} Bern(const unsigned int m, const unsigned int i) :m_minus_i(m - i) ,i_(i) ,bin_m_i_(bin(m,i)) {} ~Bern(){} Numeric operator()(const Numeric u) const { assert(u >= 0. && u <= 1.); return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i); } Numeric m_minus_i; Numeric i_; Numeric bin_m_i_; // Serialization of the class friend class boost::serialization::access; } /// \class Bernstein. /// \brief Computes a Bernstein polynome. /// template struct Bern : public serialization::Serializable< Bern > { Bern(){} Bern(const unsigned int m, const unsigned int i) :m_minus_i(m - i), i_(i), bin_m_i_(bin(m,i)) {} ~Bern(){} Numeric operator()(const Numeric u) const { assert(u >= 0. && u <= 1.); return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i); } template void serialize(Archive& ar, const unsigned int version){ if (version) { // Do something depending on version ? /* Attributes */ Numeric m_minus_i; Numeric i_; Numeric bin_m_i_; /* Attributes */ // Serialization of the class friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version){ if (version) { // Do something depending on version ? } ar & boost::serialization::make_nvp("m_minus_i", m_minus_i); ar & boost::serialization::make_nvp("i", i_); ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_); } ar & boost::serialization::make_nvp("m_minus_i", m_minus_i); ar & boost::serialization::make_nvp("i", i_); ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_); } }; }; // End struct Bern /// \brief Computes all Bernstein polynomes for a certain degree. /// template std::vector > makeBernstein(const unsigned int n) { /// \brief Computes all Bernstein polynomes for a certain degree. /// template std::vector > makeBernstein(const unsigned int n) { std::vector > res; for(unsigned int i = 0; i<= n; ++i) res.push_back(Bern(n, i)); { res.push_back(Bern(n, i)); } return res; } } } // namespace curves #endif //_CLASS_BERNSTEIN
This diff is collapsed.
 /** * \file bezier_curve.h * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. * \author Steve T. * \version 0.1 * \date 06/17/2013 */ #ifndef _BEZIER_POLY_CONVERSION #define _BEZIER_POLY_CONVERSION #include "curve_abc.h" #include "bernstein.h" #include "curve_constraint.h" #include "MathDefs.h" #include #include #include namespace curves { /// \brief Converts a Bezier curve to a polynomial. /// \param bezier : the Bezier curve to convert. /// \return the equivalent polynomial. template Polynomial from_bezier(const Bezier& curve) { typedef typename Polynomial::t_point_t t_point_t; typedef typename Polynomial::num_t num_t; t_point_t coefficients; Bezier current (curve); coefficients.push_back(curve(0.)); num_t fact = 1; for(std::size_t i = 1; i<= curve.degree_; ++i) { current = current.compute_derivate(1); fact *= (num_t)i; coefficients.push_back(current(0.)/fact); } return Polynomial(coefficients,curve.min(),curve.max()); } /* /// \brief Converts a polynomial to a Bezier curve. /// \param polynomial : the polynomial to convert. /// \return the equivalent Bezier curve. template Bezier from_polynomial(const Polynomial& polynomial) { typedef Bezier::point_t point_t; typedef Bezier::time_t time_t; typedef Bezier::num_t num_t; typedef Bezier::curve_constraints_t curve_constraints_t; typedef Bezier::t_point_t t_point_t; typedef Bezier::cit_point_t cit_point_t; typedef Bezier::bezier_curve_t bezier_curve_t; } */ } // namespace curves #endif //_BEZIER_POLY_CONVERSION
This diff is collapsed.
 ... ... @@ -22,26 +22,27 @@ namespace curves { /// \brief Creates coefficient vector of a cubic spline defined on the interval /// \f$[t_{min}, t_{max}]\f$. It follows the equation :
/// \f$x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$t \in [t_{min}, t_{max}] \f$ /// with a, b, c and d the control points. /// template T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d) { T_Point res; res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d); return res; } template polynomial create_cubic(Point const& a, Point const& b, Point const& c, Point const &d, const Time t_min, const Time t_max) { T_Point coeffs = make_cubic_vector(a,b,c,d); return polynomial(coeffs.begin(),coeffs.end(), t_min, t_max); } /// \brief Creates coefficient vector of a cubic spline defined on the interval /// \f$[t_{min}, t_{max}]\f$. It follows the equation :
/// \f$x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$t \in [t_{min}, t_{max}] \f$ /// with a, b, c and d the control points. /// template T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d) { T_Point res; res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d); return res; } template polynomial create_cubic(Point const& a, Point const& b, Point const& c, Point const &d, const Time t_min, const Time t_max) { T_Point coeffs = make_cubic_vector(a,b,c,d); return polynomial(coeffs.begin(),coeffs.end(), t_min, t_max); } } // namespace curves #endif //_STRUCT_CUBICSPLINE
 ... ... @@ -20,53 +20,49 @@ namespace curves { /// \struct curve_abc. /// \brief Represents a curve of dimension Dim. /// If value of parameter Safe is false, no verification is made on the evaluation of the curve. template > struct curve_abc : std::unary_function { /// \struct curve_abc. /// \brief Represents a curve of dimension Dim. /// If value of parameter Safe is false, no verification is made on the evaluation of the curve. template > struct curve_abc : std::unary_function { typedef Point point_t; typedef Time time_t; /* Constructors - destructors */ /* Constructors - destructors */ public: /// \brief Constructor. curve_abc(){} /// \brief Destructor. virtual ~curve_abc(){} /* Constructors - destructors */ /// \brief Constructor. curve_abc(){} /*Operations*/ public: /// \brief Evaluation of the cubic spline at time t. /// \param t : time when to evaluate the spine /// \return \f$x(t)\f$, point corresponding on curve at time t. virtual point_t operator()(const time_t t) const = 0; /// \brief Destructor. virtual ~curve_abc(){} /* Constructors - destructors */ /*Operations*/ /// \brief Evaluation of the cubic spline at time t. /// \param t : time when to evaluate the spine /// \return \f$x(t)\f$, point corresponding on curve at time t. virtual point_t operator()(const time_t t) const = 0; /// \brief Evaluate the derivative of order N of curve at time t. /// \param t : time when to evaluate the spline. /// \param order : order of derivative. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t. virtual point_t derivate(const time_t t, const std::size_t order) const = 0; /*Operations*/ /*Helpers*/ public: /// \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 = 0; /// \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 = 0; /// \brief Evaluate the derivative of order N of curve at time t. /// \param t : time when to evaluate the spline. /// \param order : order of derivative. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t. virtual point_t derivate(const time_t t, const std::size_t order) const = 0; /*Operations*/ std::pair timeRange() {return std::make_pair(min(), max());} /*Helpers*/ /*Helpers*/ /// \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 = 0; /// \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 = 0; std::pair timeRange() {return std::make_pair(min(), max());} /*Helpers*/ }; }; } // namespace curves #endif //_STRUCT_CURVE_ABC
 ... ... @@ -19,19 +19,19 @@ namespace curves { template struct curve_constraints { typedef Point point_t; curve_constraints(): init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){} template struct curve_constraints { typedef Point point_t; curve_constraints(): init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){} ~curve_constraints(){} ~curve_constraints(){} point_t init_vel; point_t init_acc; point_t end_vel; point_t end_acc; }; point_t init_vel; point_t init_acc; point_t end_vel; point_t end_acc; }; } // namespace curves #endif //_CLASS_CUBICZEROVELACC
 ... ... @@ -14,13 +14,12 @@ namespace curves { /// \brief Converts a cubic hermite spline or a bezier curve to a polynomial. /// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \return the equivalent polynomial. template Polynomial polynomial_from_curve(const curveTypeToConvert& curve) { /// \brief Converts a cubic hermite spline or a bezier curve to a polynomial. /// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \return the equivalent polynomial. template Polynomial polynomial_from_curve(const curveTypeToConvert& curve) { typedef typename Polynomial::t_point_t t_point_t; typedef typename Polynomial::num_t num_t; t_point_t coefficients; ... ... @@ -31,34 +30,30 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve) num_t fact = 1; for(std::size_t i = 1; i<= curve.degree_; ++i) { fact *= (num_t)i; coefficients.push_back(current.derivate(current.min(),i)/fact); fact *= (num_t)i; coefficients.push_back(current.derivate(current.min(),i)/fact); } return Polynomial(coefficients,curve.min(),curve.max()); } /// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve. /// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \return the equivalent cubic bezier curve. template Bezier bezier_from_curve(const curveTypeToConvert& curve) { } /// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve. /// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \return the equivalent cubic bezier curve. template Bezier bezier_from_curve(const curveTypeToConvert& curve) { typedef typename Bezier::point_t point_t; typedef typename Bezier::t_point_t t_point_t; typedef typename Bezier::num_t num_t; curveTypeToConvert current (curve); num_t T_min = current.min(); num_t T_max = current.max(); num_t T = T_max-T_min; // Positions and derivatives point_t p0 = current(T_min); point_t p1 = current(T_max); point_t m0 = current.derivate(T_min,1); point_t m1 = current.derivate(T_max,1); // Convert to bezier control points // for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T // so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T ... ... @@ -67,51 +62,43 @@ Bezier bezier_from_curve(const curveTypeToConvert& curve) point_t b_p3 = p1; point_t b_p1 = T*m0/3+b_p0; point_t b_p2 = -T*m1/3+b_p3; t_point_t control_points; control_points.push_back(b_p0); control_points.push_back(b_p1); control_points.push_back(b_p2); control_points.push_back(b_p3); return Bezier(control_points.begin(), control_points.end(), current.min(), current.max()); } /// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline. /// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert. /// \return the equivalent cubic hermite spline. template Hermite hermite_from_curve(const curveTypeToConvert& curve) { } /// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline. /// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert. /// \return the equivalent cubic hermite spline. template Hermite hermite_from_curve(const curveTypeToConvert& curve) { typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t; typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t; typedef typename Hermite::point_t point_t; typedef typename Hermite::num_t num_t; curveTypeToConvert current (curve); num_t T_min = current.min(); num_t T_max = current.max(); num_t T = T_max-T_min; // Positions and derivatives point_t p0 = current(T_min); point_t p1 = current(T_max); point_t m0 = current.derivate(T_min,1); point_t m1 = current.derivate(T_max,1); // Create pairs pos/vel pair_point_tangent_t pair0(p0,m0); pair_point_tangent_t pair1(p1,m1); t_pair_point_tangent_t control_points; control_points.push_back(pair0); control_points.push_back(pair1); std::vector< double > time_control_points; time_control_points.push_back(T_min); time_control_points.push_back(T_max); return Hermite(control_points.begin(), control_points.end(), time_control_points); } } } // namespace curve #endif //_CLASS_CURVE_CONVERSION \ No newline at end of file
 ... ... @@ -37,16 +37,17 @@ namespace curves { /// \class ExactCubic. /// \brief Represents a set of cubic splines defining a continuous function /// crossing each of the waypoint given in its initialization. /// template, typename T_Point =std::vector > , typename SplineBase=polynomial > struct exact_cubic : public piecewise_curve//, //public serialization::Serializable< exact_cubic > { /// \class ExactCubic. /// \brief Represents a set of cubic splines defining a continuous function /// crossing each of the waypoint given in its initialization. /// template, typename T_Point =std::vector >, typename SplineBase=polynomial > struct exact_cubic : public piecewise_curve//, //public serialization::Serializable< exact_cubic > { typedef Point point_t; typedef T_Point t_point_t; typedef Eigen::Matrix MatrixX; ... ... @@ -65,68 +66,67 @@ struct exact_cubic : public piecewise_curve exact_cubic(In wayPointsBegin, In wayPointsEnd) /// \brief Constructor. /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container. /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container. /// template exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t(computeWayPoints(wayPointsBegin, wayPointsEnd)) {} /// \brief Constructor. /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container. /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container. /// \param constraints : constraints on the init and end velocity / accelerations of the spline. /// template exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) {} /// \brief Constructor. /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container. /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container. /// \param constraints : constraints on the init and end velocity / accelerations of the spline. /// template exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) : piecewise_curve_t(computeWayPoints(wayPointsBegin, wayPointsEnd, constraints)) {} {} /// \brief Constructor.