Format

stack-of-tasks · Oct 9, 2019 · 0a78aff · 0a78aff
1 parent 83b4a88
commit 0a78aff
Show file tree

Hide file tree

Showing 22 changed files with 2,393 additions and 2,892 deletions.
diff --git a/include/parametric-curves/MathDefs.h b/include/parametric-curves/MathDefs.h
@@ -1,16 +1,16 @@
 /**
-* \file Math.h
-* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*
-* This file contains math definitions used
-* used throughout the library.
-* Preprocessors definition are used to use either float 
-* or double values, and 3 dimensional vectors for
-* the Point structure.
-*/
+ * \file Math.h
+ * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
+ * \author Steve T.
+ * \version 0.1
+ * \date 06/17/2013
+ *
+ * This file contains math definitions used
+ * used throughout the library.
+ * Preprocessors definition are used to use either float
+ * or double values, and 3 dimensional vectors for
+ * the Point structure.
+ */
 
 #ifndef _SPLINEMATH
 #define _SPLINEMATH
@@ -19,28 +19,24 @@
 #include <Eigen/SVD>
 
 #include <vector>
-#include <utility> 
-
-namespace parametriccurves{
-
-//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
-template<typename _Matrix_Type_>
-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<m_sigma.rows(); ++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());
-}
+#include <utility>
+
+namespace parametriccurves {
+
+// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
+template <typename _Matrix_Type_>
+void PseudoInverse(_Matrix_Type_& pinvmat) {
+  Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  _Matrix_Type_ m_sigma = svd.singularValues();
 
-} // namespace spline
-#endif //_SPLINEMATH
+  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 < m_sigma.rows(); ++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 parametriccurves
+#endif  //_SPLINEMATH
diff --git a/include/parametric-curves/abstract-curve.hpp b/include/parametric-curves/abstract-curve.hpp
@@ -1,36 +1,35 @@
 /**
-* \file AbstractCurve.hpp
-* \brief interface for a Curve of arbitrary dimension.
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*
-* Interface for a curve
-*/
+ * \file AbstractCurve.hpp
+ * \brief interface for a Curve of arbitrary dimension.
+ * \author Steve T.
+ * \version 0.1
+ * \date 06/17/2013
+ *
+ * Interface for a curve
+ */
 #include <cstddef>
 #include <iostream>
 
 #ifndef _parameteric_curves_abstract_curve_hpp
 #define _parameteric_curves_abstract_curve_hpp
 
-namespace parametriccurves
-{
+namespace parametriccurves {
 /// \struct AbstractCurve
 /// \brief Represents a curve of dimension Dim
 /// is Safe is false, no verification is made on the evaluation of the curve.
-template<typename Numeric, typename Point >
-struct  AbstractCurve
-{
-  typedef Point   point_t;
+template <typename Numeric, typename Point>
+struct AbstractCurve {
+  typedef Point point_t;
   typedef Numeric time_t;
   typedef Numeric num_t;
-public:
+
+ public:
   /* Constructors - destructors */
-  AbstractCurve(time_t t_min_, time_t t_max_):  t_min(t_min_), t_max(t_max_) { }
-  AbstractCurve(){ }
-  virtual ~AbstractCurve(){}  
-public:
-  
+  AbstractCurve(time_t t_min_, time_t t_max_) : t_min(t_min_), t_max(t_max_) {}
+  AbstractCurve() {}
+  virtual ~AbstractCurve() {}
+
+ public:
   ///  \brief Evaluation of the cubic spline at time t.
   ///  \param t : the time when to evaluate the spine
   ///  \param return : the value x(t)
@@ -42,27 +41,25 @@ struct  AbstractCurve
   ///  \param return : the value x(t)
   virtual const point_t derivate(const time_t& t, const std::size_t& order) const = 0;
 
-public:
+ public:
   /*Getters*/
   virtual const time_t tmin() const { return t_min; }
   virtual const time_t tmax() const { return t_max; }
-  virtual bool checkRange(const time_t t) const { return (t>=t_min)&&(t<=t_max); }
+  virtual bool checkRange(const time_t t) const { return (t >= t_min) && (t <= t_max); }
 
   /* Setters */
   virtual bool setInitialPoint(const point_t& /*x_init*/) = 0;
   virtual bool setInitialPoint(const num_t& /*x_init*/) = 0;
 
-  virtual bool setTimePeriod(const time_t& traj_time_)
-  {
+  virtual bool setTimePeriod(const time_t& traj_time_) {
     t_min = 0.0;
-    t_max = traj_time_;  
+    t_max = traj_time_;
     return true;
   }
 
-protected:
+ protected:
   time_t t_min;
   time_t t_max;
-
 };
-}
-#endif //_STRUCT_CURVE_ABC
+}  // namespace parametriccurves
+#endif  //_STRUCT_CURVE_ABC
diff --git a/include/parametric-curves/bernstein.h b/include/parametric-curves/bernstein.h
@@ -1,11 +1,10 @@
 /**
-* \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
-*/
-
+ * \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 _CLASS_BERNSTEIN
 #define _CLASS_BERNSTEIN
@@ -18,63 +17,49 @@
 #include <vector>
 #include <stdexcept>
 
-namespace spline
-{
+namespace spline {
 ///
 /// \brief Computes factorial of a number
 ///
-unsigned int fact(const unsigned int n)
-{
-    assert(n>=0);
-    int res = 1;
-    for (int i=2 ; i <= n ; ++i)
-       res *= i;
-    return res;
+unsigned int fact(const unsigned int n) {
+  assert(n >= 0);
+  int res = 1;
+  for (int i = 2; i <= n; ++i) res *= i;
+  return res;
 }
 
 ///
 /// \brief Computes a binomal coefficient
 ///
-unsigned int bin(const unsigned  int n, const unsigned  int k)
-{
-    return fact(n) / (fact(k) * fact(n - k));
-}
+unsigned int bin(const unsigned int n, const unsigned int k) { return fact(n) / (fact(k) * fact(n - k)); }
 
 /// \class Bernstein
 /// \brief Computes a Bernstein polynome
 ///
 template <typename Numeric = double>
-struct Bern{
-Bern(const unsigned int m, const unsigned int i)
-    :m_minus_i(m - i)
-    ,i_(i)
-    ,bin_m_i_(bin(m,i)) {}
+struct Bern {
+  Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
 
-~Bern(){}
+  ~Bern() {}
 
-Numeric operator()(const Numeric u) const
-{
+  Numeric operator()(const Numeric u) const {
     assert(u >= 0. && u <= 1.);
-    return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
-}
+    return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
+  }
 
-Numeric m_minus_i;
-Numeric i_;
-Numeric bin_m_i_;
+  Numeric m_minus_i;
+  Numeric i_;
+  Numeric bin_m_i_;
 };
 
-
 ///
 /// \brief Computes all Bernstein polynomes for a certain degree
 ///
 template <typename Numeric>
-std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
-{
-    std::vector<Bern<Numeric> > res;
-    for(unsigned int i = 0; i<= n; ++i)
-        res.push_back(Bern<Numeric>(n, i));
-    return res;
+std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) {
+  std::vector<Bern<Numeric> > res;
+  for (unsigned int i = 0; i <= n; ++i) res.push_back(Bern<Numeric>(n, i));
+  return res;
 }
-} // namespace spline
-#endif //_CLASS_BERNSTEIN
-
+}  // namespace spline
+#endif  //_CLASS_BERNSTEIN