Compare revisions

Guilhem Saurel · Guilhem Saurel · Guilhem Saurel · Guilhem Saurel · Guilhem Saurel · Guilhem Saurel
--- a/.git-blame-ignore-revs
+++ b/.git-blame-ignore-revs
+# format (Guilhem Saurel, 2022-04-05)
+6e4505ec181290ffad5bf832b51dec30f04abf52
+# [Python] allow Python 3 & fix format (Guilhem Saurel, 2019-09-25)
+c9d5314ca82524d5aab0dffd75c40564262495b1
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
+ci:
+    autoupdate_branch: 'devel'
+repos:
+-   repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v13.0.1
+    hooks:
+    -   id: clang-format
+        args: [-i, --style=Google]
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.2.0
+    hooks:
+    -   id: check-added-large-files
+    -   id: check-ast
+    -   id: check-executables-have-shebangs
+    -   id: check-json
+    -   id: check-merge-conflict
+    -   id: check-symlinks
+    -   id: check-toml
+    -   id: check-yaml
+    -   id: debug-statements
+    -   id: destroyed-symlinks
+    -   id: detect-private-key
+    -   id: end-of-file-fixer
+    -   id: fix-byte-order-marker
+    -   id: mixed-line-ending
+    -   id: trailing-whitespace
+-   repo: https://github.com/psf/black
+    rev: 22.3.0
+    hooks:
+    -   id: black
+-   repo: https://github.com/PyCQA/flake8
+    rev: 4.0.1
+    hooks:
+    -   id: flake8
--- a/README.md
+++ b/README.md
 #  bezier_COM_Traj
-[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/commits/master)
+[![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/commits/master)
-[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-bezier-com-traj/master/coverage/)
+[![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp-bezier-com-traj/master/coverage/)
+[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
+[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/humanoid-path-planner/hpp-bezier-com-traj/master.svg)](https://results.pre-commit.ci/latest/github/humanoid-path-planner/hpp-bezier-com-traj)
 Copyright 2018-2020 LAAS-CNRS

--- a/cmake @ e77c9c32
+++ b/cmake @ e77c9c32
-Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456
+Subproject commit e77c9c32b1d69b21e447cf64f1ad1590aab61159
--- a/include/hpp/bezier-com-traj/common_solve_methods.hh
+++ b/include/hpp/bezier-com-traj/common_solve_methods.hh
@@ -6,29 +6,30 @@
 #ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
 #define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
-#include <hpp/bezier-com-traj/local_config.hh>
+#include <Eigen/Dense>
 #include <hpp/bezier-com-traj/data.hh>
-#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <hpp/bezier-com-traj/local_config.hh>
 #include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
+#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
-#include <Eigen/Dense>
 namespace bezier_com_traj {
 /**
- * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
+ * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier
- * computes a discretization of the curve
+ * curve, computes a discretization of the curve
 * @param wps original waypoints
 * @param bernstein berstein polynoms for
 * @param numSteps desired number of wayoints
 * @return a vector of waypoint representing the discretization of the curve
 */
 BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
-    const std::vector<waypoint6_t>& wps, const std::vector<ndcurves::Bern<double> >& bernstein, int numSteps);
+    const std::vector<waypoint6_t>& wps,
+    const std::vector<ndcurves::Bern<double> >& bernstein, int numSteps);
 /**
- * @brief compute6dControlPointInequalities Given linear and angular control waypoints,
+ * @brief compute6dControlPointInequalities Given linear and angular control
- * compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
+ * waypoints, compute the inequality matrices A and b, A x <= b that constrain
+ * the desired control point x.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
@@ -36,14 +37,16 @@ BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
 * @param fail set to true if problem is found infeasible
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
+BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX>
-    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
+compute6dControlPointInequalities(const ContactData& cData,
-    const bool useAngMomentum, bool& fail);
+                                  const std::vector<waypoint6_t>& wps,
+                                  const std::vector<waypoint6_t>& wpL,
+                                  const bool useAngMomentum, bool& fail);
 /**
- * @brief compute6dControlPointEqualities Given linear and angular control waypoints,
+ * @brief compute6dControlPointEqualities Given linear and angular control
- * compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact
+ * waypoints, compute the equality matrices D and d, D [x; Beta]' = d that
- * forces Beta.
+ * constrain the desired control point x and contact forces Beta.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
@@ -51,9 +54,11 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequal
 * @param fail set to true if problem is found infeasible
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEqualities(
+BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX>
-    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
+compute6dControlPointEqualities(const ContactData& cData,
-    const bool useAngMomentum, bool& fail);
+                                const std::vector<waypoint6_t>& wps,
+                                const std::vector<waypoint6_t>& wpL,
+                                const bool useAngMomentum, bool& fail);
 /**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog
@@ -62,16 +67,20 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEqualit
 * @param H Cost matrix
 * @param g cost Vector
 * @param x initGuess initial guess
- * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements
- * a size equal to x. Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP;
+ * of x are unbounded in that direction, or a size equal to x. Unbounded
- * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
+ * elements should be lesser or equal to solvers::UNBOUNDED_UP;
- * a size equal to x Unbounded elements should be higher or lower than solvers::UNBOUNDED_DOWN;
+ * @param maxBounds upper bounds on x values. Can be of size 0 if all elements
- * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
+ * of x are unbounded in that direction, or a size equal to x Unbounded elements
+ * should be higher or lower than solvers::UNBOUNDED_DOWN;
+ * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not
+ * considered as an lp is solved
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g,
+BEZIER_COM_TRAJ_DLLAPI ResultData
-                                        Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
+solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g,
-                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+      Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
+      const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
 /**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog
 * @param A Inequality matrix
@@ -82,45 +91,55 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_ma
 * @param g cost Vector
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d,
+BEZIER_COM_TRAJ_DLLAPI ResultData
-                                        Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
+solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d,
-                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+      Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
+      const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
 /**
- * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3
+ * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of
+ * fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Hg Cost matrix and vector
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Hg,
+BEZIER_COM_TRAJ_DLLAPI ResultData
-                                        const VectorX& init,
+solve(const std::pair<MatrixXX, VectorX>& Ab,
-                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+      const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
+      const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
 /**
- * @brief solve x' h x + 2 g' x, subject to A*x <= b  and D*x = c using quadprog, with x of fixed dimension 3
+ * @brief solve x' h x + 2 g' x, subject to A*x <= b  and D*x = c using
+ * quadprog, with x of fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Dd Equality matrix and vector
 * @param Hg Cost matrix and vector
- * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements
- * a size equal to x. Unbounded elements should be equal to -std::numeric_limits<double>::infinity();
+ * of x are unbounded in that direction, or a size equal to x. Unbounded
- * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
+ * elements should be equal to -std::numeric_limits<double>::infinity();
- * a size equal to x Unbounded elements should be equal to  std::numeric_limits<double>::infinity();
+ * @param maxBounds upper bounds on x values. Can be of size 0 if all elements
- * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
+ * of x are unbounded in that direction, or a size equal to x Unbounded elements
+ * should be equal to  std::numeric_limits<double>::infinity();
+ * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not
+ * considered as an lp is solved
 * @return
 */
-BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Dd,
+BEZIER_COM_TRAJ_DLLAPI ResultData
-                                        const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
+solve(const std::pair<MatrixXX, VectorX>& Ab,
-                                        Cref_vectorX maxBounds, const VectorX& init,
+      const std::pair<MatrixXX, VectorX>& Dd,
-                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+      const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
+      Cref_vectorX maxBounds, const VectorX& init,
+      const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
 template <typename Point>
-BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedWaypoints(const ProblemData& pData,
+BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> >
-                                                                                          double T,
+computeDiscretizedWaypoints(const ProblemData& pData, double T,
-                                                                                          const T_time& timeArray);
+                            const T_time& timeArray);
 template <typename Point>
-BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
+BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> >
-    const ProblemData& pData, double T, const T_time& timeArray);
+computeDiscretizedAccelerationWaypoints(const ProblemData& pData, double T,
+                                        const T_time& timeArray);
 }  // end namespace bezier_com_traj

--- a/include/hpp/bezier-com-traj/common_solve_methods.inl
+++ b/include/hpp/bezier-com-traj/common_solve_methods.inl
-namespace bezier_com_traj
+namespace bezier_com_traj {
-{
 template <typename Point>
-std::vector< std::pair<double,Point> > computeDiscretizedWaypoints
+std::vector<std::pair<double, Point> > computeDiscretizedWaypoints(
-    (const ProblemData& pData, double T,const T_time& timeArray)
+    const ProblemData& pData, double T, const T_time& timeArray) {
-{
+  typedef std::pair<double, Point> coefs_t;
-    typedef std::pair<double,Point> coefs_t;
+  std::vector<coefs_t> wps;
-    std::vector<coefs_t> wps;
+  std::vector<Point> pi = computeConstantWaypoints(pData, T);
-    std::vector<Point> pi = computeConstantWaypoints(pData,T);
+  // evaluate curve work with normalized time !
-    // evaluate curve work with normalized time !
+  for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) {
-    for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
+    double t = std::min(cit->first / T, 1.);
-    {
+    wps.push_back(evaluateCurveAtTime(pData, pi, t));
-        double t = std::min(cit->first / T, 1.);
+  }
-        wps.push_back(evaluateCurveAtTime(pData,pi,t));
+  return wps;
-    }
-    return wps;
 }
 template <typename Point>
-std::vector< std::pair<double,Point> > computeDiscretizedAccelerationWaypoints
+std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
-    (const ProblemData& pData, double T,const T_time& timeArray)
+    const ProblemData& pData, double T, const T_time& timeArray) {
-{
+  typedef std::pair<double, Point> coefs_t;
-    typedef std::pair<double,Point> coefs_t;
+  std::vector<coefs_t> wps;
-    std::vector<coefs_t> wps;
+  std::vector<Point> pi = computeConstantWaypoints(pData, T);
-    std::vector<Point> pi = computeConstantWaypoints(pData,T);
+  // evaluate curve work with normalized time !
-    // evaluate curve work with normalized time !
+  for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) {
-    for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
+    double t = std::min(cit->first / T, 1.);
-    {
+    wps.push_back(evaluateAccelerationCurveAtTime(pData, pi, T, t));
-        double t = std::min(cit->first / T, 1.);
+  }
-        wps.push_back(evaluateAccelerationCurveAtTime(pData,pi,T,t));
+  return wps;
-    }
-    return wps;
 }
-} // end namespace bezier_com_traj
+}  // end namespace bezier_com_traj
--- a/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
+++ b/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
@@ -7,11 +7,13 @@
 #define BEZIER_COM_COST_WP_DEF_H
 #include <hpp/bezier-com-traj/data.hh>
 #include "boost/assign.hpp"
 namespace bezier_com_traj {
 /**
- * This file contains definitions for the different cost functions used in qp minimization
+ * This file contains definitions for the different cost functions used in qp
+ * minimization
 */
 namespace cost {
@@ -23,7 +25,8 @@ namespace cost {
 * @param H hessian cost matrix to be filled
 * @param g vector matrix
 */
-void genCostFunction(const ProblemData& pData, const VectorX& Ts, const double T, const T_time& timeArray, MatrixXX& H,
+void genCostFunction(const ProblemData& pData, const VectorX& Ts,
+                     const double T, const T_time& timeArray, MatrixXX& H,
                     VectorX& g);
 }  // namespace cost

--- a/include/hpp/bezier-com-traj/data.hh
+++ b/include/hpp/bezier-com-traj/data.hh
@@ -6,15 +6,15 @@
 #ifndef BEZIER_COM_TRAJ_LIB_DATA_H
 #define BEZIER_COM_TRAJ_LIB_DATA_H
-#include <hpp/bezier-com-traj/local_config.hh>
+#include <ndcurves/bezier_curve.h>
-#include <hpp/bezier-com-traj/flags.hh>
+#include <Eigen/Dense>
 #include <hpp/bezier-com-traj/definitions.hh>
-#include <hpp/bezier-com-traj/utils.hh>
+#include <hpp/bezier-com-traj/flags.hh>
+#include <hpp/bezier-com-traj/local_config.hh>
 #include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
-#include <ndcurves/bezier_curve.h>
+#include <hpp/bezier-com-traj/utils.hh>
 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
-#include <Eigen/Dense>
 #include <vector>
 namespace bezier_com_traj {
@@ -33,7 +33,8 @@ struct BEZIER_COM_TRAJ_DLLAPI ContactData {
        ang_(VectorX::Zero(0)) {}
  ContactData(const ContactData& other)
-      : contactPhase_(new centroidal_dynamics::Equilibrium(*(other.contactPhase_))),
+      : contactPhase_(
+            new centroidal_dynamics::Equilibrium(*(other.contactPhase_))),
        Kin_(other.Kin_),
        kin_(other.kin_),
        Ang_(other.Ang_),
@@ -56,8 +57,8 @@ struct BEZIER_COM_TRAJ_DLLAPI ContactData {
 /**
 * @brief Used to define the constraints on the trajectory generation problem.
- * Flags are used to constrain initial and terminal com positions an derivatives.
+ * Flags are used to constrain initial and terminal com positions an
- * Additionally, the maximum acceleration can be bounded.
+ * derivatives. Additionally, the maximum acceleration can be bounded.
 */
 struct BEZIER_COM_TRAJ_DLLAPI Constraints {
  Constraints()
@@ -67,7 +68,10 @@ struct BEZIER_COM_TRAJ_DLLAPI Constraints {
        reduce_h_(1e-3) {}
  Constraints(ConstraintFlag flag)
-      : flag_(flag), constraintAcceleration_(false), maxAcceleration_(10.), reduce_h_(1e-3) {}
+      : flag_(flag),
+        constraintAcceleration_(false),
+        maxAcceleration_(10.),
+        reduce_h_(1e-3) {}
  ~Constraints() {}

--- a/include/hpp/bezier-com-traj/definitions.hh
+++ b/include/hpp/bezier-com-traj/definitions.hh
@@ -6,9 +6,10 @@
 #ifndef BEZIER_COM_TRAJ_DEFINITIONS_H
 #define BEZIER_COM_TRAJ_DEFINITIONS_H
-#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
 #include <ndcurves/bezier_curve.h>
 #include <Eigen/Dense>
+#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
 namespace bezier_com_traj {

--- a/include/hpp/bezier-com-traj/flags.hh
+++ b/include/hpp/bezier-com-traj/flags.hh
@@ -39,18 +39,23 @@ enum BEZIER_COM_TRAJ_DLLAPI GIWCRepresentation {
  UNKNOWN_REPRESENTATION = 0x00004
 };
-inline ConstraintFlag operator~(ConstraintFlag a) { return static_cast<ConstraintFlag>(~static_cast<const int>(a)); }
+inline ConstraintFlag operator~(ConstraintFlag a) {
+  return static_cast<ConstraintFlag>(~static_cast<const int>(a));
+}
 inline ConstraintFlag operator|(ConstraintFlag a, ConstraintFlag b) {
-  return static_cast<ConstraintFlag>(static_cast<const int>(a) | static_cast<const int>(b));
+  return static_cast<ConstraintFlag>(static_cast<const int>(a) |
+                                     static_cast<const int>(b));
 }
 inline ConstraintFlag operator&(ConstraintFlag a, ConstraintFlag b) {
-  return static_cast<ConstraintFlag>(static_cast<const int>(a) & static_cast<const int>(b));
+  return static_cast<ConstraintFlag>(static_cast<const int>(a) &
+                                     static_cast<const int>(b));
 }
 inline ConstraintFlag operator^(ConstraintFlag a, ConstraintFlag b) {
-  return static_cast<ConstraintFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));
+  return static_cast<ConstraintFlag>(static_cast<const int>(a) ^
+                                     static_cast<const int>(b));
 }
 inline ConstraintFlag& operator|=(ConstraintFlag& a, ConstraintFlag b) {

--- a/include/hpp/bezier-com-traj/solve.hh
+++ b/include/hpp/bezier-com-traj/solve.hh
@@ -6,52 +6,62 @@
 #ifndef BEZIER_COM_TRAJ_LIB_SOLVE_H
 #define BEZIER_COM_TRAJ_LIB_SOLVE_H
-#include <hpp/bezier-com-traj/local_config.hh>
-#include <hpp/bezier-com-traj/data.hh>
 #include <Eigen/Dense>
+#include <hpp/bezier-com-traj/data.hh>
+#include <hpp/bezier-com-traj/local_config.hh>
 namespace bezier_com_traj {
 /**
- * @brief solve0step Tries to solve the 0-step capturability problem. Given the current contact phase,
+ * @brief solve0step Tries to solve the 0-step capturability problem. Given the
- * a COM position, and an initial velocity, tries to compute a feasible COM trajectory that
+ * current contact phase, a COM position, and an initial velocity, tries to
- * stops the character without falling.
+ * compute a feasible COM trajectory that stops the character without falling.
 * In this specific implementation, the considered constraints are:
- * init position and velocity, 0 velocity constraints (acceleration constraints are ignored)
+ * init position and velocity, 0 velocity constraints (acceleration constraints
+ * are ignored)
 * @param pData problem Data. Should contain only one contact phase.
 * @param Ts timelength of each contact phase. Should only contain one value
 * @param timeStep time that the solver has to stop.
- * @return ResultData a struct containing the resulting trajectory, if success is true.
+ * @return ResultData a struct containing the resulting trajectory, if success
+ * is true.
 */
-BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts,
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj
-                                                    const double timeStep = -1);
+solve0step(const ProblemData& pData, const std::vector<double>& Ts,
+           const double timeStep = -1);
 /**
- * @brief computeCOMTraj Tries to solve the one step problem :  Given two or three contact phases,
+ * @brief computeCOMTraj Tries to solve the one step problem :  Given two or
- * an initial and final com position and velocity,
+ * three contact phases, an initial and final com position and velocity, try to
- * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * compute the CoM trajectory (as a Bezier curve) that connect them
 * @param pData problem Data.
- * @param Ts timelength of each contact phase. Should be the same legnth as pData.contacts
+ * @param Ts timelength of each contact phase. Should be the same legnth as
+ * pData.contacts
 * @param timeStep time step used by the discretization
- * @return ResultData a struct containing the resulting trajectory, if success is true.
+ * @return ResultData a struct containing the resulting trajectory, if success
+ * is true.
 */
-BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const VectorX& Ts,
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj
-                                                                 const unsigned int pointsPerPhase = 3);
+computeCOMTrajFixedSize(const ProblemData& pData, const VectorX& Ts,
+                        const unsigned int pointsPerPhase = 3);
 /**
- * @brief computeCOMTraj Tries to solve the one step problem :  Given two or three contact phases,
+ * @brief computeCOMTraj Tries to solve the one step problem :  Given two or
- * an initial and final com position and velocity,
+ * three contact phases, an initial and final com position and velocity, try to
- * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * compute the CoM trajectory (as a Bezier curve) that connect them
 * @param pData problem Data.
- * @param Ts timelength of each contact phase. Should be the same length as pData.contacts
+ * @param Ts timelength of each contact phase. Should be the same length as
- * @param timeStep time step used by the discretization, if -1 : use the continuous fomulation
+ * pData.contacts
- * @param solver solver used to perform optimization. WARNING: if the continuous force formulation is
+ * @param timeStep time step used by the discretization, if -1 : use the
- * used, it is highly recommended to use the SOLVER_GLPK solver if available and a quadratic
+ * continuous fomulation
- * cost is not necessary, as these solvers are increasely more computationnaly efficient for the problem
+ * @param solver solver used to perform optimization. WARNING: if the continuous
- * @return ResultData a struct containing the resulting trajectory, if success is true.
+ * force formulation is used, it is highly recommended to use the SOLVER_GLPK
+ * solver if available and a quadratic cost is not necessary, as these solvers
+ * are increasely more computationnaly efficient for the problem
+ * @return ResultData a struct containing the resulting trajectory, if success
+ * is true.
 */
-BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(
-                                                        const double timeStep = -1,
+    const ProblemData& pData, const VectorX& Ts, const double timeStep = -1,
-                                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+    const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
 }  // end namespace bezier_com_traj

--- a/include/hpp/bezier-com-traj/solve_end_effector.hh
+++ b/include/hpp/bezier-com-traj/solve_end_effector.hh
--- a/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
+++ b/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
@@ -50,7 +50,8 @@
 #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
 #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
 #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
-#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
+#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
+  "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
 #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
 #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
 #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
@@ -135,17 +136,22 @@ class EiquadprogFast {
   * s.t. CE x + ce0 = 0
   *      CI x + ci0 >= 0
   */
-  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
+  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
-                                       const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
+                                       const MatrixXd& CE, const VectorXd& ce0,
+                                       const MatrixXd& CI, const VectorXd& ci0,
+                                       VectorXd& x);
  /**
   * solves the sparse problem
   * min. x' Hess x + 2 g0' x
   * s.t. CE x + ce0 = 0
   *      CI x + ci0 >= 0
   */
-  EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess, const VectorXd& g0, const MatrixXd& CE,
+  EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess,
-                                              const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
+                                              const VectorXd& g0,
-                                              VectorXd& x);
+                                              const MatrixXd& CE,
+                                              const VectorXd& ce0,
+                                              const MatrixXd& CI,
+                                              const VectorXd& ci0, VectorXd& x);
  MatrixXd m_J;  // J * J' = Hessian <nVars,nVars>::d
  bool is_inverse_provided_;
@@ -161,7 +167,8 @@ class EiquadprogFast {
  Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d
  // Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d
-  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
+  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
+  /// matrix of active constraints
  MatrixXd R;  // <nVars,nVars>::d
  /// CI*x+ci0
@@ -195,8 +202,8 @@ class EiquadprogFast {
  /// initialized as [1, ..., 1, .]
  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
  /// if adding ineq constraint i fails => iaexcl(i)=0
-  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
+  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
-  /// iaexcl(i)=1 otherwise
+  /// constraints iaexcl(i)=1 otherwise
  VectorXi iaexcl;  //<nIneqCon>::i
  VectorXd x_old;  // old value of x <nVars>::d
@@ -207,7 +214,8 @@ class EiquadprogFast {
  VectorXd T1;  /// tmp variable used in add_constraint
 #endif
-  /// size of the active set A (containing the indices of the active constraints)
+  /// size of the active set A (containing the indices of the active
+  /// constraints)
  int q;
  /// number of active-set iterations
@@ -221,7 +229,8 @@ class EiquadprogFast {
 #endif
  }
-  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq) {
+  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
+                       int iq) {
 #ifdef OPTIMIZE_UPDATE_Z
    z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
 #else
@@ -229,14 +238,18 @@ class EiquadprogFast {
 #endif
  }
-  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq) {
+  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
+                       int iq) {
    r.head(iq) = d.head(iq);
-    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
+    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
+        r.head(iq));
  }
-  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);
+  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq,
+                             double& R_norm);
-  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int nEqCon, int& iq, int l);
+  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
+                                VectorXd& u, int nEqCon, int& iq, int l);
 };
 } /* namespace solvers */

--- a/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
+++ b/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
@@ -18,17 +18,18 @@
 #ifndef GLPKWRAPPER_HH_
 #define GLPKWRAPPER_HH_
-#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
 #include <Eigen/Dense>
+#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
 namespace solvers {
 // min g'x
 // st  CIx <= ci0
 //     CEx  = ce0
-int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
+int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
-              solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds, VectorXd& x, double& cost);
+              const MatrixXd& CI, const VectorXd& ci0,
+              solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
+              VectorXd& x, double& cost);
 } /* namespace solvers */

--- a/include/hpp/bezier-com-traj/solver/solver-abstract.hpp
+++ b/include/hpp/bezier-com-traj/solver/solver-abstract.hpp
@@ -18,8 +18,8 @@
 #ifndef SOLVERABSTRACT_HH_
 #define SOLVERABSTRACT_HH_
-#include <hpp/bezier-com-traj/local_config.hh>
 #include <Eigen/Dense>
+#include <hpp/bezier-com-traj/local_config.hh>
 namespace solvers {
@@ -52,9 +52,11 @@ enum BEZIER_COM_TRAJ_DLLAPI SolverType {
 struct BEZIER_COM_TRAJ_DLLAPI ResultData {
  ResultData() : success_(false), cost_(-1.), x(VectorXd::Zero(0)) {}
-  ResultData(const bool success, const double cost, Cref_vectorX x) : success_(success), cost_(cost), x(x) {}
+  ResultData(const bool success, const double cost, Cref_vectorX x)
+      : success_(success), cost_(cost), x(x) {}
-  ResultData(const ResultData& other) : success_(other.success_), cost_(other.cost_), x(other.x) {}
+  ResultData(const ResultData& other)
+      : success_(other.success_), cost_(other.cost_), x(other.x) {}
  ~ResultData() {}
  ResultData& operator=(const ResultData& other) {
@@ -73,15 +75,18 @@ struct BEZIER_COM_TRAJ_DLLAPI ResultData {
 //     CEx  = ce0
 /**
 * @brief solve Solve a QP or LP given
- * init position and velocity, 0 velocity constraints (acceleration constraints are ignored)
+ * init position and velocity, 0 velocity constraints (acceleration constraints
+ * are ignored)
 * @param pData problem Data. Should contain only one contact phase.
 * @param Ts timelength of each contact phase. Should only contain one value
 * @param timeStep time that the solver has to stop.
- * @return ResultData a struct containing the resulting trajectory, if success is true.
+ * @return ResultData a struct containing the resulting trajectory, if success
+ * is true.
 */
-ResultData BEZIER_COM_TRAJ_DLLAPI solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d,
+ResultData BEZIER_COM_TRAJ_DLLAPI solve(
-                                        const MatrixXd& Hess, const VectorXd& g, const VectorXd& initGuess,
+    const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d,
-                                        Cref_vectorX minBounds, Cref_vectorX maxBounds, const SolverType solver);
+    const MatrixXd& Hess, const VectorXd& g, const VectorXd& initGuess,
+    Cref_vectorX minBounds, Cref_vectorX maxBounds, const SolverType solver);
 } /* namespace solvers */

--- a/include/hpp/bezier-com-traj/utils.hh
+++ b/include/hpp/bezier-com-traj/utils.hh
@@ -6,12 +6,10 @@
 #ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
 #define BEZIER_COM_TRAJ_LIB_UTILS_H
-#include <hpp/bezier-com-traj/local_config.hh>
+#include <Eigen/Dense>
 #include <hpp/bezier-com-traj/definitions.hh>
 #include <hpp/bezier-com-traj/flags.hh>
+#include <hpp/bezier-com-traj/local_config.hh>
-#include <Eigen/Dense>
 #include <vector>
 namespace bezier_com_traj {
@@ -37,8 +35,10 @@ struct waypoint_t {
  size_t size() const { return second.size(); }
  bool isApprox(const waypoint_t& other,
-                const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const {
+                const value_type prec =
-    return first.isApprox(other.first, prec) && second.isApprox(other.second, prec);
+                    Eigen::NumTraits<value_type>::dummy_precision()) const {
+    return first.isApprox(other.first, prec) &&
+           second.isApprox(other.second, prec);
  }
  bool operator==(const waypoint_t& other) const { return isApprox(other); }
@@ -51,7 +51,8 @@ struct waypoint_t {
 * @param degree required degree
 * @return the bernstein polynoms
 */
-BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
+BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> >
+ComputeBersteinPolynoms(const unsigned int degree);
 /**
 * @brief given the constraints of the problem, and a set of waypoints, return
@@ -62,18 +63,21 @@ BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> > ComputeBersteinPolyn
 * @return the bezier curve
 */
 template <typename Bezier, typename Point>
-BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag, const double T,
+BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag,
-                                                 const std::vector<Point>& pi, const Point& x);
+                                                 const double T,
+                                                 const std::vector<Point>& pi,
+                                                 const Point& x);
 /**
 * @brief computeDiscretizedTime build an array of discretized points in time,
- * such that there is the same number of point in each phase. Doesn't contain t=0,
+ * such that there is the same number of point in each phase. Doesn't contain
- * is of size pointsPerPhase*phaseTimings.size()
+ * t=0, is of size pointsPerPhase*phaseTimings.size()
 * @param phaseTimings
 * @param pointsPerPhase
 * @return
 */
-T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase);
+T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
+                                   const unsigned int pointsPerPhase);
 /**
 * @brief computeDiscretizedTime build an array of discretized points in time,
@@ -82,15 +86,16 @@ T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned i
 * @param phaseTimings
 * @param timeStep
 * @return */
-T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep);
+T_time computeDiscretizedTime(const VectorX& phaseTimings,
+                              const double timeStep);
 /**
 * @brief write a polytope describe by A x <= b linear constraints in
 * a given filename
 * @return the bernstein polynoms
 */
-void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint, const std::string& fileName,
+void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
-                    bool clipZ = false);
+                    const std::string& fileName, bool clipZ = false);
 /**
 * @brief skew symmetric matrix
@@ -105,7 +110,9 @@ int Normalize(Ref_matrixXX A, Ref_vectorX b);
 }  // end namespace bezier_com_traj
 template <typename Bezier, typename Point>
-Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const double T, const std::vector<Point>& pi,
+Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag,
+                                           const double T,
+                                           const std::vector<Point>& pi,
                                           const Point& x) {
  std::vector<Point> wps;
  size_t i = 0;
@@ -128,12 +135,16 @@ Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const dou
    i++;
  } else {
    if (flag & END_ACC) {
-      assert(flag & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
+      assert(flag & END_VEL &&
+             "You cannot constrain final acceleration if final velocity is not "
+             "constrained.");
      wps.push_back(pi[i]);
      i++;
    }
    if (flag & END_VEL) {
-      assert(flag & END_POS && "You cannot constrain final velocity if final position is not constrained.");
+      assert(flag & END_POS &&
+             "You cannot constrain final velocity if final position is not "
+             "constrained.");
      wps.push_back(pi[i]);
      i++;
    }

--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -13,12 +13,14 @@ namespace c0_dc0_c1 {
 static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_POS;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND final position (DEGREE = 3)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND final position
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
+/// (DEGREE = 3)
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * at t, defined by the waypoint pi and one free waypoint (x)
 * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
 * @param t param (normalized !)
- * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ * @return the expression of the waypoint such that wp.first . x + wp.second =
+ * point on curve
 */
 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  coefs_t wp;
@@ -26,23 +28,29 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  double t3 = t2 * t;
  // equation found with sympy
  wp.first = -3.0 * t3 + 3.0 * t2;
-  wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t + 1.0 * pi[0] + 3.0 * pi[1] * t3 -
+  wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t +
-              6.0 * pi[1] * t2 + 3.0 * pi[1] * t + 1.0 * pi[3] * t3;
+              1.0 * pi[0] + 3.0 * pi[1] * t3 - 6.0 * pi[1] * t2 +
+              3.0 * pi[1] * t + 1.0 * pi[3] * t3;
  return wp;
 }
-inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
+                                               double T, double t) {
  coefs_t wp;
  double alpha = 1. / (T * T);
  // equation found with sympy
  wp.first = (-18.0 * t + 6.0) * alpha;
-  wp.second = (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1] + 6.0 * pi[3] * t) * alpha;
+  wp.second = (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t -
+               12.0 * pi[1] + 6.0 * pi[3] * t) *
+              alpha;
  return wp;
 }
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
-  // equation for constraint on initial and final position and velocity (degree 4, 4 constant waypoint and one free
+                                                     double T) {
-  // (p2)) first, compute the constant waypoints that only depend on pData :
+  // equation for constraint on initial and final position and velocity (degree
+  // 4, 4 constant waypoint and one free (p2)) first, compute the constant
+  // waypoints that only depend on pData :
  int n = 3;
  std::vector<point_t> pi;
  pi.push_back(pData.c0_);                         // p0
@@ -53,7 +61,8 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, d
  return pi;
 }
-inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
+                                                double T) {
  bezier_wp_t::t_point_t wps;
  const int DIM_POINT = 6;
  const int DIM_VAR = 3;
@@ -71,24 +80,28 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
  w0.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
  w0.first.block<3, 3>(3, 0) = 6.0 * Cpi[0] * alpha;
  w0.second.head<3>() = (6 * pi[0] - 12 * pi[1]) * alpha;
-  w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1]) * alpha;
+  w0.second.tail<3>() =
+      1.0 * (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1]) * alpha;
  wps.push_back(w0);
  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
  w1.first.block<3, 3>(3, 0) = 1.0 * (-6.0 * Cpi[0] + 6.0 * Cpi[1]) * alpha;
  w1.second.head<3>() = 1.0 * (4.0 * pi[0] - 6.0 * pi[1] + 2.0 * pi[3]) * alpha;
-  w1.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[1] + 2.0 * Cpi[0] * pi[3]) * alpha;
+  w1.second.tail<3>() =
+      1.0 * (1.0 * Cg * T2 * pi[1] + 2.0 * Cpi[0] * pi[3]) * alpha;
  wps.push_back(w1);
  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
  w2.first.block<3, 3>(0, 0) = -6.0 * alpha * Matrix3::Identity();
  w2.first.block<3, 3>(3, 0) = 1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1]) * alpha;
  w2.second.head<3>() = 1.0 * (2.0 * pi[0] + 4.0 * pi[3]) * alpha;
-  w2.second.tail<3>() = 1.0 * (-2.0 * Cpi[0] * pi[3] + 6.0 * Cpi[1] * pi[3]) * alpha;
+  w2.second.tail<3>() =
+      1.0 * (-2.0 * Cpi[0] * pi[3] + 6.0 * Cpi[1] * pi[3]) * alpha;
  wps.push_back(w2);
  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
  w3.first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity();
  w3.first.block<3, 3>(3, 0) = -12.0 * Cpi[3] * alpha;
  w3.second.head<3>() = (6 * pi[1] + 6 * pi[3]) * alpha;
-  w3.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[3] - 6.0 * Cpi[1] * pi[3]) * alpha;
+  w3.second.tail<3>() =
+      1.0 * (1.0 * Cg * T2 * pi[3] - 6.0 * Cpi[1] * pi[3]) * alpha;
  wps.push_back(w3);
  return wps;
 }

--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
@@ -13,12 +13,14 @@ namespace c0_dc0_dc1 {
 static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_VEL;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE = 4)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
+/// = 4)
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * at t, defined by the waypoint pi and one free waypoint (x)
 * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
 * @param t param (normalized !)
- * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ * @return the expression of the waypoint such that wp.first . x + wp.second =
+ * point on curve
 */
 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  coefs_t wp;
@@ -26,24 +28,32 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  double t3 = t2 * t;
  // equation found with sympy
  wp.first = -2.0 * t3 + 3.0 * t2;
-  wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t + 1.0 * pi[0] + 3.0 * pi[1] * t3 -
+  wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t +
-              6.0 * pi[1] * t2 + 3.0 * pi[1] * t;
+              1.0 * pi[0] + 3.0 * pi[1] * t3 - 6.0 * pi[1] * t2 +
+              3.0 * pi[1] * t;
  return wp;
 }
-inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
+                                               double T, double t) {
  coefs_t wp;
  double alpha = 1. / (T * T);
  // equation found with sympy
  wp.first = (-12.0 * t + 6.0) * alpha;
-  wp.second = (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1]) * alpha;
+  wp.second =
+      (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1]) *
+      alpha;
  return wp;
 }
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
-  // equation for constraint on initial and final position and velocity (degree 3, 2 constant waypoints and two free
+                                                     double T) {
-  // (p2 = p3)) first, compute the constant waypoints that only depend on pData :
+  // equation for constraint on initial and final position and velocity (degree
-  if (pData.dc1_.norm() != 0.) throw std::runtime_error("Capturability not implemented for spped different than 0");
+  // 3, 2 constant waypoints and two free (p2 = p3)) first, compute the constant
+  // waypoints that only depend on pData :
+  if (pData.dc1_.norm() != 0.)
+    throw std::runtime_error(
+        "Capturability not implemented for spped different than 0");
  std::vector<point_t> pi;
  pi.push_back(pData.c0_);                          // p0
  pi.push_back((pData.dc0_ * T / 3.) + pData.c0_);  // p1
@@ -52,7 +62,8 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, d
  return pi;
 }
-inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
+                                                double T) {
  bezier_wp_t::t_point_t wps;
  const int DIM_POINT = 6;
  const int DIM_VAR = 3;
@@ -78,11 +89,13 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
  w1.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
  w1.first.block<3, 3>(3, 0) = skew(1.5 * (3 * pi[1] - pi[0])) * alpha;
  w1.second.head<3>() = 1.5 * alpha * (3 * pi[0] - 5 * pi[1]);
-  w1.second.tail<3>() = (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0]));
+  w1.second.tail<3>() =
+      (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0]));
  wps.push_back(w1);
  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
  w2.first.block<3, 3>(0, 0) = 0 * alpha * Matrix3::Identity();
-  w2.first.block<3, 3>(3, 0) = skew(0.5 * g - 3 * alpha * pi[0] + 3 * alpha * pi[1]);
+  w2.first.block<3, 3>(3, 0) =
+      skew(0.5 * g - 3 * alpha * pi[0] + 3 * alpha * pi[1]);
  w2.second.head<3>() = 3 * alpha * (pi[0] - pi[1]);
  w2.second.tail<3>() = 0.5 * Cg * pi[1];
  wps.push_back(w2);

--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -13,12 +13,14 @@ namespace c0_dc0_dc1_c1 {
 static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_POS | END_VEL;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE = 4)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
+/// = 4)
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * at t, defined by the waypoint pi and one free waypoint (x)
 * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
 * @param t param (normalized !)
- * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ * @return the expression of the waypoint such that wp.first . x + wp.second =
+ * point on curve
 */
 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  coefs_t wp;
@@ -27,26 +29,31 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  double t4 = t3 * t;
  // equation found with sympy
  wp.first = (6.0 * t4 - 12.0 * t3 + 6.0 * t2);
-  wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 - 4.0 * pi[0] * t + 1.0 * pi[0] -
+  wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 -
-              4.0 * pi[1] * t4 + 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t - 4.0 * pi[3] * t4 +
+              4.0 * pi[0] * t + 1.0 * pi[0] - 4.0 * pi[1] * t4 +
-              4.0 * pi[3] * t3 + 1.0 * pi[4] * t4;
+              12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t -
+              4.0 * pi[3] * t4 + 4.0 * pi[3] * t3 + 1.0 * pi[4] * t4;
  return wp;
 }
-inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
+                                               double T, double t) {
  coefs_t wp;
  double alpha = 1. / (T * T);
  // equation found with sympy
  wp.first = (72.0 * t * t - 72.0 * t + 12.0) * alpha;
-  wp.second = (12.0 * pi[0] * t * t - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t * t + 72.0 * pi[1] * t -
+  wp.second = (12.0 * pi[0] * t * t - 24.0 * pi[0] * t + 12.0 * pi[0] -
-               24.0 * pi[1] - 48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
+               48.0 * pi[1] * t * t + 72.0 * pi[1] * t - 24.0 * pi[1] -
+               48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
              alpha;
  return wp;
 }
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
-  // equation for constraint on initial and final position and velocity (degree 4, 4 constant waypoint and one free
+                                                     double T) {
-  // (p2)) first, compute the constant waypoints that only depend on pData :
+  // equation for constraint on initial and final position and velocity (degree
+  // 4, 4 constant waypoint and one free (p2)) first, compute the constant
+  // waypoints that only depend on pData :
  int n = 4;
  std::vector<point_t> pi;
  pi.push_back(pData.c0_);                          // p0
@@ -57,7 +64,8 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, d
  return pi;
 }
-inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
+                                                double T) {
  bezier_wp_t::t_point_t wps;
  const int DIM_POINT = 6;
  const int DIM_VAR = 3;
@@ -81,27 +89,33 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
  w1.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
  w1.first.block<3, 3>(3, 0) = (-12.0 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
  w1.second.head<3>() = (7.2 * pi[0] - 9.6 * pi[1] + 4.8 * pi[3]) * alpha;
-  w1.second.tail<3>() = (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] + 4.8 * Cpi[0] * pi[3]) * alpha;
+  w1.second.tail<3>() =
+      (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] + 4.8 * Cpi[0] * pi[3]) *
+      alpha;
  wps.push_back(w1);
  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
  w2.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
  w2.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[1]) * alpha;
  w2.second.head<3>() = (3.6 * pi[0] + 4.8 * pi[3] + 1.2 * pi[4]) * alpha;
-  w2.second.tail<3>() =
+  w2.second.tail<3>() = (0.4 * Cg * T2 * pi[1] - 4.8 * Cpi[0] * pi[3] +
-      (0.4 * Cg * T2 * pi[1] - 4.8 * Cpi[0] * pi[3] + 1.2 * Cpi[0] * pi[4] + 9.6 * Cpi[1] * pi[3]) * alpha;
+                         1.2 * Cpi[0] * pi[4] + 9.6 * Cpi[1] * pi[3]) *
+                        alpha;
  wps.push_back(w2);
  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
  w3.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
  w3.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[3]) * alpha;
  w3.second.head<3>() = (1.2 * pi[0] + 4.8 * pi[1] + 3.6 * pi[4]) * alpha;
-  w3.second.tail<3>() =
+  w3.second.tail<3>() = (0.4 * Cg * T2 * pi[3] - 1.2 * Cpi[0] * pi[4] -
-      (0.4 * Cg * T2 * pi[3] - 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[3] + 4.8 * Cpi[1] * pi[4]) * alpha;
+                         9.6 * Cpi[1] * pi[3] + 4.8 * Cpi[1] * pi[4]) *
+                        alpha;
  wps.push_back(w3);
  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
  w4.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
  w4.first.block<3, 3>(3, 0) = (9.6 * Cpi[3] - 12.0 * Cpi[4]) * alpha;
  w4.second.head<3>() = (4.8 * pi[1] - 9.6 * pi[3] + 7.2 * pi[4]) * alpha;
-  w4.second.tail<3>() = (0.8 * Cg * T2 * pi[3] + 0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4]) * alpha;
+  w4.second.tail<3>() =
+      (0.8 * Cg * T2 * pi[3] + 0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4]) *
+      alpha;
  wps.push_back(w4);
  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
  w5.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();

--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh
@@ -13,51 +13,61 @@ namespace c0_dc0_ddc0 {
 static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC;
-/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration, and only final position (degree = 4)
+/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration,
+/// and only final position (degree = 4)
 /**
- * @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
+ * @brief evaluateCurveAtTime compute the expression of the point on the curve
- * free waypoint (x)
+ * at t, defined by the waypoint pi and one free waypoint (x)
 * @param pi constant waypoints of the curve, assume pi[0] pi[1] x pi[2] p3
 * @param t param (normalized !)
- * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ * @return the expression of the waypoint such that wp.first . x + wp.second =
+ * point on curve
 */
 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  coefs_t wp;
  double t2 = t * t;
  double t3 = t2 * t;
  // equation found with sympy
-  // (-1.0*pi[0] + 3.0*pi[1] - 3.0*pi[2] + 1.0*x)*t**3 + (3.0*pi[0] - 6.0*pi[1] + 3.0*pi[2])*T2 +
+  // (-1.0*pi[0] + 3.0*pi[1] - 3.0*pi[2] + 1.0*x)*t**3 + (3.0*pi[0] - 6.0*pi[1]
+  // + 3.0*pi[2])*T2 +
  // (-3.0*pi[0] + 3.0*pi[1])*t + 1.0*pi[0],
  wp.first = t3;
-  wp.second =
+  wp.second = t3 * (3 * (pi[1] - pi[2]) - pi[0]) +
-      t3 * (3 * (pi[1] - pi[2]) - pi[0]) + t2 * (3 * (pi[0] + pi[2]) - 6 * pi[1]) + 3 * t * (pi[1] - pi[0]) + pi[0];
+              t2 * (3 * (pi[0] + pi[2]) - 6 * pi[1]) + 3 * t * (pi[1] - pi[0]) +
+              pi[0];
  return wp;
 }
-inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
+                                               double T, double t) {
  coefs_t wp;
  double alpha = 1. / (T * T);
  // equation found with sympy
-  // 6.0*t*alpha*x + (-6.0*pi[0] + 18.0*pi[1] - 18.0*pi[2])/T2*t + (6.0*pi[0] - 12.0*pi[1] + 6.0*pi[2])/T2
+  // 6.0*t*alpha*x + (-6.0*pi[0] + 18.0*pi[1] - 18.0*pi[2])/T2*t + (6.0*pi[0]
+  // - 12.0*pi[1] + 6.0*pi[2])/T2
  wp.first = 6.0 * t * alpha;
-  wp.second = (18. * (pi[1] - pi[2]) - 6. * pi[0]) * alpha * t + (6. * (pi[0] + pi[2]) - 12.0 * pi[1]) * alpha;
+  wp.second = (18. * (pi[1] - pi[2]) - 6. * pi[0]) * alpha * t +
+              (6. * (pi[0] + pi[2]) - 12.0 * pi[1]) * alpha;
  return wp;
 }
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
-  // equation for constraint on initial position, velocity and acceleration, and only final position (degree =
+                                                     double T) {
-  // 4)(degree 4, 4 constant waypoint and one free (p3)) first, compute the constant waypoints that only depend on
+  // equation for constraint on initial position, velocity and acceleration, and
-  // pData :
+  // only final position (degree = 4)(degree 4, 4 constant waypoint and one free
+  // (p3)) first, compute the constant waypoints that only depend on pData :
  double n = 3.;
  std::vector<point_t> pi;
-  pi.push_back(pData.c0_);                                                                      // pi[0]
+  pi.push_back(pData.c0_);                         // pi[0]
-  pi.push_back((pData.dc0_ * T / n) + pData.c0_);                                               // pi[1]
+  pi.push_back((pData.dc0_ * T / n) + pData.c0_);  // pi[1]
-  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_);  // pi[2]
+  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
-  pi.push_back(point_t::Zero());                                                                // x
+               (2. * pData.dc0_ * T / n) + pData.c0_);  // pi[2]
+  pi.push_back(point_t::Zero());                        // x
  return pi;
 }
-inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
+                                                double T) {
  bezier_wp_t::t_point_t wps;
  const int DIM_POINT = 6;
  const int DIM_VAR = 3;
@@ -74,23 +84,31 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
  // equation of waypoints for curve w found with sympy
  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
  w0.second.head<3>() = (6 * pi[0] - 12 * pi[1] + 6 * pi[2]) * alpha;
-  w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1] + 6.0 * Cpi[0] * pi[2]) * alpha;
+  w0.second.tail<3>() =
+      1.0 *
+      (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1] + 6.0 * Cpi[0] * pi[2]) *
+      alpha;
  wps.push_back(w0);
  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
  w1.first.block<3, 3>(0, 0) = 2.0 * alpha * Id;
  w1.first.block<3, 3>(3, 0) = 2.0 * Cpi[0] * alpha;
  w1.second.head<3>() = 1.0 * (4.0 * pi[0] - 6.0 * pi[1]) * alpha;
-  w1.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[1] - 6.0 * Cpi[0] * pi[2] + 6.0 * Cpi[1] * pi[2]) * alpha;
+  w1.second.tail<3>() =
+      1.0 *
+      (1.0 * Cg * T2 * pi[1] - 6.0 * Cpi[0] * pi[2] + 6.0 * Cpi[1] * pi[2]) *
+      alpha;
  wps.push_back(w1);
  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
  w2.first.block<3, 3>(0, 0) = 4.0 * alpha * Id;
  w2.first.block<3, 3>(3, 0) = 1.0 * (-2.0 * Cpi[0] + 6.0 * Cpi[1]) * alpha;
  w2.second.head<3>() = 1.0 * (2.0 * pi[0] - 6.0 * pi[2]) * alpha;
-  w2.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[2] - 6.0 * Cpi[1] * pi[2]) * alpha;
+  w2.second.tail<3>() =
+      1.0 * (1.0 * Cg * T2 * pi[2] - 6.0 * Cpi[1] * pi[2]) * alpha;
  wps.push_back(w2);
  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
  w3.first.block<3, 3>(0, 0) = 6 * alpha * Id;
-  w3.first.block<3, 3>(3, 0) = 1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1] + 12.0 * Cpi[2]) * alpha;
+  w3.first.block<3, 3>(3, 0) =
+      1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1] + 12.0 * Cpi[2]) * alpha;
  w3.second.head<3>() = (6 * pi[1] - 12 * pi[2]) * alpha;
  // w3.second.head<3>() = 0;
  wps.push_back(w3);
No results found