From 6e4505ec181290ffad5bf832b51dec30f04abf52 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 5 Apr 2022 11:02:28 +0200
Subject: [PATCH] format
---
.../bezier-com-traj/common_solve_methods.hh | 115 +++---
.../bezier-com-traj/common_solve_methods.inl | 56 ++-
.../cost/costfunction_definition.hh | 7 +-
include/hpp/bezier-com-traj/data.hh | 24 +-
include/hpp/bezier-com-traj/definitions.hh | 3 +-
include/hpp/bezier-com-traj/flags.hh | 13 +-
include/hpp/bezier-com-traj/solve.hh | 66 ++--
.../hpp/bezier-com-traj/solve_end_effector.hh | 249 +++++++-----
.../solver/eiquadprog-fast.hpp | 43 +-
.../bezier-com-traj/solver/glpk-wrapper.hpp | 9 +-
.../solver/solver-abstract.hpp | 21 +-
include/hpp/bezier-com-traj/utils.hh | 47 ++-
.../waypoints/waypoints_c0_dc0_c1.hh | 45 ++-
.../waypoints/waypoints_c0_dc0_dc1.hh | 43 +-
.../waypoints/waypoints_c0_dc0_dc1_c1.hh | 54 ++-
.../waypoints/waypoints_c0_dc0_ddc0.hh | 64 +--
.../waypoints/waypoints_c0_dc0_ddc0_c1.hh | 88 +++--
.../waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh | 149 ++++---
.../waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh | 196 ++++++----
...waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh | 311 +++++++++------
...points_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh | 308 +++++++++------
...points_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh | 367 +++++++++++-------
.../waypoints/waypoints_definition.hh | 92 +++--
python/bezier_com_traj.cpp | 199 +++++++---
python/test/binding_tests.py | 80 ++--
python/test/compare_pin_inv_dyn.py | 323 ++++++++++-----
src/common_solve_methods.cpp | 65 ++--
src/computeCOMTraj.cpp | 326 ++++++++++------
src/costfunction_definition.cpp | 43 +-
src/eiquadprog-fast.cpp | 148 ++++---
src/glpk-wrapper.cpp | 43 +-
src/solve_0_step.cpp | 80 ++--
src/solver-abstract.cpp | 41 +-
src/utils.cpp | 43 +-
src/waypoints_definition.cpp | 358 ++++++++++-------
tests/test-bezier-symbolic.cpp | 41 +-
tests/test-transition-quasi-static.cpp | 29 +-
tests/test-transition.cpp | 274 ++++++++-----
tests/test-zero-step-capturability.cpp | 157 +++++---
tests/test_helper.hh | 73 ++--
40 files changed, 2974 insertions(+), 1719 deletions(-)
diff --git a/include/hpp/bezier-com-traj/common_solve_methods.hh b/include/hpp/bezier-com-traj/common_solve_methods.hh
index 4719bae..38de1df 100644
--- 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 <Eigen/Dense>
+#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
namespace bezier_com_traj {
/**
- * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
- * computes a discretization of the curve
+ * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier
+ * 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,
- * compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
+ * @brief compute6dControlPointInequalities Given linear and angular control
+ * 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(
- const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
- const bool useAngMomentum, bool& fail);
+BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX>
+compute6dControlPointInequalities(const ContactData& cData,
+ 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,
- * compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact
- * forces Beta.
+ * @brief compute6dControlPointEqualities Given linear and angular control
+ * waypoints, compute the equality matrices D and d, D [x; Beta]' = d that
+ * 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(
- const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
- const bool useAngMomentum, bool& fail);
+BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX>
+compute6dControlPointEqualities(const ContactData& cData,
+ 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
- * a size equal to x. Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP;
- * @param maxBounds upper bounds on x values. Can be of size 0 if all elements 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
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements
+ * of x are unbounded in that direction, or a size equal to x. Unbounded
+ * elements should be lesser or equal to solvers::UNBOUNDED_UP;
+ * @param maxBounds upper bounds on x values. Can be of size 0 if all elements
+ * 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,
- Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
- const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+BEZIER_COM_TRAJ_DLLAPI ResultData
+solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g,
+ 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,
- Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
- const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+BEZIER_COM_TRAJ_DLLAPI ResultData
+solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d,
+ 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,
- const VectorX& init,
- const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+BEZIER_COM_TRAJ_DLLAPI ResultData
+solve(const std::pair<MatrixXX, VectorX>& Ab,
+ 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
- * 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 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
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements
+ * 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 maxBounds upper bounds on x values. Can be of size 0 if all elements
+ * 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,
- const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
- Cref_vectorX maxBounds, const VectorX& init,
- const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+BEZIER_COM_TRAJ_DLLAPI ResultData
+solve(const std::pair<MatrixXX, VectorX>& Ab,
+ const std::pair<MatrixXX, VectorX>& Dd,
+ 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,
- double T,
- const T_time& timeArray);
+BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> >
+computeDiscretizedWaypoints(const ProblemData& pData, double T,
+ const T_time& timeArray);
template <typename Point>
-BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
- const ProblemData& pData, double T, const T_time& timeArray);
+BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> >
+computeDiscretizedAccelerationWaypoints(const ProblemData& pData, double T,
+ const T_time& timeArray);
} // end namespace bezier_com_traj
diff --git a/include/hpp/bezier-com-traj/common_solve_methods.inl b/include/hpp/bezier-com-traj/common_solve_methods.inl
index 5717f26..3437af3 100644
--- a/include/hpp/bezier-com-traj/common_solve_methods.inl
+++ b/include/hpp/bezier-com-traj/common_solve_methods.inl
@@ -1,40 +1,32 @@
-namespace bezier_com_traj
-{
+namespace bezier_com_traj {
template <typename Point>
-std::vector< std::pair<double,Point> > computeDiscretizedWaypoints
- (const ProblemData& pData, double T,const T_time& timeArray)
-{
- typedef std::pair<double,Point> coefs_t;
- std::vector<coefs_t> wps;
- std::vector<Point> pi = computeConstantWaypoints(pData,T);
- // evaluate curve work with normalized time !
- 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));
- }
- return wps;
+std::vector<std::pair<double, Point> > computeDiscretizedWaypoints(
+ const ProblemData& pData, double T, const T_time& timeArray) {
+ typedef std::pair<double, Point> coefs_t;
+ std::vector<coefs_t> wps;
+ std::vector<Point> pi = computeConstantWaypoints(pData, T);
+ // evaluate curve work with normalized time !
+ 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));
+ }
+ return wps;
}
-
template <typename Point>
-std::vector< std::pair<double,Point> > computeDiscretizedAccelerationWaypoints
- (const ProblemData& pData, double T,const T_time& timeArray)
-{
- typedef std::pair<double,Point> coefs_t;
- std::vector<coefs_t> wps;
- std::vector<Point> pi = computeConstantWaypoints(pData,T);
- // evaluate curve work with normalized time !
- 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));
- }
- return wps;
+std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
+ const ProblemData& pData, double T, const T_time& timeArray) {
+ typedef std::pair<double, Point> coefs_t;
+ std::vector<coefs_t> wps;
+ std::vector<Point> pi = computeConstantWaypoints(pData, T);
+ // evaluate curve work with normalized time !
+ 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));
+ }
+ return wps;
}
-} // end namespace bezier_com_traj
-
-
+} // end namespace bezier_com_traj
diff --git a/include/hpp/bezier-com-traj/cost/costfunction_definition.hh b/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
index 349757c..3a2dfb1 100644
--- 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
diff --git a/include/hpp/bezier-com-traj/data.hh b/include/hpp/bezier-com-traj/data.hh
index c32ccda..0f3912b 100644
--- 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 <hpp/bezier-com-traj/flags.hh>
+#include <ndcurves/bezier_curve.h>
+
+#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.
- * Additionally, the maximum acceleration can be bounded.
+ * Flags are used to constrain initial and terminal com positions an
+ * 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() {}
diff --git a/include/hpp/bezier-com-traj/definitions.hh b/include/hpp/bezier-com-traj/definitions.hh
index 9f3c647..6e3b26f 100644
--- 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 {
diff --git a/include/hpp/bezier-com-traj/flags.hh b/include/hpp/bezier-com-traj/flags.hh
index b155e5a..31b7810 100644
--- 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) {
diff --git a/include/hpp/bezier-com-traj/solve.hh b/include/hpp/bezier-com-traj/solve.hh
index da247b7..6c9192e 100644
--- 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,
- * a COM position, and an initial velocity, tries to compute a feasible COM trajectory that
- * stops the character without falling.
+ * @brief solve0step Tries to solve the 0-step capturability problem. Given the
+ * current contact phase, a COM position, and an initial velocity, tries to
+ * 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,
- const double timeStep = -1);
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj
+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,
- * an initial and final com position and velocity,
- * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * @brief computeCOMTraj Tries to solve the one step problem : Given two or
+ * three contact phases, an initial and final com position and velocity, try to
+ * 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,
- const unsigned int pointsPerPhase = 3);
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj
+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,
- * an initial and final com position and velocity,
- * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * @brief computeCOMTraj Tries to solve the one step problem : Given two or
+ * three contact phases, an initial and final com position and velocity, try to
+ * 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 timeStep time step used by the discretization, if -1 : use the continuous fomulation
- * @param solver solver used to perform optimization. WARNING: if the continuous 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.
+ * @param Ts timelength of each contact phase. Should be the same length as
+ * pData.contacts
+ * @param timeStep time step used by the discretization, if -1 : use the
+ * continuous fomulation
+ * @param solver solver used to perform optimization. WARNING: if the continuous
+ * 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,
- const double timeStep = -1,
- const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
+BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(
+ const ProblemData& pData, const VectorX& Ts, const double timeStep = -1,
+ const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
} // end namespace bezier_com_traj
diff --git a/include/hpp/bezier-com-traj/solve_end_effector.hh b/include/hpp/bezier-com-traj/solve_end_effector.hh
index da53324..de59e16 100644
--- a/include/hpp/bezier-com-traj/solve_end_effector.hh
+++ b/include/hpp/bezier-com-traj/solve_end_effector.hh
@@ -3,10 +3,10 @@
* Author: Pierre Fernbach
*/
-#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
-#include <limits>
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <limits>
using namespace bezier_com_traj;
@@ -17,19 +17,23 @@ const int DIM_POINT = 3;
const bool verbose = false;
/**
- * @brief solveEndEffector Tries to produce a trajectory represented as a bezier curve
- * that satisfy position, velocity and acceleration constraint for the initial and final point
- * and that follow as close as possible the input trajectory
+ * @brief solveEndEffector Tries to produce a trajectory represented as a bezier
+ * curve that satisfy position, velocity and acceleration constraint for the
+ * initial and final point and that follow as close as possible the input
+ * trajectory
* @param pData problem Data.
- * @param path the path to follow, the class Path must implement the operator (double t) , t \in [0,1] return a Vector3
- * that give the position on the path for a given time
+ * @param path the path to follow, the class Path must implement the operator
+ * (double t) , t \in [0,1] return a Vector3 that give the position on the path
+ * for a given time
* @param T time lenght of the trajectory
* @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.
*/
template <typename Path>
-ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path, const double T,
- const double weightDistance, bool useVelCost = true);
+ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path,
+ const double T, const double weightDistance,
+ bool useVelCost = true);
coefs_t initCoefs() {
coefs_t c;
@@ -38,84 +42,105 @@ coefs_t initCoefs() {
return c;
}
-// up to jerk second derivativ constraints for init, pos vel and acc constraint for goal
-std::vector<bezier_t::point_t> computeConstantWaypointsInitPredef(const ProblemData& pData, double T) {
+// up to jerk second derivativ constraints for init, pos vel and acc constraint
+// for goal
+std::vector<bezier_t::point_t> computeConstantWaypointsInitPredef(
+ const ProblemData& pData, double T) {
const double n = 4;
std::vector<bezier_t::point_t> pts;
pts.push_back(pData.c0_); // c0
pts.push_back((pData.dc0_ * T / n) + pData.c0_); // dc0
pts.push_back(
- (n * n * pData.c0_ - n * pData.c0_ + 2 * n * pData.dc0_ * T - 2 * pData.dc0_ * T + pData.ddc0_ * T * T) /
+ (n * n * pData.c0_ - n * pData.c0_ + 2 * n * pData.dc0_ * T -
+ 2 * pData.dc0_ * T + pData.ddc0_ * T * T) /
(n * (n - 1))); // ddc0 // * T because derivation make a T appear
- pts.push_back(
- (n * n * pData.c0_ - n * pData.c0_ + 3 * n * pData.dc0_ * T - 3 * pData.dc0_ * T + 3 * pData.ddc0_ * T * T) /
- (n * (n - 1))); // j0
- // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T - 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1)))
- // ; //dj0
- // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 5*n*pData.dc0_*T - 5*pData.dc0_ *T+ 10*pData.ddc0_*T*T)/(n*(n -
- // 1))) ; //ddj0
-
- // pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T + 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1))) ;
+ pts.push_back((n * n * pData.c0_ - n * pData.c0_ + 3 * n * pData.dc0_ * T -
+ 3 * pData.dc0_ * T + 3 * pData.ddc0_ * T * T) /
+ (n * (n - 1))); // j0
+ // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T -
+ // 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1))) ; //dj0
+ // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 5*n*pData.dc0_*T -
+ // 5*pData.dc0_ *T+ 10*pData.ddc0_*T*T)/(n*(n - 1))) ; //ddj0
+
+ // pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T +
+ // 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1))) ;
// //ddc1 // * T ?? pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // dc1
pts.push_back(pData.c1_); // c1
return pts;
}
-// up to jerk second derivativ constraints for goal, pos vel and acc constraint for init
-std::vector<bezier_t::point_t> computeConstantWaypointsGoalPredef(const ProblemData& pData, double T) {
+// up to jerk second derivativ constraints for goal, pos vel and acc constraint
+// for init
+std::vector<bezier_t::point_t> computeConstantWaypointsGoalPredef(
+ const ProblemData& pData, double T) {
const double n = 4;
std::vector<bezier_t::point_t> pts;
pts.push_back(pData.c0_); // c0
// pts.push_back((pData.dc0_ * T / n )+ pData.c0_); //dc0
- // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T - 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1)));
+ // pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T -
+ // 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1)));
// //ddc0 // * T because derivation make a T appear
- // pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 5*n*pData.dc1_*T + 5*pData.dc1_*T + 10*pData.ddc1_*T*T)/(n*(n - 1)))
- // ; //ddj1 pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T + 4*pData.dc1_*T +
- // 6*pData.ddc1_*T*T)/(n*(n - 1))) ; //dj1
- pts.push_back(
- (n * n * pData.c1_ - n * pData.c1_ - 3 * n * pData.dc1_ * T + 3 * pData.dc1_ * T + 3 * pData.ddc1_ * T * T) /
- (n * (n - 1))); // j1
- pts.push_back(
- (n * n * pData.c1_ - n * pData.c1_ - 2 * n * pData.dc1_ * T + 2 * pData.dc1_ * T + pData.ddc1_ * T * T) /
- (n * (n - 1))); // ddc1 * T ??
+ // pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 5*n*pData.dc1_*T +
+ // 5*pData.dc1_*T + 10*pData.ddc1_*T*T)/(n*(n - 1))) ; //ddj1
+ // pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T +
+ // 4*pData.dc1_*T + 6*pData.ddc1_*T*T)/(n*(n - 1))) ; //dj1
+ pts.push_back((n * n * pData.c1_ - n * pData.c1_ - 3 * n * pData.dc1_ * T +
+ 3 * pData.dc1_ * T + 3 * pData.ddc1_ * T * T) /
+ (n * (n - 1))); // j1
+ pts.push_back((n * n * pData.c1_ - n * pData.c1_ - 2 * n * pData.dc1_ * T +
+ 2 * pData.dc1_ * T + pData.ddc1_ * T * T) /
+ (n * (n - 1))); // ddc1 * T ??
pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // dc1
pts.push_back(pData.c1_); // c1
return pts;
}
-void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoint_t>& wps_acc,
- const std::vector<waypoint_t>& wps_vel, const VectorX& acc_bounds,
- const VectorX& vel_bounds, MatrixXX& A, VectorX& b,
- const std::vector<waypoint_t>& wps_jerk = std::vector<waypoint_t>(),
- const VectorX& jerk_bounds = VectorX(DIM_POINT)) {
- assert(acc_bounds.size() == DIM_POINT && "Acceleration bounds should have the same dimension as the points");
- assert(vel_bounds.size() == DIM_POINT && "Velocity bounds should have the same dimension as the points");
- assert(jerk_bounds.size() == DIM_POINT && "Jerk bounds should have the same dimension as the points");
+void computeConstraintsMatrix(
+ const ProblemData& pData, const std::vector<waypoint_t>& wps_acc,
+ const std::vector<waypoint_t>& wps_vel, const VectorX& acc_bounds,
+ const VectorX& vel_bounds, MatrixXX& A, VectorX& b,
+ const std::vector<waypoint_t>& wps_jerk = std::vector<waypoint_t>(),
+ const VectorX& jerk_bounds = VectorX(DIM_POINT)) {
+ assert(acc_bounds.size() == DIM_POINT &&
+ "Acceleration bounds should have the same dimension as the points");
+ assert(vel_bounds.size() == DIM_POINT &&
+ "Velocity bounds should have the same dimension as the points");
+ assert(jerk_bounds.size() == DIM_POINT &&
+ "Jerk bounds should have the same dimension as the points");
const int DIM_VAR = dimVar(pData);
int empty_acc = 0;
int empty_vel = 0;
int empty_jerk = 0;
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
- if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_acc++;
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
+ wpcit != wps_acc.end(); ++wpcit) {
+ if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
+ empty_acc++;
}
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
- if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_vel++;
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
+ wpcit != wps_vel.end(); ++wpcit) {
+ if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
+ empty_vel++;
}
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
- if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_jerk++;
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
+ wpcit != wps_jerk.end(); ++wpcit) {
+ if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
+ empty_jerk++;
}
A = MatrixXX::Zero(
- (2 * DIM_POINT * (wps_acc.size() - empty_acc + wps_vel.size() - empty_vel + wps_jerk.size() - empty_jerk)) +
+ (2 * DIM_POINT *
+ (wps_acc.size() - empty_acc + wps_vel.size() - empty_vel +
+ wps_jerk.size() - empty_jerk)) +
DIM_VAR,
- DIM_VAR); // *2 because we have to put the lower and upper bound for each one, +DIM_VAR for the constraint on
- // x[z]
+ DIM_VAR); // *2 because we have to put the lower and upper bound for each
+ // one, +DIM_VAR for the constraint on x[z]
b = VectorX::Zero(A.rows());
int i = 0;
// upper acc bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
+ wpcit != wps_acc.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = acc_bounds - wpcit->second;
@@ -123,7 +148,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
}
// lower acc bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
+ wpcit != wps_acc.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = acc_bounds + wpcit->second;
@@ -132,7 +158,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
// upper velocity bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
+ wpcit != wps_vel.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds - wpcit->second;
@@ -140,7 +167,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
}
// lower velocity bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
+ wpcit != wps_vel.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds + wpcit->second;
@@ -149,7 +177,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
// upper jerk bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
+ wpcit != wps_jerk.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds - wpcit->second;
@@ -157,7 +186,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
}
// lower jerk bounds
- for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
+ wpcit != wps_jerk.end(); ++wpcit) {
if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
b.segment<DIM_POINT>(i * DIM_POINT) = jerk_bounds + wpcit->second;
@@ -166,7 +196,8 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
}
// test : constraint x[z] to be always higher than init[z] and goal[z].
- // TODO replace z with the direction of the contact normal ... need to change the API
+ // TODO replace z with the direction of the contact normal ... need to change
+ // the API
MatrixXX mxz = MatrixXX::Zero(DIM_VAR, DIM_VAR);
int j = DIM_POINT - 1;
VectorX nxz = VectorX::Zero(DIM_VAR);
@@ -178,9 +209,10 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
A.block(i * DIM_POINT, 0, DIM_VAR, DIM_VAR) = mxz;
b.segment(i * DIM_POINT, DIM_VAR) = nxz;
- // std::cout<<"(i*DIM_POINT + DIM_VAR) = " << (i*DIM_POINT + DIM_VAR)<<std::endl;
- // std::cout<<"A rows = "<<A.rows()<<std::endl;
- assert((i * DIM_POINT + DIM_VAR) == A.rows() && "Constraints matrix were not correctly initialized");
+ // std::cout<<"(i*DIM_POINT + DIM_VAR) = " << (i*DIM_POINT +
+ // DIM_VAR)<<std::endl; std::cout<<"A rows = "<<A.rows()<<std::endl;
+ assert((i * DIM_POINT + DIM_VAR) == A.rows() &&
+ "Constraints matrix were not correctly initialized");
// TEST :
/* A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = Matrix3::Identity();
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);
@@ -189,9 +221,11 @@ void computeConstraintsMatrix(const ProblemData& pData, const std::vector<waypoi
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);*/
}
-std::pair<MatrixXX, VectorX> computeDistanceCostFunction(size_t numPoints, const ProblemData& pData, double T,
- std::vector<point3_t> pts_path) {
- assert(numPoints == pts_path.size() && "Pts_path size must be equal to numPoints");
+std::pair<MatrixXX, VectorX> computeDistanceCostFunction(
+ size_t numPoints, const ProblemData& pData, double T,
+ std::vector<point3_t> pts_path) {
+ assert(numPoints == pts_path.size() &&
+ "Pts_path size must be equal to numPoints");
double step = 1. / (double)(numPoints - 1);
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
waypoint_t c_wp;
@@ -214,11 +248,12 @@ std::pair<MatrixXX, VectorX> computeDistanceCostFunction(size_t numPoints, const
}
template <typename Path>
-std::pair<MatrixXX, VectorX> computeDistanceCostFunction(size_t numPoints, const ProblemData& pData, double T,
- const Path& path) {
- double step = 1. /(double)(numPoints - 1);
+std::pair<MatrixXX, VectorX> computeDistanceCostFunction(
+ size_t numPoints, const ProblemData& pData, double T, const Path& path) {
+ double step = 1. / (double)(numPoints - 1);
std::vector<point3_t> pts_path;
- for (size_t i = 0; i < numPoints; ++i) pts_path.push_back(path(((double)i * step)));
+ for (size_t i = 0; i < numPoints; ++i)
+ pts_path.push_back(path(((double)i * step)));
return computeDistanceCostFunction(numPoints, pData, T, pts_path);
}
@@ -238,11 +273,14 @@ void computeC_of_T(const ProblemData& pData, double T, ResultDataCOMTraj& res) {
wps[7] = res.x.segment<3>(9);
wps[8] = res.x.segment<3>(12);
}
- res.c_of_t_ = bezier_t(wps.begin(), wps.end(),0, T);
- if (verbose) std::cout << "bezier curve created, size = " << res.c_of_t_.size_ << std::endl;
+ res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0, T);
+ if (verbose)
+ std::cout << "bezier curve created, size = " << res.c_of_t_.size_
+ << std::endl;
}
-void computeVelCostFunctionDiscretized(int numPoints, const ProblemData& pData, double T, MatrixXX& H, VectorX& g) {
+void computeVelCostFunctionDiscretized(int numPoints, const ProblemData& pData,
+ double T, MatrixXX& H, VectorX& g) {
double step = 1. / (numPoints - 1);
std::vector<waypoint_t> cks;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
@@ -251,12 +289,15 @@ void computeVelCostFunctionDiscretized(int numPoints, const ProblemData& pData,
}
H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
g = VectorX::Zero(dimVar(pData));
- for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
+ for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
+ ckcit != cks.end(); ++ckcit) {
// H+=(ckcit->first.transpose() * ckcit->first);
// g+=ckcit->second.transpose() * ckcit->first;
for (int i = 0; i < (dimVar(pData) / 3); ++i) {
- H.block<3, 3>(i * 3, i * 3) += Matrix3::Identity() * ckcit->first(0, i * 3) * ckcit->first(0, i * 3);
- g.segment<3>(i * 3) += ckcit->second.segment<3>(0) * ckcit->first(0, i * 3);
+ H.block<3, 3>(i * 3, i * 3) +=
+ Matrix3::Identity() * ckcit->first(0, i * 3) * ckcit->first(0, i * 3);
+ g.segment<3>(i * 3) +=
+ ckcit->second.segment<3>(0) * ckcit->first(0, i * 3);
}
}
// TEST : don't consider z axis for minimum acceleration cost
@@ -268,17 +309,21 @@ void computeVelCostFunctionDiscretized(int numPoints, const ProblemData& pData,
// g /= norm;
}
-void computeAccelerationCostFunctionDiscretized(int numPoints, const ProblemData& pData, double T, MatrixXX& H,
+void computeAccelerationCostFunctionDiscretized(int numPoints,
+ const ProblemData& pData,
+ double T, MatrixXX& H,
VectorX& g) {
double step = 1. / (numPoints - 1);
std::vector<waypoint_t> cks;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
for (int i = 0; i < numPoints; ++i) {
- cks.push_back(evaluateAccelerationCurveWaypointAtTime(pData, T, pi, i * step));
+ cks.push_back(
+ evaluateAccelerationCurveWaypointAtTime(pData, T, pi, i * step));
}
H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
g = VectorX::Zero(dimVar(pData));
- for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
+ for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
+ ckcit != cks.end(); ++ckcit) {
H += (ckcit->first.transpose() * ckcit->first);
g += ckcit->first.transpose() * ckcit->second;
}
@@ -291,7 +336,8 @@ void computeAccelerationCostFunctionDiscretized(int numPoints, const ProblemData
// g /= norm;
}
-void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData& pData, double T, MatrixXX& H, VectorX& g) {
+void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData& pData,
+ double T, MatrixXX& H, VectorX& g) {
double step = 1. / (numPoints - 1);
std::vector<waypoint_t> cks;
@@ -301,7 +347,8 @@ void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData& pData,
}
H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
g = VectorX::Zero(dimVar(pData));
- for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
+ for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
+ ckcit != cks.end(); ++ckcit) {
H += (ckcit->first.transpose() * ckcit->first);
g += ckcit->first.transpose() * ckcit->second;
}
@@ -314,29 +361,35 @@ void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData& pData,
// g /= norm;
}
-std::pair<MatrixXX, VectorX> computeEndEffectorConstraints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi) {
+std::pair<MatrixXX, VectorX> computeEndEffectorConstraints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi) {
std::vector<waypoint_t> wps_jerk = computeJerkWaypoints(pData, T, pi);
std::vector<waypoint_t> wps_acc = computeAccelerationWaypoints(pData, T, pi);
std::vector<waypoint_t> wps_vel = computeVelocityWaypoints(pData, T, pi);
// stack the constraint for each waypoint :
- Vector3 jerk_bounds(10000, 10000, 10000); // TODO : read it from somewhere (ProblemData ?)
+ Vector3 jerk_bounds(10000, 10000,
+ 10000); // TODO : read it from somewhere (ProblemData ?)
Vector3 acc_bounds(10000, 10000, 10000);
Vector3 vel_bounds(10000, 10000, 10000);
MatrixXX A;
VectorX b;
- computeConstraintsMatrix(pData, wps_acc, wps_vel, acc_bounds, vel_bounds, A, b, wps_jerk, jerk_bounds);
+ computeConstraintsMatrix(pData, wps_acc, wps_vel, acc_bounds, vel_bounds, A,
+ b, wps_jerk, jerk_bounds);
return std::make_pair(A, b);
}
template <typename Path>
-std::pair<MatrixXX, VectorX> computeEndEffectorCost(const ProblemData& pData, const Path& path, const double T,
- const double weightDistance, bool /*useVelCost*/,
- std::vector<bezier_t::point_t> pi) {
- assert(weightDistance >= 0. && weightDistance <= 1. && "WeightDistance must be between 0 and 1");
+std::pair<MatrixXX, VectorX> computeEndEffectorCost(
+ const ProblemData& pData, const Path& path, const double T,
+ const double weightDistance, bool /*useVelCost*/,
+ std::vector<bezier_t::point_t> pi) {
+ assert(weightDistance >= 0. && weightDistance <= 1. &&
+ "WeightDistance must be between 0 and 1");
double weightSmooth = 1. - weightDistance;
const int DIM_VAR = dimVar(pData);
- // compute distance cost function (discrete integral under the curve defined by 'path')
+ // compute distance cost function (discrete integral under the curve defined
+ // by 'path')
MatrixXX H;
VectorX g;
std::pair<MatrixXX, VectorX> Hg_smooth, Hg_rrt;
@@ -367,28 +420,33 @@ std::pair<MatrixXX, VectorX> computeEndEffectorCost(const ProblemData& pData, co
}
template <typename Path>
-ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path, const double T,
- const double weightDistance, bool useVelCost) {
+ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path,
+ const double T, const double weightDistance,
+ bool useVelCost) {
if (verbose) std::cout << "solve end effector, T = " << T << std::endl;
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, T);
std::pair<MatrixXX, VectorX> Ab = computeEndEffectorConstraints(pData, T, pi);
- std::pair<MatrixXX, VectorX> Hg = computeEndEffectorCost(pData, path, T, weightDistance, useVelCost, pi);
+ std::pair<MatrixXX, VectorX> Hg =
+ computeEndEffectorCost(pData, path, T, weightDistance, useVelCost, pi);
if (verbose) {
std::cout << "End eff A = " << std::endl << Ab.first << std::endl;
std::cout << "End eff b = " << std::endl << Ab.second << std::endl;
std::cout << "End eff H = " << std::endl << Hg.first << std::endl;
std::cout << "End eff g = " << std::endl << Hg.second << std::endl;
std::cout << "Dim Var = " << dimVar(pData) << std::endl;
- std::cout << "Dim H = " << Hg.first.rows() << " x " << Hg.first.cols() << std::endl;
+ std::cout << "Dim H = " << Hg.first.rows() << " x " << Hg.first.cols()
+ << std::endl;
std::cout << "Dim g = " << Hg.second.rows() << std::endl;
- std::cout << "Dim A = " << Ab.first.rows() << " x " << Ab.first.cols() << std::endl;
+ std::cout << "Dim A = " << Ab.first.rows() << " x " << Ab.first.cols()
+ << std::endl;
std::cout << "Dim b = " << Ab.first.rows() << std::endl;
}
VectorX init = VectorX(dimVar(pData));
// init = (pData.c0_ + pData.c1_)/2.;
// init =pData.c0_;
- if (verbose) std::cout << "Init = " << std::endl << init.transpose() << std::endl;
+ if (verbose)
+ std::cout << "Init = " << std::endl << init.transpose() << std::endl;
ResultData resQp = solve(Ab, Hg, init);
@@ -401,7 +459,8 @@ ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path, c
// computedL_of_T(pData,Ts,res);
}
if (verbose) {
- std::cout << "Solved, success = " << res.success_ << " x = " << res.x.transpose() << std::endl;
+ std::cout << "Solved, success = " << res.success_
+ << " x = " << res.x.transpose() << std::endl;
std::cout << "Final cost : " << resQp.cost_ << std::endl;
}
return res;
diff --git a/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp b/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
index 168bd04..29979b3 100644
--- 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,
- const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
+ EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
+ 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,
- const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
- VectorXd& x);
+ EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess,
+ const VectorXd& g0,
+ 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)=1 otherwise
+ /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
+ /// 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 */
diff --git a/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp b/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
index fb0ebbb..4e9de2c 100644
--- 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,
- solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds, VectorXd& x, double& cost);
+int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
+ const MatrixXd& CI, const VectorXd& ci0,
+ solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
+ VectorXd& x, double& cost);
} /* namespace solvers */
diff --git a/include/hpp/bezier-com-traj/solver/solver-abstract.hpp b/include/hpp/bezier-com-traj/solver/solver-abstract.hpp
index 546df5d..5b8e875 100644
--- 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,
- const MatrixXd& Hess, const VectorXd& g, const VectorXd& initGuess,
- Cref_vectorX minBounds, Cref_vectorX maxBounds, const SolverType solver);
+ResultData BEZIER_COM_TRAJ_DLLAPI solve(
+ const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d,
+ const MatrixXd& Hess, const VectorXd& g, const VectorXd& initGuess,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds, const SolverType solver);
} /* namespace solvers */
diff --git a/include/hpp/bezier-com-traj/utils.hh b/include/hpp/bezier-com-traj/utils.hh
index 879322d..20a58d2 100644
--- 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 <Eigen/Dense>
-
+#include <hpp/bezier-com-traj/local_config.hh>
#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 {
- return first.isApprox(other.first, prec) && second.isApprox(other.second, prec);
+ const value_type 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,
- const std::vector<Point>& pi, const Point& x);
+BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag,
+ 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,
- * is of size pointsPerPhase*phaseTimings.size()
+ * such that there is the same number of point in each phase. Doesn't contain
+ * 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,
- bool clipZ = false);
+void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
+ 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++;
}
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index e549d63..b8a9219 100644
--- 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)
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
- * one free waypoint (x)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND final position
+/// (DEGREE = 3)
+/** @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 -
- 6.0 * pi[1] * t2 + 3.0 * pi[1] * t + 1.0 * pi[3] * t3;
+ 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 - 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) {
- // 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 :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // 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;
}
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
index 3fb4578..0b0a5e8 100644
--- 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)
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
- * one free waypoint (x)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE
+/// = 4)
+/** @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 -
- 6.0 * pi[1] * t2 + 3.0 * pi[1] * t;
+ 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 - 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) {
- // equation for constraint on initial and final position and velocity (degree 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");
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity (degree
+ // 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);
diff --git 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
index c238d6a..29a2f05 100644
--- 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)
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and
- * one free waypoint (x)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE
+/// = 4)
+/** @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] -
- 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[3] * t3 + 1.0 * pi[4] * t4;
+ 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] - 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[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 -
- 24.0 * pi[1] - 48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
+ 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 - 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) {
- // 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 :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // 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>() =
- (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;
+ w2.second.tail<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;
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>() =
- (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;
+ w3.second.tail<3>() = (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;
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();
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh
index 2c9267c..e82136f 100644
--- 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
- * 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 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 =
- 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];
+ wp.second = 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];
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) {
- // equation for constraint on initial position, velocity and acceleration, and only final position (degree =
- // 4)(degree 4, 4 constant waypoint and one free (p3)) first, compute the constant waypoints that only depend on
- // pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial position, velocity and acceleration, and
+ // 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.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(point_t::Zero()); // x
+ pi.push_back(pData.c0_); // pi[0]
+ 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(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);
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index 819d9fd..8d9f5a3 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -13,13 +13,15 @@ namespace c0_dc0_ddc0_c1 {
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_POS;
-/// ### 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
- * 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;
@@ -28,39 +30,47 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t4 = t3 * t;
// equation found with sympy
wp.first = -4.0 * t4 + 4.0 * t3;
- 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] -
- 4.0 * pi[1] * t4 + 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t + 6.0 * pi[2] * t4 -
- 12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 + 1.0 * pi[4] * t4;
+ 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] - 4.0 * pi[1] * t4 +
+ 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t +
+ 6.0 * pi[2] * t4 - 12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 +
+ 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);
double t2 = t * t;
// equation found with sympy
wp.first = (-48.0 * t2 + 24.0 * t) * alpha;
- wp.second = (12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 + 72.0 * pi[1] * t -
- 24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t + 12.0 * pi[2] + 12.0 * pi[4] * t2) *
- alpha;
+ wp.second =
+ (12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 +
+ 72.0 * pi[1] * t - 24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t +
+ 12.0 * pi[2] + 12.0 * pi[4] * t2) *
+ alpha;
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial position, velocity and acceleration, and only final position (degree =
- // 4)(degree 4, 4 constant waypoint and one free (p3)) first, compute the constant waypoints that only depend on
- // pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial position, velocity and acceleration, and
+ // 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 = 4.;
std::vector<point_t> pi;
- pi.push_back(pData.c0_); // p0
- pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
- pi.push_back(point_t::Zero()); // x
- pi.push_back(pData.c1_); // p4
+ pi.push_back(pData.c0_); // p0
+ pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2. * pData.dc0_ * T / n) + pData.c0_); // p2
+ pi.push_back(point_t::Zero()); // x
+ pi.push_back(pData.c1_); // p4
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;
@@ -77,38 +87,54 @@ 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>() = (12 * pi[0] - 24 * pi[1] + 12 * pi[2]) * alpha;
- w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.0 * Cpi[0] * pi[2]) * alpha;
+ w0.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.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) = 4.8 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 4.8 * Cpi[0] * alpha;
w1.second.head<3>() = 1.0 * (7.2 * pi[0] - 9.6 * pi[1] - 2.4 * pi[2]) * alpha;
- w1.second.tail<3>() =
- 1.0 * (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] - 12.0 * Cpi[0] * pi[2] + 9.6 * Cpi[1] * pi[2]) * alpha;
+ w1.second.tail<3>() = 1.0 *
+ (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] -
+ 12.0 * Cpi[0] * pi[2] + 9.6 * 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.8 * alpha * Matrix3::Identity();
w2.first.block<3, 3>(3, 0) = 1.0 * (-4.8 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 * (3.6 * pi[0] - 9.6 * pi[2] + 1.2 * pi[4]) * alpha;
- w2.second.tail<3>() =
- 1.0 * (0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] + 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) * alpha;
+ w2.second.tail<3>() = 1.0 *
+ (0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] +
+ 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) *
+ alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
- w3.first.block<3, 3>(3, 0) = 1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha;
- w3.second.head<3>() = 1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha;
- w3.second.tail<3>() = 1.0 * (0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 4.8 * Cpi[1] * pi[4]) * alpha;
+ w3.first.block<3, 3>(3, 0) =
+ 1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha;
+ w3.second.head<3>() =
+ 1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha;
+ w3.second.tail<3>() =
+ 1.0 *
+ (0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 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) = -9.6 * alpha * Matrix3::Identity();
w4.first.block<3, 3>(3, 0) = 1.0 * (0.8 * Cg * T2 - 9.6 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 * (4.8 * pi[1] - 2.4 * pi[2] + 7.2 * pi[4]) * alpha;
- w4.second.tail<3>() = 1.0 * (0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) * alpha;
+ w4.second.tail<3>() =
+ 1.0 *
+ (0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) *
+ alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -24 * alpha * Matrix3::Identity();
w5.first.block<3, 3>(3, 0) = 1.0 * (-24.0 * Cpi[4]) * alpha;
w5.second.head<3>() = (12 * pi[2] + 12 * pi[4]) * alpha;
- w5.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha;
+ w5.second.tail<3>() =
+ 1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha;
wps.push_back(w5);
return wps;
}
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index 31e33e8..ec38e74 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -11,16 +11,19 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_dc1_c1 {
-static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
+static const ConstraintFlag flag =
+ INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND INIT ACCELERATION (DEGREE = 5)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
+/// INIT ACCELERATION (DEGREE = 5)
///
/**
- * @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and 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;
@@ -30,14 +33,17 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t5 = t4 * t;
// equation found with sympy
wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
- wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 + 10.0 * pi[0] * t2 - 5.0 * pi[0] * t +
- 1.0 * pi[0] + 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 - 20.0 * pi[1] * t2 +
- 5.0 * pi[1] * t - 10.0 * pi[2] * t5 + 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
+ wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 +
+ 10.0 * pi[0] * t2 - 5.0 * pi[0] * t + 1.0 * pi[0] +
+ 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 -
+ 20.0 * pi[1] * t2 + 5.0 * pi[1] * t - 10.0 * pi[2] * t5 +
+ 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
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);
double t2 = t * t;
@@ -45,28 +51,34 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
// equation found with sympy
wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
wp.second = 1.0 *
- (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t + 20.0 * pi[0] + 100.0 * pi[1] * t3 -
- 240.0 * pi[1] * t2 + 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 + 360.0 * pi[2] * t2 -
- 180.0 * pi[2] * t + 20.0 * pi[2] - 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
+ (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t +
+ 20.0 * pi[0] + 100.0 * pi[1] * t3 - 240.0 * pi[1] * t2 +
+ 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 +
+ 360.0 * pi[2] * t2 - 180.0 * pi[2] * t + 20.0 * pi[2] -
+ 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
alpha;
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
- // waypoint and one free (p3)) first, compute the constant waypoints that only depend on pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity and
+ // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
+ // first, compute the constant waypoints that only depend on pData :
double n = 5.;
std::vector<point_t> pi;
- pi.push_back(pData.c0_); // p0
- pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
- pi.push_back(point_t::Zero()); // x
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
- pi.push_back(pData.c1_); // p5
+ pi.push_back(pData.c0_); // p0
+ pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2. * pData.dc0_ * T / n) + pData.c0_); // p2
+ pi.push_back(point_t::Zero()); // x
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
+ pi.push_back(pData.c1_); // p5
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;
@@ -83,78 +95,113 @@ 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>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
- w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) * alpha;
+ w0.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.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) = 8.57142857142857 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
- w1.second.head<3>() = 1.0 * (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] - 5.71428571428572 * pi[2]) * alpha;
- w1.second.tail<3>() = 1.0 *
- (0.285714285714286 * Cg * T2 * pi[0] + 0.714285714285714 * Cg * T2 * pi[1] -
- 20.0 * Cpi[0] * pi[2] + 14.2857142857143 * Cpi[1] * pi[2]) *
+ w1.second.head<3>() = 1.0 *
+ (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
+ 5.71428571428572 * pi[2]) *
alpha;
+ w1.second.tail<3>() =
+ 1.0 *
+ (0.285714285714286 * Cg * T2 * pi[0] +
+ 0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
+ 14.2857142857143 * Cpi[1] * pi[2]) *
+ alpha;
wps.push_back(w1);
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
- w2.first.block<3, 3>(3, 0) = 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
- w2.second.head<3>() = 1.0 * (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] + 2.85714285714286 * pi[4]) * alpha;
+ w2.first.block<3, 3>(3, 0) =
+ 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
+ w2.second.head<3>() = 1.0 *
+ (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
+ 2.85714285714286 * pi[4]) *
+ alpha;
w2.second.tail<3>() =
1.0 *
- (0.0476190476190479 * Cg * T2 * pi[0] + 0.476190476190476 * Cg * T2 * pi[1] +
- 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] - 14.2857142857143 * Cpi[1] * pi[2]) *
+ (0.0476190476190479 * Cg * T2 * pi[0] +
+ 0.476190476190476 * Cg * T2 * pi[1] +
+ 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
+ 14.2857142857143 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
w3.first.block<3, 3>(3, 0) =
- 1.0 * (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] + 11.4285714285714 * Cpi[2]) * alpha;
+ 1.0 *
+ (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
+ 11.4285714285714 * Cpi[2]) *
+ alpha;
w3.second.head<3>() = 1.0 *
- (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] - 11.4285714285714 * pi[2] +
- 5.71428571428571 * pi[4] + 0.571428571428571 * pi[5]) *
+ (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
+ 11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
+ 0.571428571428571 * pi[5]) *
alpha;
w3.second.tail<3>() =
1.0 *
- (0.142857142857143 * Cg * T2 * pi[1] + 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
+ (0.142857142857143 * Cg * T2 * pi[1] +
+ 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
- w4.first.block<3, 3>(3, 0) = 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
+ w4.first.block<3, 3>(3, 0) =
+ 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
- (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] - 2.85714285714286 * pi[2] +
- 5.71428571428571 * pi[4] + 2.28571428571429 * pi[5]) *
+ (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
+ 2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
+ 2.28571428571429 * pi[5]) *
alpha;
w4.second.tail<3>() =
1.0 *
- (0.285714285714286 * Cg * T2 * pi[2] + 0.142857142857143 * Cg * T2 * pi[4] - 0.571428571428572 * Cpi[0] * pi[5] -
- 8.57142857142857 * Cpi[1] * pi[4] + 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
+ (0.285714285714286 * Cg * T2 * pi[2] +
+ 0.142857142857143 * Cg * T2 * pi[4] -
+ 0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
+ 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
- w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
- w5.second.head<3>() = 1.0 * (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] + 5.71428571428571 * pi[5]) * alpha;
+ w5.first.block<3, 3>(3, 0) =
+ 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
+ w5.second.head<3>() = 1.0 *
+ (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
+ 5.71428571428571 * pi[5]) *
+ alpha;
w5.second.tail<3>() =
1.0 *
- (0.476190476190476 * Cg * T2 * pi[4] + 0.0476190476190476 * Cg * T2 * pi[5] - 2.85714285714286 * Cpi[1] * pi[5] -
- 14.2857142857143 * Cpi[2] * pi[4] + 8.57142857142857 * Cpi[2] * pi[5]) *
+ (0.476190476190476 * Cg * T2 * pi[4] +
+ 0.0476190476190476 * Cg * T2 * pi[5] -
+ 2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
+ 8.57142857142857 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
- w6.first.block<3, 3>(3, 0) = 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
- w6.second.head<3>() = 1.0 * (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] + 11.4285714285714 * pi[5]) * alpha;
- w6.second.tail<3>() =
- 1.0 *
- (0.714285714285714 * Cg * T2 * pi[4] + 0.285714285714286 * Cg * T2 * pi[5] - 8.57142857142858 * Cpi[2] * pi[5]) *
- alpha;
+ w6.first.block<3, 3>(3, 0) =
+ 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
+ w6.second.head<3>() = 1.0 *
+ (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
+ 11.4285714285714 * pi[5]) *
+ alpha;
+ w6.second.tail<3>() = 1.0 *
+ (0.714285714285714 * Cg * T2 * pi[4] +
+ 0.285714285714286 * Cg * T2 * pi[5] -
+ 8.57142857142858 * Cpi[2] * pi[5]) *
+ alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
w7.second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
- w7.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
+ w7.second.tail<3>() =
+ 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
wps.push_back(w7);
return wps;
}
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index a119b18..e06e309 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -11,16 +11,19 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_ddc1_dc1_c1 {
-static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
+static const ConstraintFlag flag =
+ INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION (DEGREE = 6)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
+/// ACCELERATION (DEGREE = 6)
///
/**
- * @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and 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 p2 x p3 p4 p5
* @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;
@@ -31,15 +34,19 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t6 = t5 * t;
// equation found with sympy
wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
- wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 - 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 -
- 6.0 * pi[0] * t + 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 - 60.0 * pi[1] * t4 +
- 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 + 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
- 90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 + 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 +
- 15.0 * pi[4] * t4 - 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
+ wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 -
+ 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 - 6.0 * pi[0] * t +
+ 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 -
+ 60.0 * pi[1] * t4 + 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 +
+ 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
+ 90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 +
+ 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 + 15.0 * pi[4] * t4 -
+ 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
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);
double t2 = t * t;
@@ -48,31 +55,38 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
// equation found with sympy
wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
wp.second = 1.0 *
- (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 - 120.0 * pi[0] * t + 30.0 * pi[0] -
- 180.0 * pi[1] * t4 + 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t - 60.0 * pi[1] +
- 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 + 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
- 450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 - 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 +
- 30.0 * pi[6] * t4) *
+ (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 -
+ 120.0 * pi[0] * t + 30.0 * pi[0] - 180.0 * pi[1] * t4 +
+ 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t -
+ 60.0 * pi[1] + 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 +
+ 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
+ 450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 -
+ 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 + 30.0 * pi[6] * t4) *
alpha;
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
- // waypoint and one free (p3)) first, compute the constant waypoints that only depend on pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity and
+ // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
+ // first, compute the constant waypoints that only depend on pData :
double n = 6.;
std::vector<point_t> pi;
- pi.push_back(pData.c0_); // p0
- pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
- pi.push_back(point_t::Zero()); // x
- pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_);
+ pi.push_back(pData.c0_); // p0
+ pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2. * pData.dc0_ * T / n) + pData.c0_); // p2
+ pi.push_back(point_t::Zero()); // x
+ pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
+ (2 * pData.dc1_ * T / n) + pData.c1_);
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
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;
@@ -89,101 +103,143 @@ 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>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
- w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
+ w0.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.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) = 13.3333333333333 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
- w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
+ w1.second.head<3>() =
+ 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
- (0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
+ (0.333333333333333 * Cg * T2 * pi[0] +
+ 0.666666666666667 * Cg * T2 * pi[1] -
30.0 * Cpi[0] * pi[2] + 20.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) = 6.66666666666667 * alpha * Matrix3::Identity();
- w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
- w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
- w2.second.tail<3>() = 1.0 *
- (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
- 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
- alpha;
+ w2.first.block<3, 3>(3, 0) =
+ 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
+ w2.second.head<3>() =
+ 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
+ w2.second.tail<3>() =
+ 1.0 *
+ (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
+ 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
+ 20.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) = -5.71428571428572 * alpha * Matrix3::Identity();
- w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
+ w3.first.block<3, 3>(3, 0) = 1.0 *
+ (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
+ 14.2857142857143 * Cpi[2]) *
+ alpha;
w3.second.head<3>() = 1.0 *
- (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
- 7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
- alpha;
- w3.second.tail<3>() = 1.0 *
- (0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
- 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
- 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
+ (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
+ 14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
+ 1.42857142857143 * pi[5]) *
alpha;
+ w3.second.tail<3>() =
+ 1.0 *
+ (0.0119047619047619 * Cg * T2 * pi[0] +
+ 0.214285714285714 * Cg * T2 * pi[1] +
+ 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
+ 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
+ alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
- w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
+ w4.first.block<3, 3>(3, 0) =
+ 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
- (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
+ (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
+ 3.57142857142857 * pi[2] + 5.0 * pi[4] +
4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
alpha;
w4.second.tail<3>() =
1.0 *
- (0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
- 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
- 12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
+ (0.0476190476190471 * Cg * T2 * pi[1] +
+ 0.357142857142857 * Cg * T2 * pi[2] +
+ 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
+ 0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
+ 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
- w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
+ w5.first.block<3, 3>(3, 0) =
+ 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 *
- (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
- 3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
+ (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
+ 5.0 * pi[2] - 3.57142857142857 * pi[4] +
+ 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
alpha;
w5.second.tail<3>() =
1.0 *
- (+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
- 0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
- 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
+ (+0.11904761904762 * Cg * T2 * pi[2] +
+ 0.357142857142857 * Cg * T2 * pi[4] +
+ 0.0476190476190476 * Cg * T2 * pi[5] -
+ 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
+ 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
+ 12.8571428571429 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
- w6.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
+ w6.first.block<3, 3>(3, 0) = 1.0 *
+ (0.238095238095238 * Cg * T2 +
+ 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
+ alpha;
w6.second.head<3>() = 1.0 *
- (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
- 7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
- alpha;
- w6.second.tail<3>() = 1.0 *
- (0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
- 0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
- 12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
+ (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
+ 14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
+ 3.57142857142857 * pi[6]) *
alpha;
+ w6.second.tail<3>() =
+ 1.0 *
+ (0.535714285714286 * Cg * T2 * pi[4] +
+ 0.214285714285714 * Cg * T2 * pi[5] +
+ 0.0119047619047619 * Cg * T2 * pi[6] -
+ 1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
+ 5.0 * Cpi[2] * pi[6]) *
+ alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
- w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
- w7.second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
- w7.second.tail<3>() = 1.0 *
- (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
- 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
- alpha;
+ w7.first.block<3, 3>(3, 0) =
+ 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
+ w7.second.head<3>() =
+ 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
+ w7.second.tail<3>() =
+ 1.0 *
+ (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
+ 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
+ 20.0 * Cpi[4] * pi[5]) *
+ alpha;
wps.push_back(w7);
waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
- w8.second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
+ w8.second.head<3>() =
+ 1.0 *
+ (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
+ alpha;
w8.second.tail<3>() = 1.0 *
- (0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
+ (0.666666666666667 * Cg * T2 * pi[5] +
+ 0.333333333333333 * Cg * T2 * pi[6] -
20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
alpha;
wps.push_back(w8);
waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
- w9.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
+ w9.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
+ alpha;
wps.push_back(w9);
return wps;
}
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
index d478104..54480f4 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
@@ -11,17 +11,21 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1 {
-static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK;
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
+ END_VEL | END_POS | INIT_JERK | END_JERK;
static const size_t DIM_VAR = 3;
static const size_t DIM_POINT = 3;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK (DEGREE = 8)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
+/// ACCELERATION AND JERK (DEGREE = 8)
///
/**
- * @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 pi[8] pi[1] pi[2] pi[3] x pi[4] pi[5] pi[6] pi[7]
+ * @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 pi[8] pi[1] pi[2] pi[3] x
+ * pi[4] pi[5] pi[6] pi[7]
* @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;
@@ -34,19 +38,25 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
const double t8 = t7 * t;
// equation found with sympy
wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
- wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 - 56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 -
- 56.0 * pi[8] * t3 + 28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] - 8.0 * pi[1] * t8 +
- 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 + 280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
- 56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 - 168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 -
- 560.0 * pi[2] * t5 + 420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
- 56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 - 560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 -
- 280.0 * pi[3] * t4 + 56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 - 168.0 * pi[5] * t6 +
- 56.0 * pi[5] * pow(t, 5) + 28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 - 8.0 * pi[7] * t8 +
- 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
+ wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 -
+ 56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 - 56.0 * pi[8] * t3 +
+ 28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] -
+ 8.0 * pi[1] * t8 + 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 +
+ 280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
+ 56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 -
+ 168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 - 560.0 * pi[2] * t5 +
+ 420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
+ 56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 -
+ 560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 - 280.0 * pi[3] * t4 +
+ 56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 -
+ 168.0 * pi[5] * t6 + 56.0 * pi[5] * pow(t, 5) +
+ 28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 -
+ 8.0 * pi[7] * t8 + 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
return wp;
}
-inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi,
+ double T, double t) {
coefs_t wp;
const double alpha = 1. / (T);
const double t2 = t * t;
@@ -56,21 +66,28 @@ inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi, doubl
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
- wp.first = (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) * alpha;
- wp.second =
- (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 - 280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 -
- 168.0 * pi[8] * t2 + 56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 + 392.0 * pi[1] * t6 -
- 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 - 1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
- 8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 + 2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 +
- 1680.0 * pi[2] * t3 - 504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 + 1960.0 * pi[3] * t6 -
- 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 - 1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
- 1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 + 224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 +
- 168.0 * pi[6] * t5 - 64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
+ wp.first =
+ (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) *
alpha;
+ wp.second = (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 -
+ 280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 - 168.0 * pi[8] * t2 +
+ 56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 +
+ 392.0 * pi[1] * t6 - 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 -
+ 1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
+ 8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 +
+ 2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 + 1680.0 * pi[2] * t3 -
+ 504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 +
+ 1960.0 * pi[3] * t6 - 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 -
+ 1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
+ 1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 +
+ 224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 + 168.0 * pi[6] * t5 -
+ 64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
+ alpha;
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;
const double alpha = 1. / (T * T);
const double t2 = t * t;
@@ -79,21 +96,28 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
const double t5 = t4 * t;
const double t6 = t5 * t;
// equation found with sympy
- wp.first = ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) * alpha;
- wp.second =
- (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 - 1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 -
- 336.0 * pi[8] * t + 56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 - 5040.0 * pi[1] * t4 +
- 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 + 1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
- 7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 - 11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
- 56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 - 16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
- 3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 + 7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 +
- 1120.0 * pi[5] * t3 + 1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 - 448.0 * pi[7] * t6 +
- 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
+ wp.first =
+ ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) *
alpha;
+ wp.second = (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 -
+ 1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 - 336.0 * pi[8] * t +
+ 56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 -
+ 5040.0 * pi[1] * t4 + 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 +
+ 1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
+ 7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 -
+ 11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
+ 56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 -
+ 16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
+ 3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 +
+ 7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 + 1120.0 * pi[5] * t3 +
+ 1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 -
+ 448.0 * pi[7] * t6 + 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
+ alpha;
return wp;
}
-inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
+ double t) {
coefs_t wp;
const double alpha = 1. / (T * T * T);
const double t2 = t * t;
@@ -101,34 +125,44 @@ inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
const double t4 = t3 * t;
const double t5 = t4 * t;
// equation found with sympy
- wp.first = (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) * alpha;
+ wp.first =
+ (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) *
+ alpha;
wp.second =
1.0 *
- (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 - 3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t -
- 336.0 * pi[0] - 2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 + 16800.0 * pi[1] * t2 -
- 6720.0 * pi[1] * t + 1008.0 * pi[1] + 9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
- 33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] - 18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 -
- 67200.0 * pi[3] * t3 + 33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] - 18816.0 * pi[5] * t5 +
- 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 + 3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
- 3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 + 336.0 * pi[8] * t5) *
+ (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 -
+ 3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t - 336.0 * pi[0] -
+ 2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 +
+ 16800.0 * pi[1] * t2 - 6720.0 * pi[1] * t + 1008.0 * pi[1] +
+ 9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
+ 33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] -
+ 18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 - 67200.0 * pi[3] * t3 +
+ 33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] -
+ 18816.0 * pi[5] * t5 + 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 +
+ 3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
+ 3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 +
+ 336.0 * pi[8] * t5) *
alpha;
return wp;
}
-inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
+inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
+ double t) {
coefs_t coef = evaluateCurveAtTime(pi, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
wp.second = coef.second;
return wp;
}
-inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateVelocityCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateVelocityCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
wp.second = coef.second;
return wp;
}
-inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateAccelerationCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
@@ -136,7 +170,8 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
return wp;
}
-inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateJerkCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateJerkCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
@@ -144,27 +179,34 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
- // waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity and
+ // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
+ // first, compute the constant waypoints that only depend on pData :
double n = 8.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
- pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
+ pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
- pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
- (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
+ (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
+ (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
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;
@@ -181,107 +223,150 @@ 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>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
- w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
+ w0.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.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) = 13.3333333333333 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
- w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
+ w1.second.head<3>() =
+ 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
- (0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
+ (0.333333333333333 * Cg * T2 * pi[0] +
+ 0.666666666666667 * Cg * T2 * pi[1] -
30.0 * Cpi[0] * pi[2] + 20.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) = 6.66666666666667 * alpha * Matrix3::Identity();
- w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
- w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
- w2.second.tail<3>() = 1.0 *
- (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
- 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
- alpha;
+ w2.first.block<3, 3>(3, 0) =
+ 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
+ w2.second.head<3>() =
+ 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
+ w2.second.tail<3>() =
+ 1.0 *
+ (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
+ 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
+ 20.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) = -5.71428571428572 * alpha * Matrix3::Identity();
- w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
+ w3.first.block<3, 3>(3, 0) = 1.0 *
+ (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
+ 14.2857142857143 * Cpi[2]) *
+ alpha;
w3.second.head<3>() = 1.0 *
- (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
- 7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
- alpha;
- w3.second.tail<3>() = 1.0 *
- (0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
- 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
- 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
+ (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
+ 14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
+ 1.42857142857143 * pi[5]) *
alpha;
+ w3.second.tail<3>() =
+ 1.0 *
+ (0.0119047619047619 * Cg * T2 * pi[0] +
+ 0.214285714285714 * Cg * T2 * pi[1] +
+ 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
+ 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
+ alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
- w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
+ w4.first.block<3, 3>(3, 0) =
+ 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
- (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
+ (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
+ 3.57142857142857 * pi[2] + 5.0 * pi[4] +
4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
alpha;
w4.second.tail<3>() =
1.0 *
- (0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
- 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
- 12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
+ (0.0476190476190471 * Cg * T2 * pi[1] +
+ 0.357142857142857 * Cg * T2 * pi[2] +
+ 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
+ 0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
+ 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
- w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
+ w5.first.block<3, 3>(3, 0) =
+ 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 *
- (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
- 3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
+ (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
+ 5.0 * pi[2] - 3.57142857142857 * pi[4] +
+ 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
alpha;
w5.second.tail<3>() =
1.0 *
- (+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
- 0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
- 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
+ (+0.11904761904762 * Cg * T2 * pi[2] +
+ 0.357142857142857 * Cg * T2 * pi[4] +
+ 0.0476190476190476 * Cg * T2 * pi[5] -
+ 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
+ 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
+ 12.8571428571429 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
- w6.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
+ w6.first.block<3, 3>(3, 0) = 1.0 *
+ (0.238095238095238 * Cg * T2 +
+ 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
+ alpha;
w6.second.head<3>() = 1.0 *
- (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
- 7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
- alpha;
- w6.second.tail<3>() = 1.0 *
- (0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
- 0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
- 12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
+ (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
+ 14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
+ 3.57142857142857 * pi[6]) *
alpha;
+ w6.second.tail<3>() =
+ 1.0 *
+ (0.535714285714286 * Cg * T2 * pi[4] +
+ 0.214285714285714 * Cg * T2 * pi[5] +
+ 0.0119047619047619 * Cg * T2 * pi[6] -
+ 1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
+ 5.0 * Cpi[2] * pi[6]) *
+ alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
- w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
- w7.second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
- w7.second.tail<3>() = 1.0 *
- (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
- 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
- alpha;
+ w7.first.block<3, 3>(3, 0) =
+ 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
+ w7.second.head<3>() =
+ 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
+ w7.second.tail<3>() =
+ 1.0 *
+ (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
+ 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
+ 20.0 * Cpi[4] * pi[5]) *
+ alpha;
wps.push_back(w7);
waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
- w8.second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
+ w8.second.head<3>() =
+ 1.0 *
+ (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
+ alpha;
w8.second.tail<3>() = 1.0 *
- (0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
+ (0.666666666666667 * Cg * T2 * pi[5] +
+ 0.333333333333333 * Cg * T2 * pi[6] -
20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
alpha;
wps.push_back(w8);
waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
- w9.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
+ w9.second.tail<3>() =
+ 1.0 *
+ (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
+ alpha;
wps.push_back(w9);
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -326,7 +411,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -368,8 +454,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
-std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+std::vector<waypoint_t> computeJerkWaypoints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -419,12 +506,14 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
- const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(3, 3);
VectorX g = VectorX::Zero(3);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
- g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] + 9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
+ g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] +
+ 9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
(2 * T);
H = Matrix3::Identity() * 6.52680652684107 / (T);
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
index 6993d76..3310ae9 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
@@ -11,22 +11,26 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1 {
-static const ConstraintFlag flag =
- INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK | THREE_FREE_VAR;
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
+ END_VEL | END_POS | INIT_JERK | END_JERK |
+ THREE_FREE_VAR;
static const size_t DIM_VAR = 9;
static const size_t DIM_POINT = 3;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK AND 3 variables in
-/// the middle (DEGREE = 10)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
+/// ACCELERATION AND JERK AND 3 variables in the middle (DEGREE = 10)
///
/**
- * @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 pi[8] pi[1] pi[2] pi[3] x0 x1 x2 pi[4] pi[5] pi[6] pi[7]
+ * @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 pi[8] pi[1] pi[2] pi[3] x0
+ * x1 x2 pi[4] pi[5] pi[6] pi[7]
* @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
*/
// TODO
-inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
+inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
+ double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double t2 = t * t;
const double t3 = t2 * t;
@@ -39,28 +43,37 @@ inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, do
const double t10 = t9 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 +
- 3150.0 * t8 - 1260.0 * t9 + 210.0 * t10); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() *
+ (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 + 3150.0 * t8 -
+ 1260.0 * t9 + 210.0 * t10); // x0
wp.first.block<3, 3>(0, 3) =
- Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 - 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10); // x1
+ Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 -
+ 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10); // x1
wp.first.block<3, 3>(0, 6) =
- Matrix3::Identity() * (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
- wp.second = 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) + t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
- t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
- t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
- t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
- t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
- t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] + 120.0 * pi[7]) +
- t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] - 360.0 * pi[7] + 45.0 * pi[8]) +
- t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] + 360.0 * pi[7] - 90.0 * pi[8] +
- 10.0 * pi[9]) +
- t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] - 120.0 * pi[3] - 120.0 * pi[7] +
- 45.0 * pi[8] - 10.0 * pi[9]);
+ Matrix3::Identity() *
+ (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
+ wp.second =
+ 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) +
+ t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
+ t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
+ t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
+ t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
+ t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
+ t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] +
+ 120.0 * pi[7]) +
+ t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] -
+ 360.0 * pi[7] + 45.0 * pi[8]) +
+ t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] +
+ 360.0 * pi[7] - 90.0 * pi[8] + 10.0 * pi[9]) +
+ t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] -
+ 120.0 * pi[3] - 120.0 * pi[7] + 45.0 * pi[8] - 10.0 * pi[9]);
return wp;
}
// TODO
-inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateVelocityCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T);
const double t2 = t * t;
@@ -72,32 +85,46 @@ inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>
const double t8 = t7 * t;
const double t9 = t8 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
- (1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 + 18900.0 * t5 -
- 6300.0 * t4 + 840.0 * t3)); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 +
+ 18900.0 * t5 - 6300.0 * t4 + 840.0 * t3)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
- (1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 - 7560.0 * t5 + 1260.0 * t4)); // x1
- wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha *
- (1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 + 1260.0 * t5)); // x2
- wp.second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
- t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
- t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
- 810.0 * pi[8] + 90.0 * pi[9])) +
- t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
- t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
- 1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
- t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
- t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
- t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
- t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
- 360.0 * pi[8]))) *
- alpha;
+ (1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 -
+ 7560.0 * t5 + 1260.0 * t4)); // x1
+ wp.first.block<3, 3>(0, 6) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 +
+ 1260.0 * t5)); // x2
+ wp.second =
+ (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
+ t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
+ t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
+ 360.0 * pi[3])) +
+ t3 * (1.0 *
+ (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
+ 3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
+ t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
+ 3360.0 * pi[3])) +
+ t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
+ 450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
+ 450.0 * pi[8] - 100.0 * pi[9])) +
+ t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
+ 12600.0 * pi[3])) +
+ t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
+ 25200.0 * pi[3])) +
+ t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
+ 29400.0 * pi[3] + 840.0 * pi[7])) +
+ t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
+ 20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
+ alpha;
return wp;
}
// TODO
-inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T * T);
const double t2 = t * t;
@@ -108,34 +135,45 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
const double t7 = t6 * t;
const double t8 = t7 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
- (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
- 25200.0 * t3 + 2520.0 * t2)); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
+ 94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
- (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
+ (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
+ 37800.0 * t4 + 5040.0 * t3)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
- (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
+ (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
+ 6300.0 * t4)); // x2
wp.second =
(1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
- t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
- t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
- t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
+ t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
+ 720.0 * pi[3])) +
+ t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
+ 10080.0 * pi[3])) +
+ t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
+ 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
4050.0 * pi[8] - 900.0 * pi[9])) +
- t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
- t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
- t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
- t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
- 2520.0 * pi[8])) +
- t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
- 6480.0 * pi[8] + 720.0 * pi[9]))) *
+ t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
+ 50400.0 * pi[3])) +
+ t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
+ 126000.0 * pi[3])) +
+ t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
+ 176400.0 * pi[3] + 5040.0 * pi[7])) +
+ t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
+ 141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
+ t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
+ 60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
+ 720.0 * pi[9]))) *
alpha;
return wp;
}
// TODO
-inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateJerkCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T * T * T);
const double t2 = t * t;
@@ -145,73 +183,91 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
- (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
- 75600.0 * t2 + 5040.0 * t)); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
+ 378000.0 * t3 - 75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
- (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
+ (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
+ 151200.0 * t3 + 15120.0 * t2)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
- (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
+ (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
+ 25200.0 * t3)); // x2
wp.second =
- (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
- t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
- t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
- t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
- t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
- t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
- 15120.0 * pi[8])) +
- t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
- 45360.0 * pi[8] + 5040.0 * pi[9])) +
- t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
- 86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
+ (1.0 *
+ (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
+ t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
+ 20160.0 * pi[3])) +
+ t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
+ 151200.0 * pi[3])) +
+ t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
+ 504000.0 * pi[3])) +
+ t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
+ 882000.0 * pi[3] + 25200.0 * pi[7])) +
+ t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
+ 846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
+ t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
+ 423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
+ 5040.0 * pi[9])) +
+ t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
+ 32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
+ 32400.0 * pi[8] - 7200.0 * pi[9]))) *
alpha;
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
- // waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity and
+ // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
+ // first, compute the constant waypoints that only depend on pData :
double n = 10.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
- pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
+ pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
- pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
- (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
+ (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
+ (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
return pi;
}
// TODO
-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 = 9;
+ // const int DIM_POINT = 6;
+ // const int DIM_VAR = 9;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
std::vector<Matrix3> Cpi;
for (std::size_t i = 0; i < pi.size(); ++i) {
Cpi.push_back(skew(pi[i]));
}
- //const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
- //const Matrix3 Cg = skew(g);
- //const double T2 = T * T;
- //const double alpha = 1 / (T2);
+ // const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
+ // const Matrix3 Cg = skew(g);
+ // const double T2 = T * T;
+ // const double alpha = 1 / (T2);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -267,7 +323,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -323,8 +380,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
-std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+std::vector<waypoint_t> computeJerkWaypoints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -391,33 +449,43 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
- const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(9, 9);
VectorX g = VectorX::Zero(9);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
- g.segment<3>(0) =
- (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] - 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
- 12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] - 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
- (2 * T); // x0
- g.segment<3>(3) =
- (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] - 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
- 2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] - 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
- (2 * T); // x1
- g.segment<3>(6) =
- (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] - 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
- 4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] + 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
- (2 * T); // x2
-
- H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
- H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
- H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
- H.block<3, 3>(0, 3) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
- H.block<3, 3>(3, 0) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
- H.block<3, 3>(0, 6) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
- H.block<3, 3>(6, 0) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
- H.block<3, 3>(3, 6) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
- H.block<3, 3>(6, 3) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
+ g.segment<3>(0) = (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] -
+ 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
+ 12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] -
+ 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
+ (2 * T); // x0
+ g.segment<3>(3) = (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] -
+ 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
+ 2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] -
+ 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
+ (2 * T); // x1
+ g.segment<3>(6) = (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] -
+ 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
+ 4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] +
+ 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
+ (2 * T); // x2
+
+ H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
+ H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
+ H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
+ H.block<3, 3>(0, 3) =
+ Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
+ H.block<3, 3>(3, 0) =
+ Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
+ H.block<3, 3>(0, 6) =
+ Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
+ H.block<3, 3>(6, 0) =
+ Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
+ H.block<3, 3>(3, 6) =
+ Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
+ H.block<3, 3>(6, 3) =
+ Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
double norm = H.norm();
H /= norm;
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
index 8d3e9a0..830be88 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
@@ -11,21 +11,25 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
-static const ConstraintFlag flag =
- INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK | FIVE_FREE_VAR;
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
+ END_VEL | END_POS | INIT_JERK | END_JERK |
+ FIVE_FREE_VAR;
static const size_t DIM_VAR = 3 * 5;
static const size_t DIM_POINT = 3;
-/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK AND 5 variables in
-/// the middle (DEGREE = 10)
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
+/// ACCELERATION AND JERK AND 5 variables in the middle (DEGREE = 10)
///
/**
- * @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 pi[8] pi[1] pi[2] pi[3] x0 x1 x2 x3 x4 pi[4] pi[5] pi[6] pi[7]
+ * @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 pi[8] pi[1] pi[2] pi[3] x0
+ * x1 x2 x3 x4 pi[4] pi[5] pi[6] pi[7]
* @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 waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
+inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
+ double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double t2 = t * t;
const double t3 = t2 * t;
@@ -41,36 +45,50 @@ inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, do
// equation found with sympy
wp.first.block<3, 3>(0, 0) =
- Matrix3::Identity() * (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 - 27720.0 * t9 +
- 13860.0 * t10 - 3960.0 * t11 + 495.0 * t12); // x0
+ Matrix3::Identity() *
+ (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 -
+ 27720.0 * t9 + 13860.0 * t10 - 3960.0 * t11 + 495.0 * t12); // x0
wp.first.block<3, 3>(0, 3) =
- Matrix3::Identity() * (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 - 16632.0 * t10 +
- 5544.0 * t11 - 792.0 * t12); // x1
- wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 +
- 13860.0 * t10 - 5544.0 * t11 + 924.0 * t12); // x2
- wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 - 7920.0 * t10 +
- 3960.0 * t11 - 792.0 * t12); // x3
+ Matrix3::Identity() *
+ (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 -
+ 16632.0 * t10 + 5544.0 * t11 - 792.0 * t12); // x1
+ wp.first.block<3, 3>(0, 6) =
+ Matrix3::Identity() *
+ (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 + 13860.0 * t10 -
+ 5544.0 * t11 + 924.0 * t12); // x2
+ wp.first.block<3, 3>(0, 9) =
+ Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 -
+ 7920.0 * t10 + 3960.0 * t11 - 792.0 * t12); // x3
wp.first.block<3, 3>(0, 12) =
- Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 - 1980.0 * t11 + 495.0 * t12); // x4
-
- wp.second = 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) + t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
- t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
- t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
- t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
- t6 * (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
- t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] + 27720.0 * pi[3]) +
- t8 * (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
- t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] + 220.0 * pi[9]) +
- t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] - 7920.0 * pi[3] - 660.0 * pi[9]) +
- t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] - 660.0 * pi[2] + 1980.0 * pi[3] +
- 660.0 * pi[9]) +
- t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] - 12.0 * pi[1] + 66.0 * pi[2] -
- 220.0 * pi[3] - 220.0 * pi[9]);
+ Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 -
+ 1980.0 * t11 + 495.0 * t12); // x4
+
+ wp.second =
+ 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) +
+ t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
+ t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
+ t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
+ t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
+ t6 *
+ (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
+ t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] +
+ 27720.0 * pi[3]) +
+ t8 *
+ (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
+ t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] +
+ 220.0 * pi[9]) +
+ t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] -
+ 7920.0 * pi[3] - 660.0 * pi[9]) +
+ t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] -
+ 660.0 * pi[2] + 1980.0 * pi[3] + 660.0 * pi[9]) +
+ t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] -
+ 12.0 * pi[1] + 66.0 * pi[2] - 220.0 * pi[3] - 220.0 * pi[9]);
return wp;
}
// TODO
-inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateVelocityCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
@@ -91,24 +109,34 @@ inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>
wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha; // x3
wp.first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha; // x4
- wp.second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
- t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
- t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
- 810.0 * pi[8] + 90.0 * pi[9])) +
- t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
- t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
- 1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
- t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
- t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
- t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
- t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
- 360.0 * pi[8]))) *
- alpha;
+ wp.second =
+ (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
+ t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
+ t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
+ 360.0 * pi[3])) +
+ t3 * (1.0 *
+ (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
+ 3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
+ t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
+ 3360.0 * pi[3])) +
+ t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
+ 450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
+ 450.0 * pi[8] - 100.0 * pi[9])) +
+ t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
+ 12600.0 * pi[3])) +
+ t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
+ 25200.0 * pi[3])) +
+ t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
+ 29400.0 * pi[3] + 840.0 * pi[7])) +
+ t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
+ 20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
+ alpha;
return wp;
}
// TODO
-inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
@@ -121,34 +149,45 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
const double t7 = t6 * t;
const double t8 = t7 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
- (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
- 25200.0 * t3 + 2520.0 * t2)); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
+ 94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
- (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
+ (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
+ 37800.0 * t4 + 5040.0 * t3)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
- (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
+ (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
+ 6300.0 * t4)); // x2
wp.second =
(1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
- t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
- t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
- t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
+ t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
+ 720.0 * pi[3])) +
+ t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
+ 10080.0 * pi[3])) +
+ t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
+ 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
4050.0 * pi[8] - 900.0 * pi[9])) +
- t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
- t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
- t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
- t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
- 2520.0 * pi[8])) +
- t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
- 6480.0 * pi[8] + 720.0 * pi[9]))) *
+ t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
+ 50400.0 * pi[3])) +
+ t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
+ 126000.0 * pi[3])) +
+ t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
+ 176400.0 * pi[3] + 5040.0 * pi[7])) +
+ t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
+ 141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
+ t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
+ 60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
+ 720.0 * pi[9]))) *
alpha;
return wp;
}
// TODO
-inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
+inline waypoint_t evaluateJerkCurveWaypointAtTime(
+ const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
@@ -160,75 +199,93 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
- wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
- (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
- 75600.0 * t2 + 5040.0 * t)); // x0
+ wp.first.block<3, 3>(0, 0) =
+ Matrix3::Identity() * alpha *
+ (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
+ 378000.0 * t3 - 75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
- (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
+ (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
+ 151200.0 * t3 + 15120.0 * t2)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
- (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
+ (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
+ 25200.0 * t3)); // x2
wp.second =
- (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
- t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
- t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
- t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
- t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
- t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
- 15120.0 * pi[8])) +
- t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
- 45360.0 * pi[8] + 5040.0 * pi[9])) +
- t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
- 86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
+ (1.0 *
+ (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
+ t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
+ 20160.0 * pi[3])) +
+ t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
+ 151200.0 * pi[3])) +
+ t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
+ 504000.0 * pi[3])) +
+ t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
+ 882000.0 * pi[3] + 25200.0 * pi[7])) +
+ t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
+ 846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
+ t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
+ 423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
+ 5040.0 * pi[9])) +
+ t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
+ 32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
+ 32400.0 * pi[8] - 7200.0 * pi[9]))) *
alpha;
return wp;
}
-inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
- // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
- // waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
+ // equation for constraint on initial and final position and velocity and
+ // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
+ // first, compute the constant waypoints that only depend on pData :
double n = 12.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
- pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
+ pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
+ (2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
- pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
+ pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
- pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
- (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
+ (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
+ (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
+ (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
return pi;
}
// TODO
-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 = 15;
+ // const int DIM_POINT = 6;
+ // const int DIM_VAR = 15;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
std::vector<Matrix3> Cpi;
for (std::size_t i = 0; i < pi.size(); ++i) {
Cpi.push_back(skew(pi[i]));
}
- //const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
- //const Matrix3 Cg = skew(g);
- //const double T2 = T * T;
- //const double alpha = 1 / (T2);
+ // const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
+ // const Matrix3 Cg = skew(g);
+ // const double T2 = T * T;
+ // const double alpha = 1 / (T2);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -294,7 +351,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -362,8 +420,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
-std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+std::vector<waypoint_t> computeJerkWaypoints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
@@ -445,31 +504,37 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
// TODO
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
- const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
+ const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
VectorX g = VectorX::Zero(DIM_VAR);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
- g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] - 1.80981866649436 * pi[11] -
- 0.408668730537654 * pi[12] - 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
+ g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] -
+ 1.80981866649436 * pi[11] - 0.408668730537654 * pi[12] -
+ 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
16.0548432515434 * pi[3] - 6.66893486967885 * pi[9])) /
(2 * T); // x0
- g.segment<3>(3) =
- ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] - 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
- 12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] + 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
- (2 * T); // x1
- g.segment<3>(6) =
- (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] - 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
- 7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] - 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
- (2 * T); // x2
- g.segment<3>(9) =
- (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] - 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
- 4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] - 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
- (2 * T); // x3
- g.segment<3>(12) =
- (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] - 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
- 1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] - 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
- (2 * T); // x4
+ g.segment<3>(3) = ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] -
+ 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
+ 12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] +
+ 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
+ (2 * T); // x1
+ g.segment<3>(6) = (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] -
+ 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
+ 7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] -
+ 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
+ (2 * T); // x2
+ g.segment<3>(9) = (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] -
+ 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
+ 4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] -
+ 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
+ (2 * T); // x3
+ g.segment<3>(12) = (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] -
+ 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
+ 1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] -
+ 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
+ (2 * T); // x4
H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T); // x0^2
H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T); // x1^2
@@ -477,29 +542,49 @@ inline std::pair<MatrixXX, VectorX> computeVelocityCost(
H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T); // x3^2
H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T); // x4^2
- H.block<3, 3>(0, 3) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
- H.block<3, 3>(3, 0) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
- H.block<3, 3>(0, 6) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
- H.block<3, 3>(6, 0) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
- H.block<3, 3>(0, 9) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
- H.block<3, 3>(9, 0) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
- H.block<3, 3>(0, 12) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
- H.block<3, 3>(12, 0) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
-
- H.block<3, 3>(3, 6) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
- H.block<3, 3>(6, 3) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
- H.block<3, 3>(3, 9) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
- H.block<3, 3>(9, 3) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
- H.block<3, 3>(3, 12) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
- H.block<3, 3>(12, 3) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
-
- H.block<3, 3>(6, 9) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
- H.block<3, 3>(9, 6) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
- H.block<3, 3>(6, 12) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
- H.block<3, 3>(12, 6) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
-
- H.block<3, 3>(9, 12) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
- H.block<3, 3>(12, 9) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
+ H.block<3, 3>(0, 3) =
+ Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
+ H.block<3, 3>(3, 0) =
+ Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
+ H.block<3, 3>(0, 6) =
+ Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
+ H.block<3, 3>(6, 0) =
+ Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
+ H.block<3, 3>(0, 9) =
+ Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
+ H.block<3, 3>(9, 0) =
+ Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
+ H.block<3, 3>(0, 12) =
+ Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
+ H.block<3, 3>(12, 0) =
+ Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
+
+ H.block<3, 3>(3, 6) =
+ Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
+ H.block<3, 3>(6, 3) =
+ Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
+ H.block<3, 3>(3, 9) =
+ Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
+ H.block<3, 3>(9, 3) =
+ Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
+ H.block<3, 3>(3, 12) =
+ Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
+ H.block<3, 3>(12, 3) =
+ Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
+
+ H.block<3, 3>(6, 9) =
+ Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
+ H.block<3, 3>(9, 6) =
+ Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
+ H.block<3, 3>(6, 12) =
+ Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
+ H.block<3, 3>(12, 6) =
+ Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
+
+ H.block<3, 3>(9, 12) =
+ Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
+ H.block<3, 3>(12, 9) =
+ Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
double norm = H.norm();
H /= norm;
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
index 368f976..3236829 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -14,37 +14,48 @@ namespace bezier_com_traj {
* depending on the options set in ProblemData.constraints
*/
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t,
- * defined by the waypoint pi and one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
-coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t);
+coefs_t evaluateCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi, double t);
-/** @brief evaluateVelocityCurveAtTime compute the expression of the point on the curve dc at t,
- * defined by the waypoint pi and one free waypoint (x)
+/** @brief evaluateVelocityCurveAtTime compute the expression of the point on
+ * the curve dc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
-coefs_t evaluateVelocityCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
+coefs_t evaluateVelocityCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi, double T,
+ double t);
-/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t,
- * defined by the waypoint pi and one free waypoint (x)
+/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
+ * on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
-coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
+coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi,
+ double T, double t);
-/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t,
- * defined by the waypoint pi and one free waypoint (x)
+/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
+ * on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
-coefs_t evaluateJerkCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
+coefs_t evaluateJerkCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi, double T,
+ double t);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
@@ -53,16 +64,18 @@ coefs_t evaluateJerkCurveAtTime(const ProblemData& pData, const std::vector<poin
* @param T
* @return
*/
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T);
+std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T);
/**
- * @brief computeConstantWaypointsSymbolic compute the constant waypoints of c(t)
- * defined by the constraints on initial and final states
+ * @brief computeConstantWaypointsSymbolic compute the constant waypoints of
+ * c(t) defined by the constraints on initial and final states
* @param pData
* @param T
* @return the waypoints expressed as a polynom of the free waypoint
*/
-bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData, double T);
+bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(
+ const ProblemData& pData, double T);
/**
* @brief computeWwaypoints compute the constant waypoints of dc(t)
@@ -71,8 +84,9 @@ bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData
* @param T
* @return
*/
-std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+std::vector<waypoint_t> computeVelocityWaypoints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
* @brief computeWwaypoints compute the constant waypoints of ddc(t)
@@ -82,7 +96,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, const
* @return
*/
std::vector<waypoint_t> computeAccelerationWaypoints(
- const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
* @brief computeWwaypoints compute the constant waypoints of dddc(t)
@@ -91,15 +106,23 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
* @param T
* @return
*/
-std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
-
-waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t);
-waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const double T,
- const std::vector<point_t>& pi, double t);
-waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, const double T,
- const std::vector<point_t>& pi, double t);
-waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const double T, const std::vector<point_t>& pi,
+std::vector<waypoint_t> computeJerkWaypoints(
+ const ProblemData& pData, const double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+
+waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi,
+ double t);
+waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData,
+ const double T,
+ const std::vector<point_t>& pi,
+ double t);
+waypoint_t evaluateAccelerationCurveWaypointAtTime(
+ const ProblemData& pData, const double T, const std::vector<point_t>& pi,
+ double t);
+waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData,
+ const double T,
+ const std::vector<point_t>& pi,
double t);
/**
@@ -115,8 +138,9 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T);
int dimVar(const ProblemData& pData);
-std::pair<MatrixXX, VectorX> computeVelocityCost(const ProblemData& pData, double T,
- std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+std::pair<MatrixXX, VectorX> computeVelocityCost(
+ const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
} // namespace bezier_com_traj
diff --git a/python/bezier_com_traj.cpp b/python/bezier_com_traj.cpp
index 4dc41e0..dff09bd 100644
--- a/python/bezier_com_traj.cpp
+++ b/python/bezier_com_traj.cpp
@@ -1,24 +1,27 @@
-#include <hpp/bezier-com-traj/solve.hh>
-#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
-#include <hpp/bezier-com-traj/solve_end_effector.hh>
-#include <eigenpy/memory.hpp>
-#include <eigenpy/eigenpy.hpp>
-
#include <boost/python.hpp>
#include <boost/python/exception_translator.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include <eigenpy/memory.hpp>
+#include <hpp/bezier-com-traj/solve.hh>
+#include <hpp/bezier-com-traj/solve_end_effector.hh>
+#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ContactData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ProblemData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultData)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultDataCOMTraj)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(
+ bezier_com_traj::ResultDataCOMTraj)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::bezier_t)
namespace bezier_com_traj {
using namespace boost::python;
typedef double real;
-ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, const Vector3& com, const Vector3& dCom,
- const Vector3& l0, const bool useAngMomentum, const double timeDuration,
+ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq,
+ const Vector3& com,
+ const Vector3& dCom, const Vector3& l0,
+ const bool useAngMomentum,
+ const double timeDuration,
const double timeStep) {
bezier_com_traj::ContactData data;
data.contactPhase_ = eq;
@@ -34,11 +37,11 @@ ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, c
return new ResultDataCOMTraj(res);
}
-ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(centroidal_dynamics::Equilibrium* eq, const Vector3& com,
- const Vector3& dCom, const Vector3& l0,
- const bool useAngMomentum, const double timeDuration,
- const double timeStep, const MatrixXX& Kin,
- const MatrixXX& kin) {
+ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(
+ centroidal_dynamics::Equilibrium* eq, const Vector3& com,
+ const Vector3& dCom, const Vector3& l0, const bool useAngMomentum,
+ const double timeDuration, const double timeStep, const MatrixXX& Kin,
+ const MatrixXX& kin) {
bezier_com_traj::ContactData data;
data.Kin_ = Kin;
data.kin_ = kin;
@@ -56,7 +59,9 @@ ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(centroidal_dynamics::
}
struct res_data_exception : std::exception {
- char const* what() const throw() { return "attributes not accessible for false resData"; }
+ char const* what() const throw() {
+ return "attributes not accessible for false resData";
+ }
};
void translate(const res_data_exception& e) {
@@ -65,7 +70,9 @@ void translate(const res_data_exception& e) {
}
struct contact_data_exception : std::exception {
- char const* what() const throw() { return "attribute not defined yet for ContactData"; }
+ char const* what() const throw() {
+ return "attribute not defined yet for ContactData";
+ }
};
void translateContactData(const contact_data_exception& e) {
@@ -83,9 +90,13 @@ double get_costD(const ResultData& res) { return res.cost_; }
bool get_succD(const ResultData& res) { return res.success_; }
-bezier_t* getC_of_t(const ResultDataCOMTraj& res) { return new bezier_t(res.c_of_t_); }
+bezier_t* getC_of_t(const ResultDataCOMTraj& res) {
+ return new bezier_t(res.c_of_t_);
+}
-bezier_t* getDL_of_t(const ResultDataCOMTraj& res) { return new bezier_t(res.dL_of_t_); }
+bezier_t* getDL_of_t(const ResultDataCOMTraj& res) {
+ return new bezier_t(res.dL_of_t_);
+}
VectorX get_x(const ResultDataCOMTraj& res) {
if (res.x.size() > 0) return res.x;
@@ -104,7 +115,9 @@ centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data) {
throw contact_data_exception();
}
-void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq) { data.contactPhase_ = eq; }
+void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq) {
+ data.contactPhase_ = eq;
+}
boost::python::tuple get_Ang(const ContactData& res) {
if (res.ang_.size() == 0) {
@@ -124,7 +137,8 @@ boost::python::tuple get_Kin(const ContactData& res) {
void set_Kin(ContactData& res, const MatrixX3& val, const VectorX& val2) {
if (val2.size() != val.rows()) {
- std::cout << " Kinematic inequality matrix sizes do not match " << std::endl;
+ std::cout << " Kinematic inequality matrix sizes do not match "
+ << std::endl;
throw contact_data_exception();
}
res.Kin_ = val;
@@ -144,14 +158,22 @@ void set_Ang(ContactData& res, const MatrixX3& val, const VectorX& val2) {
/** BEGIN Constraints**/
int get_Flag(const Constraints& res) { return (int)res.flag_; }
-bool get_ConstrainAcc(const Constraints& res) { return res.constraintAcceleration_; }
+bool get_ConstrainAcc(const Constraints& res) {
+ return res.constraintAcceleration_;
+}
double get_MaxAcc(const Constraints& res) { return res.maxAcceleration_; }
double get_ReduceH(const Constraints& res) { return res.reduce_h_; }
-void set_Flag(Constraints& res, const int val) { res.flag_ = (ConstraintFlag)val; }
-void set_ConstrainAcc(Constraints& res, const bool val) { res.constraintAcceleration_ = val; }
+void set_Flag(Constraints& res, const int val) {
+ res.flag_ = (ConstraintFlag)val;
+}
+void set_ConstrainAcc(Constraints& res, const bool val) {
+ res.constraintAcceleration_ = val;
+}
-void set_MaxAcc(Constraints& res, const double val) { res.maxAcceleration_ = val; }
+void set_MaxAcc(Constraints& res, const double val) {
+ res.maxAcceleration_ = val;
+}
void set_ReduceH(Constraints& res, const double val) { res.reduce_h_ = val; }
@@ -181,24 +203,42 @@ void set_dc1_(ProblemData& res, const point_t& val) { res.dc1_ = val; }
void set_ddc1_(ProblemData& res, const point_t& val) { res.ddc1_ = val; }
-bool get_useAngularMomentum_(const ProblemData& res) { return res.useAngularMomentum_; }
-void set_useAngularMomentum_(ProblemData& res, const bool val) { res.useAngularMomentum_ = val; }
+bool get_useAngularMomentum_(const ProblemData& res) {
+ return res.useAngularMomentum_;
+}
+void set_useAngularMomentum_(ProblemData& res, const bool val) {
+ res.useAngularMomentum_ = val;
+}
-CostFunction get_costFunction_(const ProblemData& res) { return res.costFunction_; }
+CostFunction get_costFunction_(const ProblemData& res) {
+ return res.costFunction_;
+}
-void set_costFunction_(ProblemData& res, const CostFunction val) { res.costFunction_ = val; }
+void set_costFunction_(ProblemData& res, const CostFunction val) {
+ res.costFunction_ = val;
+}
-GIWCRepresentation get_GIWC_representation_(const ProblemData& res) { return res.representation_; }
+GIWCRepresentation get_GIWC_representation_(const ProblemData& res) {
+ return res.representation_;
+}
-void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) { res.representation_ = val; }
+void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) {
+ res.representation_ = val;
+}
Constraints* get_constraints_(ProblemData& res) { return &res.constraints_; }
-void set_constraints_(ProblemData& res, const Constraints& val) { res.constraints_ = val; }
+void set_constraints_(ProblemData& res, const Constraints& val) {
+ res.constraints_ = val;
+}
-std::vector<ContactData> get_contacts_(const ProblemData& res) { return res.contacts_; }
+std::vector<ContactData> get_contacts_(const ProblemData& res) {
+ return res.contacts_;
+}
-void addContact(ProblemData& res, const ContactData& val) { res.contacts_.push_back(ContactData(val)); }
+void addContact(ProblemData& res, const ContactData& val) {
+ res.contacts_.push_back(ContactData(val));
+}
void clearContacts(ProblemData& res) { res.contacts_.clear(); }
@@ -206,13 +246,16 @@ void clearContacts(ProblemData& res) { res.contacts_.clear(); }
/** BEGIN computeCOMTraj **/
-ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData, const VectorX& Ts, const double timeStep) {
+ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData,
+ const VectorX& Ts,
+ const double timeStep) {
ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep);
return new ResultDataCOMTraj(res);
}
-ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(const ProblemData& pData, const VectorX& Ts,
- const double timeStep, const solvers::SolverType solver) {
+ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(
+ const ProblemData& pData, const VectorX& Ts, const double timeStep,
+ const solvers::SolverType solver) {
ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep, solver);
return new ResultDataCOMTraj(res);
}
@@ -224,7 +267,7 @@ struct DummyPath {
point3_t operator()(double /*u*/) const { return point3_t::Zero(); }
};
-typedef std::pair<MatrixXX,VectorX> linear_points_t;
+typedef std::pair<MatrixXX, VectorX> linear_points_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
struct MatrixVector {
@@ -233,26 +276,32 @@ struct MatrixVector {
VectorX b() { return res.second; }
};
-ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time) {
- ResultDataCOMTraj res = solveEndEffector<DummyPath>(pData, DummyPath(), time, 0);
+ResultDataCOMTraj* computeEndEffector(const ProblemData& pData,
+ const double time) {
+ ResultDataCOMTraj res =
+ solveEndEffector<DummyPath>(pData, DummyPath(), time, 0);
return new ResultDataCOMTraj(res);
}
-MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData, const double time) {
+MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData,
+ const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
MatrixVector* res = new MatrixVector();
res->res = computeEndEffectorConstraints(pData, time, pi);
return res;
}
-MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, const double time) {
+MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData,
+ const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
MatrixVector* res = new MatrixVector();
res->res = computeVelocityCost(pData, time, pi);
return res;
}
-MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData, const double time, const int numPoints,
+MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData,
+ const double time,
+ const int numPoints,
point_list_t pts_l) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
// transform the matrice 3xN in a std::vector<point3_t> of size N :
@@ -265,12 +314,14 @@ MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData, con
return res;
}
-Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> computeEndEffectorConstantWaypoints(const ProblemData& pData,
- const double time) {
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>
+computeEndEffectorConstantWaypoints(const ProblemData& pData,
+ const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res(3, pi.size());
int col = 0;
- for (std::vector<bezier_t::point_t>::const_iterator cit = pi.begin(); cit != pi.end(); ++cit, ++col)
+ for (std::vector<bezier_t::point_t>::const_iterator cit = pi.begin();
+ cit != pi.end(); ++cit, ++col)
res.block<3, 1>(0, col) = *cit;
return res;
}
@@ -294,8 +345,12 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
eigenpy::exposeQuaternion();*/
class_<ResultDataCOMTraj>("ResultDataCOMTraj", init<>())
- .add_property("c_of_t", make_function(&getC_of_t, return_value_policy<manage_new_object>()))
- .add_property("dL_of_t", make_function(&getDL_of_t, return_value_policy<manage_new_object>()))
+ .add_property(
+ "c_of_t",
+ make_function(&getC_of_t, return_value_policy<manage_new_object>()))
+ .add_property(
+ "dL_of_t",
+ make_function(&getDL_of_t, return_value_policy<manage_new_object>()))
.add_property("success", &get_succ)
.add_property("cost", &get_cost)
.add_property("x", &get_x);
@@ -305,10 +360,15 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.add_property("cost", &get_costD)
.add_property("x", &get_xD);
- class_<ContactData>("ContactData", init<centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1, 2>()])
- .add_property("contactPhase_",
- make_function(&getContactPhase_, return_value_policy<reference_existing_object>()),
- &setContactPhase_)
+ class_<ContactData>(
+ "ContactData",
+ init<
+ centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1, 2>()])
+ .add_property(
+ "contactPhase_",
+ make_function(&getContactPhase_,
+ return_value_policy<reference_existing_object>()),
+ &setContactPhase_)
.add_property("Kin_", &get_Kin)
.add_property("Ang_", &get_Ang)
.def("setKinematicConstraints", &set_Kin)
@@ -321,17 +381,23 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.add_property("c1_", &get_c1_, &set_c1_)
.add_property("dc1_", &get_dc1_, &set_dc1_)
.add_property("ddc1_", &get_ddc1_, &set_ddc1_)
- .add_property("useAngularMomentum_", &get_useAngularMomentum_, &set_useAngularMomentum_)
+ .add_property("useAngularMomentum_", &get_useAngularMomentum_,
+ &set_useAngularMomentum_)
.add_property("costFunction_", &get_costFunction_, &set_costFunction_)
- .add_property("GIWCrepresentation_", &get_GIWC_representation_, &set_GIWC_representation_)
- .add_property("constraints_", make_function(&get_constraints_, return_value_policy<reference_existing_object>()),
- &set_constraints_)
+ .add_property("GIWCrepresentation_", &get_GIWC_representation_,
+ &set_GIWC_representation_)
+ .add_property(
+ "constraints_",
+ make_function(&get_constraints_,
+ return_value_policy<reference_existing_object>()),
+ &set_constraints_)
.def("clearContacts", clearContacts)
.def("addContact", addContact);
class_<Constraints>("Constraints", init<>())
.add_property("flag_", &get_Flag, &set_Flag)
- .add_property("constrainAcceleration_", &get_ConstrainAcc, &set_ConstrainAcc)
+ .add_property("constrainAcceleration_", &get_ConstrainAcc,
+ &set_ConstrainAcc)
.add_property("maxAcceleration_", &get_MaxAcc, &set_MaxAcc)
.add_property("reduce_h_", &get_ReduceH, &set_ReduceH);
@@ -377,17 +443,24 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.def_readonly("A", &MatrixVector::A)
.def_readonly("b", &MatrixVector::b);
- def("zeroStepCapturability", &zeroStepCapturability, return_value_policy<manage_new_object>());
- def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints, return_value_policy<manage_new_object>());
- def("computeCOMTraj", &computeCOMTrajPointer, return_value_policy<manage_new_object>());
- def("computeCOMTraj", &computeCOMTrajPointerChooseSolver, return_value_policy<manage_new_object>());
- def("computeEndEffector", &computeEndEffector, return_value_policy<manage_new_object>());
- def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython, return_value_policy<manage_new_object>());
+ def("zeroStepCapturability", &zeroStepCapturability,
+ return_value_policy<manage_new_object>());
+ def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints,
+ return_value_policy<manage_new_object>());
+ def("computeCOMTraj", &computeCOMTrajPointer,
+ return_value_policy<manage_new_object>());
+ def("computeCOMTraj", &computeCOMTrajPointerChooseSolver,
+ return_value_policy<manage_new_object>());
+ def("computeEndEffector", &computeEndEffector,
+ return_value_policy<manage_new_object>());
+ def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython,
+ return_value_policy<manage_new_object>());
def("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython,
return_value_policy<manage_new_object>());
def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython,
return_value_policy<manage_new_object>());
- def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);
+ def("computeEndEffectorConstantWaypoints",
+ &computeEndEffectorConstantWaypoints);
}
} // namespace bezier_com_traj
diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py
index f21667a..4cee6c7 100644
--- a/python/test/binding_tests.py
+++ b/python/test/binding_tests.py
@@ -2,27 +2,34 @@ import ndcurves # noqa - necessary to register ndcurves::bezier_curve
import numpy as np
from numpy import array
from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP
-from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData,
- computeCOMTraj, zeroStepCapturability)
+from hpp_bezier_com_traj import (
+ SOLVER_QUADPROG,
+ ConstraintFlag,
+ Constraints,
+ ContactData,
+ ProblemData,
+ computeCOMTraj,
+ zeroStepCapturability,
+)
# testing constructors
-eq = Equilibrium("test", 54., 4)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
+eq = Equilibrium("test", 54.0, 4)
+eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
+eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
+eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False)
+eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
+eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
# whether useWarmStart is enable (True by default)
previous = eq.useWarmStart()
# enable warm start in solver (only for QPOases)
eq.setUseWarmStart(False)
-assert (previous != eq.useWarmStart())
+assert previous != eq.useWarmStart()
# access solver name
-assert (eq.getName() == "test")
+assert eq.getName() == "test"
-z = array([0., 0., 1.])
+z = array([0.0, 0.0, 1.0])
P = array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]])
N = array([z for _ in range(4)])
@@ -31,16 +38,16 @@ eq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# eq.setNewContacts(P,N,0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
# setting up optimization problem
-c0 = array([0., 0., 1.])
+c0 = array([0.0, 0.0, 1.0])
# dc0 = array(np.random.uniform(-1, 1, size=3));
-dc0 = array([0.1, 0., 0.])
-l0 = array([0., 0., 0.])
+dc0 = array([0.1, 0.0, 0.0])
+l0 = array([0.0, 0.0, 0.0])
T = 1.2
-tstep = -1.
+tstep = -1.0
a = zeroStepCapturability(eq, c0, dc0, l0, False, T, tstep)
-assert (a.success)
+assert a.success
a.c_of_t(0)
a.dL_of_t(T)
@@ -57,13 +64,14 @@ kin = 10 * np.ones(3)
# a = zeroStepCapturability(eq, c0, dc0, l0, True, T, tstep, Kin, matrix(kin))
# testing contactData
-cData = ContactData(Equilibrium("test", 54., 4))
+cData = ContactData(Equilibrium("test", 54.0, 4))
ceq = cData.contactPhase_
ceq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
-assert cData.contactPhase_.getAlgorithm(
-) == EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP, "modifying ceq should modify cData.contactPhase_"
+assert (
+ cData.contactPhase_.getAlgorithm() == EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
+), "modifying ceq should modify cData.contactPhase_"
-Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+Id = array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
excep = False
try:
@@ -71,12 +79,12 @@ try:
except RuntimeError:
excep = True
assert excep, "[ERROR] No kin assigned should have raised exception"
-cData.setKinematicConstraints(Id, array([0., 0., 1.]))
+cData.setKinematicConstraints(Id, array([0.0, 0.0, 1.0]))
cData.Kin_
excep = False
try:
- cData.setKinematicConstraints(Id, array([0., 0., 0., 1.]))
+ cData.setKinematicConstraints(Id, array([0.0, 0.0, 0.0, 1.0]))
except RuntimeError:
excep = True
assert excep, "[ERROR] Miss matching matrix and vector should raise an error"
@@ -87,12 +95,12 @@ try:
except RuntimeError:
excep = True
assert excep, "[ERROR] No Ang_ assigned should have raised exception"
-cData.setAngularConstraints(Id, array([0., 0., 1.]))
+cData.setAngularConstraints(Id, array([0.0, 0.0, 1.0]))
cData.Ang_
excep = False
try:
- cData.setAngularConstraints(Id, array([0., 0., 0., 1.]))
+ cData.setAngularConstraints(Id, array([0.0, 0.0, 0.0, 1.0]))
except RuntimeError:
excep = True
assert excep, "[ERROR] Missmatching matrix and vector should raise an error"
@@ -103,20 +111,26 @@ old = c.constrainAcceleration_
c.constrainAcceleration_ = not old
assert c.constrainAcceleration_ != old
old = c.flag_
-assert c.flag_ == ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL | ConstraintFlag.END_POS
+assert (
+ c.flag_
+ == ConstraintFlag.INIT_POS
+ | ConstraintFlag.INIT_VEL
+ | ConstraintFlag.END_VEL
+ | ConstraintFlag.END_POS
+)
c.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL
assert c.flag_ != old
old = c.maxAcceleration_
-c.maxAcceleration_ = .235
+c.maxAcceleration_ = 0.235
assert c.maxAcceleration_ != old
old = c.reduce_h_
-c.reduce_h_ = .235
+c.reduce_h_ = 0.235
assert c.reduce_h_ != old
# testing problem data
c = ProblemData()
-nv = array([0., 0., 10.])
+nv = array([0.0, 0.0, 10.0])
old = c.c0_
c.c0_ = nv
assert (c.c0_ != old).any()
@@ -145,12 +159,16 @@ c.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL
assert pD.constraints_.flag_ != old
pD = ProblemData()
-pD.constraints_.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL
+pD.constraints_.flag_ = (
+ ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL
+)
def initContactData(pD):
- cData = ContactData(Equilibrium("test", 54., 4))
- cData.contactPhase_.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
+ cData = ContactData(Equilibrium("test", 54.0, 4))
+ cData.contactPhase_.setNewContacts(
+ P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
+ )
pD.addContact(cData)
diff --git a/python/test/compare_pin_inv_dyn.py b/python/test/compare_pin_inv_dyn.py
index db1aa6a..3d6d7cb 100644
--- a/python/test/compare_pin_inv_dyn.py
+++ b/python/test/compare_pin_inv_dyn.py
@@ -15,9 +15,15 @@ from numpy import cross as X
from numpy import matrix, zeros
from centroidal_dynamics import Equilibrium, EquilibriumAlgorithm
-from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import (BezierZeroStepCapturability,
- compute_CWC)
-from pinocchio_inv_dyn.multi_contact.utils import (check_static_eq, find_static_equilibrium_com, generate_contacts)
+from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import (
+ BezierZeroStepCapturability,
+ compute_CWC,
+)
+from pinocchio_inv_dyn.multi_contact.utils import (
+ check_static_eq,
+ find_static_equilibrium_com,
+ generate_contacts,
+)
from curves import bezier
__EPS = 1e-5
@@ -27,23 +33,27 @@ np.set_printoptions(precision=2, suppress=True, linewidth=100)
#####################
# EQUILIBRIUM CHECK #
#####################
-def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
+def compute_w(
+ c, ddc, dL=array([0.0, 0.0, 0.0]), m=54.0, g_vec=array([0.0, 0.0, -9.81])
+):
w1 = m * (ddc - g_vec)
return array(w1.tolist() + (X(c, w1) + dL).tolist())
-def is_stable(H,
- c=array([0., 0., 0.]),
- ddc=array([0., 0., 0.]),
- dL=array([0., 0., 0.]),
- m=54.,
- g_vec=array([0., 0., -9.81]),
- robustness=0.):
+def is_stable(
+ H,
+ c=array([0.0, 0.0, 0.0]),
+ ddc=array([0.0, 0.0, 0.0]),
+ dL=array([0.0, 0.0, 0.0]),
+ m=54.0,
+ g_vec=array([0.0, 0.0, -9.81]),
+ robustness=0.0,
+):
w = compute_w(c, ddc, dL, m, g_vec)
res = np.max(H.dot(w))
if res > -robustness:
print("offset ", res, dL)
- w = compute_w(c, ddc, array([0., 0., 0.]), m, g_vec)
+ w = compute_w(c, ddc, array([0.0, 0.0, 0.0]), m, g_vec)
print("offset2 ", np.max(H.dot(w)))
return res <= -robustness
@@ -65,25 +75,38 @@ def __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros
return asarray(c_t(t / T)).flatten()
def ddc_tT(t):
- return 1. / (T * T) * asarray(ddc_t(t / T)).flatten()
+ return 1.0 / (T * T) * asarray(ddc_t(t / T)).flatten()
def dL_tT(t):
- return 1. / (T) * asarray(dL(t / T)).flatten()
+ return 1.0 / (T) * asarray(dL(t / T)).flatten()
for i in range(resolution):
t = T * float(i) / float(resolution)
- if not (is_stable(H, c=c_tT(t), ddc=ddc_tT(t), dL=dL_tT(t), m=mass, g_vec=g, robustness=-0.00001)):
+ if not (
+ is_stable(
+ H,
+ c=c_tT(t),
+ ddc=ddc_tT(t),
+ dL=dL_tT(t),
+ m=mass,
+ g_vec=g,
+ robustness=-0.00001,
+ )
+ ):
if t > 0.1:
raise ValueError("trajectory is not stale ! at ", t)
else:
print(
- is_stable(H,
- c=c_tT(t),
- ddc=ddc_tT(t),
- dL=asarray(dL(t / T)).flatten(),
- m=mass,
- g_vec=g,
- robustness=-0.00001))
+ is_stable(
+ H,
+ c=c_tT(t),
+ ddc=ddc_tT(t),
+ dL=asarray(dL(t / T)).flatten(),
+ m=mass,
+ g_vec=g,
+ robustness=-0.00001,
+ )
+ )
print("failed at 0")
@@ -92,7 +115,7 @@ def __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros
###################
-def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0):
+def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver="qpoases", verb=0):
mu = 0.5
# friction coefficient
@@ -114,9 +137,19 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
Y_MARG = 0.07
succeeded = False
- while (not succeeded):
- p, N = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
- RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False)
+ while not succeeded:
+ p, N = generate_contacts(
+ N_CONTACTS,
+ lx,
+ ly,
+ mu,
+ CONTACT_POINT_LOWER_BOUNDS,
+ CONTACT_POINT_UPPER_BOUNDS,
+ RPY_LOWER_BOUNDS,
+ RPY_UPPER_BOUNDS,
+ MIN_CONTACT_DISTANCE,
+ False,
+ )
X_LB = np.min(p[:, 0] - X_MARG)
X_UB = np.max(p[:, 0] + X_MARG)
Y_LB = np.min(p[:, 1] - Y_MARG)
@@ -126,7 +159,9 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
# (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu)
h = zeros(H.shape[0])
- (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h)
+ (succeeded, c0) = find_static_equilibrium_com(
+ mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h
+ )
dc0 = np.random.uniform(-1, 1, size=3)
@@ -136,56 +171,67 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
ineq_kin = zeros(3)
ineq_kin[2] = -Z_MIN
- bezierSolver = BezierZeroStepCapturability("ss",
- c0,
- dc0,
- p,
- N,
- mu,
- g_vector,
- mass,
- verb=verb,
- solver=solver,
- kinematic_constraints=[Ineq_kin, ineq_kin])
+ bezierSolver = BezierZeroStepCapturability(
+ "ss",
+ c0,
+ dc0,
+ p,
+ N,
+ mu,
+ g_vector,
+ mass,
+ verb=verb,
+ solver=solver,
+ kinematic_constraints=[Ineq_kin, ineq_kin],
+ )
eqCpp = Equilibrium("dyn_eq2", mass, 4)
- eqCpp.setNewContacts(asmatrix(p), asmatrix(N), mu, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
- # bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver,
+ eqCpp.setNewContacts(
+ asmatrix(p), asmatrix(N), mu, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
+ )
+ # bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector,
+ # mass, verb=verb, solver=solver,
# kinematic_constraints = None);
- # stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver);
- window_times = [1] + [0.1 * i for i in range(1, 10)] + [0.1 * i for i in range(11, 21)] # try nominal time first
- # window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
+ # stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass,
+ # verb=verb, solver=solver);
+ window_times = (
+ [1] + [0.1 * i for i in range(1, 10)] + [0.1 * i for i in range(11, 21)]
+ ) # try nominal time first
+ # try nominal time first
+ # window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)]
# window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
# window_times = [0.7]
found = False
time_step_check = -0.2
for i, el in enumerate(window_times):
- if (found):
+ if found:
break
res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
- if (res.is_stable):
+ if res.is_stable:
found = True
# print("continuous found at ", el)
- __check_trajectory(bezierSolver._p0,
- bezierSolver._p1,
- res.c,
- res.c,
- el,
- bezierSolver._H,
- bezierSolver._mass,
- bezierSolver._g,
- time_step=time_step_check,
- dL=bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()))
+ __check_trajectory(
+ bezierSolver._p0,
+ bezierSolver._p1,
+ res.c,
+ res.c,
+ el,
+ bezierSolver._H,
+ bezierSolver._mass,
+ bezierSolver._g,
+ time_step=time_step_check,
+ dL=bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()),
+ )
if i != 0:
print("continuous Failed to stop at 1, but managed to stop at ", el)
found = False
time_step_check = 0.05
for i, el in enumerate(window_times):
- if (found):
+ if found:
break
res2 = bezierSolver.can_I_stop(T=el, time_step=time_step_check, l0=zeros(3))
- if (res2.is_stable):
+ if res2.is_stable:
found = True
# print("ang_momentum found at ", el)
__check_trajectory(
@@ -195,11 +241,13 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
res2.c,
el,
bezierSolver._H,
- # bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
+ # bezierSolver._mass, bezierSolver._g, time_step =
+ # time_step_check, dL = res2.dL_of_t)
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
- dL=bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
+ dL=bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()),
+ )
if i != 0:
print("ang_momentum Failed to stop at 1, but managed to stop at ", el)
# res2 = None
@@ -208,8 +256,8 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
# except Exception as e:
# pass
- if (res2.is_stable != res.is_stable):
- if (res.is_stable):
+ if res2.is_stable != res.is_stable:
+ if res.is_stable:
print("continuous won")
else:
print("ang_momentum won")
@@ -218,11 +266,12 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
if __name__ == "__main__":
- g_vector = np.array([0., 0., -9.81])
+ g_vector = np.array([0.0, 0.0, -9.81])
mass = 75
# mass of the robot
from matplotlib import rcParams
- rcParams.update({'font.size': 11})
+
+ rcParams.update({"font.size": 11})
mine_won = 0
mine_lose = 0
total_stop = 0
@@ -236,23 +285,26 @@ if __name__ == "__main__":
import matplotlib.pyplot as plt
- def __plot_3d_points(ax, points, c='b'):
+ def __plot_3d_points(ax, points, c="b"):
xs = [point[0] for point in points]
ys = [point[1] for point in points]
zs = [point[2] for point in points]
- ax.scatter(xs[:1], ys[:1], zs[:1], c='r')
+ ax.scatter(xs[:1], ys[:1], zs[:1], c="r")
ax.scatter(xs[1:-1], ys[1:-1], zs[1:-1], c=c)
- ax.scatter(xs[-1:], ys[-1:], zs[-1:], c='g')
- ax.set_xlabel('X Label', fontsize=11)
- ax.set_ylabel('Y Label', fontsize=11)
- ax.set_zlabel('Z Label', fontsize=11)
+ ax.scatter(xs[-1:], ys[-1:], zs[-1:], c="g")
+ ax.set_xlabel("X Label", fontsize=11)
+ ax.set_ylabel("Y Label", fontsize=11)
+ ax.set_zlabel("Z Label", fontsize=11)
def plot_support_polygon(H, h, p, N, ax, c0):
from pinocchio_inv_dyn.multi_contact.utils import compute_support_polygon
+
# (H,h) = compute_GIWC(p, N, mu);
global mass
global g_vector
- (B_sp, b_sp) = compute_support_polygon(H, h, mass, g_vector, eliminate_redundancies=False)
+ (B_sp, b_sp) = compute_support_polygon(
+ H, h, mass, g_vector, eliminate_redundancies=False
+ )
X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:, 0])
X_MIN -= 0.5 * (X_MAX - X_MIN)
@@ -264,12 +316,27 @@ if __name__ == "__main__":
num_steps = 50
dx = (X_MAX - X_MIN) / float(num_steps)
dy = (Y_MAX - Y_MIN) / float(num_steps)
- # points = [(X_MIN + dx * i, Y_MAX + dy * j, 0.) for i in range(num_steps+1) for j in range(num_steps+1) if
- # check_static_eq(H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j,0.]), g_vector) ]
- # points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, -0.5] for i in range(num_steps+1) for j in
- # range(num_steps+1) if check_static_eq(H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j,0.], g_vector)]
- points = [c0] + [[X_MIN + dx * i, Y_MIN + dy * j, 0] for i in range(num_steps + 1)
- for j in range(num_steps + 1)]
+ # points = [
+ # (X_MIN + dx * i, Y_MAX + dy * j, 0.0)
+ # for i in range(num_steps + 1)
+ # for j in range(num_steps + 1)
+ # if check_static_eq(
+ # H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j, 0.0]), g_vector
+ # )
+ # ]
+ # points = [c0] + [
+ # [X_MIN + dx * i, Y_MIN + dy * j, -0.5]
+ # for i in range(num_steps + 1)
+ # for j in range(num_steps + 1)
+ # if check_static_eq(
+ # H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j, 0.0], g_vector
+ # )
+ # ]
+ points = [c0] + [
+ [X_MIN + dx * i, Y_MIN + dy * j, 0]
+ for i in range(num_steps + 1)
+ for j in range(num_steps + 1)
+ ]
pts2 = []
for pt in points:
if check_static_eq(H, h, mass, pt, g_vector):
@@ -279,8 +346,10 @@ if __name__ == "__main__":
__plot_3d_points(ax, p, c="r")
# for i in range(num_steps):
# for j in range(num_steps):
- # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
- # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
+ # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX],
+ # ax=ax, color='b', lw=4, is_3d=False);
+ # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX],
+ # ax=ax, color='b', lw=4, is_3d=False);
# plt.show();
def plot_win_curve(n=-1, num_pts=20):
@@ -288,7 +357,22 @@ if __name__ == "__main__":
if n > len(curves_when_i_win) - 1 or n < 0:
print("n bigger than num curves or equal to -1, plotting last curve")
n = len(curves_when_i_win) - 1
- c0, dc0, c_end, dc_end, t_max, c_of_t, dc_of_t, ddc_of_t, H, h, p, N, dl_of_t, L_of_t = curves_when_i_win[n]
+ (
+ c0,
+ dc0,
+ c_end,
+ dc_end,
+ t_max,
+ c_of_t,
+ dc_of_t,
+ ddc_of_t,
+ H,
+ h,
+ p,
+ N,
+ dl_of_t,
+ L_of_t,
+ ) = curves_when_i_win[n]
print("c0 ", c0)
print("Is c0 stable ? ", check_static_eq(H, h, mass, c0, g_vector))
print("Is end stable ? ", check_static_eq(H, h, mass, c_of_t(t_max), g_vector))
@@ -296,7 +380,7 @@ if __name__ == "__main__":
w = np.zeros(6)
w[2] = -mass * 9.81
w[3:] = mass * np.cross(c_of_t(t_max), g_vector)
- print('max ', np.max(np.dot(H, w) - h))
+ print("max ", np.max(np.dot(H, w) - h))
X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:, 0])
@@ -311,30 +395,36 @@ if __name__ == "__main__":
delta = t_max / float(num_pts)
num_pts += 1
fig = plt.figure()
- ax = fig.add_subplot(221, projection='3d')
+ ax = fig.add_subplot(221, projection="3d")
# ax = fig.add_subplot(221)
__plot_3d_points(ax, [c_of_t(i * delta) for i in range(num_pts)])
- __plot_3d_points(ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y")
+ __plot_3d_points(
+ ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y"
+ )
plot_support_polygon(H, h, p, N, ax, c0)
- ax = fig.add_subplot(222, projection='3d')
+ ax = fig.add_subplot(222, projection="3d")
__plot_3d_points(ax, [dc_of_t(i * delta) for i in range(num_pts)])
- __plot_3d_points(ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)], c="y")
- ax = fig.add_subplot(223, projection='3d')
+ __plot_3d_points(
+ ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)], c="y"
+ )
+ ax = fig.add_subplot(223, projection="3d")
# __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(224, projection='3d')
__plot_3d_points(ax, [L_of_t(i * delta) for i in range(num_pts)], c="y")
# __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)], c = "y")
- ax = fig.add_subplot(224, projection='3d')
+ ax = fig.add_subplot(224, projection="3d")
__plot_3d_points(ax, [dl_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(121, projection='3d')
# __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(122, projection='3d')
# __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)])
- # print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
+ # print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )
+ # / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
# print("init acceleration ", ddc_of_t(0))
print("init velocity ", dc_of_t(0))
print("end velocity ", dc_of_t(t_max))
- # print("cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0))))
+ # print("cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) )
+ # / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0))))
# plt.show()
@@ -347,21 +437,47 @@ if __name__ == "__main__":
plot_win_curve(i, num_pts)
plt.show()
- num_tested = 0.
+ num_tested = 0.0
for i in range(1000):
num_tested = i - 1
- mine, theirs, r_mine, r_theirs, c0, dc0, H, h, p, N = test_continuous_cpp_vs_continuous_py()
- # mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = test_momentum_cpp_vs_momentum_py()
+ (
+ mine,
+ theirs,
+ r_mine,
+ r_theirs,
+ c0,
+ dc0,
+ H,
+ h,
+ p,
+ N,
+ ) = test_continuous_cpp_vs_continuous_py()
+ # mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = \
+ # test_momentum_cpp_vs_momentum_py()
# print("H test", H.shape)
- if (mine != theirs):
+ if mine != theirs:
total_disagree += 1
- if (mine):
+ if mine:
# times_disagree +=[r_mine.t]
# raise ValueError (" BITE ME " )
margin_i_win_he_lose += [r_theirs.dc]
curves_when_i_win += [
- (c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t, r_mine.dc_of_t,
- r_mine.ddc_of_t, H[:], h[:], p[:], N, r_mine.dL_of_t, r_mine.L_of_t)
+ (
+ c0[:],
+ dc0[:],
+ r_theirs.c[:],
+ r_theirs.dc[:],
+ r_mine.t,
+ r_mine.c_of_t,
+ r_mine.dc_of_t,
+ r_mine.ddc_of_t,
+ H[:],
+ h[:],
+ p[:],
+ N,
+ r_mine.dL_of_t,
+ r_mine.L_of_t,
+ )
]
# print("margin when he lost: ", norm(r_theirs.dc))
# else:
@@ -370,20 +486,25 @@ if __name__ == "__main__":
mine_won += 1
else:
mine_lose += 1
- elif (mine or theirs):
+ elif mine or theirs:
total_stop += 1
# times_agree_stop+=[r_mine.t]
margin_he_wins_i_lost += [r_theirs.ddc_min]
# margin_i_win_he_lose+=[r_theirs.dc]
- # curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t,
+ # curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:],
+ # r_theirs.dc[:], r_mine.t, r_mine.c_of_t,
# r_mine.dc_of_t, r_mine.ddc_of_t, H[:], h[:], p[:], N)]
# print("margin when he wins: ", r_theirs.ddc_min)
else:
total_not_stop += 1
- print("% of stops", 100. * float(total_stop) / num_tested, total_stop)
- print("% of total_disagree", 100. * float(total_disagree) / num_tested, total_disagree)
+ print("% of stops", 100.0 * float(total_stop) / num_tested, total_stop)
+ print(
+ "% of total_disagree",
+ 100.0 * float(total_disagree) / num_tested,
+ total_disagree,
+ )
if total_disagree > 0:
- print("% of wins", 100. * float(mine_won) / total_disagree)
- print("% of lose", 100. * float(mine_lose) / total_disagree)
+ print("% of wins", 100.0 * float(mine_won) / total_disagree)
+ print("% of lose", 100.0 * float(mine_lose) / total_disagree)
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index a74de59..7d534b9 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -5,8 +5,9 @@
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
namespace bezier_com_traj {
-std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6_t>& wps,
- const std::vector<ndcurves::Bern<double> >& berns, int numSteps) {
+std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
+ const std::vector<waypoint6_t>& wps,
+ const std::vector<ndcurves::Bern<double> >& berns, int numSteps) {
double dt = 1. / double(numSteps);
std::vector<waypoint6_t> res;
for (int i = 0; i < numSteps + 1; ++i) {
@@ -21,27 +22,30 @@ std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6
return res;
}
-MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps, const long int extraConstraintSize,
+MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps,
+ const long int extraConstraintSize,
const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
return MatrixXX::Zero(dimH * wps.size() + extraConstraintSize, dimPb);
}
-MatrixXX initMatrixD(const int colG, const std::vector<waypoint6_t>& wps, const long int extraConstraintSize,
+MatrixXX initMatrixD(const int colG, const std::vector<waypoint6_t>& wps,
+ const long int extraConstraintSize,
const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
return MatrixXX::Zero(6 + extraConstraintSize, wps.size() * colG + dimPb);
}
-void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin, Cref_vectorX kin,
- const long int otherConstraintIndex) {
+void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin,
+ Cref_vectorX kin, const long int otherConstraintIndex) {
int dimKin = (int)(kin.rows());
if (dimKin == 0) return;
A.block(A.rows() - dimKin - otherConstraintIndex, 0, dimKin, 3) = Kin;
b.segment(A.rows() - dimKin - otherConstraintIndex, dimKin) = kin;
}
-void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang, Cref_vectorX ang) {
+void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang,
+ Cref_vectorX ang) {
int dimAng = (int)(ang.rows());
if (dimAng > 0) {
A.block(A.rows() - dimAng, 3, dimAng, 3) = Ang;
@@ -50,13 +54,15 @@ void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang, Cref_v
}
int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b) {
- Eigen::Matrix<bool, Eigen::Dynamic, 1> empty = (A.array() == 0).rowwise().all();
+ Eigen::Matrix<bool, Eigen::Dynamic, 1> empty =
+ (A.array() == 0).rowwise().all();
size_t last = A.rows() - 1;
for (size_t i = 0; i < last + 1;) {
if (empty(i)) {
assert(A.row(i).norm() == 0);
if (b(i) < 0) {
- // std::cout << "empty row for A while correponding b is negative. Problem is infeasible" << b(i) << std::endl;
+ // std::cout << "empty row for A while correponding b is negative.
+ // Problem is infeasible" << b(i) << std::endl;
return -1;
}
A.row(i).swap(A.row(last));
@@ -79,16 +85,17 @@ int Normalize(Ref_matrixXX A, Ref_vectorX b) {
return zeroindex;
}
-std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData,
- const std::vector<waypoint6_t>& wps,
- const std::vector<waypoint6_t>& wpL,
- const bool useAngMomentum, bool& fail) {
+std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
+ const ContactData& cData, const std::vector<waypoint6_t>& wps,
+ const std::vector<waypoint6_t>& wpL, const bool useAngMomentum,
+ bool& fail) {
MatrixXX A;
VectorX b;
// gravity vector
const point_t& g = cData.contactPhase_->m_gravity;
// compute GIWC
- assert(cData.contactPhase_->getAlgorithm() == centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
+ assert(cData.contactPhase_->getAlgorithm() ==
+ centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
cData.contactPhase_->getPolytopeInequalities(Hrow, h);
@@ -105,7 +112,8 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
bc.head(3) = g; // constant part of Aub, Aubi = mH * (bc - wsi)
int i = 0;
std::vector<waypoint6_t>::const_iterator wpLcit = wpL.begin();
- for (std::vector<waypoint6_t>::const_iterator wpcit = wps.begin(); wpcit != wps.end(); ++wpcit) {
+ for (std::vector<waypoint6_t>::const_iterator wpcit = wps.begin();
+ wpcit != wps.end(); ++wpcit) {
A.block(i * dimH, 0, dimH, 3) = mH * wpcit->first;
b.segment(i * dimH, dimH) = mH * (bc - wpcit->second);
if (useAngMomentum) {
@@ -132,28 +140,35 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
return std::make_pair(A, b);
}
-ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g,
- Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
- const solvers::SolverType solver) {
- return solvers::solve(A, ci0, D, d, H, g, initGuess, minBounds, maxBounds, solver);
+ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D,
+ Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g,
+ Cref_vectorX initGuess, Cref_vectorX minBounds,
+ Cref_vectorX maxBounds, const solvers::SolverType solver) {
+ return solvers::solve(A, ci0, D, d, H, g, initGuess, minBounds, maxBounds,
+ solver);
}
-ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
+ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
+ Cref_vectorX g, Cref_vectorX initGuess,
const solvers::SolverType solver) {
MatrixXX D = MatrixXX::Zero(0, A.cols());
VectorX d = VectorX::Zero(0);
return solvers::solve(A, b, D, d, H, g, initGuess, d, d, solver);
}
-ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
+ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
+ const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
const solvers::SolverType solver) {
return solve(Ab.first, Ab.second, Hg.first, Hg.second, init, solver);
}
-ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Dd,
- const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds, Cref_vectorX maxBounds,
- const VectorX& init, const solvers::SolverType solver) {
- return solve(Ab.first, Ab.second, Dd.first, Dd.second, Hg.first, Hg.second, init, minBounds, maxBounds, solver);
+ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
+ const std::pair<MatrixXX, VectorX>& Dd,
+ const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
+ Cref_vectorX maxBounds, const VectorX& init,
+ const solvers::SolverType solver) {
+ return solve(Ab.first, Ab.second, Dd.first, Dd.second, Hg.first, Hg.second,
+ init, minBounds, maxBounds, solver);
}
} // namespace bezier_com_traj
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index f8619e4..c89d249 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -3,17 +3,14 @@
* Author: Pierre Fernbach
*/
-#include <hpp/bezier-com-traj/solve.hh>
+#include <algorithm>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
-#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/cost/costfunction_definition.hh>
-
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
-
+#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
-
#include <limits>
-#include <algorithm>
static const int dimVarX = 3;
static const int numRowsForce = 6;
@@ -21,11 +18,14 @@ static const int numRowsForce = 6;
namespace bezier_com_traj {
typedef std::pair<double, point3_t> coefs_t;
-bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData, double T, const T_time& timeArray) {
+bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData,
+ double T,
+ const T_time& timeArray) {
bezier_wp_t::t_point_t wps = computeWwaypoints(pData, T);
bezier_wp_t::t_point_t res;
const int DIM_VAR = (int)dimVar(pData);
- std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size() - 1);
+ std::vector<ndcurves::Bern<double> > berns =
+ ComputeBersteinPolynoms((int)wps.size() - 1);
double t, b;
for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) {
waypoint_t w = initwp(6, DIM_VAR);
@@ -40,23 +40,28 @@ bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData, do
return res;
}
-std::pair<MatrixXX, VectorX> dynamicStabilityConstraints_cross(const MatrixXX& mH, const VectorX& h, const Vector3& g,
- const coefs_t& c, const coefs_t& ddc) {
+std::pair<MatrixXX, VectorX> dynamicStabilityConstraints_cross(
+ const MatrixXX& mH, const VectorX& h, const Vector3& g, const coefs_t& c,
+ const coefs_t& ddc) {
Matrix3 S_hat;
int dimH = (int)(mH.rows());
MatrixXX A(dimH, 4);
VectorX b(dimH);
S_hat = skew(c.second * ddc.first - ddc.second * c.first + g * c.first);
- A.block(0, 0, dimH, 3) = mH.block(0, 3, dimH, 3) * S_hat + mH.block(0, 0, dimH, 3) * ddc.first;
+ A.block(0, 0, dimH, 3) =
+ mH.block(0, 3, dimH, 3) * S_hat + mH.block(0, 0, dimH, 3) * ddc.first;
b = h + mH.block(0, 0, dimH, 3) * (g - ddc.second) +
- mH.block(0, 3, dimH, 3) * (c.second.cross(g) - c.second.cross(ddc.second));
+ mH.block(0, 3, dimH, 3) *
+ (c.second.cross(g) - c.second.cross(ddc.second));
Normalize(A, b);
// add 1 for the slack variable :
A.block(0, 3, dimH, 1) = VectorX::Ones(dimH);
return std::make_pair(A, b);
}
-std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH, const VectorX& h, const Vector3& g,
+std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH,
+ const VectorX& h,
+ const Vector3& g,
const waypoint_t& w) {
int dimH = (int)(mH.rows());
MatrixXX A(dimH, dimVarX);
@@ -69,12 +74,14 @@ std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH, con
return std::make_pair(A, b);
}
-std::vector<int> stepIdPerPhase(const T_time& timeArray) // const int pointsPerPhase)
+std::vector<int> stepIdPerPhase(
+ const T_time& timeArray) // const int pointsPerPhase)
{
std::vector<int> res;
int i = 0;
CIT_time cit = timeArray.begin();
- for (CIT_time cit2 = timeArray.begin() + 1; cit2 != timeArray.end(); ++cit, ++cit2, ++i) {
+ for (CIT_time cit2 = timeArray.begin() + 1; cit2 != timeArray.end();
+ ++cit, ++cit2, ++i) {
if (cit2->second != cit->second) {
res.push_back(i);
}
@@ -83,7 +90,8 @@ std::vector<int> stepIdPerPhase(const T_time& timeArray) // const int pointsPer
return res;
}
-long int computeNumIneq(const ProblemData& pData, const VectorX& Ts, const std::vector<int>& phaseSwitch) {
+long int computeNumIneq(const ProblemData& pData, const VectorX& Ts,
+ const std::vector<int>& phaseSwitch) {
const size_t numPoints = phaseSwitch.back() + 1;
long int num_stab_ineq = 0;
long int num_kin_ineq = 0;
@@ -93,44 +101,56 @@ long int computeNumIneq(const ProblemData& pData, const VectorX& Ts, const std::
VectorX h;
for (int i = 0; i < Ts.size(); ++i) {
pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow, h);
- numStepForPhase = phaseSwitch[i] + 1 - numStepsCumulated; // pointsPerPhase;
+ numStepForPhase =
+ phaseSwitch[i] + 1 - numStepsCumulated; // pointsPerPhase;
numStepsCumulated = phaseSwitch[i] + 1;
- if (i > 0) ++numStepForPhase; // because at the switch point between phases we add the constraints of both phases.
+ if (i > 0)
+ ++numStepForPhase; // because at the switch point between phases we add
+ // the constraints of both phases.
num_stab_ineq += Hrow.rows() * numStepForPhase;
num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
}
long int res = num_stab_ineq + num_kin_ineq;
if (pData.constraints_.constraintAcceleration_)
- res += 2 * 3 *
- (numPoints); // upper and lower bound on acceleration for each discretized waypoint (exept the first one)
+ res +=
+ 2 * 3 * (numPoints); // upper and lower bound on acceleration for each
+ // discretized waypoint (exept the first one)
return res;
}
-void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH, VectorX& h, int& dimH) {
+void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH,
+ VectorX& h, int& dimH) {
VectorX hrow;
centroidal_dynamics::MatrixXX Hrow;
- centroidal_dynamics::LP_status status = phase.contactPhase_->getPolytopeInequalities(Hrow, hrow);
+ centroidal_dynamics::LP_status status =
+ phase.contactPhase_->getPolytopeInequalities(Hrow, hrow);
assert(status == centroidal_dynamics::LP_STATUS_OPTIMAL &&
"Error in centroidal dynamics lib while computing inequalities");
mH = -Hrow * phase.contactPhase_->m_mass;
mH.rowwise().normalize();
h = hrow;
dimH = (int)(mH.rows());
- if (pData.constraints_.reduce_h_ > 0) h -= VectorX::Ones(h.rows()) * pData.constraints_.reduce_h_;
+ if (pData.constraints_.reduce_h_ > 0)
+ h -= VectorX::Ones(h.rows()) * pData.constraints_.reduce_h_;
}
-void assignStabilityConstraintsForTimeStep(MatrixXX& mH, VectorX& h, const waypoint_t& wp_w, const int dimH,
- long int& id_rows, MatrixXX& A, VectorX& b, const Vector3& g) {
- std::pair<MatrixXX, VectorX> Ab_stab = dynamicStabilityConstraints(mH, h, g, wp_w);
+void assignStabilityConstraintsForTimeStep(MatrixXX& mH, VectorX& h,
+ const waypoint_t& wp_w,
+ const int dimH, long int& id_rows,
+ MatrixXX& A, VectorX& b,
+ const Vector3& g) {
+ std::pair<MatrixXX, VectorX> Ab_stab =
+ dynamicStabilityConstraints(mH, h, g, wp_w);
A.block(id_rows, 0, dimH, dimVarX) = Ab_stab.first;
b.segment(id_rows, dimH) = Ab_stab.second;
id_rows += dimH;
}
// mG is -G
-void assignStabilityConstraintsForTimeStepForce(const waypoint_t& wp_w, const long int rowIdx, long int& id_cols,
- const centroidal_dynamics::Matrix6X mG, MatrixXX& D, VectorX& d,
- const double mass, const Vector3& g) {
+void assignStabilityConstraintsForTimeStepForce(
+ const waypoint_t& wp_w, const long int rowIdx, long int& id_cols,
+ const centroidal_dynamics::Matrix6X mG, MatrixXX& D, VectorX& d,
+ const double mass, const Vector3& g) {
D.block(rowIdx, id_cols, numRowsForce, mG.cols()) = mG;
id_cols += mG.cols();
D.block(rowIdx, 0, numRowsForce, dimVarX) = mass * wp_w.first;
@@ -139,19 +159,26 @@ void assignStabilityConstraintsForTimeStepForce(const waypoint_t& wp_w, const lo
d.segment(rowIdx, numRowsForce) = mass * (g_ - wp_w.second);
}
-void switchContactPhase(const ProblemData& pData, MatrixXX& A, VectorX& b, MatrixXX& mH, VectorX& h,
- const waypoint_t& wp_w, const ContactData& phase, long int& id_rows, int& dimH) {
+void switchContactPhase(const ProblemData& pData, MatrixXX& A, VectorX& b,
+ MatrixXX& mH, VectorX& h, const waypoint_t& wp_w,
+ const ContactData& phase, long int& id_rows,
+ int& dimH) {
updateH(pData, phase, mH, h, dimH);
- // the current waypoint must have the constraints of both phases. So we add it again :
+ // the current waypoint must have the constraints of both phases. So we add it
+ // again :
// TODO : filter for redunbdant constraints ...
// add stability constraints :
- assignStabilityConstraintsForTimeStep(mH, h, wp_w, dimH, id_rows, A, b, phase.contactPhase_->m_gravity);
+ assignStabilityConstraintsForTimeStep(mH, h, wp_w, dimH, id_rows, A, b,
+ phase.contactPhase_->m_gravity);
}
-long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
- const double t_total, const std::vector<int>& stepIdForPhase) {
+long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A,
+ VectorX& b, const T_time& timeArray,
+ const double t_total,
+ const std::vector<int>& stepIdForPhase) {
long int id_rows = 0;
- bezier_wp_t::t_point_t wps_w = computeDiscretizedWwaypoints(pData, t_total, timeArray);
+ bezier_wp_t::t_point_t wps_w =
+ computeDiscretizedWwaypoints(pData, t_total, timeArray);
std::size_t id_phase = 0;
const Vector3& g = pData.contacts_[id_phase].contactPhase_->m_gravity;
@@ -166,23 +193,29 @@ long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A, Vecto
// we don't consider the first point, because it's independant of x.
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
// add stability constraints :
- assignStabilityConstraintsForTimeStep(mH, h, wps_w[id_step], dimH, id_rows, A, b, g);
+ assignStabilityConstraintsForTimeStep(mH, h, wps_w[id_step], dimH, id_rows,
+ A, b, g);
// check if we are going to switch phases :
- for (std::vector<int>::const_iterator it_switch = stepIdForPhase.begin(); it_switch != (stepIdForPhase.end() - 1);
- ++it_switch) {
+ for (std::vector<int>::const_iterator it_switch = stepIdForPhase.begin();
+ it_switch != (stepIdForPhase.end() - 1); ++it_switch) {
if ((int)id_step == (*it_switch)) {
id_phase++;
- switchContactPhase(pData, A, b, mH, h, wps_w[id_step], pData.contacts_[id_phase], id_rows, dimH);
+ switchContactPhase(pData, A, b, mH, h, wps_w[id_step],
+ pData.contacts_[id_phase], id_rows, dimH);
}
}
}
return id_rows;
}
-void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
- const double t_total, const std::vector<int>& stepIdForPhase, long int& id_rows) {
+void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A,
+ VectorX& b, const T_time& timeArray,
+ const double t_total,
+ const std::vector<int>& stepIdForPhase,
+ long int& id_rows) {
std::size_t id_phase = 0;
- std::vector<coefs_t> wps_c = computeDiscretizedWaypoints<point_t>(pData, t_total, timeArray);
+ std::vector<coefs_t> wps_c =
+ computeDiscretizedWaypoints<point_t>(pData, t_total, timeArray);
long int current_size;
id_phase = 0;
// assign the Kinematics constraints for each discretized waypoints :
@@ -190,12 +223,15 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
// add constraints for wp id_step, on current phase :
// add kinematics constraints :
- // constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <= b - As
+ // constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <=
+ // b - As
current_size = (int)pData.contacts_[id_phase].kin_.rows();
if (current_size > 0) {
- A.block(id_rows, 0, current_size, 3) = (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
+ A.block(id_rows, 0, current_size, 3) =
+ (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
b.segment(id_rows, current_size) =
- pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
+ pData.contacts_[id_phase].kin_ -
+ (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
id_rows += current_size;
}
@@ -204,13 +240,16 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
if (id_step == (std::size_t)stepIdForPhase[i]) {
// switch to phase i
id_phase = i + 1;
- // the current waypoint must have the constraints of both phases. So we add it again :
+ // the current waypoint must have the constraints of both phases. So we
+ // add it again :
// TODO : filter for redunbdant constraints ...
current_size = pData.contacts_[id_phase].kin_.rows();
if (current_size > 0) {
- A.block(id_rows, 0, current_size, 3) = (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
+ A.block(id_rows, 0, current_size, 3) =
+ (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
b.segment(id_rows, current_size) =
- pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
+ pData.contacts_[id_phase].kin_ -
+ (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
id_rows += current_size;
}
}
@@ -218,63 +257,82 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
}
}
-void assignAccelerationConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
+void assignAccelerationConstraints(const ProblemData& pData, MatrixXX& A,
+ VectorX& b, const T_time& timeArray,
const double t_total, long int& id_rows) {
if (pData.constraints_
- .constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
- std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints<point_t>(pData, t_total, timeArray);
+ .constraintAcceleration_) { // assign the acceleration constraints
+ // for each discretized waypoints :
+ std::vector<coefs_t> wps_ddc =
+ computeDiscretizedAccelerationWaypoints<point_t>(pData, t_total,
+ timeArray);
Vector3 acc_bounds = Vector3::Ones() * pData.constraints_.maxAcceleration_;
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
- A.block(id_rows, 0, 3, 3) = Matrix3::Identity() * wps_ddc[id_step].first; // upper
+ A.block(id_rows, 0, 3, 3) =
+ Matrix3::Identity() * wps_ddc[id_step].first; // upper
b.segment(id_rows, 3) = acc_bounds - wps_ddc[id_step].second;
- A.block(id_rows + 3, 0, 3, 3) = -Matrix3::Identity() * wps_ddc[id_step].first; // lower
+ A.block(id_rows + 3, 0, 3, 3) =
+ -Matrix3::Identity() * wps_ddc[id_step].first; // lower
b.segment(id_rows + 3, 3) = acc_bounds + wps_ddc[id_step].second;
id_rows += 6;
}
}
}
-std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData, const VectorX& Ts,
- const double t_total, const T_time& timeArray) {
+std::pair<MatrixXX, VectorX> computeConstraintsOneStep(
+ const ProblemData& pData, const VectorX& Ts, const double t_total,
+ const T_time& timeArray) {
// Compute all the discretized wayPoint
std::vector<int> stepIdForPhase = stepIdPerPhase(timeArray);
- // stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
+ // stepIdForPhase[i] is the id of the last step of phase i / first step of
+ // phase i+1 (overlap)
// init constraints matrix :
long int num_ineq = computeNumIneq(pData, Ts, stepIdForPhase);
MatrixXX A = MatrixXX::Zero(num_ineq, dimVarX);
VectorX b(num_ineq);
- // assign dynamic and kinematic constraints per timestep, including additional acceleration
- // constraints.
- long int id_rows = assignStabilityConstraints(pData, A, b, timeArray, t_total, stepIdForPhase);
- assignKinematicConstraints(pData, A, b, timeArray, t_total, stepIdForPhase, id_rows);
+ // assign dynamic and kinematic constraints per timestep, including additional
+ // acceleration constraints.
+ long int id_rows = assignStabilityConstraints(pData, A, b, timeArray, t_total,
+ stepIdForPhase);
+ assignKinematicConstraints(pData, A, b, timeArray, t_total, stepIdForPhase,
+ id_rows);
assignAccelerationConstraints(pData, A, b, timeArray, t_total, id_rows);
- assert(id_rows == (A.rows()) && "The constraints matrices were not fully filled.");
+ assert(id_rows == (A.rows()) &&
+ "The constraints matrices were not fully filled.");
return std::make_pair(A, b);
}
-void computeFinalAcceleration(ResultDataCOMTraj& res) { res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2); }
+void computeFinalAcceleration(ResultDataCOMTraj& res) {
+ res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2);
+}
-void computeFinalVelocity(ResultDataCOMTraj& res) { res.dc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 1); }
+void computeFinalVelocity(ResultDataCOMTraj& res) {
+ res.dc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 1);
+}
-std::pair<MatrixXX, VectorX> genCostFunction(const ProblemData& pData, const VectorX& Ts, const double T,
- const T_time& timeArray, const long int& dim) {
+std::pair<MatrixXX, VectorX> genCostFunction(const ProblemData& pData,
+ const VectorX& Ts, const double T,
+ const T_time& timeArray,
+ const long int& dim) {
MatrixXX H = MatrixXX::Identity(dim, dim) * 1e-6;
VectorX g = VectorX::Zero(dim);
cost::genCostFunction(pData, Ts, T, timeArray, H, g);
return std::make_pair(H, g);
}
-ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const double T) {
+ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData,
+ const double T) {
ResultDataCOMTraj res;
if (resQp.success_) {
res.success_ = true;
res.x = resQp.x.head<3>();
res.cost_ = resQp.cost_;
std::vector<Vector3> pis = computeConstantWaypoints(pData, T);
- res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(pData.constraints_.flag_, T, pis, res.x);
+ res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(
+ pData.constraints_.flag_, T, pis, res.x);
computeFinalVelocity(res);
computeFinalAcceleration(res);
res.dL_of_t_ = bezier_t::zero(3, T);
@@ -283,21 +341,25 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub
}
/**
- * @brief computeNumIneqContinuous compute the number of inequalitie required by all the phases
+ * @brief computeNumIneqContinuous compute the number of inequalitie required by
+ * all the phases
* @param pData
* @param Ts
* @param degree
- * @param w_degree //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf
- * waypoints_c0_dc0_dc1 )
- * @param useDD whether double description or force formulation is used to compute dynamic constraints)
+ * @param w_degree //FIXME : cannot use 2n+3 because for capturability the
+ * degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
+ * @param useDD whether double description or force formulation is used to
+ * compute dynamic constraints)
* @return
*/
-long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts, const int degree, const int w_degree,
+long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts,
+ const int degree, const int w_degree,
const bool useDD) {
long int num_ineq = 0;
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
- for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin(); it != pData.contacts_.end(); ++it) {
+ for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
+ it != pData.contacts_.end(); ++it) {
// kinematics :
num_ineq += it->kin_.rows() * (degree + 1);
// stability :
@@ -312,28 +374,34 @@ long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts, c
}
/**
- * @brief computeNumEqContinuous compute the number of equalities required by all the phases
+ * @brief computeNumEqContinuous compute the number of equalities required by
+ * all the phases
* @param pData
- * @param w_degree //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf
- * waypoints_c0_dc0_dc1 )
- * @param useForce whether double description or force formulation is used to compute dynamic constraints)
+ * @param w_degree //FIXME : cannot use 2n+3 because for capturability the
+ * degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
+ * @param useForce whether double description or force formulation is used to
+ * compute dynamic constraints)
* @param forceVarDim reference value that stores the total force variable size
* @return
*/
-long int computeNumEqContinuous(const ProblemData& pData, const int w_degree, const bool useForce,
- long int& forceVarDim) {
+long int computeNumEqContinuous(const ProblemData& pData, const int w_degree,
+ const bool useForce, long int& forceVarDim) {
long int numEq = 0;
if (useForce) {
long int cSize = 0;
- int currentDegree; // remove redundant equalities depending on more constraining problem
+ int currentDegree; // remove redundant equalities depending on more
+ // constraining problem
std::vector<ContactData>::const_iterator it2 = pData.contacts_.begin();
++it2;
- for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin(); it != pData.contacts_.end();
- ++it, ++it2) {
+ for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
+ it != pData.contacts_.end(); ++it, ++it2) {
currentDegree = w_degree + 1;
- if (cSize != 0 && it->contactPhase_->m_G_centr.cols() > cSize) currentDegree -= 1;
+ if (cSize != 0 && it->contactPhase_->m_G_centr.cols() > cSize)
+ currentDegree -= 1;
cSize = it->contactPhase_->m_G_centr.cols();
- if (it2 != pData.contacts_.end() && it2->contactPhase_->m_G_centr.cols() <= cSize) currentDegree -= 1;
+ if (it2 != pData.contacts_.end() &&
+ it2->contactPhase_->m_G_centr.cols() <= cSize)
+ currentDegree -= 1;
forceVarDim += cSize * (currentDegree);
numEq += (numRowsForce) * (currentDegree);
}
@@ -341,13 +409,16 @@ long int computeNumEqContinuous(const ProblemData& pData, const int w_degree, co
return numEq;
}
-std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pData, const VectorX& Ts,
- std::pair<MatrixXX, VectorX>& Dd, VectorX& minBounds,
- VectorX& /*maxBounds*/) {
+std::pair<MatrixXX, VectorX> computeConstraintsContinuous(
+ const ProblemData& pData, const VectorX& Ts,
+ std::pair<MatrixXX, VectorX>& Dd, VectorX& minBounds,
+ VectorX& /*maxBounds*/) {
// determine whether to use force or double description
// based on equilibrium value
- bool useDD = pData.representation_ == DOUBLE_DESCRIPTION; //!(pData.contacts_.begin()->contactPhase_->getAlgorithm()
- //!== centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
+ bool useDD =
+ pData.representation_ ==
+ DOUBLE_DESCRIPTION; //!(pData.contacts_.begin()->contactPhase_->getAlgorithm()
+ //!== centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
double T = Ts.sum();
@@ -359,9 +430,11 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
bezier_wp_t w(wps_w.begin(), wps_w.end(), 0., T);
// for each splitted curves : add the constraints for each waypoints
- const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_, (int)w.degree_, useDD);
+ const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_,
+ (int)w.degree_, useDD);
long int forceVarDim = 0;
- const long int num_eq = computeNumEqContinuous(pData, (int)w.degree_, !useDD, forceVarDim);
+ const long int num_eq =
+ computeNumEqContinuous(pData, (int)w.degree_, !useDD, forceVarDim);
long int totalVarDim = dimVarX + forceVarDim;
long int id_rows = 0;
long int id_cols = dimVarX; // start after x variable
@@ -383,7 +456,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
long int cSize = 0;
long int rowIdx = 0;
- // for each phases, split the curve and compute the waypoints of the splitted curves
+ // for each phases, split the curve and compute the waypoints of the splitted
+ // curves
for (size_t id_phase = 0; id_phase < (size_t)Ts.size(); ++id_phase) {
bezier_wp_t cs = c.extract(current_t, Ts[id_phase] + current_t);
bezier_wp_t ddcs = ddc.extract(current_t, Ts[id_phase] + current_t);
@@ -394,7 +468,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add kinematics constraints :
size_kin = (int)phase.kin_.rows();
- for (bezier_wp_t::cit_point_t wpit = cs.waypoints().begin(); wpit != cs.waypoints().end(); ++wpit) {
+ for (bezier_wp_t::cit_point_t wpit = cs.waypoints().begin();
+ wpit != cs.waypoints().end(); ++wpit) {
A.block(id_rows, 0, size_kin, 3) = (phase.Kin_ * wpit->first);
b.segment(id_rows, size_kin) = phase.kin_ - (phase.Kin_ * wpit->second);
id_rows += size_kin;
@@ -402,22 +477,28 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add stability constraints
if (useDD) {
updateH(pData, phase, mH, h, dimH);
- for (bezier_wp_t::cit_point_t wpit = ws.waypoints().begin(); wpit != ws.waypoints().end(); ++wpit)
- assignStabilityConstraintsForTimeStep(mH, h, *wpit, dimH, id_rows, A, b, phase.contactPhase_->m_gravity);
+ for (bezier_wp_t::cit_point_t wpit = ws.waypoints().begin();
+ wpit != ws.waypoints().end(); ++wpit)
+ assignStabilityConstraintsForTimeStep(mH, h, *wpit, dimH, id_rows, A, b,
+ phase.contactPhase_->m_gravity);
} else {
bezier_wp_t::cit_point_t start = ws.waypoints().begin();
bezier_wp_t::cit_point_t stop = ws.waypoints().end();
if (id_phase + 1 < (size_t)Ts.size() &&
- pData.contacts_[id_phase + 1].contactPhase_->m_G_centr.cols() <= phase.contactPhase_->m_G_centr.cols())
+ pData.contacts_[id_phase + 1].contactPhase_->m_G_centr.cols() <=
+ phase.contactPhase_->m_G_centr.cols())
--stop;
if (cSize > 0 && cSize < phase.contactPhase_->m_G_centr.cols()) ++start;
cSize = phase.contactPhase_->m_G_centr.cols();
- for (bezier_wp_t::cit_point_t wpit = start; wpit != stop; ++wpit, rowIdx += 6)
- assignStabilityConstraintsForTimeStepForce(*wpit, rowIdx, id_cols, phase.contactPhase_->m_G_centr, D, d,
- phase.contactPhase_->m_mass, phase.contactPhase_->m_gravity);
+ for (bezier_wp_t::cit_point_t wpit = start; wpit != stop;
+ ++wpit, rowIdx += 6)
+ assignStabilityConstraintsForTimeStepForce(
+ *wpit, rowIdx, id_cols, phase.contactPhase_->m_G_centr, D, d,
+ phase.contactPhase_->m_mass, phase.contactPhase_->m_gravity);
}
// add acceleration constraints :
- for (bezier_wp_t::cit_point_t wpit = ddcs.waypoints().begin(); wpit != ddcs.waypoints().end(); ++wpit) {
+ for (bezier_wp_t::cit_point_t wpit = ddcs.waypoints().begin();
+ wpit != ddcs.waypoints().end(); ++wpit) {
A.block(id_rows, 0, 3, 3) = wpit->first; // upper
b.segment(id_rows, 3) = acc_bounds - wpit->second;
A.block(id_rows + 3, 0, 3, 3) = -wpit->first; // lower
@@ -429,27 +510,37 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add positive constraints on forces
// A.block(id_rows,3,forceVarDim,forceVarDim)=MatrixXX::Identity(forceVarDim,forceVarDim)*(-1);
// id_rows += forceVarDim;
- minBounds = VectorX::Zero(A.cols()); // * (-std::numeric_limits<double>::infinity());
+ minBounds = VectorX::Zero(
+ A.cols()); // * (-std::numeric_limits<double>::infinity());
minBounds.head<3>() = VectorX::Ones(3) * (solvers::UNBOUNDED_DOWN);
minBounds.tail(forceVarDim) = VectorX::Zero(forceVarDim);
}
- assert(id_rows == (A.rows()) && "The inequality constraints matrices were not fully filled.");
- assert(id_rows == (b.rows()) && "The inequality constraints matrices were not fully filled.");
- assert(id_cols == (D.cols()) && "The equality constraints matrices were not fully filled.");
- assert(rowIdx == (d.rows()) && "The equality constraints matrices were not fully filled.");
+ assert(id_rows == (A.rows()) &&
+ "The inequality constraints matrices were not fully filled.");
+ assert(id_rows == (b.rows()) &&
+ "The inequality constraints matrices were not fully filled.");
+ assert(id_cols == (D.cols()) &&
+ "The equality constraints matrices were not fully filled.");
+ assert(rowIdx == (d.rows()) &&
+ "The equality constraints matrices were not fully filled.");
// int out = Normalize(D,d);
return std::make_pair(A, b);
}
-ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const VectorX& Ts,
+ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData,
+ const VectorX& Ts,
const unsigned int pointsPerPhase) {
assert(pData.representation_ == DOUBLE_DESCRIPTION);
if (Ts.size() != (int)pData.contacts_.size())
- throw std::runtime_error("Time phase vector has different size than the number of contact phases");
+ throw std::runtime_error(
+ "Time phase vector has different size than the number of contact "
+ "phases");
double T = Ts.sum();
T_time timeArray = computeDiscretizedTimeFixed(Ts, pointsPerPhase);
- std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData, Ts, T, timeArray);
- std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData, Ts, T, timeArray, dimVarX);
+ std::pair<MatrixXX, VectorX> Ab =
+ computeConstraintsOneStep(pData, Ts, T, timeArray);
+ std::pair<MatrixXX, VectorX> Hg =
+ genCostFunction(pData, Ts, T, timeArray, dimVarX);
VectorX x = VectorX::Zero(dimVarX);
ResultData resQp = solve(Ab, Hg, x);
#if PRINT_QHULL_INEQ
@@ -458,10 +549,13 @@ ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const Vector
return genTraj(resQp, pData, T);
}
-ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, const double timeStep,
+ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
+ const double timeStep,
const solvers::SolverType solver) {
if (Ts.size() != (int)pData.contacts_.size())
- throw std::runtime_error("Time phase vector has different size than the number of contact phases");
+ throw std::runtime_error(
+ "Time phase vector has different size than the number of contact "
+ "phases");
double T = Ts.sum();
T_time timeArray;
std::pair<MatrixXX, VectorX> Ab;
@@ -476,10 +570,12 @@ ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, co
maxBounds = VectorX::Zero(0);
} else { // continuous
Ab = computeConstraintsContinuous(pData, Ts, Dd, minBounds, maxBounds);
- timeArray = computeDiscretizedTimeFixed(Ts, 7); // FIXME : hardcoded value for discretization for cost function in
- // case of continuous formulation for the constraints
+ timeArray = computeDiscretizedTimeFixed(
+ Ts, 7); // FIXME : hardcoded value for discretization for cost function
+ // in case of continuous formulation for the constraints
}
- std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData, Ts, T, timeArray, Ab.first.cols());
+ std::pair<MatrixXX, VectorX> Hg =
+ genCostFunction(pData, Ts, T, timeArray, Ab.first.cols());
VectorX x = VectorX::Zero(Ab.first.cols());
ResultData resQp = solve(Ab, Dd, Hg, minBounds, maxBounds, x, solver);
#if PRINT_QHULL_INEQ
diff --git a/src/costfunction_definition.cpp b/src/costfunction_definition.cpp
index 2c16720..de97f85 100644
--- a/src/costfunction_definition.cpp
+++ b/src/costfunction_definition.cpp
@@ -3,52 +3,65 @@
* Author: Steve Tonneau
*/
+#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/cost/costfunction_definition.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
-#include <hpp/bezier-com-traj/common_solve_methods.hh>
namespace bezier_com_traj {
namespace cost {
// cost : min distance between x and midPoint :
-void computeCostMidPoint(const ProblemData& pData, const VectorX& /*Ts*/, const double /*T*/,
- const T_time& /*timeArray*/, MatrixXX& H, VectorX& g) {
+void computeCostMidPoint(const ProblemData& pData, const VectorX& /*Ts*/,
+ const double /*T*/, const T_time& /*timeArray*/,
+ MatrixXX& H, VectorX& g) {
// cost : x' H x + 2 x g'
- Vector3 midPoint = (pData.c0_ + pData.c1_) / 2.; // todo : replace it with point found by planning ??
+ Vector3 midPoint = (pData.c0_ + pData.c1_) /
+ 2.; // todo : replace it with point found by planning ??
H.block<3, 3>(0, 0) = Matrix3::Identity();
g.head<3>() = -midPoint;
}
// cost : min distance between end velocity and the one computed by planning
-void computeCostEndVelocity(const ProblemData& pData, const VectorX& /*Ts*/, const double T,
- const T_time& /*timeArray*/, MatrixXX& H, VectorX& g) {
+void computeCostEndVelocity(const ProblemData& pData, const VectorX& /*Ts*/,
+ const double T, const T_time& /*timeArray*/,
+ MatrixXX& H, VectorX& g) {
if (pData.constraints_.flag_ && END_VEL)
- throw std::runtime_error("Can't use computeCostEndVelocity as cost function when end velocity is a contraint");
+ throw std::runtime_error(
+ "Can't use computeCostEndVelocity as cost function when end velocity "
+ "is a contraint");
coefs_t v = computeFinalVelocityPoint(pData, T);
H.block<3, 3>(0, 0) = Matrix3::Identity() * v.first * v.first;
g.head<3>() = v.first * (v.second - pData.dc1_);
}
// TODO this is temporary.The acceleration integral can be computed analitcally
-void computeCostMinAcceleration(const ProblemData& pData, const VectorX& Ts, const double /*T*/,
- const T_time& timeArray, MatrixXX& H, VectorX& g) {
+void computeCostMinAcceleration(const ProblemData& pData, const VectorX& Ts,
+ const double /*T*/, const T_time& timeArray,
+ MatrixXX& H, VectorX& g) {
double t_total = 0.;
for (int i = 0; i < Ts.size(); ++i) t_total += Ts[i];
- std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints<point3_t>(pData, t_total, timeArray);
+ std::vector<coefs_t> wps_ddc =
+ computeDiscretizedAccelerationWaypoints<point3_t>(pData, t_total,
+ timeArray);
// cost : x' H x + 2 x g'
H.block<3, 3>(0, 0) = Matrix3::Zero();
g.head<3>() = Vector3::Zero();
for (size_t i = 0; i < wps_ddc.size(); ++i) {
- H.block<3, 3>(0, 0) += Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first;
+ H.block<3, 3>(0, 0) +=
+ Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first;
g.head<3>() += wps_ddc[i].first * wps_ddc[i].second;
}
}
-typedef void (*costCompute)(const ProblemData&, const VectorX&, const double, const T_time&, MatrixXX&, VectorX&);
+typedef void (*costCompute)(const ProblemData&, const VectorX&, const double,
+ const T_time&, MatrixXX&, VectorX&);
typedef std::map<CostFunction, costCompute> T_costCompute;
-static const T_costCompute costs = boost::assign::map_list_of(ACCELERATION, computeCostMinAcceleration)(
- DISTANCE_TRAVELED, computeCostMidPoint)(TARGET_END_VELOCITY, computeCostEndVelocity);
+static const T_costCompute costs =
+ boost::assign::map_list_of(ACCELERATION, computeCostMinAcceleration)(
+ DISTANCE_TRAVELED, computeCostMidPoint)(TARGET_END_VELOCITY,
+ computeCostEndVelocity);
-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) {
T_costCompute::const_iterator cit = costs.find(pData.costFunction_);
if (cit != costs.end())
diff --git a/src/eiquadprog-fast.cpp b/src/eiquadprog-fast.cpp
index d19de1d..7871fd5 100644
--- a/src/eiquadprog-fast.cpp
+++ b/src/eiquadprog-fast.cpp
@@ -16,6 +16,7 @@
//
#include "hpp/bezier-com-traj/solver/eiquadprog-fast.hpp"
+
#include <iostream>
namespace tsid {
namespace solvers {
@@ -38,7 +39,8 @@ inline Scalar distance(Scalar a, Scalar b) {
EiquadprogFast::EiquadprogFast() {
m_maxIter = DEFAULT_MAX_ITER;
- q = 0; // size of the active set A (containing the indices of the active constraints)
+ q = 0; // size of the active set A (containing the indices of the active
+ // constraints)
is_inverse_provided_ = false;
m_nVars = 0;
m_nEqCon = 0;
@@ -72,7 +74,8 @@ void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon) {
#endif
}
-bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm) {
+bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
+ int& iq, double& R_norm) {
long int nVars = J.rows();
#ifdef TRACE_SOLVER
std::cout << "Add constraint " << iq << '/';
@@ -90,13 +93,12 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int&
decreasing j */
for (j = d.size() - 1; j >= iq + 1; j--) {
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
- If cc is one, then element (j) of d is zero compared with element
- (j - 1). Hence we don't have to do anything.
- If cc is zero, then we just have to switch column (j) and column (j - 1)
- of J. Since we only switch columns in J, we have to be careful how we
- update d depending on the sign of gs.
- Otherwise we have to apply the Givens rotation to these columns.
- The i - 1 element of d has to be updated to h. */
+ If cc is one, then element (j) of d is zero compared with
+ element (j - 1). Hence we don't have to do anything. If cc is zero, then
+ we just have to switch column (j) and column (j - 1) of J. Since we only
+ switch columns in J, we have to be careful how we update d depending on
+ the sign of gs. Otherwise we have to apply the Givens rotation to these
+ columns. The i - 1 element of d has to be updated to h. */
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
@@ -113,7 +115,8 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int&
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
-#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original
+#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
+ // the original
T1 = J.col(j - 1);
cc_ss(0) = cc;
cc_ss(1) = ss;
@@ -146,7 +149,8 @@ into column iq - 1 of R
return true;
}
-void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int nEqCon, int& iq,
+void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
+ VectorXd& u, int nEqCon, int& iq,
int l) {
const long int nVars = R.rows();
#ifdef TRACE_SOLVER
@@ -223,14 +227,15 @@ void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cout << name << std::endl << x << std::endl;
}
-EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
- const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
- VectorXd& x) {
+EiquadprogFast_status EiquadprogFast::solve_quadprog(
+ const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
+ const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
const int nVars = (int)g0.size();
const int nEqCon = (int)ce0.size();
const int nIneqCon = (int)ci0.size();
- if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
+ if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
+ reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
assert(g0.size() == m_nVars);
@@ -272,7 +277,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
R.setZero(nVars, nVars);
R_norm = 1.0;
- /* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
+ /* compute the inverse of the factorized matrix Hess^-1, this is the initial
+ * value for H */
// m_J = L^-T
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
@@ -293,9 +299,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
/* c1 * c2 is an estimate for cond(Hess) */
/*
- * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
- * this is a feasible point in the dual space
- * x = Hess^-1 * g0
+ * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
+ * x this is a feasible point in the dual space x = Hess^-1 * g0
*/
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
if (is_inverse_provided_) {
@@ -336,10 +341,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
print_vector("d", d, nVars);
#endif
- /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
- becomes feasible */
+ /* compute full step length t2: i.e., the minimum step in primal space s.t.
+ the contraint becomes feasible */
t2 = 0.0;
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
@@ -366,7 +372,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
for (i = 0; i < nIneqCon; i++) iai(i) = i;
#ifdef USE_WARM_START
- // DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
+ // DEBUG_STREAM("Gonna warm start using previous active
+ // set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
@@ -375,10 +382,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
- /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
- becomes feasible */
+ /* compute full step length t2: i.e., the minimum step in primal space s.t.
+ the contraint becomes feasible */
t2 = 0.0;
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
else
DEBUG_STREAM("[WARM START] z=0\n")
@@ -445,10 +453,12 @@ l1:
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
- if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
+ if (std::abs(psi) <=
+ nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
/* numerically there are not infeasibilities anymore */
q = iq;
- // DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
+ // DEBUG_STREAM("Optimal active
+ // set:\n"<<A.head(iq).transpose()<<"\n\n")
return EIQUADPROG_FAST_OPTIMAL;
}
@@ -459,7 +469,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
- // find constraint with highest violation (what about normalizing constraints?)
+ // find constraint with highest violation (what about normalizing
+ // constraints?)
for (i = 0; i < nIneqCon; i++) {
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
ss = s(i);
@@ -489,7 +500,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
- /* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
+ /* compute z = H np: the step direction in the primal space (through m_J, see
+ * the paper) */
compute_d(d, m_J, np);
// update_z(z, m_J, d, iq);
if (iq >= nVars) {
@@ -498,7 +510,8 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
- /* compute N* np (if q > 0): the negative of the step direction in the dual space */
+ /* compute N* np (if q > 0): the negative of the step direction in the dual
+ * space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
@@ -513,7 +526,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
l = 0;
- /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
+ /* Compute t1: partial step length (maximum step in dual space without
+ * violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
// l: index of constraint to drop (maybe)
@@ -524,8 +538,10 @@ l2a: /* Step 2a: determine step direction */
l = A(k);
}
}
- /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ /* Compute t2: full step length (minimum step in primal space such that the
+ * constraint ip becomes feasible */
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
@@ -533,7 +549,8 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
- std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
+ std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
+ << ") ";
#endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
@@ -635,14 +652,15 @@ double trace_sparse(const EiquadprogFast::SpMat& Hess) {
return sum;
}
-EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast::SpMat& Hess, const VectorXd& g0,
- const MatrixXd& CE, const VectorXd& ce0,
- const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
+EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(
+ const EiquadprogFast::SpMat& Hess, const VectorXd& g0, const MatrixXd& CE,
+ const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
const int nVars = (int)g0.size();
const int nEqCon = (int)ce0.size();
const int nIneqCon = (int)ci0.size();
- if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
+ if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
+ reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
assert(g0.size() == m_nVars);
@@ -681,7 +699,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
R.setZero(nVars, nVars);
R_norm = 1.0;
- /* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
+ /* compute the inverse of the factorized matrix Hess^-1, this is the initial
+ * value for H */
// m_J = L^-T
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
@@ -702,9 +721,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
/* c1 * c2 is an estimate for cond(Hess) */
/*
- * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
- * this is a feasible point in the dual space
- * x = Hess^-1 * g0
+ * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
+ * x this is a feasible point in the dual space x = Hess^-1 * g0
*/
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
if (is_inverse_provided_) {
@@ -745,10 +763,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
print_vector("d", d, nVars);
#endif
- /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
- becomes feasible */
+ /* compute full step length t2: i.e., the minimum step in primal space s.t.
+ the contraint becomes feasible */
t2 = 0.0;
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
@@ -775,7 +794,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
for (i = 0; i < nIneqCon; i++) iai(i) = i;
#ifdef USE_WARM_START
- // DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
+ // DEBUG_STREAM("Gonna warm start using previous active
+ // set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
@@ -784,10 +804,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
- /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
- becomes feasible */
+ /* compute full step length t2: i.e., the minimum step in primal space s.t.
+ the contraint becomes feasible */
t2 = 0.0;
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
else
DEBUG_STREAM("[WARM START] z=0\n")
@@ -854,10 +875,12 @@ l1:
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
- if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
+ if (std::abs(psi) <=
+ nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
/* numerically there are not infeasibilities anymore */
q = iq;
- // DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
+ // DEBUG_STREAM("Optimal active
+ // set:\n"<<A.head(iq).transpose()<<"\n\n")
return EIQUADPROG_FAST_OPTIMAL;
}
@@ -868,7 +891,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
- // find constraint with highest violation (what about normalizing constraints?)
+ // find constraint with highest violation (what about normalizing
+ // constraints?)
for (i = 0; i < nIneqCon; i++) {
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
ss = s(i);
@@ -898,7 +922,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
- /* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
+ /* compute z = H np: the step direction in the primal space (through m_J, see
+ * the paper) */
compute_d(d, m_J, np);
// update_z(z, m_J, d, iq);
if (iq >= nVars) {
@@ -907,7 +932,8 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
- /* compute N* np (if q > 0): the negative of the step direction in the dual space */
+ /* compute N* np (if q > 0): the negative of the step direction in the dual
+ * space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
@@ -922,7 +948,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
l = 0;
- /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
+ /* Compute t1: partial step length (maximum step in dual space without
+ * violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
// l: index of constraint to drop (maybe)
@@ -933,8 +960,10 @@ l2a: /* Step 2a: determine step direction */
l = A(k);
}
}
- /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ /* Compute t2: full step length (minimum step in primal space such that the
+ * constraint ip becomes feasible */
+ if (std::abs(z.dot(z)) >
+ std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
@@ -942,7 +971,8 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
- std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
+ std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
+ << ") ";
#endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
diff --git a/src/glpk-wrapper.cpp b/src/glpk-wrapper.cpp
index 138d7ae..f331379 100644
--- a/src/glpk-wrapper.cpp
+++ b/src/glpk-wrapper.cpp
@@ -16,7 +16,9 @@
//
#include "hpp/bezier-com-traj/solver/glpk-wrapper.hpp"
+
#include <glpk.h>
+
#include <iostream>
namespace solvers {
@@ -34,20 +36,23 @@ int getType(const VectorXd& mib, const VectorXd& mab, const int i) {
return type;
}
-int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
- solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds, VectorXd& x, double& cost) {
+int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
+ const MatrixXd& CI, const VectorXd& ci0,
+ solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
+ VectorXd& x, double& cost) {
glp_smcp opts;
glp_init_smcp(&opts);
opts.msg_lev = GLP_MSG_OFF;
glp_prob* lp;
- int ia[1 + 20000]; // Row indices of each element
- int ja[1 + 20000]; // column indices of each element
- double ar[1 + 20000]; // numerical values of corresponding elements
- lp = glp_create_prob(); // creates a problem object
- glp_set_prob_name(lp, "sample"); // assigns a symbolic name to the problem object
- glp_set_obj_dir(lp, GLP_MIN); // calls the routine glp_set_obj_dir to set the
- // omptimization direction flag,
- // where GLP_MAX means maximization
+ int ia[1 + 20000]; // Row indices of each element
+ int ja[1 + 20000]; // column indices of each element
+ double ar[1 + 20000]; // numerical values of corresponding elements
+ lp = glp_create_prob(); // creates a problem object
+ glp_set_prob_name(lp,
+ "sample"); // assigns a symbolic name to the problem object
+ glp_set_obj_dir(lp, GLP_MIN); // calls the routine glp_set_obj_dir to set the
+ // omptimization direction flag,
+ // where GLP_MAX means maximization
// ROWS
const int numEqConstraints = (int)(CE.rows());
@@ -62,7 +67,8 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
glp_set_row_bnds(lp, idrow, GLP_UP, 0.0, ci0(i));
for (int j = 0; j < xsize; ++j, ++idcol) {
if (CI(i, j) != 0.) {
- ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CI(i, j); /* a[1,1] = 1 */
+ ia[idConsMat] = idrow, ja[idConsMat] = idcol,
+ ar[idConsMat] = CI(i, j); /* a[1,1] = 1 */
++idConsMat;
}
}
@@ -72,7 +78,8 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
glp_set_row_bnds(lp, idrow, GLP_FX, ce0(i), ce0(i));
for (int j = 0; j < xsize; ++j, ++idcol) {
if (CE(i, j) != 0.) {
- ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CE(i, j); /* a[1,1] = 1 */
+ ia[idConsMat] = idrow, ja[idConsMat] = idcol,
+ ar[idConsMat] = CE(i, j); /* a[1,1] = 1 */
++idConsMat;
}
}
@@ -81,8 +88,10 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
// COLUMNS
glp_add_cols(lp, xsize);
- VectorXd miB = minBounds.size() > 0 ? minBounds : VectorXd::Ones(xsize) * UNBOUNDED_DOWN;
- VectorXd maB = maxBounds.size() > 0 ? maxBounds : VectorXd::Ones(xsize) * UNBOUNDED_UP;
+ VectorXd miB =
+ minBounds.size() > 0 ? minBounds : VectorXd::Ones(xsize) * UNBOUNDED_DOWN;
+ VectorXd maB =
+ maxBounds.size() > 0 ? maxBounds : VectorXd::Ones(xsize) * UNBOUNDED_UP;
for (int i = 0; i < xsize; ++i, ++idcol) {
glp_set_col_bnds(lp, idcol, getType(miB, maB, i), miB(i), maB(i));
glp_set_obj_coef(lp, idcol, g0(i));
@@ -92,11 +101,13 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
int res = glp_simplex(lp, &opts);
res = glp_get_status(lp);
if (res == GLP_OPT) {
- cost = glp_get_obj_val(lp); // obtains a computed value of the objective function
+ cost = glp_get_obj_val(
+ lp); // obtains a computed value of the objective function
idrow = 1;
for (int i = 0; i < xsize; ++i, ++idrow) x(i) = glp_get_col_prim(lp, idrow);
}
- glp_delete_prob(lp); // calls the routine glp_delete_prob, which frees all the memory
+ glp_delete_prob(
+ lp); // calls the routine glp_delete_prob, which frees all the memory
glp_free_env();
return res;
}
diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp
index 309477f..e9a3b64 100644
--- a/src/solve_0_step.cpp
+++ b/src/solve_0_step.cpp
@@ -3,14 +3,15 @@
* Author: Steve Tonneau
*/
+#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/utils.hh>
-#include <hpp/bezier-com-traj/common_solve_methods.hh>
using namespace bezier_com_traj;
namespace bezier_com_traj {
-waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, const Matrix3& /*p1X*/,
- const Matrix3& /*gX*/, const double alpha) {
+waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X,
+ const Matrix3& /*p1X*/, const Matrix3& /*gX*/,
+ const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
w.first.block<3, 3>(3, 0) = 6 * alpha * p0X;
@@ -19,7 +20,8 @@ waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, c
return w;
}
-waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
+waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/,
+ const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& gX, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
@@ -29,7 +31,8 @@ waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*
return w;
}
-waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
+waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g,
+ const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& gX, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
// w.first.block<3,3>(0,0) = 0;
@@ -39,7 +42,8 @@ waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*
return w;
}
-waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
+waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g,
+ const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& /*gX*/, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
@@ -49,7 +53,8 @@ waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*
return w;
}
-waypoint6_t w4(point_t_tC /*p0*/, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
+waypoint6_t w4(point_t_tC /*p0*/, point_t_tC p1, point_t_tC g,
+ const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& /*gX*/, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
@@ -103,9 +108,12 @@ waypoint6_t u4(point_t_tC /*l0*/, const double /*alpha*/) {
return w;
}
-int computeNumSteps(const double T, const double timeStep) { return timeStep > 0. ? int(T / timeStep) : -1; }
+int computeNumSteps(const double T, const double timeStep) {
+ return timeStep > 0. ? int(T / timeStep) : -1;
+}
-std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point_t_tC g, const double T,
+std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0,
+ point_t_tC g, const double T,
const double timeStep) {
int numSteps = computeNumSteps(T, timeStep);
static const double n = 3.; // degree
@@ -127,7 +135,8 @@ std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, poin
return wps;
}
-std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const double T, const double timeStep) {
+std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(
+ point_t_tC l0, const double T, const double timeStep) {
int numSteps = computeNumSteps(T, timeStep);
double alpha = 1. / (T);
std::vector<waypoint6_t> wps;
@@ -145,23 +154,28 @@ std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const
/* compute the inequality methods that determine the 6D bezier curve w(t)
as a function of a variable waypoint for the 3D COM trajectory.
-The initial curve is of degree 3 (init pos and velocity, 0 velocity constraints + one free variable).
-The 6d curve is of degree 2*n-2 = 4, thus 5 control points are to be computed.
-Each control point produces a 6 * 3 inequality matrix wix, and a 6 *1 column right member wsi.
-Premultiplying it by H gives mH w_xi * x <= mH_wsi where m is the mass
-Stacking all of these results in a big inequality matrix A and a column vector x that determines the constraints
-On the 6d curves, Ain x <= Aub
+The initial curve is of degree 3 (init pos and velocity, 0 velocity constraints
++ one free variable). The 6d curve is of degree 2*n-2 = 4, thus 5 control points
+are to be computed. Each control point produces a 6 * 3 inequality matrix wix,
+and a 6 *1 column right member wsi. Premultiplying it by H gives mH w_xi * x <=
+mH_wsi where m is the mass Stacking all of these results in a big inequality
+matrix A and a column vector x that determines the constraints On the 6d curves,
+Ain x <= Aub
*/
-std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData, point_t_tC c0, point_t_tC dc0,
- point_t_tC l0, const bool useAngMomentum,
- const double T, const double timeStep, bool& fail) {
+std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
+ const ContactData& cData, point_t_tC c0, point_t_tC dc0, point_t_tC l0,
+ const bool useAngMomentum, const double T, const double timeStep,
+ bool& fail) {
std::vector<waypoint6_t> wps, wpL;
- wps = ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
+ wps =
+ ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
if (useAngMomentum) wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
- return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum, fail);
+ return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum,
+ fail);
}
-std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0, const bool useAngMomentum) {
+std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0,
+ const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
std::pair<MatrixXX, VectorX> res;
res.first = MatrixXX::Zero(dimPb, dimPb);
@@ -185,12 +199,14 @@ std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0, c
void computeRealCost(const ProblemData& pData, ResultData& resData) {
if (pData.useAngularMomentum_) {
Vector3 xL = resData.x.tail(3);
- resData.cost_ = (1. / 5.) * (9. * pData.l0_.dot(pData.l0_) - 9. * pData.l0_.dot(xL) + 6. * xL.dot(xL));
+ resData.cost_ = (1. / 5.) * (9. * pData.l0_.dot(pData.l0_) -
+ 9. * pData.l0_.dot(xL) + 6. * xL.dot(xL));
} else
resData.cost_ = (pData.c0_ - resData.x).norm();
}
-void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) {
+void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts,
+ ResultDataCOMTraj& res) {
std::vector<Vector3> wps;
wps.push_back(pData.c0_);
wps.push_back(pData.dc0_ * Ts[0] / 3 + pData.c0_);
@@ -199,7 +215,8 @@ void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, Resu
res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0]);
}
-void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) {
+void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts,
+ ResultDataCOMTraj& res) {
if (pData.useAngularMomentum_) {
std::vector<Vector3> wps;
wps.push_back(3 * (res.x.tail(3) - pData.l0_));
@@ -211,17 +228,20 @@ void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, Res
}
// no angular momentum for now
-ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep) {
+ResultDataCOMTraj solve0step(const ProblemData& pData,
+ const std::vector<double>& Ts,
+ const double timeStep) {
assert(pData.representation_ == DOUBLE_DESCRIPTION);
assert(pData.contacts_.size() == 1);
assert(Ts.size() == pData.contacts_.size());
bool fail = true;
- std::pair<MatrixXX, VectorX> Ab =
- compute6dControlPointInequalities(pData.contacts_.front(), pData.c0_, pData.dc0_, pData.l0_,
- pData.useAngularMomentum_, Ts.front(), timeStep, fail);
+ std::pair<MatrixXX, VectorX> Ab = compute6dControlPointInequalities(
+ pData.contacts_.front(), pData.c0_, pData.dc0_, pData.l0_,
+ pData.useAngularMomentum_, Ts.front(), timeStep, fail);
ResultDataCOMTraj res;
if (fail) return res;
- std::pair<MatrixXX, VectorX> Hg = computeCostFunction(pData.c0_, pData.l0_, pData.useAngularMomentum_);
+ std::pair<MatrixXX, VectorX> Hg =
+ computeCostFunction(pData.c0_, pData.l0_, pData.useAngularMomentum_);
int dimPb = pData.useAngularMomentum_ ? 6 : 3;
VectorX init = VectorX(dimPb);
init.head(3) = pData.c0_;
diff --git a/src/solver-abstract.cpp b/src/solver-abstract.cpp
index f3cb30d..3d9e513 100644
--- a/src/solver-abstract.cpp
+++ b/src/solver-abstract.cpp
@@ -17,12 +17,12 @@
#include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
#ifdef USE_GLPK_SOLVER
-#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
#include <glpk.h>
-#endif
-#include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
+#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
+#endif
#include <Eigen/Sparse>
+#include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
#include <stdexcept>
namespace solvers {
@@ -38,37 +38,41 @@ typedef Eigen::SparseVector<double> SpVec;
typedef Eigen::SparseVector<int> SpVeci;
namespace {
-void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds, std::pair<MatrixXd, VectorXd>& data) {
+void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds,
+ std::pair<MatrixXd, VectorXd>& data) {
if (minBounds.size() == 0) return;
MatrixXd& res = data.first;
VectorXd& resv = data.second;
MatrixXd D(res.rows() + res.cols(), res.cols());
VectorXd d(resv.rows() + res.cols());
D.block(0, 0, res.rows(), res.cols()) = res;
- D.block(res.rows(), 0, res.cols(), res.cols()) = (-1.) * MatrixXd::Identity(res.cols(), res.cols());
+ D.block(res.rows(), 0, res.cols(), res.cols()) =
+ (-1.) * MatrixXd::Identity(res.cols(), res.cols());
d.head(resv.size()) = resv;
d.tail(res.cols()) = -minBounds;
data.first = D;
data.second = d;
}
-void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds, std::pair<MatrixXd, VectorXd>& data) {
+void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds,
+ std::pair<MatrixXd, VectorXd>& data) {
if (maxBounds.size() == 0) return;
MatrixXd& res = data.first;
VectorXd& resv = data.second;
MatrixXd D(res.rows() + res.cols() - 3, res.cols());
VectorXd d(resv.rows() + res.cols());
D.block(0, 0, res.rows(), res.cols()) = res;
- D.block(res.rows(), 0, res.cols(), res.cols()) = MatrixXd::Identity(res.cols(), res.cols());
+ D.block(res.rows(), 0, res.cols(), res.cols()) =
+ MatrixXd::Identity(res.cols(), res.cols());
d.head(resv.size()) = resv;
d.tail(res.cols()) = maxBounds;
data.first = D;
data.second = d;
}
-std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vectorX minBounds,
- solvers::Cref_vectorX maxBounds, const MatrixXd& CI,
- const VectorXd& ci0) {
+std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(
+ solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
+ const MatrixXd& CI, const VectorXd& ci0) {
std::pair<MatrixXd, VectorXd> data;
data.first = CI;
data.second = ci0;
@@ -78,8 +82,9 @@ std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vecto
}
} // namespace
-ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d, const MatrixXd& Hess,
- const VectorXd& g, const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
+ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D,
+ const VectorXd& d, const MatrixXd& Hess, const VectorXd& g,
+ const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
solvers::Cref_vectorX maxBounds, const SolverType solver) {
assert(!(is_nan(A)));
assert(!(is_nan(b)));
@@ -101,12 +106,15 @@ ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const
// case SOLVER_QUADPROG_SPARSE:
{
assert(!(is_nan(Hess)));
- std::pair<MatrixXd, VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
+ std::pair<MatrixXd, VectorXd> CIp =
+ addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
VectorXd ce0 = -d;
- tsid::solvers::EiquadprogFast QPsolver = tsid::solvers::EiquadprogFast();
+ tsid::solvers::EiquadprogFast QPsolver =
+ tsid::solvers::EiquadprogFast();
tsid::solvers::EiquadprogFast_status status;
// if(solver == SOLVER_QUADPROG)
- status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first, CIp.second, res.x);
+ status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first,
+ CIp.second, res.x);
/* else
{
SpMat Hsp = Hess.sparseView();
@@ -118,7 +126,8 @@ ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const
}
#ifdef USE_GLPK_SOLVER
case SOLVER_GLPK: {
- res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds, res.x, res.cost_) == GLP_OPT);
+ res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds,
+ res.x, res.cost_) == GLP_OPT);
return res;
}
#endif
diff --git a/src/utils.cpp b/src/utils.cpp
index ea84d2a..b1217d2 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -14,20 +14,26 @@ waypoint_t initwp(const size_t rows, const size_t cols) {
}
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2) {
- if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
+ if (w1.second.rows() != w2.second.rows() ||
+ w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first + w2.first, w1.second + w2.second);
}
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2) {
- if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
+ if (w1.second.rows() != w2.second.rows() ||
+ w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first - w2.first, w1.second - w2.second);
}
-waypoint_t operator*(const double k, const waypoint_t& w) { return waypoint_t(k * w.first, k * w.second); }
+waypoint_t operator*(const double k, const waypoint_t& w) {
+ return waypoint_t(k * w.first, k * w.second);
+}
-waypoint_t operator*(const waypoint_t& w, const double k) { return waypoint_t(k * w.first, k * w.second); }
+waypoint_t operator*(const waypoint_t& w, const double k) {
+ return waypoint_t(k * w.first, k * w.second);
+}
template <>
waypoint9_t initwp<waypoint9_t>() {
@@ -64,13 +70,16 @@ Matrix3 skew(point_t_tC x) {
return res;
}
-std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) {
+std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(
+ const unsigned int degree) {
std::vector<ndcurves::Bern<double> > res;
- for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(ndcurves::Bern<double>(degree, i));
+ for (unsigned int i = 0; i <= (unsigned int)degree; ++i)
+ res.push_back(ndcurves::Bern<double>(degree, i));
return res;
}
-T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase) {
+T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
+ const unsigned int pointsPerPhase) {
T_time timeArray;
double t = 0;
double t_total = phaseTimings.sum();
@@ -83,17 +92,20 @@ T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned i
}
}
timeArray.pop_back();
- timeArray.push_back(std::make_pair(t_total, phaseTimings.size() - 1)); // avoid numerical errors
+ timeArray.push_back(std::make_pair(
+ t_total, phaseTimings.size() - 1)); // avoid numerical errors
return timeArray;
}
-T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep) {
+T_time computeDiscretizedTime(const VectorX& phaseTimings,
+ const double timeStep) {
T_time timeArray;
double t = 0;
double currentTiming = 0.;
for (int i = 0; i < phaseTimings.size(); ++i) {
assert(timeStep * 2 <= phaseTimings[i] &&
- "Time step too high: should allow to contain at least 2 points per phase");
+ "Time step too high: should allow to contain at least 2 points per "
+ "phase");
t = currentTiming;
currentTiming += phaseTimings[i];
while (t < currentTiming) {
@@ -105,20 +117,21 @@ T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep
return timeArray;
}
-void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint, const std::string& fileName,
- bool clipZ) {
+void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
+ const std::string& fileName, bool clipZ) {
std::ofstream file;
using std::endl;
std::string path("/local/fernbac/bench_iros18/constraints_obj/");
path.append(fileName);
file.open(path.c_str(), std::ios::out | std::ios::trunc);
file << "3 1" << endl;
- file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2] << endl;
+ file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2]
+ << endl;
file << "4" << endl;
clipZ ? file << Ab.first.rows() + 2 << endl : file << Ab.first.rows() << endl;
for (int i = 0; i < Ab.first.rows(); ++i) {
- file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t" << Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001
- << endl;
+ file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t"
+ << Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001 << endl;
}
if (clipZ) {
file << "\t" << 0 << "\t" << 0 << "\t" << 1. << "\t" << -3. << endl;
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index 0899610..a5b9db1 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -7,7 +7,6 @@
#define BEZIER_COM_TRAJ_WP_DEF_H
#include <hpp/bezier-com-traj/data.hh>
-#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
@@ -18,13 +17,14 @@
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh>
+#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include "boost/assign.hpp"
namespace bezier_com_traj {
/**
- * This file is used to choose the correct expressions of the curves waypoints, depending on the options set in
- * ProblemData.constraints
+ * This file is used to choose the correct expressions of the curves waypoints,
+ * depending on the options set in ProblemData.constraints
*/
int dimVar(const ProblemData& pData) {
@@ -44,22 +44,28 @@ typedef std::pair<double, point3_t> coefs_t;
typedef coefs_t (*evalCurveAtTime)(const std::vector<point_t>& pi, double t);
typedef std::map<ConstraintFlag, evalCurveAtTime> T_evalCurveAtTime;
typedef T_evalCurveAtTime::const_iterator CIT_evalCurveAtTime;
-static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of(
- c0_dc0_c1::flag, c0_dc0_c1::evaluateCurveAtTime)(c0_dc0_dc1::flag, c0_dc0_dc1::evaluateCurveAtTime)(
- c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateCurveAtTime)(c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateCurveAtTime)(
- c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateCurveAtTime)(
- c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)(c0_dc0_ddc0_ddc1_dc1_c1::flag,
- c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
+static const T_evalCurveAtTime evalCurveAtTimes =
+ boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::evaluateCurveAtTime)(
+ c0_dc0_dc1::flag, c0_dc0_dc1::evaluateCurveAtTime)(
+ c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateCurveAtTime)(
+ c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateCurveAtTime)(
+ c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateCurveAtTime)(
+ c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)(
+ c0_dc0_ddc0_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
// TODOin C++ 10, all these methods could be just one function :)
-coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t) {
+coefs_t evaluateCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi, double t) {
CIT_evalCurveAtTime cit = evalCurveAtTimes.find(pData.constraints_.flag_);
if (cit != evalCurveAtTimes.end())
return cit->second(pi, t);
@@ -69,27 +75,36 @@ coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>
}
}
-typedef coefs_t (*evalAccCurveAtTime)(const std::vector<point_t>& pi, double T, double t);
+typedef coefs_t (*evalAccCurveAtTime)(const std::vector<point_t>& pi, double T,
+ double t);
typedef std::map<ConstraintFlag, evalAccCurveAtTime> T_evalAccCurveAtTime;
typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
static const T_evalAccCurveAtTime evalAccCurveAtTimes =
- boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::evaluateAccelerationCurveAtTime)(
- c0_dc0_dc1::flag, c0_dc0_dc1::evaluateAccelerationCurveAtTime)(c0_dc0_dc1_c1::flag,
- c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)(
+ boost::assign::map_list_of(c0_dc0_c1::flag,
+ c0_dc0_c1::evaluateAccelerationCurveAtTime)(
+ c0_dc0_dc1::flag, c0_dc0_dc1::evaluateAccelerationCurveAtTime)(
+ c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)(
- c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)(
- c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
+ c0_dc0_ddc0_dc1_c1::flag,
+ c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)(
+ c0_dc0_ddc0_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
-/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the
- * waypoint pi and one free waypoint (x)
+/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
+ * on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
-coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t) {
- CIT_evalAccCurveAtTime cit = evalAccCurveAtTimes.find(pData.constraints_.flag_);
+coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi,
+ double T, double t) {
+ CIT_evalAccCurveAtTime cit =
+ evalAccCurveAtTimes.find(pData.constraints_.flag_);
if (cit != evalAccCurveAtTimes.end())
return cit->second(pi, T, t);
else {
@@ -98,23 +113,33 @@ coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vec
}
}
-typedef waypoint_t (*evalCurveWaypointAtTime)(const std::vector<point_t>& pi, double t);
-typedef std::map<ConstraintFlag, evalCurveWaypointAtTime> T_evalCurveWaypointAtTime;
+typedef waypoint_t (*evalCurveWaypointAtTime)(const std::vector<point_t>& pi,
+ double t);
+typedef std::map<ConstraintFlag, evalCurveWaypointAtTime>
+ T_evalCurveWaypointAtTime;
typedef T_evalCurveWaypointAtTime::const_iterator CIT_evalCurveWaypointAtTime;
-static const T_evalCurveWaypointAtTime evalCurveWaypointAtTimes = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime);
+static const T_evalCurveWaypointAtTime evalCurveWaypointAtTimes =
+ boost::assign::map_list_of(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime);
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
// TODOin C++ 10, all these methods could be just one function :)
-waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t) {
- CIT_evalCurveWaypointAtTime cit = evalCurveWaypointAtTimes.find(pData.constraints_.flag_);
+waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData,
+ const std::vector<point_t>& pi,
+ double t) {
+ CIT_evalCurveWaypointAtTime cit =
+ evalCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalCurveWaypointAtTimes.end())
return cit->second(pi, t);
else {
@@ -122,24 +147,35 @@ waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vect
throw std::runtime_error("Current constraints set are not implemented");
}
}
-typedef waypoint_t (*evalVelCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
-typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime> T_evalVelCurveWaypointAtTime;
-typedef T_evalVelCurveWaypointAtTime::const_iterator CIT_evalVelCurveWaypointAtTime;
-static const T_evalVelCurveWaypointAtTime evalVelCurveWaypointAtTimes = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime);
+typedef waypoint_t (*evalVelCurveWaypointAtTime)(const std::vector<point_t>& pi,
+ const double T, double t);
+typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime>
+ T_evalVelCurveWaypointAtTime;
+typedef T_evalVelCurveWaypointAtTime::const_iterator
+ CIT_evalVelCurveWaypointAtTime;
+static const T_evalVelCurveWaypointAtTime evalVelCurveWaypointAtTimes =
+ boost::assign::map_list_of(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime);
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
// TODOin C++ 10, all these methods could be just one function :)
-waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const double T,
- const std::vector<point_t>& pi, double t) {
- CIT_evalVelCurveWaypointAtTime cit = evalVelCurveWaypointAtTimes.find(pData.constraints_.flag_);
+waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData,
+ const double T,
+ const std::vector<point_t>& pi,
+ double t) {
+ CIT_evalVelCurveWaypointAtTime cit =
+ evalVelCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalVelCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
@@ -147,24 +183,36 @@ waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const d
throw std::runtime_error("Current constraints set are not implemented");
}
}
-typedef waypoint_t (*evalAccCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
-typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime> T_evalAccCurveWaypointAtTime;
-typedef T_evalAccCurveWaypointAtTime::const_iterator CIT_evalAccCurveWaypointAtTime;
-static const T_evalAccCurveWaypointAtTime evalAccCurveWaypointAtTimes = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime);
+typedef waypoint_t (*evalAccCurveWaypointAtTime)(const std::vector<point_t>& pi,
+ const double T, double t);
+typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime>
+ T_evalAccCurveWaypointAtTime;
+typedef T_evalAccCurveWaypointAtTime::const_iterator
+ CIT_evalAccCurveWaypointAtTime;
+static const T_evalAccCurveWaypointAtTime evalAccCurveWaypointAtTimes =
+ boost::assign::map_list_of(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::
+ evaluateAccelerationCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::
+ evaluateAccelerationCurveWaypointAtTime);
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
// TODOin C++ 10, all these methods could be just one function :)
-waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, const double T,
- const std::vector<point_t>& pi, double t) {
- CIT_evalAccCurveWaypointAtTime cit = evalAccCurveWaypointAtTimes.find(pData.constraints_.flag_);
+waypoint_t evaluateAccelerationCurveWaypointAtTime(
+ const ProblemData& pData, const double T, const std::vector<point_t>& pi,
+ double t) {
+ CIT_evalAccCurveWaypointAtTime cit =
+ evalAccCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalAccCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
@@ -172,24 +220,35 @@ waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, con
throw std::runtime_error("Current constraints set are not implemented");
}
}
-typedef waypoint_t (*evalJerkCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
-typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime> T_evalJerkCurveWaypointAtTime;
-typedef T_evalJerkCurveWaypointAtTime::const_iterator CIT_evalJerkCurveWaypointAtTime;
-static const T_evalJerkCurveWaypointAtTime evalJerkCurveWaypointAtTimes = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime);
+typedef waypoint_t (*evalJerkCurveWaypointAtTime)(
+ const std::vector<point_t>& pi, const double T, double t);
+typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime>
+ T_evalJerkCurveWaypointAtTime;
+typedef T_evalJerkCurveWaypointAtTime::const_iterator
+ CIT_evalJerkCurveWaypointAtTime;
+static const T_evalJerkCurveWaypointAtTime evalJerkCurveWaypointAtTimes =
+ boost::assign::map_list_of(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime);
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
- * one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve
+ * c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @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
*/
// TODOin C++ 10, all these methods could be just one function :)
-waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const double T, const std::vector<point_t>& pi,
+waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData,
+ const double T,
+ const std::vector<point_t>& pi,
double t) {
- CIT_evalJerkCurveWaypointAtTime cit = evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
+ CIT_evalJerkCurveWaypointAtTime cit =
+ evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalJerkCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
@@ -202,24 +261,30 @@ typedef std::vector<point_t> (*compConsWp)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compConsWp> T_compConsWp;
typedef T_compConsWp::const_iterator CIT_compConsWp;
static const T_compConsWp compConsWps = boost::assign::map_list_of(
- c0_dc0_c1::flag, c0_dc0_c1::computeConstantWaypoints)(c0_dc0_dc1::flag, c0_dc0_dc1::computeConstantWaypoints)(
- c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeConstantWaypoints)(c0_dc0_ddc0::flag,
- c0_dc0_ddc0::computeConstantWaypoints)(
- c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeConstantWaypoints)(c0_dc0_ddc0_dc1_c1::flag,
- c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)(
- c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeConstantWaypoints);
+ c0_dc0_c1::flag, c0_dc0_c1::computeConstantWaypoints)(
+ c0_dc0_dc1::flag, c0_dc0_dc1::computeConstantWaypoints)(
+ c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0::flag, c0_dc0_ddc0::computeConstantWaypoints)(
+ c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeConstantWaypoints);
/**
- * @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
- * final states
+ * @brief computeConstantWaypoints compute the constant waypoints of c(t)
+ * defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
+std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
+ double T) {
CIT_compConsWp cit = compConsWps.find(pData.constraints_.flag_);
if (cit != compConsWps.end())
return cit->second(pData, T);
@@ -229,12 +294,14 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T
}
}
-bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData, double T) {
+bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(
+ const ProblemData& pData, double T) {
const int DIM_POINT = 3; // FIXME : always true ??
const int DIM_VAR = dimVar(pData);
std::vector<point_t> pts = computeConstantWaypoints(pData, T);
bezier_wp_t::t_point_t wps;
- for (std::vector<point_t>::const_iterator pit = pts.begin(); pit != pts.end(); ++pit) {
+ for (std::vector<point_t>::const_iterator pit = pts.begin(); pit != pts.end();
+ ++pit) {
waypoint_t w = initwp(DIM_POINT, DIM_VAR);
if (pit->isZero()) {
w.first = MatrixXX::Identity(DIM_POINT, DIM_VAR);
@@ -246,23 +313,27 @@ bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData
return wps;
}
-typedef std::vector<waypoint_t> (*compVelWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
+typedef std::vector<waypoint_t> (*compVelWp)(const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compVelWp> T_compVelWp;
typedef T_compVelWp::const_iterator CIT_compVelWp;
static const T_compVelWp compVelWps = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityWaypoints);
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityWaypoints);
/**
- * @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
- * final states
+ * @brief computeConstantWaypoints compute the constant waypoints of c(t)
+ * defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, double T,
- std::vector<bezier_t::point_t> pi) {
+std::vector<waypoint_t> computeVelocityWaypoints(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
if (cit != compVelWps.end())
return cit->second(pData, T, pi);
@@ -272,23 +343,27 @@ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, doubl
}
}
-typedef std::vector<waypoint_t> (*compAccWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
+typedef std::vector<waypoint_t> (*compAccWp)(const ProblemData& pData, double T,
+ std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compAccWp> T_compAccWp;
typedef T_compAccWp::const_iterator CIT_compAccWp;
static const T_compAccWp compAccWps = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
/**
- * @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
- * final states
+ * @brief computeConstantWaypoints compute the constant waypoints of c(t)
+ * defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData, double T,
- std::vector<bezier_t::point_t> pi) {
+std::vector<waypoint_t> computeAccelerationWaypoints(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
if (cit != compAccWps.end())
return cit->second(pData, T, pi);
@@ -298,22 +373,27 @@ std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData, d
}
}
-typedef std::vector<waypoint_t> (*compJerkWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
+typedef std::vector<waypoint_t> (*compJerkWp)(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compJerkWp> T_compJerkWp;
typedef T_compJerkWp::const_iterator CIT_compJerkWp;
static const T_compJerkWp compJerkWps = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeJerkWaypoints);
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeJerkWaypoints);
/**
- * @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
- * final states
+ * @brief computeConstantWaypoints compute the constant waypoints of c(t)
+ * defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
+std::vector<waypoint_t> computeJerkWaypoints(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compJerkWp cit = compJerkWps.find(pData.constraints_.flag_);
if (cit != compJerkWps.end())
return cit->second(pData, T, pi);
@@ -326,16 +406,20 @@ std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, double T,
typedef bezier_wp_t::t_point_t (*compWp)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compWp> T_compWp;
typedef T_compWp::const_iterator CIT_compWp;
-static const T_compWp compWps = boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::computeWwaypoints)(
- c0_dc0_dc1::flag, c0_dc0_dc1::computeWwaypoints)(c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeWwaypoints)(
- c0_dc0_ddc0::flag, c0_dc0_ddc0::computeWwaypoints)(c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeWwaypoints)(
- c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeWwaypoints)(c0_dc0_ddc0_ddc1_dc1_c1::flag,
- c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
+static const T_compWp compWps = boost::assign::map_list_of(
+ c0_dc0_c1::flag, c0_dc0_c1::computeWwaypoints)(
+ c0_dc0_dc1::flag, c0_dc0_dc1::computeWwaypoints)(
+ c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeWwaypoints)(
+ c0_dc0_ddc0::flag, c0_dc0_ddc0::computeWwaypoints)(
+ c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeWwaypoints)(
+ c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeWwaypoints)(
+ c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)(
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
/**
- * @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final
- * states
+ * @brief computeWwaypoints compute the constant waypoints of w(t) defined by
+ * the constraints on initial and final states
* @param pData
* @param T
* @return
@@ -354,12 +438,14 @@ typedef coefs_t (*compFinalVelP)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compFinalVelP> T_compFinalVelP;
typedef T_compFinalVelP::const_iterator CIT_compFinalVelP;
static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of(
- c0_dc0_c1::flag, c0_dc0_c1::computeFinalVelocityPoint)(c0_dc0_dc1::flag, c0_dc0_dc1::computeFinalVelocityPoint)(
- c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeFinalVelocityPoint)(c0_dc0_ddc0::flag,
- c0_dc0_ddc0::computeFinalVelocityPoint)(
- c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeFinalVelocityPoint)(c0_dc0_ddc0_dc1_c1::flag,
- c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)(
- c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
+ c0_dc0_c1::flag, c0_dc0_c1::computeFinalVelocityPoint)(
+ c0_dc0_dc1::flag, c0_dc0_dc1::computeFinalVelocityPoint)(
+ c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeFinalVelocityPoint)(
+ c0_dc0_ddc0::flag, c0_dc0_ddc0::computeFinalVelocityPoint)(
+ c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeFinalVelocityPoint)(
+ c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)(
+ c0_dc0_ddc0_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
@@ -371,23 +457,27 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
}
-typedef std::pair<MatrixXX, VectorX> (*compVelCost)(const ProblemData& pData, double T,
- std::vector<bezier_t::point_t> pi);
+typedef std::pair<MatrixXX, VectorX> (*compVelCost)(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compVelCost> T_compVelCost;
typedef T_compVelCost::const_iterator CIT_compVelCost;
static const T_compVelCost compVelCosts = boost::assign::map_list_of(
- c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityCost)(
- c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityCost)(
- c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityCost);
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityCost)(
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityCost)(
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
+ c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityCost);
/**
- * @brief computeVelocityCost the matrices H and g defining a cost that minimise the integral of the squared velocity
+ * @brief computeVelocityCost the matrices H and g defining a cost that minimise
+ * the integral of the squared velocity
* @param pData
* @param T
* @return
*/
-std::pair<MatrixXX, VectorX> computeVelocityCost(const ProblemData& pData, double T,
- std::vector<bezier_t::point_t> pi) {
+std::pair<MatrixXX, VectorX> computeVelocityCost(
+ const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compVelCost cit = compVelCosts.find(pData.constraints_.flag_);
if (cit != compVelCosts.end())
return cit->second(pData, T, pi);
diff --git a/tests/test-bezier-symbolic.cpp b/tests/test-bezier-symbolic.cpp
index 3154d05..2d26755 100644
--- a/tests/test-bezier-symbolic.cpp
+++ b/tests/test-bezier-symbolic.cpp
@@ -17,19 +17,21 @@
// <http://www.gnu.org/licenses/>.
#define BOOST_TEST_MODULE transition
+#include <ndcurves/bezier_curve.h>
+
#include <boost/test/included/unit_test.hpp>
-#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
+
#include "test_helper.hh"
-#include <ndcurves/bezier_curve.h>
using namespace bezier_com_traj;
const double T = 1.5;
-ProblemData buildPData(
- const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
+ProblemData buildPData(const centroidal_dynamics::EquilibriumAlgorithm algo =
+ centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
ProblemData pData;
pData.c0_ = Vector3(0, 0.5, 5.);
pData.c1_ = Vector3(2, -0.5, 5.);
@@ -44,18 +46,25 @@ ProblemData buildPData(
positions.block<1, 3>(0, 0) = Vector3(0, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- pData.contacts_.push_back(
- new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second, algo)));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ pData.contacts_.push_back(new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second, algo)));
return pData;
}
-std::vector<point_t> generate_wps() { return computeConstantWaypoints(buildPData(), T); }
+std::vector<point_t> generate_wps() {
+ return computeConstantWaypoints(buildPData(), T);
+}
-bezier_wp_t::t_point_t generate_wps_symbolic() { return computeConstantWaypointsSymbolic(buildPData(), T); }
+bezier_wp_t::t_point_t generate_wps_symbolic() {
+ return computeConstantWaypointsSymbolic(buildPData(), T);
+}
-VectorX eval(const waypoint_t& w, const point_t& x) { return w.first * x + w.second; }
+VectorX eval(const waypoint_t& w, const point_t& x) {
+ return w.first * x + w.second;
+}
void vectorEqual(const VectorX& a, const VectorX& b, const double EPS = 1e-14) {
BOOST_CHECK_EQUAL(a.size(), b.size());
@@ -177,12 +186,14 @@ BOOST_AUTO_TEST_CASE(symbolic_split_c_bench) {
clock_t s0, e0;
std::pair<bezier_wp_t, bezier_wp_t> splitted = c_sym.split(0.5);
s0 = clock();
- for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
+ for (std::vector<double>::const_iterator cit = values.begin();
+ cit != values.end(); ++cit) {
splitted = c_sym.split(*cit);
}
e0 = clock();
- std::cout << "Time required to split a c curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
+ std::cout << "Time required to split a c curve : "
+ << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
<< std::endl;
}
@@ -220,12 +231,14 @@ BOOST_AUTO_TEST_CASE(symbolic_split_w_bench) {
clock_t s0, e0;
std::pair<bezier_wp_t, bezier_wp_t> splitted = w.split(0.5);
s0 = clock();
- for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
+ for (std::vector<double>::const_iterator cit = values.begin();
+ cit != values.end(); ++cit) {
splitted = w.split(*cit);
}
e0 = clock();
- std::cout << "Time required to split a w curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
+ std::cout << "Time required to split a w curve : "
+ << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
<< std::endl;
}
diff --git a/tests/test-transition-quasi-static.cpp b/tests/test-transition-quasi-static.cpp
index 159661b..1ec79dc 100644
--- a/tests/test-transition-quasi-static.cpp
+++ b/tests/test-transition-quasi-static.cpp
@@ -18,9 +18,10 @@
#define BOOST_TEST_MODULE transition - quasiStatic
#include <boost/test/included/unit_test.hpp>
-#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
+
#include "test_helper.hh"
BOOST_AUTO_TEST_SUITE(quasiStatic)
@@ -31,19 +32,20 @@ BOOST_AUTO_TEST_CASE(single_support) {
normal << 0, 0, 1;
MatrixX3 position(1, 3);
position << 0, 0, 0;
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
BOOST_CHECK(res.success_);
- // sample positions for second foot inside kinematics constraints and check for feasibility :
+ // sample positions for second foot inside kinematics constraints and check
+ // for feasibility :
}
BOOST_AUTO_TEST_CASE(quasiStatic_exist) {
- // sample positions for second foot inside kinematics constraints and check for feasibility :
- // Vector3 firstLeg_n(0,0,1);
- // Vector3 firstLeg_p(0,0,0);
+ // sample positions for second foot inside kinematics constraints and check
+ // for feasibility : Vector3 firstLeg_n(0,0,1); Vector3 firstLeg_p(0,0,0);
for (size_t i = 0; i < 500; ++i) {
MatrixX3 normal(1, 3);
@@ -55,7 +57,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_exist) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
@@ -74,7 +77,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upX) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
@@ -93,7 +97,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downX) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
@@ -112,7 +117,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downY) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
@@ -131,7 +137,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upY) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
- std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
+ std::pair<MatrixX3, VectorX> Ab = generateConstraints(
+ normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
diff --git a/tests/test-transition.cpp b/tests/test-transition.cpp
index 7a255d8..857861a 100644
--- a/tests/test-transition.cpp
+++ b/tests/test-transition.cpp
@@ -18,15 +18,17 @@
#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
-#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
+
#include "test_helper.hh"
#define NOMINAL_COM_HEIGHT 0.795
-std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings, const int pointsPerPhase) {
+std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,
+ const int pointsPerPhase) {
std::vector<double> timeArray;
double t = 0;
double t_total = 0;
@@ -44,15 +46,21 @@ std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings, const in
return timeArray;
}
-bool check_constraints(const bezier_com_traj::ContactData& contactPhase, Vector3 c, Vector3 dc, Vector3 ddc) {
- BOOST_CHECK(verifyKinematicConstraints(std::make_pair(contactPhase.Kin_, contactPhase.kin_), c));
- BOOST_CHECK(verifyStabilityConstraintsDLP(*contactPhase.contactPhase_, c, dc, ddc));
- BOOST_CHECK(verifyStabilityConstraintsPP(*contactPhase.contactPhase_, c, dc, ddc));
+bool check_constraints(const bezier_com_traj::ContactData& contactPhase,
+ Vector3 c, Vector3 dc, Vector3 ddc) {
+ BOOST_CHECK(verifyKinematicConstraints(
+ std::make_pair(contactPhase.Kin_, contactPhase.kin_), c));
+ BOOST_CHECK(
+ verifyStabilityConstraintsDLP(*contactPhase.contactPhase_, c, dc, ddc));
+ BOOST_CHECK(
+ verifyStabilityConstraintsPP(*contactPhase.contactPhase_, c, dc, ddc));
return true;
}
-void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX& Ts,
- const bezier_com_traj::ResultDataCOMTraj& res, const int pointsPerPhase, const double t_total) {
+void discretized_check(const bezier_com_traj::ProblemData& pData,
+ const VectorX& Ts,
+ const bezier_com_traj::ResultDataCOMTraj& res,
+ const int pointsPerPhase, const double t_total) {
// check if timing is respected
std::vector<double> timings = computeDiscretizedTime(Ts, pointsPerPhase);
BOOST_CHECK_EQUAL(timings.back(), t_total);
@@ -72,8 +80,10 @@ void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX&
// check if each discretized point is feasible :
std::vector<int>
- stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
- for (int i = 0; i < Ts.size(); ++i) stepIdForPhase.push_back(pointsPerPhase * (i + 1) - 1);
+ stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase
+ // i / first step of phase i+1 (overlap)
+ for (int i = 0; i < Ts.size(); ++i)
+ stepIdForPhase.push_back(pointsPerPhase * (i + 1) - 1);
std::size_t id_phase = 0;
bezier_com_traj::ContactData phase = pData.contacts_[id_phase];
@@ -96,8 +106,9 @@ void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX&
}
}
-void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shouldFail = false,
- bool continuous = false, bool test_continuous = true) {
+void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,
+ bool shouldFail = false, bool continuous = false,
+ bool test_continuous = true) {
BOOST_CHECK_EQUAL(pData.contacts_.size(), Ts.size());
double t_total = 0;
@@ -109,7 +120,8 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
bezier_com_traj::ResultDataCOMTraj res;
if (continuous) {
// testing all available solvers
- res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
+ res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
+ solvers::SOLVER_QUADPROG);
if (pData.representation_ == bezier_com_traj::FORCE) {
/*bezier_com_traj::ResultDataCOMTraj res2 =
bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG_SPARSE);
@@ -119,14 +131,17 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
std::cout << " x " << res.cost_ << std::endl;
std::cout << " x2 " << res2.cost_ << std::endl;
discretized_check(pData,Ts,res2,pointsPerPhase,t_total);
- BOOST_CHECK(!res.success_ || (res.x.head<3>() - res2.x.head<3>()).norm() < EPSILON);
+ BOOST_CHECK(!res.success_ || (res.x.head<3>() -
+ res2.x.head<3>()).norm() < EPSILON);
}*/
#ifdef USE_GLPK_SOLVER
// clock_t s0,e0;
// s0 = clock();
- bezier_com_traj::ResultDataCOMTraj res3 = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
+ bezier_com_traj::ResultDataCOMTraj res3 =
+ bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
// e0 = clock();
- // std::cout<<"Time to perform full lp : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
+ // std::cout<<"Time to perform full lp :
+ // "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
BOOST_CHECK(res.success_ == res3.success_);
if (res3.success_) {
discretized_check(pData, Ts, res3, pointsPerPhase, t_total);
@@ -139,13 +154,16 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
if (shouldFail) {
BOOST_CHECK(!res.success_);
if (test_continuous) {
- res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
+ res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
+ solvers::SOLVER_QUADPROG);
BOOST_CHECK(!res.success_);
pData.representation_ = bezier_com_traj::FORCE;
- res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
+ res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
+ solvers::SOLVER_QUADPROG);
BOOST_CHECK(!res.success_);
#ifdef USE_GLPK_SOLVER
- res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
+ res =
+ bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
BOOST_CHECK(!res.success_);
#endif
}
@@ -155,7 +173,8 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
BOOST_CHECK(res.success_);
if (res.success_) discretized_check(pData, Ts, res, pointsPerPhase, t_total);
- if (continuous && pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION) {
+ if (continuous &&
+ pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION) {
pData.representation_ = bezier_com_traj::FORCE;
check_transition(pData, Ts, shouldFail, true);
} else if (!continuous && test_continuous)
@@ -180,11 +199,16 @@ bezier_com_traj::ContactData phase0_flat() {
positions.block<1, 3>(0, 0) = Vector3(0, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
-
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
- ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
+
+ ConstraintsPair kin1 =
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
+ ConstraintsPair kin = stackConstraints(
+ kin1,
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -196,10 +220,13 @@ bezier_com_traj::ContactData phase1_flat() {
MatrixX3 normals(1, 3), positions(1, 3);
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(0, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
+ ConstraintsPair kin =
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -213,12 +240,16 @@ bezier_com_traj::ContactData phase2_flat() {
positions.block<1, 3>(0, 0) = Vector3(0.3, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
-
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
- ConstraintsPair kin =
- stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
+
+ ConstraintsPair kin1 =
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
+ ConstraintsPair kin = stackConstraints(
+ kin1,
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -243,11 +274,14 @@ bezier_com_traj::ProblemData gen_problem_data_flat() {
BOOST_AUTO_TEST_CASE(quasi_static) {
// compute kinematic constraints for the right foot :
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
- ConstraintsPair kin0 =
- stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
- ConstraintsPair kin2 =
- stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
+ ConstraintsPair kin1 =
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
+ ConstraintsPair kin0 = stackConstraints(
+ kin1,
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
+ ConstraintsPair kin2 = stackConstraints(
+ kin1,
+ generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
bezier_com_traj::ContactData cDataMid = phase1_flat();
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
@@ -256,11 +290,13 @@ BOOST_AUTO_TEST_CASE(quasi_static) {
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin0, stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ bezier_com_traj::ResultData res_first =
+ bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin2, stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ bezier_com_traj::ResultData res_second =
+ bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataMid.contactPhase_;
}
@@ -274,7 +310,9 @@ BOOST_AUTO_TEST_CASE(transition) {
BOOST_AUTO_TEST_CASE(transition_noc1) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
+ pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
+ bezier_com_traj::INIT_VEL |
+ bezier_com_traj::END_VEL;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts);
@@ -284,7 +322,9 @@ BOOST_AUTO_TEST_CASE(transition_no_terminal_constraints) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.contacts_.pop_back();
pData.contacts_.pop_back();
- pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::INIT_ACC;
+ pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
+ bezier_com_traj::INIT_VEL |
+ bezier_com_traj::INIT_ACC;
VectorX Ts(1);
Ts << 0.2;
check_transition(pData, Ts, false, false, true);
@@ -342,7 +382,8 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_noAcc) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
+ pData.constraints_.flag_ |=
+ bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.constraintAcceleration_ = false;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
@@ -354,7 +395,8 @@ BOOST_AUTO_TEST_CASE(transition_Acc1) {
pData.constraints_.maxAcceleration_ = 1.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
- check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+ check_transition(pData, Ts, false, false,
+ false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1) {
@@ -363,7 +405,8 @@ BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1) {
pData.constraints_.maxAcceleration_ = 1.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
- check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+ check_transition(pData, Ts, false, false,
+ false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
@@ -371,12 +414,14 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2) {
pData.constraints_.maxAcceleration_ = 2.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
- check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+ check_transition(pData, Ts, false, false,
+ false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
+ pData.constraints_.flag_ |=
+ bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 2.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
@@ -385,11 +430,13 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc05) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
+ pData.constraints_.flag_ |=
+ bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 0.5;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
- check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+ check_transition(pData, Ts, false, false,
+ false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_Acc05) {
@@ -397,7 +444,8 @@ BOOST_AUTO_TEST_CASE(transition_Acc05) {
pData.constraints_.maxAcceleration_ = 0.5;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
- check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+ check_transition(pData, Ts, false, false,
+ false); // fail with continuous formulation
}
// constraints that should fails :
@@ -433,7 +481,8 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc02) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
+ pData.constraints_.flag_ |=
+ bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 0.2;
pData.constraints_.constraintAcceleration_ = true;
VectorX Ts(3);
@@ -446,7 +495,8 @@ BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE(platform)
// platform : first step :
-// (0.35,0.1,0) ; (0.35,-0.1,0) -> (0.775, 0.23, -0.02);(0.35,-0.1,0) (normal : 0.0, -0.423, 0.906)
+// (0.35,0.1,0) ; (0.35,-0.1,0) -> (0.775, 0.23, -0.02);(0.35,-0.1,0) (normal :
+// 0.0, -0.423, 0.906)
// second step :
// (0.775, 0.23, -0.02);(0.35,-0.1,0) -> (0.775, 0.23, -0.02);(1.15,-0.1,0)
@@ -462,12 +512,16 @@ bezier_com_traj::ContactData phase0_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.35, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0.35, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
+ ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(0.35, -0.1, 0));
ConstraintsPair kin =
- stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, 0.1, 0)));
+ stackConstraints(kin1, generateKinematicsConstraints(
+ Matrix3::Identity(), Vector3(0.35, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -479,10 +533,13 @@ bezier_com_traj::ContactData phase1_platform() {
MatrixX3 normals(1, 3), positions(1, 3);
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(0.35, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
+ ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(0.35, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -496,14 +553,18 @@ bezier_com_traj::ContactData phase2_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0.35, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
-
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
- Eigen::Quaterniond quat =
- Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
+
+ ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(0.35, -0.1, 0));
+ Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
+ Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
- ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
+ ConstraintsPair kin = stackConstraints(
+ kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -516,13 +577,16 @@ bezier_com_traj::ContactData phase3_platform() {
normals.block<1, 3>(0, 0) = Vector3(0.0, -0.423, 0.906).normalized();
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- Eigen::Quaterniond quat =
- Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
+ Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
+ Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
- ConstraintsPair kin = generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02));
+ ConstraintsPair kin =
+ generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -536,14 +600,18 @@ bezier_com_traj::ContactData phase4_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(1.15, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
-
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
- Eigen::Quaterniond quat =
- Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
+
+ ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(1.15, -0.1, 0));
+ Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
+ Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
- ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
+ ConstraintsPair kin = stackConstraints(
+ kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -556,10 +624,13 @@ bezier_com_traj::ContactData phase5_platform() {
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(1.15, -0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
+ ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(1.15, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -574,12 +645,16 @@ bezier_com_traj::ContactData phase6_platform() {
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(1.15, 0.1, 0);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
+ ComputeContactCone(contacts.first, contacts.second));
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
+ ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
+ Vector3(1.15, -0.1, 0));
ConstraintsPair kin =
- stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, 0.1, 0)));
+ stackConstraints(kin1, generateKinematicsConstraints(
+ Matrix3::Identity(), Vector3(1.15, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
@@ -642,17 +717,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_0) {
bezier_com_traj::ContactData cDataSecond = phase2_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
- ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
+ ConstraintsPair kin_second =
+ std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ bezier_com_traj::ResultData res_first =
+ bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ bezier_com_traj::ResultData res_second =
+ bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataFirst.contactPhase_;
@@ -668,17 +746,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_1) {
bezier_com_traj::ContactData cDataSecond = phase4_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
- ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
+ ConstraintsPair kin_second =
+ std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ bezier_com_traj::ResultData res_first =
+ bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(!res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ bezier_com_traj::ResultData res_second =
+ bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(!res_second.success_);
delete cDataFirst.contactPhase_;
@@ -694,17 +775,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_2) {
bezier_com_traj::ContactData cDataSecond = phase6_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
- ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
+ ConstraintsPair kin_second =
+ std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ bezier_com_traj::ResultData res_first =
+ bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ bezier_com_traj::ResultData res_second =
+ bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataFirst.contactPhase_;
diff --git a/tests/test-zero-step-capturability.cpp b/tests/test-zero-step-capturability.cpp
index 82dc62c..d60b2c3 100644
--- a/tests/test-zero-step-capturability.cpp
+++ b/tests/test-zero-step-capturability.cpp
@@ -3,13 +3,14 @@
* Author: Steve Tonneau
*/
-#include <vector>
-#include <iostream>
-#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
-#include <hpp/bezier-com-traj/solve.hh>
-#include <hpp/bezier-com-traj/data.hh>
#include <math.h>
+#include <hpp/bezier-com-traj/data.hh>
+#include <hpp/bezier-com-traj/solve.hh>
+#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
+#include <iostream>
+#include <vector>
+
using namespace centroidal_dynamics;
using namespace Eigen;
using namespace std;
@@ -25,9 +26,12 @@ using namespace std;
#define EPS 1e-3 // required precision
-void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, double LX, double LY,
- RVector3& CONTACT_POINT_LOWER_BOUNDS, RVector3& CONTACT_POINT_UPPER_BOUNDS,
- RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS, MatrixX3& p, MatrixX3& N) {
+void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE,
+ double LX, double LY,
+ RVector3& CONTACT_POINT_LOWER_BOUNDS,
+ RVector3& CONTACT_POINT_UPPER_BOUNDS,
+ RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS,
+ MatrixX3& p, MatrixX3& N) {
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
p.setZero(4 * N_CONTACTS, 3); // contact points
@@ -38,27 +42,31 @@ void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, doub
for (unsigned int i = 0; i < N_CONTACTS; i++) {
while (true) // generate contact position
{
- uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
+ uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
+ contact_pos.row(i));
if (i == 0) break;
collision = false;
for (unsigned int j = 0; j < i - 1; j++)
- if ((contact_pos.row(i) - contact_pos.row(j)).norm() < MIN_CONTACT_DISTANCE) collision = true;
+ if ((contact_pos.row(i) - contact_pos.row(j)).norm() <
+ MIN_CONTACT_DISTANCE)
+ collision = true;
if (collision == false) break;
}
// generate contact orientation
uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
- generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i), p.middleRows<4>(i * 4),
- N.middleRows<4>(i * 4));
- // printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1),
- // contact_pos(i,2)); printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1),
- // contact_rpy(i,2));
+ generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
+ p.middleRows<4>(i * 4), N.middleRows<4>(i * 4));
+ // printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i,
+ // contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
+ // printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0),
+ // contact_rpy(i,1), contact_rpy(i,2));
}
// for(int i=0; i<p.rows(); i++)
// {
- // printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
- // printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
+ // printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1),
+ // p(i,2)); printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
// }
}
@@ -67,7 +75,8 @@ double fRandom(double fMin, double fMax) {
return fMin + f * (fMax - fMin);
}
-bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB, const RVector3& com_UB, Vector3& c0) {
+bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB,
+ const RVector3& com_UB, Vector3& c0) {
int trials = 0;
while (trials < 1000) {
++trials;
@@ -84,7 +93,8 @@ bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB, const RVector3&
return false;
}
-Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c, const Vector3 ddc, const Vector3 dL) {
+Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c,
+ const Vector3 ddc, const Vector3 dL) {
Vector6 w;
Vector3 w1;
w1 = eq->m_mass * (ddc - eq->m_gravity);
@@ -93,8 +103,10 @@ Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c, const
return w;
}
-bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilibrium* eq,
- const bezier_com_traj::ResultDataCOMTraj& resData, const double T, const int num_steps = 100) {
+bool checkTrajectory(const Vector3& c0, const std::string& solver,
+ const Equilibrium* eq,
+ const bezier_com_traj::ResultDataCOMTraj& resData,
+ const double T, const int num_steps = 100) {
// retrieve H
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
@@ -112,9 +124,11 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
VectorX res = H * w;
for (long j = 0; j < res.size(); j++)
if (res(j) > 0.0001) {
- std::cout << "check trajectory failed for solver " << solver << " " << res(j) << "; iteration " << dt
- << "; numsteps " << num_steps << " c_of_t(dt) " << (c_of_t)(dt).transpose() << " dc_of_t(dt) "
- << c_of_t.derivate(dt, 1).transpose() << " dL_of_t(dt) " << (dL_of_t)(dt).transpose() << std::endl;
+ std::cout << "check trajectory failed for solver " << solver << " "
+ << res(j) << "; iteration " << dt << "; numsteps "
+ << num_steps << " c_of_t(dt) " << (c_of_t)(dt).transpose()
+ << " dc_of_t(dt) " << c_of_t.derivate(dt, 1).transpose()
+ << " dL_of_t(dt) " << (dL_of_t)(dt).transpose() << std::endl;
/*std::cout << "c0 " << c0.transpose() << std::endl;
std::cout << "H row " << H.row(j) << std::endl;
std::cout << "w " << w << std::endl;
@@ -137,7 +151,8 @@ int main() {
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
- /************************************** USER PARAMETERS *******************************/
+ /************************************** USER PARAMETERS
+ * *******************************/
unsigned int N_TESTS = 500;
double mass = 55.0;
double mu = 0.3; // friction coefficient
@@ -153,16 +168,19 @@ int main() {
RPY_UPPER_BOUNDS << +2 * gamma, +2 * gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
- /************************************ END USER PARAMETERS *****************************/
+ /************************************ END USER PARAMETERS
+ * *****************************/
MatrixX3 p, N;
RVector3 com_LB, com_UB;
- Equilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES, false, 10, false);
- int succContinuous = 0, succDiscretize = 0, succdL = 0, succDiscretizedL = 0, succKin = 0, succdLKin = 0,
- succdLAng = 0;
+ Equilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES,
+ false, 10, false);
+ int succContinuous = 0, succDiscretize = 0, succdL = 0, succDiscretizedL = 0,
+ succKin = 0, succdLKin = 0, succdLAng = 0;
int numSol = 0;
for (unsigned n_test = 0; n_test < N_TESTS; n_test++) {
- generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
+ generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY,
+ CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
// compute upper and lower bounds of com positions to test
@@ -182,7 +200,9 @@ int main() {
bezier_com_traj::ContactData data;
data.contactPhase_ = &solver_PP;
bezier_com_traj::ProblemData pData;
- pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
+ pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
+ bezier_com_traj::INIT_VEL |
+ bezier_com_traj::END_VEL;
pData.c0_ = c0;
pData.dc0_ << fRandom(-1., 1.), fRandom(-1., 1.), fRandom(-1., 1.);
// pData.dc0_ *= 1;
@@ -195,12 +215,14 @@ int main() {
{
pData.useAngularMomentum_ = false;
pData.contacts_.front().kin_ = VectorX::Zero(0);
- bool succCont = false, succDisc = false, succdLbool = false, succDiscdL = false, succKinBool = false,
- succdLKinBool = false, succdLAngBool = false;
+ bool succCont = false, succDisc = false, succdLbool = false,
+ succDiscdL = false, succKinBool = false, succdLKinBool = false,
+ succdLAngBool = false;
T = 1. + 0.3 * k;
std::vector<double> Ts;
Ts.push_back(T);
- bezier_com_traj::ResultDataCOMTraj rData = bezier_com_traj::solve0step(pData, Ts);
+ bezier_com_traj::ResultDataCOMTraj rData =
+ bezier_com_traj::solve0step(pData, Ts);
if (rData.success_) {
assert((rData.c_of_t_(0.) - pData.c0_).norm() < 0.0001);
succCont = true;
@@ -213,19 +235,26 @@ int main() {
// try discretize
VectorX t2(1);
t2 << Ts[0];
- bezier_com_traj::ResultDataCOMTraj rData0 = bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
+ bezier_com_traj::ResultDataCOMTraj rData0 =
+ bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
if (rData0.success_) {
- bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
- // bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::computeCOMTraj(pData,t2,tg,(int)(round(T
- // / DISCRETIZATION_STEP)));
+ bezier_com_traj::ResultDataCOMTraj rDatatest =
+ bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
+ // bezier_com_traj::ResultDataCOMTraj rDatatest =
+ // bezier_com_traj::computeCOMTraj(pData,t2,tg,(int)(round(T /
+ // DISCRETIZATION_STEP)));
assert(rDatatest.success_);
assert((rData0.c_of_t_(0.) - c0).norm() < 0.0001);
- assert((rData0.c_of_t_.compute_derivate(1)(0.) - pData.dc0_).norm() < 0.0001);
+ assert(
+ (rData0.c_of_t_.compute_derivate(1)(0.) - pData.dc0_).norm() <
+ 0.0001);
succDisc = true;
succDiscretize += 1;
std::string solverName("discretize");
- checkTrajectory(c0, solverName, &solver_PP, rData0, T, (int)(round(T / DISCRETIZATION_STEP)));
- // checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T / DISCRETIZATION_STEP));
+ checkTrajectory(c0, solverName, &solver_PP, rData0, T,
+ (int)(round(T / DISCRETIZATION_STEP)));
+ // checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T /
+ // DISCRETIZATION_STEP));
} else {
VectorX t2(1);
t2 << Ts[0];
@@ -233,13 +262,15 @@ int main() {
bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
assert(!rDatatest.success_);
if (succCont)
- std::cout << "error: Solver discretize failed while a solution was found for the continuous case"
+ std::cout << "error: Solver discretize failed while a solution "
+ "was found for the continuous case"
<< std::endl;
succDisc = false;
}
pData.useAngularMomentum_ = true;
- bezier_com_traj::ResultDataCOMTraj rData1 = bezier_com_traj::solve0step(pData, Ts);
+ bezier_com_traj::ResultDataCOMTraj rData1 =
+ bezier_com_traj::solve0step(pData, Ts);
if (rData1.success_) {
assert((rData1.c_of_t_(0.) - c0).norm() < 0.0001);
succdLbool = true;
@@ -248,28 +279,33 @@ int main() {
checkTrajectory(c0, solverName, &solver_PP, rData1, T);
} else {
// if(succCont)
- // std::cout << "error: Solver Ang momentum failed while a solution was found without angular momentum"
+ // std::cout << "error: Solver Ang momentum failed while a
+ // solution was found without angular momentum"
// << std::endl;
succDisc = false;
}
- bezier_com_traj::ResultDataCOMTraj rData2 = bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
+ bezier_com_traj::ResultDataCOMTraj rData2 =
+ bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
if (rData2.success_) {
assert((rData2.c_of_t_(0.) - c0).norm() < 0.0001);
succDiscdL = true;
succDiscretizedL += 1;
std::string solverName("discretize AngularMomentum");
- checkTrajectory(c0, solverName, &solver_PP, rData2, T, int(T / DISCRETIZATION_STEP));
+ checkTrajectory(c0, solverName, &solver_PP, rData2, T,
+ int(T / DISCRETIZATION_STEP));
} else {
if (succCont || succDisc || succdLbool)
- std::cout << "error: Solver discretize with angular momentum failed while a solution was found for "
+ std::cout << "error: Solver discretize with angular momentum "
+ "failed while a solution was found for "
"another case"
<< std::endl;
succDiscdL = false;
}
pData.contacts_.front().Kin_ = Eigen::Matrix3d::Identity();
pData.contacts_.front().kin_ = Vector3::Constant(3, 0.5);
- bezier_com_traj::ResultDataCOMTraj rData3 = bezier_com_traj::solve0step(pData, Ts);
+ bezier_com_traj::ResultDataCOMTraj rData3 =
+ bezier_com_traj::solve0step(pData, Ts);
pData.contacts_.front().Ang_ = Eigen::Matrix3d::Identity();
pData.contacts_.front().ang_ = Vector3::Constant(3, 0.1);
if (rData3.success_) {
@@ -281,7 +317,8 @@ int main() {
}
pData.contacts_.front().kin_ = VectorX::Zero(0);
- bezier_com_traj::ResultDataCOMTraj rData31 = bezier_com_traj::solve0step(pData, Ts);
+ bezier_com_traj::ResultDataCOMTraj rData31 =
+ bezier_com_traj::solve0step(pData, Ts);
if (rData31.success_) {
assert((rData31.c_of_t_(0.) - c0).norm() < 0.0001);
succdLAngBool = true;
@@ -293,7 +330,8 @@ int main() {
pData.useAngularMomentum_ = false;
pData.contacts_.front().ang_ = VectorX::Zero(0);
pData.contacts_.front().kin_ = Vector3::Constant(3, 0.5);
- bezier_com_traj::ResultDataCOMTraj rData4 = bezier_com_traj::solve0step(pData, Ts);
+ bezier_com_traj::ResultDataCOMTraj rData4 =
+ bezier_com_traj::solve0step(pData, Ts);
if (rData4.success_) {
assert((rData4.c_of_t_(0.) - c0).norm() < 0.0001);
succKinBool = true;
@@ -307,14 +345,21 @@ int main() {
}
std::cout << "sucesses continunous" << succContinuous << std::endl;
- std::cout << "sucesses continuous with Kinematic constraints " << succKin << std::endl;
+ std::cout << "sucesses continuous with Kinematic constraints " << succKin
+ << std::endl;
std::cout << "sucesses discretize " << succDiscretize << std::endl;
- std::cout << "sucesses continunous with angular momentum" << succdL << std::endl;
- std::cout << "sucesses discretize with angular momentum" << succDiscretizedL << std::endl;
- std::cout << "sucesses continunous with angular momentum and angular constraints" << succdLAng << std::endl;
- std::cout << "sucesses continunous with angular momentum and kinematic and angular constraints" << succdLKin
+ std::cout << "sucesses continunous with angular momentum" << succdL
+ << std::endl;
+ std::cout << "sucesses discretize with angular momentum" << succDiscretizedL
+ << std::endl;
+ std::cout
+ << "sucesses continunous with angular momentum and angular constraints"
+ << succdLAng << std::endl;
+ std::cout << "sucesses continunous with angular momentum and kinematic and "
+ "angular constraints"
+ << succdLKin << std::endl;
+ std::cout << "sucesses discretize with angular momentum" << succDiscretizedL
<< std::endl;
- std::cout << "sucesses discretize with angular momentum" << succDiscretizedL << std::endl;
std::cout << "eq static point found " << numSol << std::endl;
return 0;
diff --git a/tests/test_helper.hh b/tests/test_helper.hh
index 443c15a..1876efa 100644
--- a/tests/test_helper.hh
+++ b/tests/test_helper.hh
@@ -1,10 +1,10 @@
#ifndef TEST_HELPER_HH
#define TEST_HELPER_HH
-#include <hpp/bezier-com-traj/solve.hh>
+#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
+#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
-#include <boost/test/included/unit_test.hpp>
using bezier_com_traj::Matrix3;
using bezier_com_traj::MatrixX3;
@@ -48,7 +48,8 @@ std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints() {
return std::make_pair(N, V);
}
-std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotation, Vector3 endEffTranslation) {
+std::pair<MatrixXX, VectorX> generateKinematicsConstraints(
+ Matrix3 endEffRotation, Vector3 endEffTranslation) {
std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
MatrixX3 N = NV.first;
MatrixX3 V = NV.second;
@@ -67,9 +68,12 @@ std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotatio
return std::make_pair(A, b);
}
-std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, MatrixX3 positions, double size_X,
+std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals,
+ MatrixX3 positions,
+ double size_X,
double size_Y) {
- // TODO : consider normal != z (see code in rbprm :: stability.cc (or add it as dependency ?)
+ // TODO : consider normal != z (see code in rbprm :: stability.cc (or add it
+ // as dependency ?)
BOOST_CHECK(normals.rows() == positions.rows());
MatrixX3 rec_normals(normals.rows() * 4, 3);
@@ -83,7 +87,8 @@ std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, Matri
for (long int ic = 0; ic < normals.rows(); ++ic) {
for (long int i = 0; i < 4; ++i) {
rec_normals.block<1, 3>(ic * 4 + i, 0) = normals.block<1, 3>(ic, 0);
- rec_positions.block<1, 3>(ic * 4 + i, 0) = positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
+ rec_positions.block<1, 3>(ic * 4 + i, 0) =
+ positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
}
}
return std::make_pair(rec_normals, rec_positions);
@@ -91,16 +96,20 @@ std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, Matri
centroidal_dynamics::Equilibrium ComputeContactCone(
MatrixX3 normals, MatrixX3 positions,
- const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
- centroidal_dynamics::Equilibrium contactCone("test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES,
- true, 10, false);
- // centroidal_dynamics::EquilibriumAlgorithm alg = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
+ const centroidal_dynamics::EquilibriumAlgorithm algo =
+ centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
+ centroidal_dynamics::Equilibrium contactCone(
+ "test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES, true,
+ 10, false);
+ // centroidal_dynamics::EquilibriumAlgorithm alg =
+ // centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
contactCone.setNewContacts(positions, normals, MU, algo);
return contactCone;
}
-std::pair<MatrixXX, VectorX> generateStabilityConstraints(centroidal_dynamics::Equilibrium contactPhase,
- Vector3 acc = Vector3::Zero()) {
+std::pair<MatrixXX, VectorX> generateStabilityConstraints(
+ centroidal_dynamics::Equilibrium contactPhase,
+ Vector3 acc = Vector3::Zero()) {
const Vector3& g = contactPhase.m_gravity;
const Matrix3 gSkew = bezier_com_traj::skew(g);
const Matrix3 accSkew = bezier_com_traj::skew(acc);
@@ -122,9 +131,12 @@ std::pair<MatrixXX, VectorX> generateStabilityConstraints(centroidal_dynamics::E
std::pair<MatrixXX, VectorX> generateStabilityConstraints(
MatrixX3 normals, MatrixX3 positions, Vector3 acc = Vector3::Zero(),
- const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
- centroidal_dynamics::Equilibrium contactPhase = ComputeContactCone(contacts.first, contacts.second, algo);
+ const centroidal_dynamics::EquilibriumAlgorithm algo =
+ centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
+ std::pair<MatrixX3, MatrixX3> contacts =
+ computeRectangularContacts(normals, positions, LX, LY);
+ centroidal_dynamics::Equilibrium contactPhase =
+ ComputeContactCone(contacts.first, contacts.second, algo);
return generateStabilityConstraints(contactPhase, acc);
}
@@ -134,10 +146,14 @@ std::pair<Matrix3, Vector3> computeCost() {
return std::make_pair(H, g);
}
-std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals, MatrixX3 positions, Matrix3 endEffRotation,
+std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals,
+ MatrixX3 positions,
+ Matrix3 endEffRotation,
Vector3 endEffTranslation) {
- std::pair<MatrixX3, VectorX> Ab = generateKinematicsConstraints(endEffRotation, endEffTranslation);
- std::pair<MatrixX3, VectorX> Cd = generateStabilityConstraints(normals, positions);
+ std::pair<MatrixX3, VectorX> Ab =
+ generateKinematicsConstraints(endEffRotation, endEffTranslation);
+ std::pair<MatrixX3, VectorX> Cd =
+ generateStabilityConstraints(normals, positions);
size_t numIneq = Ab.first.rows() + Cd.first.rows();
MatrixXX M(numIneq, 3);
VectorX n(numIneq);
@@ -153,7 +169,8 @@ double fRandom(double fMin, double fMax) {
return fMin + f * (fMax - fMin);
}
-ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPair& Cd) {
+ConstraintsPair stackConstraints(const ConstraintsPair& Ab,
+ const ConstraintsPair& Cd) {
size_t numIneq = Ab.first.rows() + Cd.first.rows();
MatrixX3 M(numIneq, 3);
VectorX n(numIneq);
@@ -164,7 +181,8 @@ ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPai
return std::make_pair(M, n);
}
-bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3& point) {
+bool verifyKinematicConstraints(const ConstraintsPair& Ab,
+ const Vector3& point) {
for (long int i = 0; i < Ab.second.size(); ++i) {
if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
return false;
@@ -173,21 +191,24 @@ bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3& point)
return true;
}
-bool verifyStabilityConstraintsDLP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
- Vector3 ddc) {
+bool verifyStabilityConstraintsDLP(
+ centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
+ Vector3 ddc) {
bool success(false);
double res;
centroidal_dynamics::Equilibrium contactPhaseDLP(contactPhase);
contactPhaseDLP.setAlgorithm(centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
- centroidal_dynamics::LP_status status = contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
- success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL || status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
+ centroidal_dynamics::LP_status status =
+ contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
+ success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL ||
+ status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
if (success) success = res >= -EPSILON;
if (!success) std::cout << "fail level " << res << std::endl;
return success;
}
-bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
- Vector3 acc) {
+bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase,
+ Vector3 c, Vector3 /*dc*/, Vector3 acc) {
// compute inequalities :
const Vector3& g = contactPhase.m_gravity;
const Matrix3 gSkew = bezier_com_traj::skew(g);
--
GitLab