diff --git a/include/hpp/bezier-com-traj/common_solve_methods.hh b/include/hpp/bezier-com-traj/common_solve_methods.hh
index e2055939acccd86df82f658afe0e8d90c544495b..7d6bf248b96910559651dbb7bcb936e6b6dc641b 100644
--- a/include/hpp/bezier-com-traj/common_solve_methods.hh
+++ b/include/hpp/bezier-com-traj/common_solve_methods.hh
@@ -13,8 +13,7 @@
#include <Eigen/Dense>
-namespace bezier_com_traj
-{
+namespace bezier_com_traj {
/**
* @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
@@ -24,8 +23,8 @@ namespace bezier_com_traj
* @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<spline::Bern<double> >& bernstein, int numSteps);
+BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
+ const std::vector<waypoint6_t>& wps, const std::vector<spline::Bern<double> >& bernstein, int numSteps);
/**
* @brief compute6dControlPointInequalities Given linear and angular control waypoints,
@@ -37,14 +36,14 @@ 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.
+ * 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
@@ -52,9 +51,9 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequa
* @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
@@ -63,17 +62,16 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEquali
* @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 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
@@ -84,11 +82,9 @@ 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
@@ -96,39 +92,38 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b,
* @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
* @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 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
+} // end namespace bezier_com_traj
#include "common_solve_methods.inl"
-
#endif
diff --git a/include/hpp/bezier-com-traj/cost/costfunction_definition.hh b/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
index 56bb81d97492bcc0bff407ccfcce49dc316d71f7..349757c051b9b95f20cf270eee56b36380d73b7a 100644
--- a/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
+++ b/include/hpp/bezier-com-traj/cost/costfunction_definition.hh
@@ -9,15 +9,12 @@
#include <hpp/bezier-com-traj/data.hh>
#include "boost/assign.hpp"
-
-
-namespace bezier_com_traj{
+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 {
-
/** @brief genCostFunction generate a cost function according to the constraints
* of the problem, and the flag selected in ProblemData.
* The cost has the form x' H x + 2 g' x.
@@ -26,10 +23,10 @@ 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, VectorX& g);
+void genCostFunction(const ProblemData& pData, const VectorX& Ts, const double T, const T_time& timeArray, MatrixXX& H,
+ VectorX& g);
-} // namespace cost
-} // namespace bezier_com_traj
+} // namespace cost
+} // namespace bezier_com_traj
#endif
diff --git a/include/hpp/bezier-com-traj/data.hh b/include/hpp/bezier-com-traj/data.hh
index 2f4c5ef312e36bcf72263d6b4344e966fce0c2be..12b1363151fcec1895937e160ecc13ebe15fc03a 100644
--- a/include/hpp/bezier-com-traj/data.hh
+++ b/include/hpp/bezier-com-traj/data.hh
@@ -17,126 +17,117 @@
#include <vector>
-namespace bezier_com_traj
-{
- /**
- * @brief Contact data contains all the contact information
- * relative to a contact phase: contact points and normals
- * (within Equilibrium object), as well as any additional
- * kinematic and angular constraints.
- */
- struct BEZIER_COM_TRAJ_DLLAPI ContactData
- {
- ContactData()
- : contactPhase_(0)
- , Kin_(Eigen::Matrix3d::Zero())
- , kin_(VectorX::Zero(0))
- , Ang_(Eigen::Matrix3d::Zero())
- , ang_(VectorX::Zero(0)) {}
-
- ContactData(const ContactData& other)
- : contactPhase_(new centroidal_dynamics::Equilibrium(*(other.contactPhase_)))
- , Kin_(other.Kin_)
- , kin_(other.kin_)
- , Ang_(other.Ang_)
- , ang_(other.ang_){}
-
- ContactData(centroidal_dynamics::Equilibrium* contactPhase)
- : contactPhase_(contactPhase)
- , Kin_(Eigen::Matrix3d::Zero())
- , kin_(VectorX::Zero(0))
- , Ang_(Eigen::Matrix3d::Zero())
- , ang_(VectorX::Zero(0)) {}
- ~ContactData(){}
-
- centroidal_dynamics::Equilibrium* contactPhase_;
- MatrixX3 Kin_; // inequality kinematic constraints
- VectorX kin_;
- MatrixX3 Ang_; // inequality angular momentum constraints
- VectorX ang_;
- };
-
- /**
- * @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.
- */
- struct BEZIER_COM_TRAJ_DLLAPI Constraints
- {
- Constraints()
- : flag_(INIT_POS | INIT_VEL | END_VEL | END_POS)
- , constraintAcceleration_(false)
- , maxAcceleration_(10.)
- , reduce_h_(1e-3) {}
-
- Constraints(ConstraintFlag flag)
- : flag_(flag)
- , constraintAcceleration_(false)
- , maxAcceleration_(10.)
- , reduce_h_(1e-3) {}
-
- ~Constraints(){}
-
- ConstraintFlag flag_;
- bool constraintAcceleration_;
- double maxAcceleration_;
- double reduce_h_;
- };
-
-
- /**
- * @brief Defines all the inputs of the problem:
- * Initial and terminal constraints, as well as selected
- * cost functions. Also,a list of ContactData defines the
- * different phases of the problem. While the method
- * can handle any phase greater than one, using more
- * than three phases is probably too constraining.
- */
- struct BEZIER_COM_TRAJ_DLLAPI ProblemData
- {
- ProblemData()
- : c0_ (point_t::Zero())
- ,dc0_ (point_t::Zero())
- ,ddc0_(point_t::Zero())
- ,j0_(point_t::Zero())
- , c1_ (point_t::Zero())
- ,dc1_ (point_t::Zero())
- ,ddc1_(point_t::Zero())
- ,j1_(point_t::Zero())
- ,useAngularMomentum_(false)
- ,costFunction_(ACCELERATION)
- ,representation_(DOUBLE_DESCRIPTION) {}
-
- std::vector<ContactData> contacts_;
- point_t c0_,dc0_,ddc0_,j0_,c1_,dc1_,ddc1_,j1_;
- point_t l0_;
- bool useAngularMomentum_;
- Constraints constraints_;
- CostFunction costFunction_;
- GIWCRepresentation representation_;
- };
-
- typedef solvers::ResultData ResultData;
-
- /**
- * @brief Specialized ResultData that computes the Bezier curves
- * corresponding to the computed trajectory
- */
- struct BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj : public ResultData
- {
- ResultDataCOMTraj():
- ResultData()
- , c_of_t_(bezier_t::zero())
- , dL_of_t_(bezier_t::zero())
- , dc1_(point_t::Zero())
- , ddc1_(point_t::Zero()) {}
- ~ResultDataCOMTraj(){}
-
- bezier_t c_of_t_; // center of mass trajectory
- bezier_t dL_of_t_; // angular momentum derivative trajectory
- point_t dc1_; // terminal velocity
- point_t ddc1_; //terminal acceleration
- };
-} // end namespace bezier_com_traj
+namespace bezier_com_traj {
+/**
+ * @brief Contact data contains all the contact information
+ * relative to a contact phase: contact points and normals
+ * (within Equilibrium object), as well as any additional
+ * kinematic and angular constraints.
+ */
+struct BEZIER_COM_TRAJ_DLLAPI ContactData {
+ ContactData()
+ : contactPhase_(0),
+ Kin_(Eigen::Matrix3d::Zero()),
+ kin_(VectorX::Zero(0)),
+ Ang_(Eigen::Matrix3d::Zero()),
+ ang_(VectorX::Zero(0)) {}
+
+ ContactData(const ContactData& other)
+ : contactPhase_(new centroidal_dynamics::Equilibrium(*(other.contactPhase_))),
+ Kin_(other.Kin_),
+ kin_(other.kin_),
+ Ang_(other.Ang_),
+ ang_(other.ang_) {}
+
+ ContactData(centroidal_dynamics::Equilibrium* contactPhase)
+ : contactPhase_(contactPhase),
+ Kin_(Eigen::Matrix3d::Zero()),
+ kin_(VectorX::Zero(0)),
+ Ang_(Eigen::Matrix3d::Zero()),
+ ang_(VectorX::Zero(0)) {}
+ ~ContactData() {}
+
+ centroidal_dynamics::Equilibrium* contactPhase_;
+ MatrixX3 Kin_; // inequality kinematic constraints
+ VectorX kin_;
+ MatrixX3 Ang_; // inequality angular momentum constraints
+ VectorX ang_;
+};
+
+/**
+ * @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.
+ */
+struct BEZIER_COM_TRAJ_DLLAPI Constraints {
+ Constraints()
+ : flag_(INIT_POS | INIT_VEL | END_VEL | END_POS),
+ constraintAcceleration_(false),
+ maxAcceleration_(10.),
+ reduce_h_(1e-3) {}
+
+ Constraints(ConstraintFlag flag)
+ : flag_(flag), constraintAcceleration_(false), maxAcceleration_(10.), reduce_h_(1e-3) {}
+
+ ~Constraints() {}
+
+ ConstraintFlag flag_;
+ bool constraintAcceleration_;
+ double maxAcceleration_;
+ double reduce_h_;
+};
+
+/**
+ * @brief Defines all the inputs of the problem:
+ * Initial and terminal constraints, as well as selected
+ * cost functions. Also,a list of ContactData defines the
+ * different phases of the problem. While the method
+ * can handle any phase greater than one, using more
+ * than three phases is probably too constraining.
+ */
+struct BEZIER_COM_TRAJ_DLLAPI ProblemData {
+ ProblemData()
+ : c0_(point_t::Zero()),
+ dc0_(point_t::Zero()),
+ ddc0_(point_t::Zero()),
+ j0_(point_t::Zero()),
+ c1_(point_t::Zero()),
+ dc1_(point_t::Zero()),
+ ddc1_(point_t::Zero()),
+ j1_(point_t::Zero()),
+ useAngularMomentum_(false),
+ costFunction_(ACCELERATION),
+ representation_(DOUBLE_DESCRIPTION) {}
+
+ std::vector<ContactData> contacts_;
+ point_t c0_, dc0_, ddc0_, j0_, c1_, dc1_, ddc1_, j1_;
+ point_t l0_;
+ bool useAngularMomentum_;
+ Constraints constraints_;
+ CostFunction costFunction_;
+ GIWCRepresentation representation_;
+};
+
+typedef solvers::ResultData ResultData;
+
+/**
+ * @brief Specialized ResultData that computes the Bezier curves
+ * corresponding to the computed trajectory
+ */
+struct BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj : public ResultData {
+ ResultDataCOMTraj()
+ : ResultData(),
+ c_of_t_(bezier_t::zero()),
+ dL_of_t_(bezier_t::zero()),
+ dc1_(point_t::Zero()),
+ ddc1_(point_t::Zero()) {}
+ ~ResultDataCOMTraj() {}
+
+ bezier_t c_of_t_; // center of mass trajectory
+ bezier_t dL_of_t_; // angular momentum derivative trajectory
+ point_t dc1_; // terminal velocity
+ point_t ddc1_; // terminal acceleration
+};
+} // end namespace bezier_com_traj
#endif
diff --git a/include/hpp/bezier-com-traj/definitions.hh b/include/hpp/bezier-com-traj/definitions.hh
index 3d86ecf8a09fa3391643ec6aaf84b2e515ffd80c..4d68b1da4211d8afaf81b95653751cf64b09dc5c 100644
--- a/include/hpp/bezier-com-traj/definitions.hh
+++ b/include/hpp/bezier-com-traj/definitions.hh
@@ -10,29 +10,28 @@
#include <hpp/spline/bezier_curve.h>
#include <Eigen/Dense>
-namespace bezier_com_traj
-{
+namespace bezier_com_traj {
typedef double value_type;
-typedef Eigen::Matrix <value_type, 3, 3> Matrix3;
-typedef Eigen::Matrix <value_type, 6, 3> Matrix63;
-typedef Eigen::Matrix <value_type, 3, 9> Matrix39;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3> MatrixX3;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
+typedef Eigen::Matrix<value_type, 3, 3> Matrix3;
+typedef Eigen::Matrix<value_type, 6, 3> Matrix63;
+typedef Eigen::Matrix<value_type, 3, 9> Matrix39;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> MatrixX3;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
typedef centroidal_dynamics::Vector3 Vector3;
typedef centroidal_dynamics::Vector6 Vector6;
typedef centroidal_dynamics::VectorX VectorX;
-typedef Eigen::Ref<Vector3> Ref_vector3;
-typedef Eigen::Ref<VectorX> Ref_vectorX;
-typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
-typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+typedef Eigen::Ref<Vector3> Ref_vector3;
+typedef Eigen::Ref<VectorX> Ref_vectorX;
+typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
-typedef const Eigen::Ref<const Vector3> & Cref_vector3;
-typedef const Eigen::Ref<const Vector6> & Cref_vector6;
-typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
-typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
-typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
+typedef const Eigen::Ref<const Vector3>& Cref_vector3;
+typedef const Eigen::Ref<const Vector6>& Cref_vector6;
+typedef const Eigen::Ref<const VectorX>& Cref_vectorX;
+typedef const Eigen::Ref<const MatrixXX>& Cref_matrixXX;
+typedef const Eigen::Ref<const MatrixX3>& Cref_matrixX3;
typedef Matrix63 matrix6_t;
typedef Vector6 point6_t;
@@ -43,27 +42,24 @@ typedef Eigen::Vector3d point_t;
typedef const Eigen::Ref<const point_t>& point_t_tC;
/**
-* @brief waypoint_t a waypoint is composed of a 6*3 matrix that depend
-* on the variable x, and of a 6d vector independent of x, such that
-* each control point of the target bezier curve is given by pi = wix * x + wis
-*/
+ * @brief waypoint_t a waypoint is composed of a 6*3 matrix that depend
+ * on the variable x, and of a 6d vector independent of x, such that
+ * each control point of the target bezier curve is given by pi = wix * x + wis
+ */
typedef std::pair<matrix6_t, point6_t> waypoint6_t;
typedef std::pair<matrix3_t, point3_t> waypoint3_t;
typedef std::pair<Matrix39, point3_t> waypoint9_t;
-struct waypoint_t; // forward declaration
-
+struct waypoint_t; // forward declaration
-typedef spline::bezier_curve <double, double, 3, true, point_t > bezier_t;
-typedef spline::bezier_curve <double, double, 3, true, waypoint_t > bezier_wp_t;
-typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
+typedef spline::bezier_curve<double, double, 3, true, point_t> bezier_t;
+typedef spline::bezier_curve<double, double, 3, true, waypoint_t> bezier_wp_t;
+typedef spline::bezier_curve<double, double, 6, true, point6_t> bezier6_t;
-typedef std::vector< std::pair<double, int> > T_time;
+typedef std::vector<std::pair<double, int> > T_time;
typedef T_time::const_iterator CIT_time;
+typedef std::pair<double, point3_t> coefs_t;
-
-typedef std::pair<double,point3_t> coefs_t;
-
-} // end namespace bezier_com_traj
+} // end namespace bezier_com_traj
#endif
diff --git a/include/hpp/bezier-com-traj/flags.hh b/include/hpp/bezier-com-traj/flags.hh
index 59a39d1683d8d2608b29b538bc2c9b066eb8639e..b155e5aff4a2455832cf71b201f5e97faed09eda 100644
--- a/include/hpp/bezier-com-traj/flags.hh
+++ b/include/hpp/bezier-com-traj/flags.hh
@@ -8,59 +8,63 @@
#include <hpp/bezier-com-traj/local_config.hh>
-namespace bezier_com_traj
-{
- enum BEZIER_COM_TRAJ_DLLAPI CostFunction{
- ACCELERATION = 0x00001,
- DISTANCE_TRAVELED = 0x00002,
- TARGET_END_VELOCITY = 0x00004,
- UNKNOWN_COST = 0x00008
- };
+namespace bezier_com_traj {
+enum BEZIER_COM_TRAJ_DLLAPI CostFunction {
+ ACCELERATION = 0x00001,
+ DISTANCE_TRAVELED = 0x00002,
+ TARGET_END_VELOCITY = 0x00004,
+ UNKNOWN_COST = 0x00008
+};
- enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag{
- INIT_POS = 0x00001,
- INIT_VEL = 0x00002,
- INIT_ACC = 0x00004,
- END_POS = 0x00008,
- END_VEL = 0x00010,
- END_ACC = 0x00020,
- INIT_JERK = 0x00040,
- END_JERK = 0x00080,
- ONE_FREE_VAR = 0x00000,
- TWO_FREE_VAR = 0x00100,
- THREE_FREE_VAR = 0x00200,
- FOUR_FREE_VAR = 0x00400,
- FIVE_FREE_VAR = 0x00800,
- UNKNOWN = 0x01000
- };
+enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag {
+ INIT_POS = 0x00001,
+ INIT_VEL = 0x00002,
+ INIT_ACC = 0x00004,
+ END_POS = 0x00008,
+ END_VEL = 0x00010,
+ END_ACC = 0x00020,
+ INIT_JERK = 0x00040,
+ END_JERK = 0x00080,
+ ONE_FREE_VAR = 0x00000,
+ TWO_FREE_VAR = 0x00100,
+ THREE_FREE_VAR = 0x00200,
+ FOUR_FREE_VAR = 0x00400,
+ FIVE_FREE_VAR = 0x00800,
+ UNKNOWN = 0x01000
+};
- enum BEZIER_COM_TRAJ_DLLAPI GIWCRepresentation{
- DOUBLE_DESCRIPTION = 0x00001,
- FORCE = 0x00002,
- UNKNOWN_REPRESENTATION = 0x00004
- };
+enum BEZIER_COM_TRAJ_DLLAPI GIWCRepresentation {
+ DOUBLE_DESCRIPTION = 0x00001,
+ FORCE = 0x00002,
+ 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));}
+inline ConstraintFlag operator|(ConstraintFlag a, ConstraintFlag 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));}
+inline ConstraintFlag operator&(ConstraintFlag a, ConstraintFlag 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));}
+inline ConstraintFlag operator^(ConstraintFlag a, ConstraintFlag b) {
+ return static_cast<ConstraintFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));
+}
- inline ConstraintFlag& operator|=(ConstraintFlag& a, ConstraintFlag b)
- {return (ConstraintFlag&)((int&)(a) |= static_cast<const int>(b));}
+inline ConstraintFlag& operator|=(ConstraintFlag& a, ConstraintFlag b) {
+ return (ConstraintFlag&)((int&)(a) |= static_cast<const int>(b));
+}
- inline ConstraintFlag& operator&=(ConstraintFlag& a, ConstraintFlag b)
- {return (ConstraintFlag&)((int&)(a) &= static_cast<const int>(b));}
+inline ConstraintFlag& operator&=(ConstraintFlag& a, ConstraintFlag b) {
+ return (ConstraintFlag&)((int&)(a) &= static_cast<const int>(b));
+}
- inline ConstraintFlag& operator^=(ConstraintFlag& a, ConstraintFlag b)
- {return (ConstraintFlag&)((int&)(a) ^= static_cast<const int>(b));}
+inline ConstraintFlag& operator^=(ConstraintFlag& a, ConstraintFlag b) {
+ return (ConstraintFlag&)((int&)(a) ^= static_cast<const int>(b));
+}
-} // end namespace bezier_com_traj
+} // end namespace bezier_com_traj
#endif
diff --git a/include/hpp/bezier-com-traj/local_config.hh b/include/hpp/bezier-com-traj/local_config.hh
index 967616916bd382443e5d3bd9fb4414e3a60c1e79..9538a07df0537f6bf9c54bd3cf6f4f408520107c 100644
--- a/include/hpp/bezier-com-traj/local_config.hh
+++ b/include/hpp/bezier-com-traj/local_config.hh
@@ -7,7 +7,7 @@
#define _BEZIER_COM_TRAJ_LIB_CONFIG_HH
// Package version (header).
-# define BEZIER_COM_TRAJ_VERSION "UNKNOWN"
+#define BEZIER_COM_TRAJ_VERSION "UNKNOWN"
// Handle portable symbol export.
// Defining manually which symbol should be exported is required
@@ -19,39 +19,39 @@
//
// On Linux, set the visibility accordingly. If C++ symbol visibility
// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
-# if defined _WIN32 || defined __CYGWIN__
+#if defined _WIN32 || defined __CYGWIN__
// On Microsoft Windows, use dllimport and dllexport to tag symbols.
-# define BEZIER_COM_TRAJ_DLLIMPORT __declspec(dllimport)
-# define BEZIER_COM_TRAJ_DLLEXPORT __declspec(dllexport)
-# define BEZIER_COM_TRAJ_DLLLOCAL
-# else
+#define BEZIER_COM_TRAJ_DLLIMPORT __declspec(dllimport)
+#define BEZIER_COM_TRAJ_DLLEXPORT __declspec(dllexport)
+#define BEZIER_COM_TRAJ_DLLLOCAL
+#else
// On Linux, for GCC >= 4, tag symbols using GCC extension.
-# if __GNUC__ >= 4
-# define BEZIER_COM_TRAJ_DLLIMPORT __attribute__ ((visibility("default")))
-# define BEZIER_COM_TRAJ_DLLEXPORT __attribute__ ((visibility("default")))
-# define BEZIER_COM_TRAJ_DLLLOCAL __attribute__ ((visibility("hidden")))
-# else
+#if __GNUC__ >= 4
+#define BEZIER_COM_TRAJ_DLLIMPORT __attribute__((visibility("default")))
+#define BEZIER_COM_TRAJ_DLLEXPORT __attribute__((visibility("default")))
+#define BEZIER_COM_TRAJ_DLLLOCAL __attribute__((visibility("hidden")))
+#else
// Otherwise (GCC < 4 or another compiler is used), export everything.
-# define BEZIER_COM_TRAJ_DLLIMPORT
-# define BEZIER_COM_TRAJ_DLLEXPORT
-# define BEZIER_COM_TRAJ_DLLLOCAL
-# endif // __GNUC__ >= 4
-# endif // defined _WIN32 || defined __CYGWIN__
+#define BEZIER_COM_TRAJ_DLLIMPORT
+#define BEZIER_COM_TRAJ_DLLEXPORT
+#define BEZIER_COM_TRAJ_DLLLOCAL
+#endif // __GNUC__ >= 4
+#endif // defined _WIN32 || defined __CYGWIN__
-# ifdef BEZIER_COM_TRAJ_STATIC
+#ifdef BEZIER_COM_TRAJ_STATIC
// If one is using the library statically, get rid of
// extra information.
-# define BEZIER_COM_TRAJ_DLLAPI
-# define BEZIER_COM_TRAJ_LOCAL
-# else
+#define BEZIER_COM_TRAJ_DLLAPI
+#define BEZIER_COM_TRAJ_LOCAL
+#else
// Depending on whether one is building or using the
// library define DLLAPI to import or export.
-# ifdef BEZIER_COM_TRAJ_EXPORTS
-# define BEZIER_COM_TRAJ_DLLAPI BEZIER_COM_TRAJ_DLLEXPORT
-# else
-# define BEZIER_COM_TRAJ_DLLAPI BEZIER_COM_TRAJ_DLLIMPORT
-# endif // BEZIER_COM_TRAJ_EXPORTS
-# define BEZIER_COM_TRAJ_LOCAL BEZIER_COM_TRAJ_DLLLOCAL
-# endif // BEZIER_COM_TRAJ_STATIC
+#ifdef BEZIER_COM_TRAJ_EXPORTS
+#define BEZIER_COM_TRAJ_DLLAPI BEZIER_COM_TRAJ_DLLEXPORT
+#else
+#define BEZIER_COM_TRAJ_DLLAPI BEZIER_COM_TRAJ_DLLIMPORT
+#endif // BEZIER_COM_TRAJ_EXPORTS
+#define BEZIER_COM_TRAJ_LOCAL BEZIER_COM_TRAJ_DLLLOCAL
+#endif // BEZIER_COM_TRAJ_STATIC
-#endif //_BEZIER_COM_TRAJ_LIB_CONFIG_HH
+#endif //_BEZIER_COM_TRAJ_LIB_CONFIG_HH
diff --git a/include/hpp/bezier-com-traj/solve.hh b/include/hpp/bezier-com-traj/solve.hh
index 1e93a45b504e3f88361f6890c214835841f9f153..da247b7c59e3bec1cb4097527a65697db1d04827 100644
--- a/include/hpp/bezier-com-traj/solve.hh
+++ b/include/hpp/bezier-com-traj/solve.hh
@@ -10,49 +10,49 @@
#include <hpp/bezier-com-traj/data.hh>
#include <Eigen/Dense>
-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.
- * In this specific implementation, the considered constraints are:
- * 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.
- */
- 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
- * @param pData problem Data.
- * @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.
- */
- 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
- * @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.
- */
- BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, const double timeStep = -1, const solvers::SolverType solver = solvers::SOLVER_QUADPROG );
-
+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.
+ * In this specific implementation, the considered constraints are:
+ * 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.
+ */
+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
+ * @param pData problem Data.
+ * @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.
+ */
+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
+ * @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.
+ */
+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
+} // end namespace bezier_com_traj
#endif
diff --git a/include/hpp/bezier-com-traj/solve_end_effector.hh b/include/hpp/bezier-com-traj/solve_end_effector.hh
index 0d68a0fec99da51ad7ad431e35e6ae9764d3e4de..ed33fd9404062a54a8a5f1595f399e7f95780d7a 100644
--- a/include/hpp/bezier-com-traj/solve_end_effector.hh
+++ b/include/hpp/bezier-com-traj/solve_end_effector.hh
@@ -10,177 +10,178 @@
using namespace bezier_com_traj;
-namespace bezier_com_traj
-{
-typedef std::pair<double,point3_t> coefs_t;
-const int DIM_POINT=3;
-//const int NUM_DISCRETIZATION = 11;
+namespace bezier_com_traj {
+typedef std::pair<double, point3_t> coefs_t;
+const int DIM_POINT = 3;
+// const int NUM_DISCRETIZATION = 11;
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
-* @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 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.
-*/
-template<typename Path>
-ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, const double T, const double weightDistance, bool useVelCost = true);
-
-
-coefs_t initCoefs(){
- coefs_t c;
- c.first=0;
- c.second=point3_t::Zero();
- return c;
+ * @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 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.
+ */
+template <typename Path>
+ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path, const double T,
+ const double weightDistance, bool useVelCost = true);
+
+coefs_t initCoefs() {
+ coefs_t c;
+ c.first = 0;
+ c.second = point3_t::Zero();
+ 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){
- 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)));//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))) ; //ddc1 // * T ??
- // pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // dc1
- pts.push_back(pData.c1_); //c1
- return pts;
+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 - 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))) ;
+ // //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){
- 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))); //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((-pData.dc1_ * T / n) + pData.c1_); // dc1
- pts.push_back(pData.c1_); //c1
- return pts;
+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)));
+ // //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((-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");
- 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++;
+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_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++;
+ }
+
+ A = MatrixXX::Zero(
+ (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]
+ 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) {
+ 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;
+ ++i;
}
- 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++;
+ }
+ // lower acc bounds
+ 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;
+ ++i;
}
- 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++;
+ }
+
+ // upper velocity bounds
+ 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;
+ ++i;
}
-
- A = MatrixXX::Zero((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]
- 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)
- {
- 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;
- ++i;
- }
+ }
+ // lower velocity bounds
+ 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;
+ ++i;
}
- //lower acc bounds
- 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;
- ++i;
- }
+ }
+
+ // upper jerk bounds
+ 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;
+ ++i;
}
-
- //upper velocity bounds
- 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;
- ++i;
- }
- }
- //lower velocity bounds
- 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;
- ++i;
- }
- }
-
-
- //upper jerk bounds
- 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;
- ++i;
- }
+ }
+ // lower jerk bounds
+ 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;
+ ++i;
}
- //lower jerk bounds
- 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;
- ++i;
- }
- }
-
- // 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
- MatrixXX mxz = MatrixXX::Zero(DIM_VAR,DIM_VAR);
- size_t j = DIM_POINT-1;
- VectorX nxz = VectorX::Zero(DIM_VAR);
- while(j < (DIM_VAR)){
- mxz(j,j) = -1;
- nxz[j]= - std::min(pData.c0_[2],pData.c1_[2]);
- j += DIM_POINT;
- }
- A.block(i*DIM_POINT,0,DIM_VAR,DIM_VAR) = mxz;
- b.segment(i*DIM_POINT,DIM_VAR) = nxz;
+ }
+
+ // 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
+ MatrixXX mxz = MatrixXX::Zero(DIM_VAR, DIM_VAR);
+ size_t j = DIM_POINT - 1;
+ VectorX nxz = VectorX::Zero(DIM_VAR);
+ while (j < (DIM_VAR)) {
+ mxz(j, j) = -1;
+ nxz[j] = -std::min(pData.c0_[2], pData.c1_[2]);
+ j += DIM_POINT;
+ }
+ 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");
- //TEST :
+ 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);
i++;
@@ -188,228 +189,222 @@ void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoin
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);*/
}
-std::pair<MatrixXX,VectorX> computeDistanceCostFunction(int 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./(numPoints-1);
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- waypoint_t c_wp;
- MatrixXX H = MatrixXX::Zero(dimVar(pData),dimVar(pData));
- VectorX g = VectorX::Zero(dimVar(pData));
- point3_t pk;
- for(size_t i = 0 ; i < numPoints ; ++i){
- c_wp = evaluateCurveWaypointAtTime(pData,pi,i*step);
- pk = pts_path[i];
- // std::cout<<"pk = "<<pk.transpose()<<std::endl;
- // std::cout<<"coef First : "<<ckcit->first<<std::endl;
- // std::cout<<"coef second : "<<ckcit->second.transpose()<<std::endl;
- H += (c_wp.first.transpose() * c_wp.first);
- g += ((c_wp.second - pk).transpose() * c_wp.first).transpose();
- }
- double norm=H.norm();
- H /= norm;
- g /= norm;
- return std::make_pair(H,g);
+std::pair<MatrixXX, VectorX> computeDistanceCostFunction(int 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. / (numPoints - 1);
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ waypoint_t c_wp;
+ MatrixXX H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
+ VectorX g = VectorX::Zero(dimVar(pData));
+ point3_t pk;
+ for (size_t i = 0; i < numPoints; ++i) {
+ c_wp = evaluateCurveWaypointAtTime(pData, pi, i * step);
+ pk = pts_path[i];
+ // std::cout<<"pk = "<<pk.transpose()<<std::endl;
+ // std::cout<<"coef First : "<<ckcit->first<<std::endl;
+ // std::cout<<"coef second : "<<ckcit->second.transpose()<<std::endl;
+ H += (c_wp.first.transpose() * c_wp.first);
+ g += ((c_wp.second - pk).transpose() * c_wp.first).transpose();
+ }
+ double norm = H.norm();
+ H /= norm;
+ g /= norm;
+ return std::make_pair(H, g);
}
-
template <typename Path>
-std::pair<MatrixXX,VectorX> computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,const Path& path){
- double step = 1./(numPoints-1);
- std::vector<point3_t> pts_path;
- for(size_t i = 0 ; i < numPoints ; ++i)
- pts_path.push_back(path((double)(i*step)));
- return computeDistanceCostFunction(numPoints,pData,T,pts_path);
+std::pair<MatrixXX, VectorX> computeDistanceCostFunction(int numPoints, const ProblemData& pData, double T,
+ const Path& path) {
+ double step = 1. / (numPoints - 1);
+ std::vector<point3_t> pts_path;
+ for (size_t i = 0; i < numPoints; ++i) pts_path.push_back(path((double)(i * step)));
+ return computeDistanceCostFunction(numPoints, pData, T, pts_path);
}
-//TODO
-void computeC_of_T (const ProblemData& pData,double T, ResultDataCOMTraj& res){
- std::vector<Vector3> wps = computeConstantWaypoints(pData,T);
- if(dimVar(pData) == 3 )
- wps[4] = res.x; //FIXME : compute id from constraints
- else if(dimVar(pData) == 9){
- wps[4] = res.x.segment<3>(0);
- wps[5] = res.x.segment<3>(3);
- wps[6] = res.x.segment<3>(6);
- }else if(dimVar(pData) ==15){
- wps[4] = res.x.segment<3>(0);
- wps[5] = res.x.segment<3>(3);
- wps[6] = res.x.segment<3>(6);
- wps[7] = res.x.segment<3>(9);
- wps[8] = res.x.segment<3>(12);
- }
- res.c_of_t_ = bezier_t (wps.begin(), wps.end(),T);
- if(verbose)
- std::cout<<"bezier curve created, size = "<<res.c_of_t_.size_<<std::endl;
+// TODO
+void computeC_of_T(const ProblemData& pData, double T, ResultDataCOMTraj& res) {
+ std::vector<Vector3> wps = computeConstantWaypoints(pData, T);
+ if (dimVar(pData) == 3)
+ wps[4] = res.x; // FIXME : compute id from constraints
+ else if (dimVar(pData) == 9) {
+ wps[4] = res.x.segment<3>(0);
+ wps[5] = res.x.segment<3>(3);
+ wps[6] = res.x.segment<3>(6);
+ } else if (dimVar(pData) == 15) {
+ wps[4] = res.x.segment<3>(0);
+ wps[5] = res.x.segment<3>(3);
+ wps[6] = res.x.segment<3>(6);
+ wps[7] = res.x.segment<3>(9);
+ wps[8] = res.x.segment<3>(12);
+ }
+ res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 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){
- 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(evaluateVelocityCurveWaypointAtTime(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){
- // H+=(ckcit->first.transpose() * ckcit->first);
- // g+=ckcit->second.transpose() * ckcit->first;
- for(size_t 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);
- }
+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);
+ for (int i = 0; i < numPoints; ++i) {
+ cks.push_back(evaluateVelocityCurveWaypointAtTime(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) {
+ // H+=(ckcit->first.transpose() * ckcit->first);
+ // g+=ckcit->second.transpose() * ckcit->first;
+ for (size_t 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);
}
- //TEST : don't consider z axis for minimum acceleration cost
- //H(2,2) = 1e-6;
- //g[2] = 1e-6 ;
- //normalize :
+ }
+ // TEST : don't consider z axis for minimum acceleration cost
+ // H(2,2) = 1e-6;
+ // g[2] = 1e-6 ;
+ // normalize :
// double norm=H.norm(); // because H is always diagonal
// H /= norm;
// g /= norm;
}
-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));
- }
- 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){
- H+=(ckcit->first.transpose() * ckcit->first);
- g+=ckcit->first.transpose()*ckcit->second;
- }
- //TEST : don't consider z axis for minimum acceleration cost
- //H(2,2) = 1e-6;
- //g[2] = 1e-6 ;
- //normalize :
- // double norm=H.norm(); // because H is always diagonal
+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));
+ }
+ 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) {
+ H += (ckcit->first.transpose() * ckcit->first);
+ g += ckcit->first.transpose() * ckcit->second;
+ }
+ // TEST : don't consider z axis for minimum acceleration cost
+ // H(2,2) = 1e-6;
+ // g[2] = 1e-6 ;
+ // normalize :
+ // double norm=H.norm(); // because H is always diagonal
// H /= norm;
// g /= norm;
}
-void computeJerkCostFunctionDiscretized(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(evaluateJerkCurveWaypointAtTime(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){
- H+=(ckcit->first.transpose() * ckcit->first);
- g+=ckcit->first.transpose()*ckcit->second;
- }
- //TEST : don't consider z axis for minimum acceleration cost
- //H(2,2) = 1e-6;
- //g[2] = 1e-6 ;
- //normalize :
- // double norm=H.norm(); // because H is always diagonal
- // H /= norm;
- // g /= norm;
+void computeJerkCostFunctionDiscretized(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(evaluateJerkCurveWaypointAtTime(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) {
+ H += (ckcit->first.transpose() * ckcit->first);
+ g += ckcit->first.transpose() * ckcit->second;
+ }
+ // TEST : don't consider z axis for minimum acceleration cost
+ // H(2,2) = 1e-6;
+ // g[2] = 1e-6 ;
+ // normalize :
+ // double norm=H.norm(); // because H is always diagonal
+ // H /= norm;
+ // g /= norm;
}
-
-
-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 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);
- return std::make_pair(A,b);
+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 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);
+ 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");
- double weightSmooth = 1. - weightDistance;
- const int DIM_VAR = dimVar(pData);
- // compute distance cost function (discrete integral under the curve defined by 'path')
- MatrixXX H;
- VectorX g;
- std::pair<MatrixXX,VectorX> Hg_smooth,Hg_rrt;
-
- if(weightDistance>0)
- Hg_rrt = computeDistanceCostFunction<Path>(50,pData,T,path);
- else{
- Hg_rrt.first=MatrixXX::Zero(DIM_VAR,DIM_VAR);
- Hg_rrt.second=VectorX::Zero(DIM_VAR);
- }
-
- Hg_smooth = computeVelocityCost(pData,T,pi);
+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')
+ MatrixXX H;
+ VectorX g;
+ std::pair<MatrixXX, VectorX> Hg_smooth, Hg_rrt;
+
+ if (weightDistance > 0)
+ Hg_rrt = computeDistanceCostFunction<Path>(50, pData, T, path);
+ else {
+ Hg_rrt.first = MatrixXX::Zero(DIM_VAR, DIM_VAR);
+ Hg_rrt.second = VectorX::Zero(DIM_VAR);
+ }
+
+ Hg_smooth = computeVelocityCost(pData, T, pi);
/* std::cout<<"End eff H_rrt = "<<std::endl<<H_rrt<<std::endl;
std::cout<<"End eff g_rrt = "<<std::endl<<g_rrt<<std::endl;
std::cout<<"End eff H_acc = "<<std::endl<<H_acc<<std::endl;
std::cout<<"End eff g_acc = "<<std::endl<<g_acc<<std::endl;
*/
- // add the costs :
- H = MatrixXX::Zero(DIM_VAR,DIM_VAR);
- g = VectorX::Zero(DIM_VAR);
- H = weightSmooth*(Hg_smooth.first) + weightDistance*Hg_rrt.first;
- g = weightSmooth*(Hg_smooth.second) + weightDistance*Hg_rrt.second;
- // H = Hg_smooth.first;
+ // add the costs :
+ H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
+ g = VectorX::Zero(DIM_VAR);
+ H = weightSmooth * (Hg_smooth.first) + weightDistance * Hg_rrt.first;
+ g = weightSmooth * (Hg_smooth.second) + weightDistance * Hg_rrt.second;
+ // H = Hg_smooth.first;
// g = Hg_smooth.second;
- return std::make_pair(H,g);
+ return std::make_pair(H, g);
}
-
template <typename Path>
-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);
- 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 g = "<<Hg.second.rows()<<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;
-
- ResultData resQp = solve(Ab,Hg, init);
-
- ResultDataCOMTraj res;
- if(resQp.success_)
- {
- res.success_ = true;
- res.x = resQp.x;
- // computeRealCost(pData, res);
- computeC_of_T (pData,T,res);
- // computedL_of_T(pData,Ts,res);
- }
- if(verbose){
- std::cout<<"Solved, success = "<<res.success_<<" x = "<<res.x.transpose()<<std::endl;
- std::cout<<"Final cost : "<<resQp.cost_<<std::endl;
- }
- return res;
+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);
+ 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 g = " << Hg.second.rows() << 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;
+
+ ResultData resQp = solve(Ab, Hg, init);
+
+ ResultDataCOMTraj res;
+ if (resQp.success_) {
+ res.success_ = true;
+ res.x = resQp.x;
+ // computeRealCost(pData, res);
+ computeC_of_T(pData, T, res);
+ // computedL_of_T(pData,Ts,res);
+ }
+ if (verbose) {
+ std::cout << "Solved, success = " << res.success_ << " x = " << res.x.transpose() << std::endl;
+ std::cout << "Final cost : " << resQp.cost_ << std::endl;
+ }
+ return res;
}
-
-}//namespace bezier_com_traj
+} // namespace bezier_com_traj
diff --git a/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp b/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
index 65775dd11d7ec1667cf152cba664dbd5ddd56694..168bd04b3567c009cfbce31928acf3d7476fe379 100644
--- a/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
+++ b/include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp
@@ -21,7 +21,7 @@
#include <Eigen/Dense>
#include <Eigen/Sparse>
-#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
+#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
#define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
@@ -36,244 +36,210 @@
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST START_PROFILER
-#define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
+#define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
#else
#define START_PROFILER_EIQUADPROG_FAST
#define STOP_PROFILER_EIQUADPROG_FAST
#endif
#define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec"
-#define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv"
-#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
-#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
-#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
-#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_2 "EIQUADPROG_FAST STEP_2"
-#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
-#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
-#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
+#define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv"
+#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
+#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
+#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
+#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_2 "EIQUADPROG_FAST STEP_2"
+#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
+#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
+#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
#define DEFAULT_MAX_ITER 1000
-namespace tsid
-{
-
- namespace solvers
- {
-
- /**
- * Possible states of the solver.
- */
- enum EiquadprogFast_status
- {
- EIQUADPROG_FAST_OPTIMAL=0,
- EIQUADPROG_FAST_INFEASIBLE=1,
- EIQUADPROG_FAST_UNBOUNDED=2,
- EIQUADPROG_FAST_MAX_ITER_REACHED=3,
- EIQUADPROG_FAST_REDUNDANT_EQUALITIES=4
- };
-
- class EiquadprogFast
- {
- public:
- typedef Eigen::MatrixXd MatrixXd;
- typedef Eigen::VectorXd VectorXd;
- typedef Eigen::VectorXi VectorXi;
-
- typedef Eigen::SparseMatrix<double> SpMat;
- typedef Eigen::SparseVector<double> SpVec;
- typedef Eigen::SparseVector<int> SpVeci;
-
- public:
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- EiquadprogFast();
- virtual ~EiquadprogFast();
-
- void reset(int dim_qp, int num_eq, int num_ineq);
-
- int getMaxIter() const { return m_maxIter; }
-
- bool setMaxIter(int maxIter)
- {
- if(maxIter<0)
- return false;
- m_maxIter = maxIter;
- return true;
- }
-
- /**
- * @return The size of the active set, namely the number of
- * active constraints (including the equalities).
- */
- int getActiveSetSize() const { return q; }
-
- /**
- * @return The number of active-set iteratios.
- */
- int getIteratios() const { return iter; }
-
- /**
- * @return The value of the objective function.
- */
- double getObjValue() const { return f_value; }
-
- /**
- * @return The Lagrange multipliers
- */
- const VectorXd & getLagrangeMultipliers() const { return u; }
- /**
- * Return the active set, namely the indeces of active constraints.
- * The first nEqCon indexes are for the equalities and are negative.
- * The last nIneqCon indexes are for the inequalities and start from 0.
- * Only the first q elements of the return vector are valid, where q
- * is the size of the active set.
- * @return The set of indexes of the active constraints.
- */
- const VectorXi & getActiveSet() const { return A; }
-
- /**
- * solves the problem
- * min. x' Hess x + 2 g0' x
- * 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);
- /**
- * 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);
-
- MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
- bool is_inverse_provided_;
-
- private:
- int m_nVars;
- int m_nEqCon;
- int m_nIneqCon;
-
- int m_maxIter; /// max number of active-set iterations
- double f_value; /// current value of cost function
-
- 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
- MatrixXd R; // <nVars,nVars>::d
-
- /// CI*x+ci0
- VectorXd s; // <nIneqCon>::d
-
- /// infeasibility multipliers, i.e. negative step direction in dual space
- VectorXd r; // <nIneqCon+nEqCon>::d
-
- /// Lagrange multipliers
- VectorXd u; // <nIneqCon+nEqCon>::d
-
- /// step direction in primal space
- VectorXd z; // <nVars>::d
-
- /// J' np
- VectorXd d; //<nVars>::d
-
- /// current constraint normal
- VectorXd np; //<nVars>::d
-
- /// active set (indeces of active constraints)
- /// the first nEqCon indeces are for the equalities and are negative
- /// the last nIneqCon indeces are for the inequalities are start from 0
- VectorXi A; // <nIneqCon+nEqCon>
-
- /// initialized as K \ A
- /// iai(i)=-1 iff inequality constraint i is in the active set
- /// iai(i)=i otherwise
- VectorXi iai; // <nIneqCon>::i
-
- /// 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
- VectorXi iaexcl; //<nIneqCon>::i
-
- VectorXd x_old; // old value of x <nVars>::d
- VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
- VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
-
+namespace tsid {
+
+namespace solvers {
+
+/**
+ * Possible states of the solver.
+ */
+enum EiquadprogFast_status {
+ EIQUADPROG_FAST_OPTIMAL = 0,
+ EIQUADPROG_FAST_INFEASIBLE = 1,
+ EIQUADPROG_FAST_UNBOUNDED = 2,
+ EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
+ EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
+};
+
+class EiquadprogFast {
+ public:
+ typedef Eigen::MatrixXd MatrixXd;
+ typedef Eigen::VectorXd VectorXd;
+ typedef Eigen::VectorXi VectorXi;
+
+ typedef Eigen::SparseMatrix<double> SpMat;
+ typedef Eigen::SparseVector<double> SpVec;
+ typedef Eigen::SparseVector<int> SpVeci;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ EiquadprogFast();
+ virtual ~EiquadprogFast();
+
+ void reset(int dim_qp, int num_eq, int num_ineq);
+
+ int getMaxIter() const { return m_maxIter; }
+
+ bool setMaxIter(int maxIter) {
+ if (maxIter < 0) return false;
+ m_maxIter = maxIter;
+ return true;
+ }
+
+ /**
+ * @return The size of the active set, namely the number of
+ * active constraints (including the equalities).
+ */
+ int getActiveSetSize() const { return q; }
+
+ /**
+ * @return The number of active-set iteratios.
+ */
+ int getIteratios() const { return iter; }
+
+ /**
+ * @return The value of the objective function.
+ */
+ double getObjValue() const { return f_value; }
+
+ /**
+ * @return The Lagrange multipliers
+ */
+ const VectorXd& getLagrangeMultipliers() const { return u; }
+ /**
+ * Return the active set, namely the indeces of active constraints.
+ * The first nEqCon indexes are for the equalities and are negative.
+ * The last nIneqCon indexes are for the inequalities and start from 0.
+ * Only the first q elements of the return vector are valid, where q
+ * is the size of the active set.
+ * @return The set of indexes of the active constraints.
+ */
+ const VectorXi& getActiveSet() const { return A; }
+
+ /**
+ * solves the problem
+ * min. x' Hess x + 2 g0' x
+ * 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);
+ /**
+ * 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);
+
+ MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
+ bool is_inverse_provided_;
+
+ private:
+ int m_nVars;
+ int m_nEqCon;
+ int m_nIneqCon;
+
+ int m_maxIter; /// max number of active-set iterations
+ double f_value; /// current value of cost function
+
+ 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
+ MatrixXd R; // <nVars,nVars>::d
+
+ /// CI*x+ci0
+ VectorXd s; // <nIneqCon>::d
+
+ /// infeasibility multipliers, i.e. negative step direction in dual space
+ VectorXd r; // <nIneqCon+nEqCon>::d
+
+ /// Lagrange multipliers
+ VectorXd u; // <nIneqCon+nEqCon>::d
+
+ /// step direction in primal space
+ VectorXd z; // <nVars>::d
+
+ /// J' np
+ VectorXd d; //<nVars>::d
+
+ /// current constraint normal
+ VectorXd np; //<nVars>::d
+
+ /// active set (indeces of active constraints)
+ /// the first nEqCon indeces are for the equalities and are negative
+ /// the last nIneqCon indeces are for the inequalities are start from 0
+ VectorXi A; // <nIneqCon+nEqCon>
+
+ /// initialized as K \ A
+ /// iai(i)=-1 iff inequality constraint i is in the active set
+ /// iai(i)=i otherwise
+ VectorXi iai; // <nIneqCon>::i
+
+ /// 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
+ VectorXi iaexcl; //<nIneqCon>::i
+
+ VectorXd x_old; // old value of x <nVars>::d
+ VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
+ VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
+
#ifdef OPTIMIZE_ADD_CONSTRAINT
- VectorXd T1; /// tmp variable used in add_constraint
+ VectorXd T1; /// tmp variable used in add_constraint
#endif
- /// size of the active set A (containing the indices of the active constraints)
- int q;
+ /// size of the active set A (containing the indices of the active constraints)
+ int q;
- /// number of active-set iterations
- int iter;
+ /// number of active-set iterations
+ int iter;
- inline void compute_d(VectorXd & d,
- const MatrixXd & J,
- const VectorXd & np)
- {
+ inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
#ifdef OPTIMIZE_COMPUTE_D
- d.noalias() = J.adjoint() * np;
+ d.noalias() = J.adjoint() * np;
#else
- d = J.adjoint() * np;
+ d = J.adjoint() * np;
#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);
+ z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
#else
- z = J.rightCols(J.cols()-iq) * d.tail(J.cols()-iq);
+ z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
#endif
- }
-
- 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));
- }
-
- 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);
- };
-
- } /* namespace solvers */
+ }
+
+ 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));
+ }
+
+ 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);
+};
+
+} /* namespace solvers */
} /* namespace tsid */
#endif /* EIQUADPROGFAST_HH_ */
diff --git a/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp b/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
index 0f3b1ac47c0627fc87b2ce5e40c8d8068cc326ee..fb0ebbbfc23d502e841145684fc6a3b18d52cad6 100644
--- a/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
+++ b/include/hpp/bezier-com-traj/solver/glpk-wrapper.hpp
@@ -22,21 +22,13 @@
#include <Eigen/Dense>
-namespace solvers
-{
+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);
+// 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);
} /* 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 f23f46d675172bab7ee154b597c1fe216a9fe9e1..546df5d19b04584bb06e21a855de18497bbf5e20 100644
--- a/include/hpp/bezier-com-traj/solver/solver-abstract.hpp
+++ b/include/hpp/bezier-com-traj/solver/solver-abstract.hpp
@@ -21,91 +21,67 @@
#include <hpp/bezier-com-traj/local_config.hh>
#include <Eigen/Dense>
-namespace solvers
-{
+namespace solvers {
/**
-* Possible states of the solver.
-*/
-enum optim_status
-{
- OPTIM_OPTIMAL=0,
- OPTIM_INFEASIBLE=1
-};
+ * Possible states of the solver.
+ */
+enum optim_status { OPTIM_OPTIMAL = 0, OPTIM_INFEASIBLE = 1 };
-static const double UNBOUNDED_UP = 100000.;
+static const double UNBOUNDED_UP = 100000.;
static const double UNBOUNDED_DOWN = -100000.;
typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi;
-typedef const Eigen::Ref<const VectorXd> & Cref_vectorX;
+typedef const Eigen::Ref<const VectorXd>& Cref_vectorX;
-enum BEZIER_COM_TRAJ_DLLAPI SolverType
-{
- SOLVER_QUADPROG = 0x00001
- //SOLVER_QUADPROG_SPARSE = 0x00002
+enum BEZIER_COM_TRAJ_DLLAPI SolverType {
+ SOLVER_QUADPROG = 0x00001
+// SOLVER_QUADPROG_SPARSE = 0x00002
#ifdef USE_GLPK_SOLVER
- ,SOLVER_GLPK = 0x00002
+ ,
+ SOLVER_GLPK = 0x00002
#endif
};
-
/**
-* @brief Struct used to return the results of the trajectory generation
-* problem.
-*/
-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 ResultData& other):
- success_(other.success_)
- , cost_(other.cost_)
- , x(other.x){}
- ~ResultData(){}
-
- ResultData& operator=(const ResultData& other)
- {
- success_= (other.success_);
- cost_ = (other.cost_);
- x = (other.x);
- return *this;
- }
- bool success_; // whether the optimization was successful
- double cost_; // cost evaluation for the solved control point
- VectorXd x; //control point
+ * @brief Struct used to return the results of the trajectory generation
+ * problem.
+ */
+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 ResultData& other) : success_(other.success_), cost_(other.cost_), x(other.x) {}
+ ~ResultData() {}
+
+ ResultData& operator=(const ResultData& other) {
+ success_ = (other.success_);
+ cost_ = (other.cost_);
+ x = (other.x);
+ return *this;
+ }
+ bool success_; // whether the optimization was successful
+ double cost_; // cost evaluation for the solved control point
+ VectorXd x; // control point
};
// min g'x
// st CIx <= ci0
// CEx = ce0
/**
-* @brief solve Solve a QP or LP given
-* 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.
-*/
-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);
-
+ * @brief solve Solve a QP or LP given
+ * 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.
+ */
+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 bff1fc0c2558e7287a1c10824af5c043c03c9863..01239a15210843fbae7bc344b46cebe610c63409 100644
--- a/include/hpp/bezier-com-traj/utils.hh
+++ b/include/hpp/bezier-com-traj/utils.hh
@@ -14,33 +14,27 @@
#include <vector>
-namespace bezier_com_traj
-{
+namespace bezier_com_traj {
-template<typename T> T initwp();
+template <typename T>
+T initwp();
waypoint_t initwp(const size_t rows, const size_t cols);
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator*(const double k, const waypoint_t& w);
-waypoint_t operator*(const waypoint_t& w,const double k);
+waypoint_t operator*(const waypoint_t& w, const double k);
-struct waypoint_t{
- MatrixXX first;
- VectorX second;
+struct waypoint_t {
+ MatrixXX first;
+ VectorX second;
- waypoint_t():first(MatrixXX()),second(VectorX())
- {}
+ waypoint_t() : first(MatrixXX()), second(VectorX()) {}
- waypoint_t(MatrixXX A,VectorX b):first(A),second(b)
- {}
-
- static waypoint_t Zero(size_t dim){
- return initwp(dim,dim);
- }
+ waypoint_t(MatrixXX A, VectorX b) : first(A), second(b) {}
+ static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
};
-
/**
* @brief Compute the Bernstein polynoms for a given degree
* @param degree required degree
@@ -48,7 +42,6 @@ struct waypoint_t{
*/
BEZIER_COM_TRAJ_DLLAPI std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
-
/**
* @brief given the constraints of the problem, and a set of waypoints, return
* the bezier curve corresponding
@@ -57,9 +50,9 @@ BEZIER_COM_TRAJ_DLLAPI std::vector<spline::Bern<double> > ComputeBersteinPolynom
* @param pis list of waypoints
* @return the bezier curve
*/
-template<typename Bezier, typename Point>
+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);
+ const std::vector<Point>& pi, const Point& x);
/**
* @brief computeDiscretizedTime build an array of discretized points in time,
@@ -69,7 +62,7 @@ BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag, con
* @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,
@@ -80,14 +73,13 @@ T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned i
* @return */
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
@@ -99,56 +91,47 @@ BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
*/
int Normalize(Ref_matrixXX A, Ref_vectorX b);
+} // end namespace bezier_com_traj
-
-} // 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, const Point& x)
-{
- std::vector<Point> wps;
- size_t i = 0;
- if(flag & INIT_POS ){
+template <typename Bezier, typename Point>
+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;
+ if (flag & INIT_POS) {
+ wps.push_back(pi[i]);
+ i++;
+ if (flag & INIT_VEL) {
+ wps.push_back(pi[i]);
+ i++;
+ if (flag & INIT_ACC) {
wps.push_back(pi[i]);
i++;
- if(flag & INIT_VEL){
- wps.push_back(pi[i]);
- i++;
- if(flag & INIT_ACC){
- wps.push_back(pi[i]);
- i++;
- }
- }
+ }
}
+ }
+ wps.push_back(x);
+ i++;
+ if (flag & (END_VEL) && !(flag & (END_POS))) {
wps.push_back(x);
i++;
- if(flag & (END_VEL) && !(flag & (END_POS) ))
- {
- wps.push_back(x);
- i++;
+ } else {
+ if (flag & END_ACC) {
+ 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.");
+ wps.push_back(pi[i]);
+ i++;
}
- else
- {
- if(flag & END_ACC){
- 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.");
- wps.push_back(pi[i]);
- i++;
- }
- if(flag & END_POS){
- wps.push_back(pi[i]);
- i++;
- }
+ if (flag & END_POS) {
+ wps.push_back(pi[i]);
+ i++;
}
- return Bezier (wps.begin(), wps.end(),T);
+ }
+ return Bezier(wps.begin(), wps.end(), T);
}
-
-
-
#endif
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 aa5754ef0b8523fdfd3f8eb3694a013cc7e0c986..e549d6353f2847c384952cbe9d77a64c758291d7 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
@@ -3,107 +3,105 @@
* Author: Pierre Fernbach
*/
-
#ifndef BEZIER_COM_TRAJ_C0DC0C1_H
#define BEZIER_COM_TRAJ_C0DC0C1_H
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_c1{
+namespace bezier_com_traj {
+namespace c0_dc0_c1 {
-static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS;
+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)
- * @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
- */
-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
- 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;
- return wp;
+/** @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
+ */
+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
+ 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;
+ return wp;
}
-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;
- return wp;
+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;
+ 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 :
- int n = 3;
- std::vector<point_t> pi;
- pi.push_back(pData.c0_); //p0
- pi.push_back((pData.dc0_ * T / n )+ pData.c0_); // p1
- pi.push_back(point_t::Zero()); // p2 = x
- pi.push_back(pData.c1_); // p3
-
- return pi;
+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
+ pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
+ pi.push_back(point_t::Zero()); // p2 = x
+ pi.push_back(pData.c1_); // p3
+
+ return pi;
}
-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;
- 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);
- // equation of waypoints for curve w found with sympy
- waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
- 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;
- 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;
- 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;
- 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;
- wps.push_back(w3);
- return wps;
+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;
+ 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);
+ // equation of waypoints for curve w found with sympy
+ waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
+ 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;
+ 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;
+ 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;
+ 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;
+ wps.push_back(w3);
+ return wps;
}
-
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- // equation found with sympy
- v.first = -3./T;
- v.second = 3.* pData.c1_ / T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ // equation found with sympy
+ v.first = -3. / T;
+ v.second = 3. * pData.c1_ / T;
+ return v;
}
-
-}
-}
+} // namespace c0_dc0_c1
+} // namespace bezier_com_traj
#endif
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 fe8ae14a64d6586ad1aaa136dc8f4dbc2ddf6536..3fb4578acf7b1a1e03941d908d08a8f745c54cc3 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
@@ -8,109 +8,107 @@
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_dc1{
+namespace bezier_com_traj {
+namespace c0_dc0_dc1 {
-static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_VEL;
+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)
+/** @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
*/
-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
- 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;
- return wp;
+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
+ 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;
+ return wp;
}
-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;
- return wp;
+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;
+ 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");
- std::vector<point_t> pi;
- pi.push_back(pData.c0_); //p0
- pi.push_back((pData.dc0_ * T / 3. )+ pData.c0_); // p1
- pi.push_back(point_t::Zero()); // p2 = x
- pi.push_back(point_t::Zero()); // p3 = x
- return pi;
+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
+ pi.push_back(point_t::Zero()); // p2 = x
+ pi.push_back(point_t::Zero()); // p3 = x
+ return pi;
}
-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;
- std::vector<point_t> pi = c0_dc0_dc1::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);
- // equation of waypoints for curve w found with sympy
- // TODO Apparently sympy equations are false ...
-
+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;
+ std::vector<point_t> pi = c0_dc0_dc1::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);
+ // equation of waypoints for curve w found with sympy
+ // TODO Apparently sympy equations are false ...
- waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
- 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>() = (-Cpi[0])*(12.0*pi[1]*alpha + g);
- wps.push_back(w0);
- waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
- 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]));
- 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.second.head<3>() = 3*alpha*(pi[0] - pi[1]);
- w2.second.tail<3>() = 0.5 * Cg*pi[1];
- wps.push_back(w2);
- waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
- w3.first.block<3,3>(0,0) = -3*alpha*Matrix3::Identity();
- w3.first.block<3,3>(3,0) = skew(g - 1.5 *alpha* (pi[1] + pi[0]));
- w3.second.head<3>() = 1.5*alpha * (pi[1] + pi[0]);
- wps.push_back(w3);
- waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
- w4.first.block<3,3>(0,0) = -6*alpha * Matrix3::Identity();
- w4.first.block<3,3>(3,0) = skew(g - 6*alpha* pi[1]);
- w4.second.head<3>() = 6*pi[1]*alpha;
- wps.push_back(w4);
- return wps;
+ waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
+ 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>() = (-Cpi[0]) * (12.0 * pi[1] * alpha + g);
+ wps.push_back(w0);
+ waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
+ 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]));
+ 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.second.head<3>() = 3 * alpha * (pi[0] - pi[1]);
+ w2.second.tail<3>() = 0.5 * Cg * pi[1];
+ wps.push_back(w2);
+ waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
+ w3.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
+ w3.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (pi[1] + pi[0]));
+ w3.second.head<3>() = 1.5 * alpha * (pi[1] + pi[0]);
+ wps.push_back(w3);
+ waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
+ w4.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
+ w4.first.block<3, 3>(3, 0) = skew(g - 6 * alpha * pi[1]);
+ w4.second.head<3>() = 6 * pi[1] * alpha;
+ wps.push_back(w4);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = point3_t::Zero();
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = point3_t::Zero();
+ return v;
}
-
-}
-}
+} // namespace c0_dc0_dc1
+} // namespace bezier_com_traj
#endif
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 b24006ea2002bf54890929a80bd878c650b53ae4..c238d6ab49d6c2a2a2a29515964f727891b75f0d 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
@@ -8,115 +8,120 @@
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_dc1_c1{
+namespace bezier_com_traj {
+namespace c0_dc0_dc1_c1 {
-static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS | END_VEL;
+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)
+/** @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
*/
-inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
- coefs_t wp;
- double t2 = t*t;
- double t3 = t2*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;
- return wp;
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
+ coefs_t wp;
+ double t2 = t * t;
+ double t3 = t2 * 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;
+ return wp;
}
-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)*alpha;
- return wp;
+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) *
+ 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 :
- int 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(point_t::Zero()); // p2 = x
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
- pi.push_back(pData.c1_); // p4
- return pi;
+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
+ pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
+ pi.push_back(point_t::Zero()); // p2 = x
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
+ pi.push_back(pData.c1_); // p4
+ return pi;
}
-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;
- 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);
- // equation of waypoints for curve w found with sympy
- waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
- w0.first.block<3,3>(0,0) = 12.*alpha*Matrix3::Identity();
- w0.first.block<3,3>(3,0) = 12.*alpha*Cpi[0];
- w0.second.head<3>() = (12.*pi[0] - 24.*pi[1])*alpha;
- w0.second.tail<3>() = 1.0*Cg*pi[0] - (24.0*Cpi[0]*pi[1])*alpha;
- wps.push_back(w0);
- waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
- 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;
- 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;
- 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;
- 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;
- wps.push_back(w4);
- waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
- w5.first.block<3,3>(0,0) =12*alpha*Matrix3::Identity();
- w5.first.block<3,3>(3,0) =12.0*Cpi[4]*alpha;
- w5.second.head<3>() = (-24*pi[3] + 12*pi[4])*alpha;
- w5.second.tail<3>() =(Cg*T2*pi[4] + 24.0*Cpi[3]*pi[4])*alpha;
- wps.push_back(w5);
- return wps;
+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;
+ 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);
+ // equation of waypoints for curve w found with sympy
+ waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
+ w0.first.block<3, 3>(0, 0) = 12. * alpha * Matrix3::Identity();
+ w0.first.block<3, 3>(3, 0) = 12. * alpha * Cpi[0];
+ w0.second.head<3>() = (12. * pi[0] - 24. * pi[1]) * alpha;
+ w0.second.tail<3>() = 1.0 * Cg * pi[0] - (24.0 * Cpi[0] * pi[1]) * alpha;
+ wps.push_back(w0);
+ waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
+ 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;
+ 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;
+ 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;
+ 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;
+ wps.push_back(w4);
+ waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
+ w5.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
+ w5.first.block<3, 3>(3, 0) = 12.0 * Cpi[4] * alpha;
+ w5.second.head<3>() = (-24 * pi[3] + 12 * pi[4]) * alpha;
+ w5.second.tail<3>() = (Cg * T2 * pi[4] + 24.0 * Cpi[3] * pi[4]) * alpha;
+ wps.push_back(w5);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-4.0*pi[3] + 4.0*pi[4])/ T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-4.0 * pi[3] + 4.0 * pi[4]) / T;
+ return v;
}
-
-}
-}
+} // namespace c0_dc0_dc1_c1
+} // namespace bezier_com_traj
#endif
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 7c03b85b3ab051d24acabd59e008c503abd0b4ae..2c9267c7695d02baaea63bfaa57c613b72c12b91 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
@@ -3,113 +3,111 @@
* Author: Pierre Fernbach
*/
-
#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_H
#define BEZIER_COM_TRAJ_c0_dc0_ddc0_H
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0{
+namespace bezier_com_traj {
+namespace c0_dc0_ddc0 {
-static const ConstraintFlag flag =INIT_POS | INIT_VEL | INIT_ACC;
+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)
/**
- * @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
*/
-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 +
- // (-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];
- return wp;
+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 +
+ // (-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];
+ return wp;
}
-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
- 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;
- return wp;
+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
+ 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;
+ 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 :
- 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
- return pi;
+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
+ return pi;
}
-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;
- 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), Id = Matrix3::Identity();
- const double T2 = T*T;
- const double alpha = 1/(T2);
+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;
+ 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), Id = Matrix3::Identity();
+ const double T2 = T * T;
+ const double alpha = 1 / (T2);
- // 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;
- 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;
- 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;
- 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.second.head<3>() = (6*pi[1] - 12*pi[2])*alpha;
- //w3.second.head<3>() = 0;
- wps.push_back(w3);
- return wps;
+ // 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;
+ 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;
+ 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;
+ 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.second.head<3>() = (6 * pi[1] - 12 * pi[2]) * alpha;
+ // w3.second.head<3>() = 0;
+ wps.push_back(w3);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- // 3.0*(-pi[2] + x)/T
- v.first = 3. /T;
- v.second = -3. * pi[2] / T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ // 3.0*(-pi[2] + x)/T
+ v.first = 3. / T;
+ v.second = -3. * pi[2] / T;
+ return v;
}
-
-}
-}
+} // namespace c0_dc0_ddc0
+} // namespace bezier_com_traj
#endif
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 aafd2a50a18cd04ce49a5f46453632466ceefc75..819d9fdf2258085749f38c1e4783c59d577679df 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
@@ -3,120 +3,125 @@
* Author: Pierre Fernbach
*/
-
#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
#define BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_c1{
+namespace bezier_com_traj {
+namespace c0_dc0_ddc0_c1 {
-static const ConstraintFlag flag =INIT_POS | INIT_VEL | INIT_ACC | END_POS;
+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)
/**
- * @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
*/
-inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
- coefs_t wp;
- double t2 = t*t;
- double t3 = t2*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;
- return wp;
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
+ coefs_t wp;
+ double t2 = t * t;
+ double t3 = t2 * 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;
+ return wp;
}
-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;
- return wp;
+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;
+ 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 :
- 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
- return pi;
+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
+ return pi;
}
-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;
- 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);
+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;
+ 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);
- // 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;
- 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;
- 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;
- 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;
- 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;
- 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;
- wps.push_back(w5);
- return wps;
+ // 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ wps.push_back(w5);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- // equation found with sympy
- v.first = -4./T;
- v.second = 4.* pData.c1_ / T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ // equation found with sympy
+ v.first = -4. / T;
+ v.second = 4. * pData.c1_ / T;
+ return v;
}
-
-}
-}
+} // namespace c0_dc0_ddc0_c1
+} // namespace bezier_com_traj
#endif
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 bcc65f40c978748696316b9f262109bbd5c7ecc1..31e33e8fad381d4c5b579f4db7dcba39d869b513 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
@@ -3,137 +3,172 @@
* Author: Pierre Fernbach
*/
-
#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
#define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_dc1_c1{
+namespace bezier_com_traj {
+namespace c0_dc0_ddc0_dc1_c1 {
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)
///
/**
- * @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
*/
-inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t){
- coefs_t wp;
- double t2 = t*t;
- double t3 = t2*t;
- double t4 = t3*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 - 5.0*pi[4]*t5 + 5.0*pi[4]*t4 + 1.0*pi[5]*t5;
- return wp;
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
+ coefs_t wp;
+ double t2 = t * t;
+ double t3 = t2 * t;
+ double t4 = t3 * 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 -
+ 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){
- coefs_t wp;
- double alpha = 1./(T*T);
- double t2 = t*t;
- double t3 = t2*t;
- // 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)*alpha;
- return wp;
+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;
+ double t3 = t2 * t;
+ // 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) *
+ 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 :
- 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
- return pi;
+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
+ return pi;
}
-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;
- 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);
+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;
+ 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);
- // 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;
- 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])*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.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])*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;
- 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])*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.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.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])*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])*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.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])*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;
- 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;
- wps.push_back(w7);
- return wps;
+ // 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;
+ 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]) *
+ 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.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]) *
+ 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;
+ 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]) *
+ 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.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.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]) *
+ 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]) *
+ 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.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]) *
+ 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;
+ 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;
+ wps.push_back(w7);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-5.0*pi[4] + 5.0*pi[5])/ T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-5.0 * pi[4] + 5.0 * pi[5]) / T;
+ return v;
}
-}
-}
+} // namespace c0_dc0_ddc0_dc1_c1
+} // namespace bezier_com_traj
#endif
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 d5a9cfeba69eba5bdb6f95d18508f2b7d1991b4d..a119b186156f3f28f8dc73b82c35946ea14e9298 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
@@ -3,151 +3,202 @@
* Author: Pierre Fernbach
*/
-
#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
#define BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_ddc1_dc1_c1{
+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;
/// ### 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
*/
-inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
- coefs_t wp;
- double t2 = t*t;
- double t3 = t2*t;
- double t4 = t3*t;
- double t5 = t4*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;
- return wp;
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
+ coefs_t wp;
+ double t2 = t * t;
+ double t3 = t2 * t;
+ double t4 = t3 * t;
+ double t5 = t4 * 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;
+ return wp;
}
-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;
- double t3 = t2*t;
- double t4 = t3*t;
- // 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)*alpha;
- return wp;
+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;
+ double t3 = t2 * t;
+ double t4 = t3 * t;
+ // 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) *
+ 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 :
- 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.dc1_ * T / n) + pData.c1_); // p4
- pi.push_back(pData.c1_); // p5
- return pi;
+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.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){
- bezier_wp_t::t_point_t wps;
- const int DIM_POINT = 6;
- const int DIM_VAR = 3;
- 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);
+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;
+ 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);
- // 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;
- 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.tail<3>() = 1.0*(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;
- 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.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])*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.second.head<3>() = 1.0*(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])*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*(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])*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.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])*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;
- 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.tail<3>() = 1.0*( 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;
- wps.push_back(w9);
- return wps;
+ // 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;
+ 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.tail<3>() = 1.0 *
+ (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;
+ 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.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]) *
+ 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.second.head<3>() = 1.0 *
+ (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]) *
+ 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 *
+ (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]) *
+ 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.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]) *
+ 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;
+ 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.tail<3>() = 1.0 *
+ (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;
+ wps.push_back(w9);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
+ return v;
}
-}
+} // namespace c0_dc0_ddc0_ddc1_dc1_c1
-}
+} // namespace bezier_com_traj
#endif
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 1332997b0613f1bf0caf26617158e6cce8ec5d91..d478104c3d55eeba449e38e78cf518f75d3c0f91 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
@@ -8,8 +8,8 @@
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1{
+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 size_t DIM_VAR = 3;
@@ -17,355 +17,427 @@ static const size_t DIM_POINT = 3;
/// ### 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)
+ * @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
*/
-inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
- coefs_t wp;
- const double t2 = t*t;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- const double t7 = t6*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;
- return wp;
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
+ coefs_t wp;
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ const double t7 = t6 * 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;
+ return wp;
}
-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;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- 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)*alpha;
- return wp;
+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;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ 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) *
+ alpha;
+ return wp;
}
-
-
-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;
- const double t3 = t2*t;
- const double t4 = t3*t;
- 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)*alpha;
- return wp;
+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;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ 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) *
+ alpha;
+ return wp;
}
-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;
- const double t3 = t2*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.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)*alpha;
- return wp;
+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;
+ const double t3 = t2 * 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.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) *
+ alpha;
+ return wp;
}
-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 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){
- 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 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){
- coefs_t coef = evaluateAccelerationCurveAtTime(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) {
+ coefs_t coef = evaluateAccelerationCurveAtTime(pi, T, t);
+ waypoint_t wp;
+ wp.first = Matrix3::Identity() * coef.first;
+ wp.second = coef.second;
+ return wp;
}
-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;
- wp.second = coef.second;
- return wp;
+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;
+ wp.second = coef.second;
+ 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 :
- 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) + 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))) + (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.c1_);
- return pi;
+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) +
+ 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))) +
+ (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.c1_);
+ return pi;
}
-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;
- 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);
-
- // 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;
- 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.tail<3>() = 1.0*(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;
- 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.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])*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.second.head<3>() = 1.0*(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])*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*(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])*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.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])*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;
- 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.tail<3>() = 1.0*( 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;
- wps.push_back(w9);
- return wps;
+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;
+ 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);
+
+ // 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;
+ 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.tail<3>() = 1.0 *
+ (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;
+ 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.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]) *
+ 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.second.head<3>() = 1.0 *
+ (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]) *
+ 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 *
+ (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]) *
+ 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.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]) *
+ 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;
+ 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.tail<3>() = 1.0 *
+ (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;
+ 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>()){
- if(pi.size() == 0)
- pi = computeConstantWaypoints(pData,T);
-
- std::vector<waypoint_t> wps;
- assert(pi.size() == 9);
-
- double alpha = 1. / (T);
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
- // assign w0:
- w.second= alpha*8*(-pi[0]+pi[1]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second = alpha*8*(-pi[1]+pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.second = alpha*8*(-pi[2]+pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first = 8*alpha*Matrix3::Identity();
- w.second = alpha*-8*pi[3];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first = -8*alpha*Matrix3::Identity();
- w.second = alpha*8*pi[5];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.second=alpha*8*(-pi[5]+pi[6]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.second=alpha*8*(-pi[6]+pi[7]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w7:
- w.second=alpha*8*(-pi[7]+pi[8]);
- wps.push_back(w);
- 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>()) {
+ if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 9);
+
+ double alpha = 1. / (T);
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+ // assign w0:
+ w.second = alpha * 8 * (-pi[0] + pi[1]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = alpha * 8 * (-pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.second = alpha * 8 * (-pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first = 8 * alpha * Matrix3::Identity();
+ w.second = alpha * -8 * pi[3];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first = -8 * alpha * Matrix3::Identity();
+ w.second = alpha * 8 * pi[5];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.second = alpha * 8 * (-pi[5] + pi[6]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.second = alpha * 8 * (-pi[6] + pi[7]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w7:
+ w.second = alpha * 8 * (-pi[7] + pi[8]);
+ wps.push_back(w);
+ return wps;
}
-std::vector<waypoint_t> computeAccelerationWaypoints(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;
- assert(pi.size() == 9);
- double alpha = 1. / (T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= 56*alpha*(pi[0] - 2*pi[1] + pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second= 56*alpha*(pi[1] - 2*pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first = 56*alpha*Matrix3::Identity();
- w.second = (56*pi[2] - 112*pi[3])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first = -112*alpha*Matrix3::Identity();
- w.second = (56*pi[3] +56*pi[8])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first = 56*alpha*Matrix3::Identity();
- w.second = (-112*pi[5] + 56*pi[6])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.second=56*alpha*(pi[5]-2*pi[6]+pi[7]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.second=56*alpha*(pi[6]-2*pi[7]+pi[8]);
- wps.push_back(w);
- return wps;
+std::vector<waypoint_t> computeAccelerationWaypoints(
+ 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;
+ assert(pi.size() == 9);
+ double alpha = 1. / (T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = 56 * alpha * (pi[0] - 2 * pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = 56 * alpha * (pi[1] - 2 * pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first = 56 * alpha * Matrix3::Identity();
+ w.second = (56 * pi[2] - 112 * pi[3]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first = -112 * alpha * Matrix3::Identity();
+ w.second = (56 * pi[3] + 56 * pi[8]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first = 56 * alpha * Matrix3::Identity();
+ w.second = (-112 * pi[5] + 56 * pi[6]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.second = 56 * alpha * (pi[5] - 2 * pi[6] + pi[7]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.second = 56 * alpha * (pi[6] - 2 * pi[7] + pi[8]);
+ wps.push_back(w);
+ 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>()){
- if(pi.size() == 0)
- pi = computeConstantWaypoints(pData,T);
-
- std::vector<waypoint_t> wps;
- assert(pi.size() == 9);
-
- double alpha = 1. / (T*T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= 336*(-pi[0] +3*pi[1] - 3*pi[2] + pi[3])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.first = 336*alpha*Matrix3::Identity();
- w.second= 336*(-pi[1] + 3*pi[2] - 3*pi[3])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first = -3*336*alpha*Matrix3::Identity();
- w.second = 336*(-pi[2] + 3*pi[3] + pi[5])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first = 3*336*alpha*Matrix3::Identity();
- w.second = 336*(-pi[3] - 3*pi[5] + pi[6])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first = -336*alpha*Matrix3::Identity();
- w.second = 336*(3*pi[5] - 3*pi[6] + pi[7])*alpha;
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.second= 336*(-pi[5] + 3*pi[6] - 3*pi[7] + pi[8])*alpha;
- wps.push_back(w);
- 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>()) {
+ if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 9);
+
+ double alpha = 1. / (T * T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = 336 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.first = 336 * alpha * Matrix3::Identity();
+ w.second = 336 * (-pi[1] + 3 * pi[2] - 3 * pi[3]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first = -3 * 336 * alpha * Matrix3::Identity();
+ w.second = 336 * (-pi[2] + 3 * pi[3] + pi[5]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first = 3 * 336 * alpha * Matrix3::Identity();
+ w.second = 336 * (-pi[3] - 3 * pi[5] + pi[6]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first = -336 * alpha * Matrix3::Identity();
+ w.second = 336 * (3 * pi[5] - 3 * pi[6] + pi[7]) * alpha;
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.second = 336 * (-pi[5] + 3 * pi[6] - 3 * pi[7] + pi[8]) * alpha;
+ wps.push_back(w);
+ return wps;
}
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
- return v;
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
+ return v;
}
+inline std::pair<MatrixXX, VectorX> computeVelocityCost(
+ 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);
-inline std::pair<MatrixXX,VectorX> computeVelocityCost(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] -
+ 7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
+ (2 * T);
+ H = Matrix3::Identity() * 6.52680652684107 / (T);
- 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);
+ double norm = H.norm();
+ H /= norm;
+ g /= norm;
- double norm=H.norm();
- H /= norm;
- g /= norm;
-
-
- return std::make_pair(H,g);
+ return std::make_pair(H, g);
}
-}
-
-}
+} // namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1
+} // namespace bezier_com_traj
-#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
+#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
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 e512e311a18cbcd6622e51c1731fc70aec3b4f14..2a249b094945e9628b84f55d2261eca2c8494cc3 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
@@ -8,385 +8,426 @@
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1{
+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)
+ * @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
*/
-//TODO
-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;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- const double t7 = t6*t;
- const double t8 = t7*t;
- const double t9 = t8*t;
- 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,3) = 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]);
- return wp;
+// TODO
+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;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ const double t7 = t6 * t;
+ const double t8 = t7 * t;
+ const double t9 = t8 * t;
+ 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, 3) =
+ 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]);
+ return wp;
}
-//TODO
-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;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- const double t7 = t6*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,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;
- return wp;
+// TODO
+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;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ const double t7 = t6 * 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, 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;
+ return wp;
}
-
-//TODO
-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;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- 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,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
- 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
- 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] + 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])))*alpha;
- return wp;
+// TODO
+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;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ 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, 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
+ 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
+ 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] +
+ 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]))) *
+ alpha;
+ return wp;
}
-//TODO
-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;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- 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,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
- 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
- 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])))*alpha;
- return wp;
+// TODO
+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;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ 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, 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
+ 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
+ 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]))) *
+ 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 :
- 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) + 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))) + (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.c1_);
- return pi;
+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) +
+ 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))) +
+ (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.c1_);
+ return pi;
}
-//TODO
-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;
- 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);
- std::cout<<"NOT IMPLEMENTED YET"<<std::endl;
- return wps;
+// TODO
+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;
+ 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);
+ 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>()){
- if(pi.size() == 0)
- pi = computeConstantWaypoints(pData,T);
-
- std::vector<waypoint_t> wps;
- assert(pi.size() == 11);
-
- double alpha = 1. / (T);
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*10*(-pi[0] + pi[1]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second = alpha*10*(-pi[1] + pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.second = alpha*10*(-pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = 10*alpha*Matrix3::Identity(); // x0
- w.second = alpha*10*(-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = -10*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 10*alpha*Matrix3::Identity(); // x1
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = -10*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = 10*alpha*Matrix3::Identity(); // x2
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.first.block<3,3>(0,6) = -10*alpha*Matrix3::Identity(); // x2
- w.second=alpha*10*pi[7];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w7:
- w.second=alpha*10*(-pi[7] + pi[8]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w8:
- w.second=alpha*10*(-pi[8] + pi[9]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w9:
- w.second=alpha*10*(pi[10] - pi[9]);
- wps.push_back(w);
- 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>()) {
+ if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 11);
+
+ double alpha = 1. / (T);
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 10 * (-pi[0] + pi[1]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = alpha * 10 * (-pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.second = alpha * 10 * (-pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = 10 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 10 * (-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = -10 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 10 * alpha * Matrix3::Identity(); // x1
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = -10 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 10 * alpha * Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.first.block<3, 3>(0, 6) = -10 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 10 * pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w7:
+ w.second = alpha * 10 * (-pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w8:
+ w.second = alpha * 10 * (-pi[8] + pi[9]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w9:
+ w.second = alpha * 10 * (pi[10] - pi[9]);
+ wps.push_back(w);
+ return wps;
}
-std::vector<waypoint_t> computeAccelerationWaypoints(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;
- assert(pi.size() == 11);
- double alpha = 1. / (T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*90*(pi[0] - 2*pi[1] + pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second= alpha*90*(pi[1] - 2*pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first.block<3,3>(0,0) = 90*alpha*Matrix3::Identity();//x0
- w.second = alpha*90*(pi[2]-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = -180*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 90*alpha*Matrix3::Identity(); // x1
- w.second = alpha*90*pi[3];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = 90*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = -180*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = 90*alpha*Matrix3::Identity(); // x2
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = 90*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = -180*alpha*Matrix3::Identity(); // x2
- w.second=alpha*90*pi[7];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.first.block<3,3>(0,6) = 90*alpha*Matrix3::Identity(); // x2
- w.second=alpha*90*(-2*pi[7] + pi[8]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w7:
- w.second=alpha*90*(pi[7] - 2*pi[8] + pi[9]);
- wps.push_back(w);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w8:
- w.second=alpha*90*(pi[10] + pi[8] - 2*pi[9]);
- wps.push_back(w);
- return wps;
+std::vector<waypoint_t> computeAccelerationWaypoints(
+ 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;
+ assert(pi.size() == 11);
+ double alpha = 1. / (T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 90 * (pi[0] - 2 * pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = alpha * 90 * (pi[1] - 2 * pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 90 * (pi[2] - pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = -180 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
+ w.second = alpha * 90 * pi[3];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = -180 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = -180 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 90 * pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 90 * (-2 * pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w7:
+ w.second = alpha * 90 * (pi[7] - 2 * pi[8] + pi[9]);
+ wps.push_back(w);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w8:
+ w.second = alpha * 90 * (pi[10] + pi[8] - 2 * pi[9]);
+ wps.push_back(w);
+ 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>()){
- if(pi.size() == 0)
- pi = computeConstantWaypoints(pData,T);
-
- std::vector<waypoint_t> wps;
- assert(pi.size() == 11);
-
- double alpha = 1. / (T*T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*720*(-pi[0] + 3*pi[1] - 3*pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.first.block<3,3>(0,0) = 720*alpha*Matrix3::Identity(); //x0
- w.second= alpha*720*(-pi[1] + 3*pi[2] - 3*pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first.block<3,3>(0,0) = 720*-3*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 720*alpha*Matrix3::Identity(); //x1
- w.second = alpha*720*(-pi[2] + 3*pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = 720*3*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 720*-3*alpha*Matrix3::Identity(); //x1
- w.first.block<3,3>(0,6) = 720*alpha*Matrix3::Identity(); // x2
- w.second = alpha*720*(-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = -720*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 720*3*alpha*Matrix3::Identity(); //x1
- w.first.block<3,3>(0,6) = 720*-3*alpha*Matrix3::Identity(); // x2
- w.second = alpha*720*pi[7];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = -720*alpha*Matrix3::Identity(); //x1
- w.first.block<3,3>(0,6) = 720*3*alpha*Matrix3::Identity(); // x2
- w.second=alpha* 720*(-3*pi[7] + pi[8]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,6) = -720*alpha*Matrix3::Identity(); // x2
- w.second=alpha*720*(3*pi[7] - 3*pi[8] + pi[9]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.second=alpha*720*(pi[10] - pi[7] + 3*pi[8] - 3*pi[9]);
- wps.push_back(w);
- 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>()) {
+ if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 11);
+
+ double alpha = 1. / (T * T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 720 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.first.block<3, 3>(0, 0) = 720 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 720 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first.block<3, 3>(0, 0) = 720 * -3 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 720 * alpha * Matrix3::Identity(); // x1
+ w.second = alpha * 720 * (-pi[2] + 3 * pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = 720 * 3 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 720 * -3 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 720 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 720 * (-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = -720 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 720 * 3 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 720 * -3 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 720 * pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = -720 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 720 * 3 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 720 * (-3 * pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 6) = -720 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 720 * (3 * pi[7] - 3 * pi[8] + pi[9]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.second = alpha * 720 * (pi[10] - pi[7] + 3 * pi[8] - 3 * pi[9]);
+ wps.push_back(w);
+ return wps;
}
-//TODO
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
- return v;
+// TODO
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
+ return v;
}
-
-inline std::pair<MatrixXX,VectorX> computeVelocityCost(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
-
- double norm=H.norm();
- H /= norm;
- g /= norm;
-
-
- return std::make_pair(H,g);
-}
-
-
-
+inline std::pair<MatrixXX, VectorX> computeVelocityCost(
+ 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
+
+ double norm = H.norm();
+ H /= norm;
+ g /= norm;
+
+ return std::make_pair(H, g);
}
-}
+} // namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1
+} // namespace bezier_com_traj
-#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
+#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
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 43495b6a3d7cbc40e5eb5b2ffd54c1a7193a515f..7dc4ffc6cbb21418c1dae5b8e0978f1b940eba85 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
@@ -8,464 +8,510 @@
#include <hpp/bezier-com-traj/data.hh>
-namespace bezier_com_traj{
-namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1{
+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 size_t DIM_VAR = 3*5;
+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)
+ * @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
*/
-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;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- const double t7 = t6*t;
- const double t8 = t7*t;
- const double t9 = t8*t;
- const double t10 = t9*t;
- const double t11 = t10*t;
- const double t12 = t11*t;
-
- // 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
- 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
- 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]);
- return wp;
+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;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ const double t7 = t6 * t;
+ const double t8 = t7 * t;
+ const double t9 = t8 * t;
+ const double t10 = t9 * t;
+ const double t11 = t10 * t;
+ const double t12 = t11 * t;
+
+ // 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
+ 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
+ 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]);
+ return wp;
}
-//TODO
-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;
-
- const double alpha = 1./(T);
- const double t2 = t*t;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- const double t7 = t6*t;
- const double t8 = t7*t;
- const double t9 = t8*t;
- const double t10 = t9*t;
- const double t11 = t10*t;
-
- // equation found with sympy
- wp.first.block<3,3>(0,0) = Matrix3::Identity()*alpha; // x0
- wp.first.block<3,3>(0,3) = Matrix3::Identity()*alpha; //x1
- wp.first.block<3,3>(0,6) = Matrix3::Identity()*alpha; // x2
- 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;
- return wp;
+// TODO
+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;
+
+ const double alpha = 1. / (T);
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ const double t7 = t6 * t;
+ const double t8 = t7 * t;
+ const double t9 = t8 * t;
+ const double t10 = t9 * t;
+ const double t11 = t10 * t;
+
+ // equation found with sympy
+ wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha; // x0
+ wp.first.block<3, 3>(0, 3) = Matrix3::Identity() * alpha; // x1
+ wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha; // x2
+ 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;
+ return wp;
}
-
-//TODO
-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;
-
- const double alpha = 1./(T*T);
- const double t2 = t*t;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- const double t6 = t5*t;
- 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,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
- 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
- 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] + 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])))*alpha;
- return wp;
-}
-
-//TODO
-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;
-
- const double alpha = 1./(T*T*T);
- const double t2 = t*t;
- const double t3 = t2*t;
- const double t4 = t3*t;
- const double t5 = t4*t;
- 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,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
- 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
- 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])))*alpha;
- return wp;
+// TODO
+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;
+
+ const double alpha = 1. / (T * T);
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ const double t6 = t5 * t;
+ 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, 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
+ 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
+ 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] +
+ 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]))) *
+ 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 :
- 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) + 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))) + (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.c1_);
- return pi;
+// TODO
+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;
+
+ const double alpha = 1. / (T * T * T);
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ 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, 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
+ 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
+ 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]))) *
+ alpha;
+ return wp;
}
-//TODO
-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;
- 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);
- std::cout<<"NOT IMPLEMENTED YET"<<std::endl;
- return wps;
+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) +
+ 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))) +
+ (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.c1_);
+ return pi;
}
-std::vector<waypoint_t> computeVelocityWaypoints(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;
- assert(pi.size() == 13);
-
- double alpha = 1. / (T);
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*12*(-pi[0] + pi[1]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second = alpha*12*(-pi[1] + pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.second = alpha*12*(-pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = 12*alpha*Matrix3::Identity(); // x0
- w.second = alpha*12*(-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = -12*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 12*alpha*Matrix3::Identity(); // x1
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = -12*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = 12*alpha*Matrix3::Identity(); // x2
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.first.block<3,3>(0,6) = -12*alpha*Matrix3::Identity(); // x2
- w.first.block<3,3>(0,9) = 12*alpha*Matrix3::Identity(); // x3
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w7:
- w.first.block<3,3>(0,9) = -12*alpha*Matrix3::Identity(); // x3
- w.first.block<3,3>(0,12) = 12*alpha*Matrix3::Identity(); // x4
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w8:
- w.first.block<3,3>(0,12) = -12*alpha*Matrix3::Identity(); // x4
- w.second=alpha*12*pi[9];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w9:
- w.second=alpha*12*(-pi[9] + pi[10]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w10:
- w.second=alpha*12*(-pi[10] + pi[11]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w11:
- w.second=alpha*12*(-pi[11] + pi[12]);
- wps.push_back(w);
- return wps;
+// TODO
+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;
+ 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);
+ std::cout << "NOT IMPLEMENTED YET" << std::endl;
+ return wps;
}
-std::vector<waypoint_t> computeAccelerationWaypoints(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;
- assert(pi.size() == 13);
- double alpha = 1. / (T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*132*(pi[0] - 2*pi[1] + pi[2]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.second= alpha*132*(pi[1] - 2*pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first.block<3,3>(0,0) = 132*alpha*Matrix3::Identity();//x0
- w.second = alpha*132*(pi[2]-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = -264*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 132*alpha*Matrix3::Identity(); // x1
- w.second = alpha*132*pi[3];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = 132*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = -264*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = 132*alpha*Matrix3::Identity(); // x2
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = 132*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = -264*alpha*Matrix3::Identity(); // x2
- w.first.block<3,3>(0,9) = 132*alpha*Matrix3::Identity(); // x3
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.first.block<3,3>(0,6) = 132*alpha*Matrix3::Identity(); // x2
- w.first.block<3,3>(0,9) = -264*alpha*Matrix3::Identity(); // x3
- w.first.block<3,3>(0,12) = 132*alpha*Matrix3::Identity(); // x4
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w7:
- w.first.block<3,3>(0,9) = 132*alpha*Matrix3::Identity(); // x3
- w.first.block<3,3>(0,12) = -264*alpha*Matrix3::Identity(); // x4
- w.second=alpha*132*pi[7];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w8:
- w.first.block<3,3>(0,12) = 132*alpha*Matrix3::Identity(); // x4
- w.second=alpha*132*(-2*pi[9] + pi[10]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w9:
- w.second=alpha*132*(pi[9] - 2*pi[10] + pi[11]);
- wps.push_back(w);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w10:
- w.second=alpha*132*(pi[12] + pi[10] - 2*pi[11]);
- wps.push_back(w);
- 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>()) {
+ if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 13);
+
+ double alpha = 1. / (T);
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 12 * (-pi[0] + pi[1]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = alpha * 12 * (-pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.second = alpha * 12 * (-pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 12 * (-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 12 * alpha * Matrix3::Identity(); // x1
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = -12 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 12 * alpha * Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.first.block<3, 3>(0, 6) = -12 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = 12 * alpha * Matrix3::Identity(); // x3
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w7:
+ w.first.block<3, 3>(0, 9) = -12 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = 12 * alpha * Matrix3::Identity(); // x4
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w8:
+ w.first.block<3, 3>(0, 12) = -12 * alpha * Matrix3::Identity(); // x4
+ w.second = alpha * 12 * pi[9];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w9:
+ w.second = alpha * 12 * (-pi[9] + pi[10]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w10:
+ w.second = alpha * 12 * (-pi[10] + pi[11]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w11:
+ w.second = alpha * 12 * (-pi[11] + pi[12]);
+ wps.push_back(w);
+ 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>()){
- if(pi.size() == 0)
- pi = computeConstantWaypoints(pData,T);
-
- std::vector<waypoint_t> wps;
- assert(pi.size() == 13);
-
- double alpha = 1. / (T*T*T);
-
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
-
- // assign w0:
- w.second= alpha*1320*(-pi[0] + 3*pi[1] - 3*pi[2] + pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w1:
- w.first.block<3,3>(0,0) = 1320*alpha*Matrix3::Identity(); //x0
- w.second= alpha*1320*(-pi[1] + 3*pi[2] - 3*pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w2:
- w.first.block<3,3>(0,0) = 1320*-3*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 1320*alpha*Matrix3::Identity(); //x1
- w.second = alpha*1320*(-pi[2] + 3*pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w3:
- w.first.block<3,3>(0,0) = 1320*3*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 1320*-3*alpha*Matrix3::Identity(); //x1
- w.first.block<3,3>(0,6) = 1320*alpha*Matrix3::Identity(); // x2
- w.second = alpha*1320*(-pi[3]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,0) = -1320*alpha*Matrix3::Identity(); // x0
- w.first.block<3,3>(0,3) = 1320*3*alpha*Matrix3::Identity(); //x1
- w.first.block<3,3>(0,6) = 1320*-3*alpha*Matrix3::Identity(); // x2
- w.first.block<3,3>(0,9) = 1320*alpha*Matrix3::Identity(); // x3
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,3) = -1320*alpha*Matrix3::Identity(); // x1
- w.first.block<3,3>(0,6) = 1320*3*alpha*Matrix3::Identity(); //x2
- w.first.block<3,3>(0,9) = 1320*-3*alpha*Matrix3::Identity(); // x3
- w.first.block<3,3>(0,12) = 1320*alpha*Matrix3::Identity(); // x4
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w4:
- w.first.block<3,3>(0,6) = -1320*alpha*Matrix3::Identity(); // x2
- w.first.block<3,3>(0,9) = 1320*3*alpha*Matrix3::Identity(); //x3
- w.first.block<3,3>(0,12) = 1320*-3*alpha*Matrix3::Identity(); // x4
- w.second = alpha*1320*pi[9];
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,9) = -1320*alpha*Matrix3::Identity(); //x3
- w.first.block<3,3>(0,12) = 1320*3*alpha*Matrix3::Identity(); // x4
- w.second=alpha* 1320*(-3*pi[9] + pi[10]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w5:
- w.first.block<3,3>(0,12) = -1320*alpha*Matrix3::Identity(); // x6
- w.second=alpha*1320*(3*pi[9] - 3*pi[10] + pi[11]);
- wps.push_back(w);
- w = initwp(DIM_POINT,DIM_VAR);
- // assign w6:
- w.second=alpha*1320*(pi[12] - pi[9] + 3*pi[10] - 3*pi[11]);
- wps.push_back(w);
- return wps;
+std::vector<waypoint_t> computeAccelerationWaypoints(
+ 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;
+ assert(pi.size() == 13);
+ double alpha = 1. / (T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 132 * (pi[0] - 2 * pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.second = alpha * 132 * (pi[1] - 2 * pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 132 * (pi[2] - pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = -264 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
+ w.second = alpha * 132 * pi[3];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = -264 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = -264 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = -264 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w7:
+ w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = -264 * alpha * Matrix3::Identity(); // x4
+ w.second = alpha * 132 * pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w8:
+ w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
+ w.second = alpha * 132 * (-2 * pi[9] + pi[10]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w9:
+ w.second = alpha * 132 * (pi[9] - 2 * pi[10] + pi[11]);
+ wps.push_back(w);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w10:
+ w.second = alpha * 132 * (pi[12] + pi[10] - 2 * pi[11]);
+ wps.push_back(w);
+ return wps;
}
-//TODO
-inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
- return v;
+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;
+ assert(pi.size() == 13);
+
+ double alpha = 1. / (T * T * T);
+
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+
+ // assign w0:
+ w.second = alpha * 1320 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w1:
+ w.first.block<3, 3>(0, 0) = 1320 * alpha * Matrix3::Identity(); // x0
+ w.second = alpha * 1320 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w2:
+ w.first.block<3, 3>(0, 0) = 1320 * -3 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 1320 * alpha * Matrix3::Identity(); // x1
+ w.second = alpha * 1320 * (-pi[2] + 3 * pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w3:
+ w.first.block<3, 3>(0, 0) = 1320 * 3 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 1320 * -3 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 1320 * alpha * Matrix3::Identity(); // x2
+ w.second = alpha * 1320 * (-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 0) = -1320 * alpha * Matrix3::Identity(); // x0
+ w.first.block<3, 3>(0, 3) = 1320 * 3 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 1320 * -3 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = 1320 * alpha * Matrix3::Identity(); // x3
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 3) = -1320 * alpha * Matrix3::Identity(); // x1
+ w.first.block<3, 3>(0, 6) = 1320 * 3 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = 1320 * -3 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = 1320 * alpha * Matrix3::Identity(); // x4
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w4:
+ w.first.block<3, 3>(0, 6) = -1320 * alpha * Matrix3::Identity(); // x2
+ w.first.block<3, 3>(0, 9) = 1320 * 3 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = 1320 * -3 * alpha * Matrix3::Identity(); // x4
+ w.second = alpha * 1320 * pi[9];
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 9) = -1320 * alpha * Matrix3::Identity(); // x3
+ w.first.block<3, 3>(0, 12) = 1320 * 3 * alpha * Matrix3::Identity(); // x4
+ w.second = alpha * 1320 * (-3 * pi[9] + pi[10]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w5:
+ w.first.block<3, 3>(0, 12) = -1320 * alpha * Matrix3::Identity(); // x6
+ w.second = alpha * 1320 * (3 * pi[9] - 3 * pi[10] + pi[11]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT, DIM_VAR);
+ // assign w6:
+ w.second = alpha * 1320 * (pi[12] - pi[9] + 3 * pi[10] - 3 * pi[11]);
+ wps.push_back(w);
+ return wps;
}
-//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>()){
- 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] + 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
-
- 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
- H.block<3,3>(6,6) = Matrix3::Identity() * 7.92188615942945 / (T); // x2^2
- 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
-
-
- double norm=H.norm();
- H /= norm;
- g /= norm;
-
-
- return std::make_pair(H,g);
+// TODO
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
+ return v;
}
-
-
+// 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>()) {
+ 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] +
+ 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
+
+ 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
+ H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.92188615942945 / (T); // x2^2
+ 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
+
+ double norm = H.norm();
+ H /= norm;
+ g /= norm;
+
+ return std::make_pair(H, g);
}
-}
+} // namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1
+} // namespace bezier_com_traj
-#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
+#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
diff --git a/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh b/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
index 57eed5fa48047b8d5013c1ab2a3eb32ce439abc9..5ade006c920342733e2504eab48f863401b0bd23 100644
--- a/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -8,13 +8,11 @@
#include <hpp/bezier-com-traj/data.hh>
-
-
-namespace bezier_com_traj{
+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
+ */
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t,
* defined by the waypoint pi and one free waypoint (x)
@@ -22,7 +20,7 @@ namespace bezier_com_traj{
* @param t param (normalized !)
* @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)
@@ -30,24 +28,23 @@ coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>
* @param t param (normalized !)
* @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)
-* @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
-*/
-coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi,double T,double 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
+ */
+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)
-* @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
-*/
-coefs_t evaluateJerkCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi,double T,double 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
+ */
+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)
@@ -56,8 +53,7 @@ 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)
@@ -66,7 +62,7 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
* @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)
@@ -75,8 +71,8 @@ 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)
@@ -85,7 +81,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData,const
* @param T
* @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>());
+std::vector<waypoint_t> computeAccelerationWaypoints(
+ 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)
@@ -94,14 +91,16 @@ std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData,co
* @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,double 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>());
+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);
/**
* @brief computeConstantWaypoints compute the constant waypoints of w(t)
@@ -110,16 +109,15 @@ waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const doubl
* @param T
* @return
*/
-bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T);
+bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T);
-
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T);
+coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T);
size_t 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
#endif
diff --git a/python/bezier_com_traj.cpp b/python/bezier_com_traj.cpp
index 3cd2cac5347911dabe4017d7ca3c6214ef30136d..dbc179e3461da62da128e2ff88ff03481d994798 100644
--- a/python/bezier_com_traj.cpp
+++ b/python/bezier_com_traj.cpp
@@ -13,546 +13,384 @@ 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::bezier_t)
-namespace bezier_com_traj
-{
+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, const double timeStep)
-{
- bezier_com_traj::ContactData data;
- data.contactPhase_ = eq;
- bezier_com_traj::ProblemData pData;
- pData.c0_ = com;
- pData.dc0_ = dCom;
- pData.l0_ = l0;
- pData.contacts_.push_back(data);
- pData.useAngularMomentum_ = useAngMomentum;
- std::vector<double> Ts;
- Ts.push_back(timeDuration);
- ResultDataCOMTraj res = solve0step(pData, Ts, timeStep);
- 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)
-{
- bezier_com_traj::ContactData data;
- data.Kin_ = Kin;
- data.kin_ = kin;
- data.contactPhase_ = eq;
- bezier_com_traj::ProblemData pData;
- pData.c0_ = com;
- pData.dc0_ = dCom;
- pData.l0_ = l0;
- pData.contacts_.push_back(data);
- pData.useAngularMomentum_ = useAngMomentum;
- std::vector<double> Ts;
- Ts.push_back(timeDuration);
- ResultDataCOMTraj res = solve0step(pData, Ts, timeStep);
- return new ResultDataCOMTraj(res);
-}
-
-struct res_data_exception : std::exception
-{
+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;
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = com;
+ pData.dc0_ = dCom;
+ pData.l0_ = l0;
+ pData.contacts_.push_back(data);
+ pData.useAngularMomentum_ = useAngMomentum;
+ std::vector<double> Ts;
+ Ts.push_back(timeDuration);
+ ResultDataCOMTraj res = solve0step(pData, Ts, timeStep);
+ 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) {
+ bezier_com_traj::ContactData data;
+ data.Kin_ = Kin;
+ data.kin_ = kin;
+ data.contactPhase_ = eq;
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = com;
+ pData.dc0_ = dCom;
+ pData.l0_ = l0;
+ pData.contacts_.push_back(data);
+ pData.useAngularMomentum_ = useAngMomentum;
+ std::vector<double> Ts;
+ Ts.push_back(timeDuration);
+ ResultDataCOMTraj res = solve0step(pData, Ts, timeStep);
+ return new ResultDataCOMTraj(res);
+}
+
+struct res_data_exception : std::exception {
char const* what() const throw() { return "attributes not accessible for false resData"; }
};
-void translate(const res_data_exception & e)
-{
- // Use the Python 'C' API to set up an exception object
- PyErr_SetString(PyExc_RuntimeError, e.what());
+void translate(const res_data_exception& e) {
+ // Use the Python 'C' API to set up an exception object
+ PyErr_SetString(PyExc_RuntimeError, e.what());
}
-struct contact_data_exception : std::exception
-{
+struct contact_data_exception : std::exception {
char const* what() const throw() { return "attribute not defined yet for ContactData"; }
};
-void translateContactData(const contact_data_exception & e)
-{
- // Use the Python 'C' API to set up an exception object
- PyErr_SetString(PyExc_RuntimeError, e.what());
+void translateContactData(const contact_data_exception& e) {
+ // Use the Python 'C' API to set up an exception object
+ PyErr_SetString(PyExc_RuntimeError, e.what());
}
-VectorX get_xD(const ResultData& res)
-{
- if (res.x.size() > 0)
- return res.x;
- std::cout << "x is not defined" << std::endl;
- throw res_data_exception();
+VectorX get_xD(const ResultData& res) {
+ if (res.x.size() > 0) return res.x;
+ std::cout << "x is not defined" << std::endl;
+ throw res_data_exception();
}
-double get_costD(const ResultData& res)
-{
- return res.cost_;
-}
+double get_costD(const ResultData& res) { return res.cost_; }
-bool get_succD(const ResultData& res)
-{
- return res.success_;
-}
+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;
- std::cout << "x is not defined" << std::endl;
- throw res_data_exception();
+VectorX get_x(const ResultDataCOMTraj& res) {
+ if (res.x.size() > 0) return res.x;
+ std::cout << "x is not defined" << std::endl;
+ throw res_data_exception();
}
-double get_cost(const ResultDataCOMTraj& res)
-{
- return res.cost_;
-}
+double get_cost(const ResultDataCOMTraj& res) { return res.cost_; }
-bool get_succ(const ResultDataCOMTraj& res)
-{
- return res.success_;
-}
+bool get_succ(const ResultDataCOMTraj& res) { return res.success_; }
/** BEGIN CONTACT DATA **/
-centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data)
-{
- if (data.contactPhase_)
- return data.contactPhase_;
- std::cout << "contactPhase_ is not assigned" << std::endl;
- throw contact_data_exception();
+centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data) {
+ if (data.contactPhase_) return data.contactPhase_;
+ std::cout << "contactPhase_ is not assigned" << std::endl;
+ 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)
- {
- std::cout << " no angular momentum constraints assigned " << std::endl;
- throw contact_data_exception();
- }
- return boost::python::make_tuple(res.Ang_, res.ang_);
+boost::python::tuple get_Ang(const ContactData& res) {
+ if (res.ang_.size() == 0) {
+ std::cout << " no angular momentum constraints assigned " << std::endl;
+ throw contact_data_exception();
+ }
+ return boost::python::make_tuple(res.Ang_, res.ang_);
}
-boost::python::tuple get_Kin(const ContactData& res)
-{
- if(res.kin_.size() == 0)
- {
- std::cout << " no kinematic constraints assigned " << std::endl;
- throw contact_data_exception();
- }
- return boost::python::make_tuple(res.Kin_, res.kin_);
+boost::python::tuple get_Kin(const ContactData& res) {
+ if (res.kin_.size() == 0) {
+ std::cout << " no kinematic constraints assigned " << std::endl;
+ throw contact_data_exception();
+ }
+ return boost::python::make_tuple(res.Kin_, res.kin_);
}
-
-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;
- throw contact_data_exception();
- }
- res.Kin_ = val;
- res.kin_ = val2;
+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;
+ throw contact_data_exception();
+ }
+ res.Kin_ = val;
+ res.kin_ = val2;
}
-void set_Ang(ContactData& res, const MatrixX3 & val, const VectorX& val2)
-{
- if(val2.size() != val.rows())
- {
- std::cout << " Angular inequality matrix sizes do not match " << std::endl;
- throw contact_data_exception();
- }
- res.Ang_ = val;
- res.ang_ = val2;
+void set_Ang(ContactData& res, const MatrixX3& val, const VectorX& val2) {
+ if (val2.size() != val.rows()) {
+ std::cout << " Angular inequality matrix sizes do not match " << std::endl;
+ throw contact_data_exception();
+ }
+ res.Ang_ = val;
+ res.ang_ = val2;
}
/** END CONTACT DATA **/
-
/** BEGIN Constraints**/
-int get_Flag(const Constraints& res)
-{
- return (int)res.flag_;
-}
-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_;
-}
+int get_Flag(const Constraints& res) { return (int)res.flag_; }
+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;
-}
+void set_ReduceH(Constraints& res, const double val) { res.reduce_h_ = val; }
/** END Constraints **/
-
/** BEGIN ProblemData**/
-point_t get_c0_(const ProblemData& res)
-{
- return res.c0_;
-}
-point_t get_dc0_(const ProblemData& res)
-{
- return res.dc0_;
-}
+point_t get_c0_(const ProblemData& res) { return res.c0_; }
+point_t get_dc0_(const ProblemData& res) { return res.dc0_; }
-point_t get_ddc0_(const ProblemData& res)
-{
- return res.ddc0_;
-}
+point_t get_ddc0_(const ProblemData& res) { return res.ddc0_; }
-point_t get_c1_(const ProblemData& res)
-{
- return res.c1_;
-}
+point_t get_c1_(const ProblemData& res) { return res.c1_; }
-point_t get_dc1_(const ProblemData& res)
-{
- return res.dc1_;
-}
+point_t get_dc1_(const ProblemData& res) { return res.dc1_; }
-point_t get_ddc1_(const ProblemData& res)
-{
- return res.ddc1_;
-}
+point_t get_ddc1_(const ProblemData& res) { return res.ddc1_; }
+void set_c0_(ProblemData& res, const point_t& val) { res.c0_ = val; }
-void set_c0_(ProblemData& res, const point_t& val)
-{
- res.c0_ = val;
-}
+void set_dc0_(ProblemData& res, const point_t& val) { res.dc0_ = val; }
-void set_dc0_(ProblemData& res, const point_t& val)
-{
- res.dc0_ = val;
-}
+void set_ddc0_(ProblemData& res, const point_t& val) { res.ddc0_ = val; }
-void set_ddc0_(ProblemData& res, const point_t& val)
-{
- res.ddc0_ = val;
-}
+void set_c1_(ProblemData& res, const point_t& val) { res.c1_ = val; }
-void set_c1_(ProblemData& res, const point_t& val)
-{
- res.c1_ = val;
-}
+void set_dc1_(ProblemData& res, const point_t& val) { res.dc1_ = val; }
-void set_dc1_(ProblemData& res, const point_t& val)
-{
- res.dc1_ = val;
-}
+void set_ddc1_(ProblemData& res, const point_t& val) { res.ddc1_ = 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; }
+CostFunction get_costFunction_(const ProblemData& res) { return res.costFunction_; }
-bool get_useAngularMomentum_(const ProblemData& res)
-{
- return res.useAngularMomentum_;
-}
-void set_useAngularMomentum_(ProblemData& res, const bool val)
-{
- res.useAngularMomentum_ = val;
-}
+void set_costFunction_(ProblemData& res, const CostFunction val) { res.costFunction_ = val; }
-CostFunction get_costFunction_(const ProblemData& res)
-{
- return res.costFunction_;
-}
+GIWCRepresentation get_GIWC_representation_(const ProblemData& res) { return res.representation_; }
-void set_costFunction_(ProblemData& res, const CostFunction val)
-{
- res.costFunction_ = val;
-}
+void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) { res.representation_ = val; }
-GIWCRepresentation get_GIWC_representation_(const ProblemData& res)
-{
- return res.representation_;
-}
+Constraints* get_constraints_(ProblemData& res) { return &res.constraints_; }
-void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val)
-{
- res.representation_ = val;
-}
+void set_constraints_(ProblemData& res, const Constraints& val) { res.constraints_ = val; }
-Constraints* get_constraints_(ProblemData& res)
-{
- return &res.constraints_;
-}
+std::vector<ContactData> get_contacts_(const ProblemData& res) { return res.contacts_; }
-void set_constraints_(ProblemData& res, const Constraints& val)
-{
- res.constraints_ = val;
-}
+void addContact(ProblemData& res, const ContactData& val) { res.contacts_.push_back(ContactData(val)); }
-
-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 clearContacts(ProblemData& res)
-{
- res.contacts_.clear();
-}
+void clearContacts(ProblemData& res) { res.contacts_.clear(); }
/** END ProblemData **/
-
/** BEGIN computeCOMTraj **/
-ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData, const VectorX& Ts, const double timeStep)
-{
- ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep);
- return new ResultDataCOMTraj(res);
+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 res = computeCOMTraj(pData, Ts, timeStep, solver);
- return new ResultDataCOMTraj(res);
+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);
}
-
-
/** END computeCOMTraj **/
/** BEGIN end effector **/
-struct DummyPath{
-
- point3_t operator()(double /*u*/)const{
- return point3_t::Zero();
- }
-
+struct DummyPath {
+ point3_t operator()(double /*u*/) const { return point3_t::Zero(); }
};
-
typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> >
+ linear_points_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
-
-struct MatrixVector
-{
- linear_points_t res;
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() {return res.first;}
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() {return res.second;}
+struct MatrixVector {
+ linear_points_t res;
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() { return res.first; }
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() { return res.second; }
};
-
-ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time){
-
- ResultDataCOMTraj res =solveEndEffector<DummyPath>(pData,DummyPath(),time, 0);
- return new ResultDataCOMTraj(res);
+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){
- std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
- MatrixVector* res = new MatrixVector();
- res->res =computeEndEffectorConstraints(pData,time, pi);
- return res;
+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){
- std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
- MatrixVector* res = new MatrixVector();
- res->res = computeVelocityCost(pData,time,pi);
- return res;
+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,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 :
- std::vector<point3_t> pts_path;
- for(size_t c = 0 ; c < pts_l.cols() ; ++c){
- pts_path.push_back(pts_l.block<3,1>(0,c));
- }
- MatrixVector* res = new MatrixVector();
- res->res =computeDistanceCostFunction(numPoints,pData,time,pts_path);
- return res;
+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 :
+ std::vector<point3_t> pts_path;
+ for (size_t c = 0; c < pts_l.cols(); ++c) {
+ pts_path.push_back(pts_l.block<3, 1>(0, c));
+ }
+ MatrixVector* res = new MatrixVector();
+ res->res = computeDistanceCostFunction(numPoints, pData, time, pts_path);
+ return res;
}
-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)
- res.block<3,1>(0,col) = *cit;
- return res;
- }
-
+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)
+ res.block<3, 1>(0, col) = *cit;
+ return res;
+}
/** END end effector **/
-
-BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
-{
- using namespace boost::python;
- register_exception_translator<res_data_exception>(&translate);
- register_exception_translator<contact_data_exception>(&translateContactData);
- /** BEGIN eigenpy init**/
- eigenpy::enableEigenPy();
-
- eigenpy::enableEigenPySpecific<point_t,point_t>();
- eigenpy::enableEigenPySpecific<Vector3,Vector3>();
- eigenpy::enableEigenPySpecific<VectorX,VectorX>();
- eigenpy::enableEigenPySpecific<MatrixX3,MatrixX3>();
- eigenpy::enableEigenPySpecific<MatrixX3,MatrixX3>();
-
-
- /** END eigenpy init**/
- /*eigenpy::exposeAngleAxis();
- 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("success", &get_succ)
- .add_property("cost", &get_cost)
- .add_property("x", &get_x)
- ;
-
-
- class_<ResultData>("ResultData", init<> ())
- .add_property("success", &get_succD)
- .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_)
- .add_property("Kin_",&get_Kin)
- .add_property("Ang_",&get_Ang)
- .def("setKinematicConstraints", &set_Kin)
- .def("setAngularConstraints", &set_Ang)
- ;
-
- class_<ProblemData>("ProblemData", init<>())
- .add_property("c0_",&get_c0_, &set_c0_ )
- .add_property("dc0_",&get_dc0_, &set_dc0_)
- .add_property("ddc0_",&get_ddc0_, &set_ddc0_ )
- .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("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_ )
- .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("maxAcceleration_",&get_MaxAcc, &set_MaxAcc )
- .add_property("reduce_h_",&get_ReduceH, &set_ReduceH )
- ;
-
- enum_<CostFunction>("CostFunction")
- .value("ACCELERATION", ACCELERATION)
- .value("DISTANCE_TRAVELED", DISTANCE_TRAVELED)
- .value("TARGET_END_VELOCITY", TARGET_END_VELOCITY)
- .value("UNKNOWN_COST", UNKNOWN_COST)
- .export_values();
-
- enum_<GIWCRepresentation>("GIWCRepresentation")
- .value("DOUBLE_DESCRIPTION", DOUBLE_DESCRIPTION)
- .value("FORCE", FORCE)
- .value("UNKNOWN_REPRESENTATION", UNKNOWN_REPRESENTATION)
- .export_values();
-
- enum_<solvers::SolverType>("SolverType")
- .value("SOLVER_QUADPROG", solvers::SOLVER_QUADPROG)
- //.value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE)
+BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
+ using namespace boost::python;
+ register_exception_translator<res_data_exception>(&translate);
+ register_exception_translator<contact_data_exception>(&translateContactData);
+ /** BEGIN eigenpy init**/
+ eigenpy::enableEigenPy();
+
+ eigenpy::enableEigenPySpecific<point_t, point_t>();
+ eigenpy::enableEigenPySpecific<Vector3, Vector3>();
+ eigenpy::enableEigenPySpecific<VectorX, VectorX>();
+ eigenpy::enableEigenPySpecific<MatrixX3, MatrixX3>();
+ eigenpy::enableEigenPySpecific<MatrixX3, MatrixX3>();
+
+ /** END eigenpy init**/
+ /*eigenpy::exposeAngleAxis();
+ 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("success", &get_succ)
+ .add_property("cost", &get_cost)
+ .add_property("x", &get_x);
+
+ class_<ResultData>("ResultData", init<>())
+ .add_property("success", &get_succD)
+ .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_)
+ .add_property("Kin_", &get_Kin)
+ .add_property("Ang_", &get_Ang)
+ .def("setKinematicConstraints", &set_Kin)
+ .def("setAngularConstraints", &set_Ang);
+
+ class_<ProblemData>("ProblemData", init<>())
+ .add_property("c0_", &get_c0_, &set_c0_)
+ .add_property("dc0_", &get_dc0_, &set_dc0_)
+ .add_property("ddc0_", &get_ddc0_, &set_ddc0_)
+ .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("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_)
+ .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("maxAcceleration_", &get_MaxAcc, &set_MaxAcc)
+ .add_property("reduce_h_", &get_ReduceH, &set_ReduceH);
+
+ enum_<CostFunction>("CostFunction")
+ .value("ACCELERATION", ACCELERATION)
+ .value("DISTANCE_TRAVELED", DISTANCE_TRAVELED)
+ .value("TARGET_END_VELOCITY", TARGET_END_VELOCITY)
+ .value("UNKNOWN_COST", UNKNOWN_COST)
+ .export_values();
+
+ enum_<GIWCRepresentation>("GIWCRepresentation")
+ .value("DOUBLE_DESCRIPTION", DOUBLE_DESCRIPTION)
+ .value("FORCE", FORCE)
+ .value("UNKNOWN_REPRESENTATION", UNKNOWN_REPRESENTATION)
+ .export_values();
+
+ enum_<solvers::SolverType>("SolverType")
+ .value("SOLVER_QUADPROG", solvers::SOLVER_QUADPROG)
+ //.value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE)
#ifdef USE_GLPK_SOLVER
- .value("SOLVER_GLPK", solvers::SOLVER_GLPK)
+ .value("SOLVER_GLPK", solvers::SOLVER_GLPK)
#endif
- .export_values();
-
- enum_<ConstraintFlag>("ConstraintFlag")
- .value("INIT_POS", INIT_POS)
- .value("INIT_VEL", INIT_VEL)
- .value("INIT_ACC", INIT_ACC)
- .value("INIT_JERK",INIT_JERK)
- .value("END_POS", END_POS)
- .value("END_VEL", END_VEL)
- .value("END_ACC", END_ACC)
- .value("END_JERK",END_JERK)
- .value("ONE_FREE_VAR",ONE_FREE_VAR)
- .value("TWO_FREE_VAR",TWO_FREE_VAR)
- .value("THREE_FREE_VAR",THREE_FREE_VAR)
- .value("FOUR_FREE_VAR",FOUR_FREE_VAR)
- .value("FIVE_FREE_VAR",FIVE_FREE_VAR)
- .value("UNKNOWN", UNKNOWN)
- .export_values();
-
- class_<MatrixVector>("MatrixVector", no_init)
- .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("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython, return_value_policy<manage_new_object>());
- def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython, return_value_policy<manage_new_object>());
- def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);
-
-
-}
-
-} // namespace bezier_com_traj
+ .export_values();
+
+ enum_<ConstraintFlag>("ConstraintFlag")
+ .value("INIT_POS", INIT_POS)
+ .value("INIT_VEL", INIT_VEL)
+ .value("INIT_ACC", INIT_ACC)
+ .value("INIT_JERK", INIT_JERK)
+ .value("END_POS", END_POS)
+ .value("END_VEL", END_VEL)
+ .value("END_ACC", END_ACC)
+ .value("END_JERK", END_JERK)
+ .value("ONE_FREE_VAR", ONE_FREE_VAR)
+ .value("TWO_FREE_VAR", TWO_FREE_VAR)
+ .value("THREE_FREE_VAR", THREE_FREE_VAR)
+ .value("FOUR_FREE_VAR", FOUR_FREE_VAR)
+ .value("FIVE_FREE_VAR", FIVE_FREE_VAR)
+ .value("UNKNOWN", UNKNOWN)
+ .export_values();
+
+ class_<MatrixVector>("MatrixVector", no_init)
+ .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("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython,
+ return_value_policy<manage_new_object>());
+ def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython,
+ return_value_policy<manage_new_object>());
+ def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);
+}
+
+} // namespace bezier_com_traj
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index c5b27d40d58f9171b54fa954233fc47210721746..27e54c8e45ab2ca900cfec9e9cc32166d1c02d26 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -4,176 +4,156 @@
#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
#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<spline::Bern<double> >& berns, int numSteps)
-{
- double dt = 1./double(numSteps);
- std::vector<waypoint6_t> res;
- for (int i =0; i < numSteps + 1; ++i)
- {
- waypoint6_t w = initwp<waypoint6_t>();
- for (int j = 0; j <5; ++j)
- {
- double b = berns[j](i*dt);
- w.first +=b*(wps[j].first );
- w.second+=b*(wps[j].second);
- }
- res.push_back(w);
+namespace bezier_com_traj {
+std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6_t>& wps,
+ const std::vector<spline::Bern<double> >& berns, int numSteps) {
+ double dt = 1. / double(numSteps);
+ std::vector<waypoint6_t> res;
+ for (int i = 0; i < numSteps + 1; ++i) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ for (int j = 0; j < 5; ++j) {
+ double b = berns[j](i * dt);
+ w.first += b * (wps[j].first);
+ w.second += b * (wps[j].second);
}
- return res;
+ res.push_back(w);
+ }
+ return res;
}
-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 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, const bool useAngMomentum)
-{
- int dimPb = useAngMomentum ? 6 : 3;
- return MatrixXX::Zero(6 + extraConstraintSize, wps.size() * colG + dimPb);
+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)
-{
- 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 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)
-{
- int dimAng = (int)(ang.rows());
- if(dimAng > 0)
- {
- A.block(A.rows() - dimAng , 3, dimAng, 3) = Ang;
- b.tail(dimAng) = 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;
+ b.tail(dimAng) = ang;
+ }
}
-int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b)
-{
- 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;
- return -1;
- }
- A.row(i).swap(A.row(last));
- b.row(i).swap(b.row(last));
- empty.segment<1>(i).swap(empty.segment<1>(last));
- --last;
- }
- else
- ++i;
- }
- return (int)last+1;
+int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b) {
+ 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;
+ return -1;
+ }
+ A.row(i).swap(A.row(last));
+ b.row(i).swap(b.row(last));
+ empty.segment<1>(i).swap(empty.segment<1>(last));
+ --last;
+ } else
+ ++i;
+ }
+ return (int)last + 1;
}
-int Normalize(Ref_matrixXX A, Ref_vectorX b)
-{
- int zeroindex = removeZeroRows(A,b);
- if(zeroindex > 0)
- {
- Eigen::VectorXd norm = A.block(0,0,zeroindex,A.cols()).rowwise().norm();
- A.block(0,0,zeroindex,A.cols()).rowwise().normalize();
- b.head(zeroindex) = b.head(zeroindex).cwiseQuotient(norm);
- }
- return zeroindex;
+int Normalize(Ref_matrixXX A, Ref_vectorX b) {
+ int zeroindex = removeZeroRows(A, b);
+ if (zeroindex > 0) {
+ Eigen::VectorXd norm = A.block(0, 0, zeroindex, A.cols()).rowwise().norm();
+ A.block(0, 0, zeroindex, A.cols()).rowwise().normalize();
+ b.head(zeroindex) = b.head(zeroindex).cwiseQuotient(norm);
+ }
+ 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)
-{
- 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);
- centroidal_dynamics::MatrixXX Hrow; VectorX h;
- cData.contactPhase_->getPolytopeInequalities(Hrow,h);
- MatrixXX H = -Hrow;
- H.rowwise().normalize();
- int dimH = (int)(H.rows());
- MatrixXX mH = cData.contactPhase_->m_mass * H;
- // init and fill Ab matrix
- long int dimKin = cData.kin_.size();
- long int dimAng = useAngMomentum ? (long int)(cData.ang_.size()) : 0;
- A = initMatrixA(dimH, wps, dimKin + dimAng, useAngMomentum);
- b = VectorX::Zero(A.rows());
- point6_t bc = point6_t::Zero(); 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)
- {
- A.block(i*dimH,0, dimH, 3) = mH * wpcit->first;
- b.segment(i*dimH, dimH) = mH * (bc - wpcit->second);
- if(useAngMomentum)
- {
- A.block (i*dimH,3, dimH, 3) = H * wpLcit->first;
- b.segment(i*dimH, dimH) += H * (-wpLcit->second);
- ++wpLcit;
- }
- ++i;
+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);
+ centroidal_dynamics::MatrixXX Hrow;
+ VectorX h;
+ cData.contactPhase_->getPolytopeInequalities(Hrow, h);
+ MatrixXX H = -Hrow;
+ H.rowwise().normalize();
+ int dimH = (int)(H.rows());
+ MatrixXX mH = cData.contactPhase_->m_mass * H;
+ // init and fill Ab matrix
+ long int dimKin = cData.kin_.size();
+ long int dimAng = useAngMomentum ? (long int)(cData.ang_.size()) : 0;
+ A = initMatrixA(dimH, wps, dimKin + dimAng, useAngMomentum);
+ b = VectorX::Zero(A.rows());
+ point6_t bc = point6_t::Zero();
+ 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) {
+ A.block(i * dimH, 0, dimH, 3) = mH * wpcit->first;
+ b.segment(i * dimH, dimH) = mH * (bc - wpcit->second);
+ if (useAngMomentum) {
+ A.block(i * dimH, 3, dimH, 3) = H * wpLcit->first;
+ b.segment(i * dimH, dimH) += H * (-wpLcit->second);
+ ++wpLcit;
}
- addKinematic(A,b, cData.Kin_,cData.kin_, dimAng);
- if (useAngMomentum)
- addAngularMomentum(A,b,cData.Kin_, cData.kin_);
- fail = false;
- // normalization removes 0 value rows, but resizing
- // must actually be done with matrices and not the references
- /*int zeroindex = Normalize(A,b);
- if(zeroindex < 0)
- fail = true;
- else
- {
- A.conservativeResize(zeroindex, A.cols());
- b.conservativeResize(zeroindex, 1);
- fail = false;
- }*/
- return std::make_pair(A,b);
+ ++i;
+ }
+ addKinematic(A, b, cData.Kin_, cData.kin_, dimAng);
+ if (useAngMomentum) addAngularMomentum(A, b, cData.Kin_, cData.kin_);
+ fail = false;
+ // normalization removes 0 value rows, but resizing
+ // must actually be done with matrices and not the references
+ /*int zeroindex = Normalize(A,b);
+ if(zeroindex < 0)
+ fail = true;
+ else
+ {
+ A.conservativeResize(zeroindex, A.cols());
+ b.conservativeResize(zeroindex, 1);
+ fail = false;
+ }*/
+ 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, 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(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, 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>& 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
+} // namespace bezier_com_traj
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index 6680cc5ca78b60c39e05b0ab56b639545dcdce3d..1a0da0d3a8c2fff0be6743fa898affe2d69aff0e 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -12,513 +12,480 @@
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
-#include <limits>
-#include <algorithm>
+#include <limits>
+#include <algorithm>
static const int dimVarX = 3;
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 wps = computeWwaypoints(pData,T);
- bezier_wp_t::t_point_t res;
- const int DIM_VAR = (int)dimVar(pData);
- std::vector<spline::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);
- t = std::min(cit->first / T, 1.);
- for (std::size_t j = 0 ; j < wps.size() ; ++j)
- {
- b = berns[j](t);
- w.first +=b*(wps[j].first );
- w.second+=b*(wps[j].second);
- }
- res.push_back(w);
+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 wps = computeWwaypoints(pData, T);
+ bezier_wp_t::t_point_t res;
+ const int DIM_VAR = (int)dimVar(pData);
+ std::vector<spline::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);
+ t = std::min(cit->first / T, 1.);
+ for (std::size_t j = 0; j < wps.size(); ++j) {
+ b = berns[j](t);
+ w.first += b * (wps[j].first);
+ w.second += b * (wps[j].second);
}
- return res;
+ res.push_back(w);
+ }
+ 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)
-{
- 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;
- 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));
- 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_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;
+ 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));
+ 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,const waypoint_t& w){
- int dimH = (int)(mH.rows());
- MatrixXX A(dimH,dimVarX);
- VectorX b(dimH);
- VectorX g_= VectorX::Zero(6);
- g_.head<3>() = g;
- A.block(0,0,dimH,3) = mH*w.first;
- b = h + mH*(g_ - w.second);
- Normalize(A,b);
- return std::make_pair(A,b);
+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);
+ VectorX b(dimH);
+ VectorX g_ = VectorX::Zero(6);
+ g_.head<3>() = g;
+ A.block(0, 0, dimH, 3) = mH * w.first;
+ b = h + mH * (g_ - w.second);
+ Normalize(A, b);
+ 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)
- {
- if (cit2->second != cit->second)
- {
- res.push_back(i);
- }
+ 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) {
+ if (cit2->second != cit->second) {
+ res.push_back(i);
}
- res.push_back(i);
- return res;
+ }
+ res.push_back(i);
+ return res;
}
-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;
- int numStepForPhase;
- int numStepsCumulated= 0;
- centroidal_dynamics::MatrixXX Hrow; VectorX h;
- for(int i = 0 ; i < Ts.size() ; ++i)
- {
- pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow,h);
- 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.
- 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)
- return res;
+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;
+ int numStepForPhase;
+ int numStepsCumulated = 0;
+ centroidal_dynamics::MatrixXX Hrow;
+ VectorX h;
+ for (int i = 0; i < Ts.size(); ++i) {
+ pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow, h);
+ 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.
+ 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)
+ return res;
}
-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);
- 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_;
+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);
+ 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_;
}
-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 ;
+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)
-{
- D.block(rowIdx,id_cols,numRowsForce,mG.cols()) = mG;
- id_cols += mG.cols() ;
- D.block(rowIdx,0,numRowsForce,dimVarX) = mass * wp_w.first;
- VectorX g_= VectorX::Zero(6);
- g_.head<3>() = g;
- d.segment(rowIdx,numRowsForce) = mass * (g_ - wp_w.second);
+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;
+ VectorX g_ = VectorX::Zero(6);
+ g_.head<3>() = g;
+ 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)
-{
- updateH(pData, phase, mH, h, dimH);
- // 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);
+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 :
+ // TODO : filter for redunbdant constraints ...
+ // add stability constraints :
+ 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 id_rows = 0;
- 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;
-
- //reference to current stability matrix
- MatrixXX mH; VectorX h; int dimH;
- updateH(pData, pData.contacts_[id_phase], mH, h, dimH);
-
- // assign the Stability constraints for each discretized waypoints :
- // 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);
- // 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)
- {
- 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);
- }
- }
+ 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);
+
+ std::size_t id_phase = 0;
+ const Vector3& g = pData.contacts_[id_phase].contactPhase_->m_gravity;
+
+ // reference to current stability matrix
+ MatrixXX mH;
+ VectorX h;
+ int dimH;
+ updateH(pData, pData.contacts_[id_phase], mH, h, dimH);
+
+ // assign the Stability constraints for each discretized waypoints :
+ // 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);
+ // 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) {
+ 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);
+ }
}
- return id_rows;
+ }
+ 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)
-{
- std::size_t id_phase = 0;
- 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 :
- // 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 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
- 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);
- b.segment(id_rows,current_size) = pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_*wps_c[id_step].second);
- id_rows += current_size;
- }
+ 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);
+ long int current_size;
+ id_phase = 0;
+ // assign the Kinematics constraints for each discretized waypoints :
+ // 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 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
+ 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);
+ b.segment(id_rows, current_size) =
+ pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
+ id_rows += current_size;
+ }
- // check if we are going to switch phases :
- for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i)
- {
- 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 :
- // 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);
- b.segment(id_rows,current_size) = pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_*wps_c[id_step].second);
- id_rows += current_size;
- }
- }
+ // check if we are going to switch phases :
+ for (std::size_t i = 0; i < (stepIdForPhase.size() - 1); ++i) {
+ 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 :
+ // 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);
+ b.segment(id_rows, current_size) =
+ pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
+ id_rows += current_size;
}
+ }
}
+ }
}
-
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);
- 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
- 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
- b.segment(id_rows+3,3) = acc_bounds + wps_ddc[id_step].second;
- id_rows += 6;
- }
+ 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);
+ 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
+ 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
+ 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)
-{
- // 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)
-
- // 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);
- assignAccelerationConstraints(pData, A, b, timeArray, t_total, id_rows);
-
- assert(id_rows == (A.rows()) && "The constraints matrices were not fully filled.");
- return std::make_pair(A,b);
+ 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)
+
+ // 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);
+ assignAccelerationConstraints(pData, A, b, timeArray, t_total, id_rows);
+
+ 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)
-{
- 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);
+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 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);
- computeFinalVelocity(res);
- computeFinalAcceleration(res);
- res.dL_of_t_ = bezier_t::zero(T);
- }
- return res;
+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);
+ computeFinalVelocity(res);
+ computeFinalAcceleration(res);
+ res.dL_of_t_ = bezier_t::zero(T);
+ }
+ return res;
}
-
/**
* @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 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, 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){
- //kinematics :
- num_ineq += it->kin_.rows()*(degree+1);
- //stability :
- if(useDD)
- {
- it->contactPhase_->getPolytopeInequalities(Hrow,h);
- num_ineq += Hrow.rows()*(w_degree+1);
- }
+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) {
+ // kinematics :
+ num_ineq += it->kin_.rows() * (degree + 1);
+ // stability :
+ if (useDD) {
+ it->contactPhase_->getPolytopeInequalities(Hrow, h);
+ num_ineq += Hrow.rows() * (w_degree + 1);
}
- // acceleration constraints : 6 per points
- num_ineq += (degree-1)*6*Ts.size();
- return num_ineq;
+ }
+ // acceleration constraints : 6 per points
+ num_ineq += (degree - 1) * 6 * Ts.size();
+ return num_ineq;
}
/**
* @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 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 numEq = 0;
- if(useForce)
- {
- long int cSize = 0;
- 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)
- {
- currentDegree = w_degree +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;
- forceVarDim += cSize * (currentDegree);
- numEq += (numRowsForce ) * (currentDegree);
- }
+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
+ 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) {
+ currentDegree = w_degree + 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;
+ forceVarDim += cSize * (currentDegree);
+ numEq += (numRowsForce) * (currentDegree);
}
- return numEq;
+ }
+ return numEq;
}
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);
-
- double T = Ts.sum();
-
- // create the curves for c and w with symbolic waypoints (ie. depend on y)
- bezier_wp_t::t_point_t wps_c = computeConstantWaypointsSymbolic(pData,T);
- bezier_wp_t::t_point_t wps_w = computeWwaypoints(pData,T);
- bezier_wp_t c(wps_c.begin(),wps_c.end(),T);
- bezier_wp_t ddc = c.compute_derivate(2);
- bezier_wp_t w(wps_w.begin(),wps_w.end(),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);
- long int forceVarDim = 0;
- 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
- //MatrixXX A = MatrixXX::Zero(num_ineq+forceVarDim,totalVarDim);
- MatrixXX A = MatrixXX::Zero(num_ineq,totalVarDim);
- VectorX b = VectorX::Zero(num_ineq);
- MatrixXX& D = Dd.first;
- VectorX& d = Dd.second;
- D = MatrixXX::Zero(num_eq,totalVarDim);
- d = VectorX::Zero(num_eq);
-
- double current_t = 0.;
- ContactData phase;
- int size_kin;
- MatrixXX mH; VectorX h; int dimH;
- Vector3 acc_bounds = Vector3::Ones()*pData.constraints_.maxAcceleration_;
-
- long int cSize = 0;
- long int rowIdx = 0;
- // 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);
- bezier_wp_t ws = w.extract(current_t , Ts[id_phase]+current_t);
- current_t += Ts[id_phase];
-
- phase = pData.contacts_[id_phase];
-
- // 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){
- 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;
- }
- // 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);
- }
- 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())
- --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);
- }
- // add acceleration constraints :
- 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
- b.segment(id_rows+3,3) = acc_bounds + wpit->second;
- id_rows += 6;
- }
+ 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);
+
+ double T = Ts.sum();
+
+ // create the curves for c and w with symbolic waypoints (ie. depend on y)
+ bezier_wp_t::t_point_t wps_c = computeConstantWaypointsSymbolic(pData, T);
+ bezier_wp_t::t_point_t wps_w = computeWwaypoints(pData, T);
+ bezier_wp_t c(wps_c.begin(), wps_c.end(), T);
+ bezier_wp_t ddc = c.compute_derivate(2);
+ bezier_wp_t w(wps_w.begin(), wps_w.end(), 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);
+ long int forceVarDim = 0;
+ 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
+ // MatrixXX A = MatrixXX::Zero(num_ineq+forceVarDim,totalVarDim);
+ MatrixXX A = MatrixXX::Zero(num_ineq, totalVarDim);
+ VectorX b = VectorX::Zero(num_ineq);
+ MatrixXX& D = Dd.first;
+ VectorX& d = Dd.second;
+ D = MatrixXX::Zero(num_eq, totalVarDim);
+ d = VectorX::Zero(num_eq);
+
+ double current_t = 0.;
+ ContactData phase;
+ int size_kin;
+ MatrixXX mH;
+ VectorX h;
+ int dimH;
+ Vector3 acc_bounds = Vector3::Ones() * pData.constraints_.maxAcceleration_;
+
+ long int cSize = 0;
+ long int rowIdx = 0;
+ // 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);
+ bezier_wp_t ws = w.extract(current_t, Ts[id_phase] + current_t);
+ current_t += Ts[id_phase];
+
+ phase = pData.contacts_[id_phase];
+
+ // 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) {
+ 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;
+ }
+ // 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);
+ } 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())
+ --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);
}
- if(!useDD)
- {
- // 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.head<3>() = VectorX::Ones(3) * (solvers::UNBOUNDED_DOWN);
- minBounds.tail(forceVarDim) = VectorX::Zero(forceVarDim) ;
+ // add acceleration constraints :
+ 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
+ b.segment(id_rows + 3, 3) = acc_bounds + wpit->second;
+ id_rows += 6;
}
- 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);
+ }
+ if (!useDD) {
+ // 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.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.");
+ // int out = Normalize(D,d);
+ return std::make_pair(A, b);
}
-
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");
- 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);
- VectorX x = VectorX::Zero(dimVarX);
- ResultData resQp = solve(Ab,Hg, x);
+ 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");
+ 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);
+ VectorX x = VectorX::Zero(dimVarX);
+ ResultData resQp = solve(Ab, Hg, x);
#if PRINT_QHULL_INEQ
- if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
+ if (resQp.success_) printQHullFile(Ab, resQp.x, "bezier_wp.txt");
#endif
- return genTraj(resQp, pData, T);
+ return genTraj(resQp, pData, T);
}
-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");
- double T = Ts.sum();
- T_time timeArray;
- std::pair<MatrixXX, VectorX> Ab;
- std::pair<MatrixXX, VectorX> Dd;
- VectorX minBounds, maxBounds;
- if(timeStep > 0 ){ // discretized
- assert (pData.representation_ == DOUBLE_DESCRIPTION);
- timeArray = computeDiscretizedTime(Ts,timeStep);
- Ab = computeConstraintsOneStep(pData,Ts,T,timeArray);
- Dd = std::make_pair(MatrixXX::Zero(0,Ab.first.cols()),VectorX::Zero(0));
- minBounds =VectorX::Zero(0);
- 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
- }
- 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);
+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");
+ double T = Ts.sum();
+ T_time timeArray;
+ std::pair<MatrixXX, VectorX> Ab;
+ std::pair<MatrixXX, VectorX> Dd;
+ VectorX minBounds, maxBounds;
+ if (timeStep > 0) { // discretized
+ assert(pData.representation_ == DOUBLE_DESCRIPTION);
+ timeArray = computeDiscretizedTime(Ts, timeStep);
+ Ab = computeConstraintsOneStep(pData, Ts, T, timeArray);
+ Dd = std::make_pair(MatrixXX::Zero(0, Ab.first.cols()), VectorX::Zero(0));
+ minBounds = VectorX::Zero(0);
+ 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
+ }
+ 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
- if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
+ if (resQp.success_) printQHullFile(Ab, resQp.x, "bezier_wp.txt");
#endif
- return genTraj(resQp, pData, T);
+ return genTraj(resQp, pData, T);
}
-
-} // namespace
+} // namespace bezier_com_traj
diff --git a/src/costfunction_definition.cpp b/src/costfunction_definition.cpp
index 4e43e7b70897532e7f40f74eef44ce0adc4ae0a7..2c16720c925dbf522c877adf9505e9b92539c165 100644
--- a/src/costfunction_definition.cpp
+++ b/src/costfunction_definition.cpp
@@ -7,68 +7,57 @@
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
-namespace bezier_com_traj
-{
+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)
-{
- // cost : x' H x + 2 x g'
- 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 x and midPoint :
+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 ??
+ 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)
-{
- if (pData.constraints_.flag_ && END_VEL)
- 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_);
+// 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) {
+ if (pData.constraints_.flag_ && END_VEL)
+ 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)
-{
- 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);
- // 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;
- g.head<3>() += wps_ddc[i].first*wps_ddc[i].second;
- }
+ 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);
+ // 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;
+ 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 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);
-
-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())
- return cit->second(pData, Ts, T, timeArray, H, g);
- else
- {
- std::cout<<"Unknown cost function"<<std::endl;
- throw std::runtime_error("Unknown cost function");
- }
+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);
+
+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())
+ return cit->second(pData, Ts, T, timeArray, H, g);
+ else {
+ std::cout << "Unknown cost function" << std::endl;
+ throw std::runtime_error("Unknown cost function");
+ }
}
-} // namespace cost
-} // namespace bezier_com_traj
+} // namespace cost
+} // namespace bezier_com_traj
diff --git a/src/eiquadprog-fast.cpp b/src/eiquadprog-fast.cpp
index 7c0f619993e760d13904e54e5f1dc5f1ae696888..d19de1dc541655c974d691c4e8f4b2554fc77ea5 100644
--- a/src/eiquadprog-fast.cpp
+++ b/src/eiquadprog-fast.cpp
@@ -15,1142 +15,1028 @@
// <http://www.gnu.org/licenses/>.
//
-
#include "hpp/bezier-com-traj/solver/eiquadprog-fast.hpp"
#include <iostream>
-namespace tsid
-{
- namespace solvers
- {
-
- /// Compute sqrt(a^2 + b^2)
- template<typename Scalar>
- inline Scalar distance(Scalar a, Scalar b)
- {
- Scalar a1, b1, t;
- a1 = std::abs(a);
- b1 = std::abs(b);
- if (a1 > b1)
- {
- t = (b1 / a1);
- return a1 * std::sqrt(1.0 + t * t);
- }
- else
- if (b1 > a1)
- {
- t = (a1 / b1);
- return b1 * std::sqrt(1.0 + t * t);
- }
- return a1 * std::sqrt(2.0);
- }
-
- EiquadprogFast::EiquadprogFast()
- {
- m_maxIter = DEFAULT_MAX_ITER;
- 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;
- m_nIneqCon = 0;
- }
-
- EiquadprogFast::~EiquadprogFast() {}
-
- void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon)
- {
- m_nVars = nVars;
- m_nEqCon = nEqCon;
- m_nIneqCon = nIneqCon;
- m_J.setZero(nVars, nVars);
- chol_.compute(m_J);
- R.resize(nVars, nVars);
- s.resize(nIneqCon);
- r.resize(nIneqCon+nEqCon);
- u.resize(nIneqCon+nEqCon);
- z.resize(nVars);
- d.resize(nVars);
- np.resize(nVars);
- A.resize(nIneqCon+nEqCon);
- iai.resize(nIneqCon);
- iaexcl.resize(nIneqCon);
- x_old.resize(nVars);
- u_old.resize(nIneqCon+nEqCon);
- A_old.resize(nIneqCon+nEqCon);
+namespace tsid {
+namespace solvers {
+
+/// Compute sqrt(a^2 + b^2)
+template <typename Scalar>
+inline Scalar distance(Scalar a, Scalar b) {
+ Scalar a1, b1, t;
+ a1 = std::abs(a);
+ b1 = std::abs(b);
+ if (a1 > b1) {
+ t = (b1 / a1);
+ return a1 * std::sqrt(1.0 + t * t);
+ } else if (b1 > a1) {
+ t = (a1 / b1);
+ return b1 * std::sqrt(1.0 + t * t);
+ }
+ return a1 * std::sqrt(2.0);
+}
+
+EiquadprogFast::EiquadprogFast() {
+ m_maxIter = DEFAULT_MAX_ITER;
+ 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;
+ m_nIneqCon = 0;
+}
+
+EiquadprogFast::~EiquadprogFast() {}
+
+void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon) {
+ m_nVars = nVars;
+ m_nEqCon = nEqCon;
+ m_nIneqCon = nIneqCon;
+ m_J.setZero(nVars, nVars);
+ chol_.compute(m_J);
+ R.resize(nVars, nVars);
+ s.resize(nIneqCon);
+ r.resize(nIneqCon + nEqCon);
+ u.resize(nIneqCon + nEqCon);
+ z.resize(nVars);
+ d.resize(nVars);
+ np.resize(nVars);
+ A.resize(nIneqCon + nEqCon);
+ iai.resize(nIneqCon);
+ iaexcl.resize(nIneqCon);
+ x_old.resize(nVars);
+ u_old.resize(nIneqCon + nEqCon);
+ A_old.resize(nIneqCon + nEqCon);
#ifdef OPTIMIZE_ADD_CONSTRAINT
- T1.resize(nVars);
+ T1.resize(nVars);
#endif
- }
+}
-
- bool EiquadprogFast::add_constraint(MatrixXd & R,
- MatrixXd & J,
- VectorXd & d,
- int& iq,
- double& R_norm)
- {
- long int nVars = J.rows();
+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 << '/';
+ std::cout << "Add constraint " << iq << '/';
#endif
- long int j, k;
- double cc, ss, h, t1, t2, xny;
+ long int j, k;
+ double cc, ss, h, t1, t2, xny;
#ifdef OPTIMIZE_ADD_CONSTRAINT
- Eigen::Vector2d cc_ss;
+ Eigen::Vector2d cc_ss;
#endif
- /* we have to find the Givens rotation which will reduce the element
- d(j) to zero.
- if it is already zero we don't have to do anything, except of
- 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. */
- cc = d(j - 1);
- ss = d(j);
- h = distance(cc, ss);
- if (h == 0.0)
- continue;
- d(j) = 0.0;
- ss = ss / h;
- cc = cc / h;
- if (cc < 0.0)
- {
- cc = -cc;
- ss = -ss;
- d(j - 1) = -h;
- }
- else
- d(j - 1) = h;
- xny = ss / (1.0 + cc);
+ /* we have to find the Givens rotation which will reduce the element
+ d(j) to zero.
+ if it is already zero we don't have to do anything, except of
+ 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. */
+ cc = d(j - 1);
+ ss = d(j);
+ h = distance(cc, ss);
+ if (h == 0.0) continue;
+ d(j) = 0.0;
+ ss = ss / h;
+ cc = cc / h;
+ if (cc < 0.0) {
+ cc = -cc;
+ ss = -ss;
+ d(j - 1) = -h;
+ } else
+ d(j - 1) = h;
+ xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
-#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;
- J.col(j-1).noalias() = J.middleCols<2>(j-1) * cc_ss;
- J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
+#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;
+ J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
+ J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
#else
- // J.col(j-1) = J[:,j-1:j] * [cc; ss]
- for (k = 0; k < nVars; k++)
- {
- t1 = J(k,j - 1);
- t2 = J(k,j);
- J(k,j - 1) = t1 * cc + t2 * ss;
- J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
- }
+ // J.col(j-1) = J[:,j-1:j] * [cc; ss]
+ for (k = 0; k < nVars; k++) {
+ t1 = J(k, j - 1);
+ t2 = J(k, j);
+ J(k, j - 1) = t1 * cc + t2 * ss;
+ J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
+ }
#endif
- }
- /* update the number of constraints added*/
- iq++;
- /* To update R we have to put the iq components of the d vector
- into column iq - 1 of R
- */
- R.col(iq-1).head(iq) = d.head(iq);
+ }
+ /* update the number of constraints added*/
+ iq++;
+ /* To update R we have to put the iq components of the d vector
+into column iq - 1 of R
+*/
+ R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
- std::cout << iq << std::endl;
+ std::cout << iq << std::endl;
#endif
- if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
- // problem degenerate
- return false;
- R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
- return true;
- }
-
-
- void EiquadprogFast::delete_constraint(MatrixXd & R,
- MatrixXd & J,
- VectorXi & A,
- VectorXd & u,
- int nEqCon,
- int& iq,
- int l)
- {
+ if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
+ // problem degenerate
+ return false;
+ R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
+ return true;
+}
- const long int nVars = R.rows();
+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
- std::cout << "Delete constraint " << l << ' ' << iq;
+ std::cout << "Delete constraint " << l << ' ' << iq;
#endif
- int i, j, k;
- int qq =0;
- double cc, ss, h, xny, t1, t2;
-
- /* Find the index qq for active constraint l to be removed */
- for (i = nEqCon; i < iq; i++)
- if (A(i) == l)
- {
- qq = i;
- break;
- }
-
- /* remove the constraint from the active set and the duals */
- for (i = qq; i < iq - 1; i++)
- {
- A(i) = A(i + 1);
- u(i) = u(i + 1);
- R.col(i) = R.col(i+1);
- }
+ int i, j, k;
+ int qq = 0;
+ double cc, ss, h, xny, t1, t2;
+
+ /* Find the index qq for active constraint l to be removed */
+ for (i = nEqCon; i < iq; i++)
+ if (A(i) == l) {
+ qq = i;
+ break;
+ }
- A(iq - 1) = A(iq);
- u(iq - 1) = u(iq);
- A(iq) = 0;
- u(iq) = 0.0;
- for (j = 0; j < iq; j++)
- R(j,iq - 1) = 0.0;
- /* constraint has been fully removed */
- iq--;
+ /* remove the constraint from the active set and the duals */
+ for (i = qq; i < iq - 1; i++) {
+ A(i) = A(i + 1);
+ u(i) = u(i + 1);
+ R.col(i) = R.col(i + 1);
+ }
+
+ A(iq - 1) = A(iq);
+ u(iq - 1) = u(iq);
+ A(iq) = 0;
+ u(iq) = 0.0;
+ for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
+ /* constraint has been fully removed */
+ iq--;
#ifdef TRACE_SOLVER
- std::cout << '/' << iq << std::endl;
+ std::cout << '/' << iq << std::endl;
#endif
- if (iq == 0)
- return;
-
- for (j = qq; j < iq; j++)
- {
- cc = R(j,j);
- ss = R(j + 1,j);
- h = distance(cc, ss);
- if (h == 0.0)
- continue;
- cc = cc / h;
- ss = ss / h;
- R(j + 1,j) = 0.0;
- if (cc < 0.0)
- {
- R(j,j) = -h;
- cc = -cc;
- ss = -ss;
- }
- else
- R(j,j) = h;
-
- xny = ss / (1.0 + cc);
- for (k = j + 1; k < iq; k++)
- {
- t1 = R(j,k);
- t2 = R(j + 1,k);
- R(j,k) = t1 * cc + t2 * ss;
- R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
- }
- for (k = 0; k < nVars; k++)
- {
- t1 = J(k,j);
- t2 = J(k,j + 1);
- J(k,j) = t1 * cc + t2 * ss;
- J(k,j + 1) = xny * (J(k,j) + t1) - t2;
- }
- }
+ if (iq == 0) return;
+
+ for (j = qq; j < iq; j++) {
+ cc = R(j, j);
+ ss = R(j + 1, j);
+ h = distance(cc, ss);
+ if (h == 0.0) continue;
+ cc = cc / h;
+ ss = ss / h;
+ R(j + 1, j) = 0.0;
+ if (cc < 0.0) {
+ R(j, j) = -h;
+ cc = -cc;
+ ss = -ss;
+ } else
+ R(j, j) = h;
+
+ xny = ss / (1.0 + cc);
+ for (k = j + 1; k < iq; k++) {
+ t1 = R(j, k);
+ t2 = R(j + 1, k);
+ R(j, k) = t1 * cc + t2 * ss;
+ R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
}
-
-
- template<class Derived>
- void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n){
- if (x.size() < 10)
- std::cout << name << x.transpose() << std::endl;
- }
- template<class Derived>
- void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n){
- // std::cout << name << std::endl << x << std::endl;
+ for (k = 0; k < nVars; k++) {
+ t1 = J(k, j);
+ t2 = J(k, j + 1);
+ J(k, j) = t1 * cc + t2 * ss;
+ J(k, j + 1) = xny * (J(k, j) + t1) - t2;
}
-
- 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);
-
- assert(Hess.rows()==m_nVars && Hess.cols()==m_nVars);
- assert(g0.size()==m_nVars);
- assert(CE.rows()==m_nEqCon && CE.cols()==m_nVars);
- assert(ce0.size()==m_nEqCon);
- assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars);
- assert(ci0.size()==m_nIneqCon);
-
- int i, k, l; // indices
- int ip; // index of the chosen violated constraint
- int iq; // current number of active constraints
- double psi; // current sum of constraint violations
- double c1; // Hessian trace
- double c2; // Hessian Chowlesky factor trace
- double ss; // largest constraint violation (negative for violation)
- double R_norm; // norm of matrix R
- const double inf = std::numeric_limits<double>::infinity();
- double t, t1, t2;
- /* t is the step length, which is the minimum of the partial step length t1
- * and the full step length t2 */
-
- iter = 0; // active-set iteration number
-
- /*
- * Preprocessing phase
- */
- /* compute the trace of the original matrix Hess */
- c1 = Hess.trace();
-
- /* decompose the matrix Hess in the form LL^T */
- if(!is_inverse_provided_)
- {
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
- chol_.compute(Hess);
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
- }
-
-
- /* initialize the matrix R */
- d.setZero(nVars);
- R.setZero(nVars, nVars);
- R_norm = 1.0;
-
- /* 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);
- m_J.setIdentity(nVars, nVars);
+ }
+}
+
+template <class Derived>
+void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
+ if (x.size() < 10) std::cout << name << x.transpose() << std::endl;
+}
+template <class Derived>
+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) {
+ 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);
+
+ assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
+ assert(g0.size() == m_nVars);
+ assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars);
+ assert(ce0.size() == m_nEqCon);
+ assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars);
+ assert(ci0.size() == m_nIneqCon);
+
+ int i, k, l; // indices
+ int ip; // index of the chosen violated constraint
+ int iq; // current number of active constraints
+ double psi; // current sum of constraint violations
+ double c1; // Hessian trace
+ double c2; // Hessian Chowlesky factor trace
+ double ss; // largest constraint violation (negative for violation)
+ double R_norm; // norm of matrix R
+ const double inf = std::numeric_limits<double>::infinity();
+ double t, t1, t2;
+ /* t is the step length, which is the minimum of the partial step length t1
+ * and the full step length t2 */
+
+ iter = 0; // active-set iteration number
+
+ /*
+ * Preprocessing phase
+ */
+ /* compute the trace of the original matrix Hess */
+ c1 = Hess.trace();
+
+ /* decompose the matrix Hess in the form LL^T */
+ if (!is_inverse_provided_) {
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
+ chol_.compute(Hess);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
+ }
+
+ /* initialize the matrix R */
+ d.setZero(nVars);
+ R.setZero(nVars, nVars);
+ R_norm = 1.0;
+
+ /* 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);
+ m_J.setIdentity(nVars, nVars);
#ifdef OPTIMIZE_HESSIAN_INVERSE
- chol_.matrixU().solveInPlace(m_J);
+ chol_.matrixU().solveInPlace(m_J);
#else
- m_J = chol_.matrixU().solve(m_J);
+ m_J = chol_.matrixU().solve(m_J);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
+ }
- c2 = m_J.trace();
+ c2 = m_J.trace();
#ifdef _SOLVER
- print_matrix("m_J", m_J, nVars);
+ print_matrix("m_J", m_J, nVars);
#endif
- /* 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
- */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
- if(is_inverse_provided_)
- {
- x = m_J*(m_J.transpose()*g0);
- }
- else
- {
+ /* 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
+ */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
+ if (is_inverse_provided_) {
+ x = m_J * (m_J.transpose() * g0);
+ } else {
#ifdef OPTIMIZE_UNCONSTR_MINIM
- x = -g0;
- chol_.solveInPlace(x);
- }
+ x = -g0;
+ chol_.solveInPlace(x);
+ }
#else
- x = chol_.solve(g0);
- }
- x = -x;
+ x = chol_.solve(g0);
+ }
+ x = -x;
#endif
- /* and compute the current solution value */
- f_value = 0.5 * g0.dot(x);
+ /* and compute the current solution value */
+ f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
- std::cout << "Unconstrained solution: " << f_value << std::endl;
- print_vector("x", x, nVars);
+ std::cout << "Unconstrained solution: " << f_value << std::endl;
+ print_vector("x", x, nVars);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
- /* Add equality constraints to the working set A */
+ /* Add equality constraints to the working set A */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
- iq = 0;
- for (i = 0; i < nEqCon; i++)
- {
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
- np = CE.row(i);
- compute_d(d, m_J, np);
- update_z(z, m_J, d, iq);
- update_r(R, r, d, iq);
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
+ iq = 0;
+ for (i = 0; i < nEqCon; i++) {
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
+ np = CE.row(i);
+ compute_d(d, m_J, np);
+ update_z(z, m_J, d, iq);
+ update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
- print_matrix("R", R, iq);
- print_vector("z", z, nVars);
- print_vector("r", r, iq);
- print_vector("d", d, nVars);
+ print_matrix("R", R, iq);
+ print_vector("z", z, nVars);
+ print_vector("r", r, iq);
+ 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 */
- t2 = 0.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;
-
- /* set u = u+ */
- u(iq) = t2;
- u.head(iq) -= t2 * r.head(iq);
-
- /* compute the new solution value */
- f_value += 0.5 * (t2 * t2) * z.dot(np);
- A(i) = -i - 1;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
-
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- // Equality constraints are linearly dependent
- return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
- }
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
- }
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
+ /* 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
+ t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
+
+ x += t2 * z;
+
+ /* set u = u+ */
+ u(iq) = t2;
+ u.head(iq) -= t2 * r.head(iq);
- /* set iai = K \ A */
- for (i = 0; i < nIneqCon; i++)
- iai(i) = i;
+ /* compute the new solution value */
+ f_value += 0.5 * (t2 * t2) * z.dot(np);
+ A(i) = -i - 1;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
+
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ // Equality constraints are linearly dependent
+ return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
+ }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
+ }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
+
+ /* set iai = K \ A */
+ 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")
- for (i = nEqCon; i < q; i++)
- {
- iai(i-nEqCon) = -1;
- ip = A(i);
- np = CI.row(ip);
- compute_d(d, m_J, np);
- 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 */
- t2 = 0.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")
-
- x += t2 * z;
-
- /* set u = u+ */
- u(iq) = t2;
- u.head(iq) -= t2 * r.head(iq);
-
- /* compute the new solution value */
- f_value += 0.5 * (t2 * t2) * z.dot(np);
-
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- // constraints are linearly dependent
- std::cout << "[WARM START] Constraints are linearly dependent\n";
- return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
- }
- }
+ // 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);
+ np = CI.row(ip);
+ compute_d(d, m_J, np);
+ 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 */
+ t2 = 0.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")
+
+ x += t2 * z;
+
+ /* set u = u+ */
+ u(iq) = t2;
+ u.head(iq) -= t2 * r.head(iq);
+
+ /* compute the new solution value */
+ f_value += 0.5 * (t2 * t2) * z.dot(np);
+
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ // constraints are linearly dependent
+ std::cout << "[WARM START] Constraints are linearly dependent\n";
+ return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
+ }
+ }
#else
#endif
l1:
- iter++;
- if(iter>=m_maxIter)
- {
- q = iq;
- return EIQUADPROG_FAST_MAX_ITER_REACHED;
- }
+ iter++;
+ if (iter >= m_maxIter) {
+ q = iq;
+ return EIQUADPROG_FAST_MAX_ITER_REACHED;
+ }
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
#ifdef TRACE_SOLVER
- print_vector("x", x, nVars);
+ print_vector("x", x, nVars);
#endif
- /* step 1: choose a violated constraint */
- for (i = nEqCon; i < iq; i++)
- {
- ip = A(i);
- iai(ip) = -1;
- }
+ /* step 1: choose a violated constraint */
+ for (i = nEqCon; i < iq; i++) {
+ ip = A(i);
+ iai(ip) = -1;
+ }
- /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
- ss = 0.0;
- ip = 0; /* ip will be the index of the chosen violated constraint */
+ /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
+ ss = 0.0;
+ ip = 0; /* ip will be the index of the chosen violated constraint */
#ifdef OPTIMIZE_STEP_1_2
- s = ci0;
- s.noalias() += CI*x;
- iaexcl.setOnes();
- psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
+ s = ci0;
+ s.noalias() += CI * x;
+ iaexcl.setOnes();
+ psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
#else
- psi = 0.0; /* this value will contain the sum of all infeasibilities */
- for (i = 0; i < nIneqCon; i++)
- {
- iaexcl(i) = 1;
- s(i) = CI.row(i).dot(x) + ci0(i);
- psi += std::min(0.0, s(i));
- }
+ psi = 0.0; /* this value will contain the sum of all infeasibilities */
+ for (i = 0; i < nIneqCon; i++) {
+ iaexcl(i) = 1;
+ s(i) = CI.row(i).dot(x) + ci0(i);
+ psi += std::min(0.0, s(i));
+ }
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
#ifdef TRACE_SOLVER
- print_vector("s", s, nIneqCon);
+ print_vector("s", s, nIneqCon);
#endif
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
-
- 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")
- return EIQUADPROG_FAST_OPTIMAL;
- }
+ 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")
+ return EIQUADPROG_FAST_OPTIMAL;
+ }
- /* save old values for u, x and A */
- u_old.head(iq) = u.head(iq);
- A_old.head(iq) = A.head(iq);
- x_old = x;
+ /* save old values for u, x and A */
+ u_old.head(iq) = u.head(iq);
+ A_old.head(iq) = A.head(iq);
+ x_old = x;
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?)
- for (i = 0; i < nIneqCon; i++)
- {
- if (s(i) < ss && iai(i) != -1 && iaexcl(i))
- {
- ss = s(i);
- ip = i;
- }
- }
- if (ss >= 0.0)
- {
- q = iq;
- // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
- return EIQUADPROG_FAST_OPTIMAL;
- }
-
- /* set np = n(ip) */
- np = CI.row(ip);
- /* set u = (u 0)^T */
- u(iq) = 0.0;
- /* add ip to the active set A */
- A(iq) = ip;
-
- // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
+ // 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);
+ ip = i;
+ }
+ }
+ if (ss >= 0.0) {
+ q = iq;
+ // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
+ return EIQUADPROG_FAST_OPTIMAL;
+ }
+
+ /* set np = n(ip) */
+ np = CI.row(ip);
+ /* set u = (u 0)^T */
+ u(iq) = 0.0;
+ /* add ip to the active set A */
+ A(iq) = ip;
+
+ // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
#ifdef TRACE_SOLVER
- std::cout << "Trying with constraint " << ip << std::endl;
- print_vector("np", np, nVars);
+ std::cout << "Trying with constraint " << ip << std::endl;
+ print_vector("np", np, nVars);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
-
-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_d(d, m_J, np);
- // update_z(z, m_J, d, iq);
- if(iq >= nVars)
- {
- // throw std::runtime_error("iq >= m_J.cols()");
- z.setZero();
- }else{
- update_z(z, m_J, d, iq);
- }
- /* compute N* np (if q > 0): the negative of the step direction in the dual space */
- update_r(R, r, d, iq);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
+
+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_d(d, m_J, np);
+ // update_z(z, m_J, d, iq);
+ if (iq >= nVars) {
+ // throw std::runtime_error("iq >= m_J.cols()");
+ z.setZero();
+ } else {
+ update_z(z, m_J, d, iq);
+ }
+ /* 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;
- print_vector("z", z, nVars);
- print_vector("r", r, iq + 1);
- print_vector("u", u, iq + 1);
- print_vector("d", d, nVars);
- print_vector("A", A, iq + 1);
+ std::cout << "Step direction z" << std::endl;
+ print_vector("z", z, nVars);
+ print_vector("r", r, iq + 1);
+ print_vector("u", u, iq + 1);
+ print_vector("d", d, nVars);
+ print_vector("A", A, iq + 1);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
-
- /* 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 */
- t1 = inf; /* +inf */
- /* find the index l s.t. it reaches the minimum of u+(x) / r */
- // l: index of constraint to drop (maybe)
- for (k = nEqCon; k < iq; k++)
- {
- double tmp;
- if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
- {
- t1 = tmp;
- 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
- t2 = -s(ip) / z.dot(np);
- else
- t2 = inf; /* +inf */
-
- /* the step is chosen as the minimum of t1 and t2 */
- t = std::min(t1, t2);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
+
+ /* 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 */
+ t1 = inf; /* +inf */
+ /* find the index l s.t. it reaches the minimum of u+(x) / r */
+ // l: index of constraint to drop (maybe)
+ for (k = nEqCon; k < iq; k++) {
+ double tmp;
+ if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
+ t1 = tmp;
+ 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
+ t2 = -s(ip) / z.dot(np);
+ else
+ t2 = inf; /* +inf */
+
+ /* 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);
-
- /* Step 2c: determine new S-pair and take step: */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- /* case (i): no step in primal or dual space */
- if (t >= inf)
- {
- /* QPP is infeasible */
- q = iq;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- return EIQUADPROG_FAST_UNBOUNDED;
- }
- /* case (ii): step in dual space */
- if (t2 >= inf)
- {
- /* set u = u + t * [-r 1) and drop constraint l from the active set A */
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
- iai(l) = l;
- delete_constraint(R, m_J, A, u, nEqCon, iq, l);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
+
+ /* Step 2c: determine new S-pair and take step: */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ /* case (i): no step in primal or dual space */
+ if (t >= inf) {
+ /* QPP is infeasible */
+ q = iq;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ return EIQUADPROG_FAST_UNBOUNDED;
+ }
+ /* case (ii): step in dual space */
+ if (t2 >= inf) {
+ /* set u = u + t * [-r 1) and drop constraint l from the active set A */
+ u.head(iq) -= t * r.head(iq);
+ u(iq) += t;
+ iai(l) = l;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
#ifdef TRACE_SOLVER
- std::cout << " in dual space: "<< f_value << std::endl;
- print_vector("x", x, nVars);
- print_vector("z", z, nVars);
- print_vector("A", A, iq + 1);
+ std::cout << " in dual space: " << f_value << std::endl;
+ print_vector("x", x, nVars);
+ print_vector("z", z, nVars);
+ print_vector("A", A, iq + 1);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l2a;
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l2a;
+ }
- /* case (iii): step in primal and dual space */
- x += t * z;
- /* update the solution value */
- f_value += t * z.dot(np) * (0.5 * t + u(iq));
+ /* case (iii): step in primal and dual space */
+ x += t * z;
+ /* update the solution value */
+ f_value += t * z.dot(np) * (0.5 * t + u(iq));
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
+ u.head(iq) -= t * r.head(iq);
+ u(iq) += t;
#ifdef TRACE_SOLVER
- std::cout << " in both spaces: "<< f_value << std::endl;
- print_vector("x", x, nVars);
- print_vector("u", u, iq + 1);
- print_vector("r", r, iq + 1);
- print_vector("A", A, iq + 1);
+ std::cout << " in both spaces: " << f_value << std::endl;
+ print_vector("x", x, nVars);
+ print_vector("u", u, iq + 1);
+ print_vector("r", r, iq + 1);
+ print_vector("A", A, iq + 1);
#endif
- if (t == t2)
- {
+ if (t == t2) {
#ifdef TRACE_SOLVER
- std::cout << "Full step has taken " << t << std::endl;
- print_vector("x", x, nVars);
+ std::cout << "Full step has taken " << t << std::endl;
+ print_vector("x", x, nVars);
#endif
- /* full step has taken */
- /* add constraint ip to the active set*/
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- iaexcl(ip) = 0;
- delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
+ /* full step has taken */
+ /* add constraint ip to the active set*/
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ iaexcl(ip) = 0;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
#ifdef TRACE_SOLVER
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
#endif
- for (i = 0; i < nIneqCon; i++)
- iai(i) = i;
- for (i = 0; i < iq; i++)
- {
- A(i) = A_old(i);
- iai(A(i)) = -1;
- u(i) = u_old(i);
- }
- x = x_old;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l2; /* go to step 2 */
- }
- else
- iai(ip) = -1;
+ for (i = 0; i < nIneqCon; i++) iai(i) = i;
+ for (i = 0; i < iq; i++) {
+ A(i) = A_old(i);
+ iai(A(i)) = -1;
+ u(i) = u_old(i);
+ }
+ x = x_old;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l2; /* go to step 2 */
+ } else
+ iai(ip) = -1;
#ifdef TRACE_SOLVER
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l1;
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l1;
+ }
- /* a partial step has been taken => drop constraint l */
- iai(l) = l;
- delete_constraint(R, m_J, A, u, nEqCon, iq, l);
- s(ip) = CI.row(ip).dot(x) + ci0(ip);
+ /* a partial step has been taken => drop constraint l */
+ iai(l) = l;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
+ s(ip) = CI.row(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER
- std::cout << "Partial step has taken " << t << std::endl;
- print_vector("x", x, nVars);
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
- print_vector("s", s, nIneqCon);
+ std::cout << "Partial step has taken " << t << std::endl;
+ print_vector("x", x, nVars);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
+ print_vector("s", s, nIneqCon);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
-
- goto l2a;
- }
-
- double trace_sparse(const EiquadprogFast::SpMat& Hess)
- {
- double sum =0;
- for (int k=0; k<Hess.outerSize(); ++k)
- sum += Hess.coeff(k,k);
- 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)
- {
- 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);
-
- assert(Hess.rows()==m_nVars && Hess.cols()==m_nVars);
- assert(g0.size()==m_nVars);
- assert(CE.rows()==m_nEqCon && CE.cols()==m_nVars);
- assert(ce0.size()==m_nEqCon);
- assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars);
- assert(ci0.size()==m_nIneqCon);
-
- int i, k, l; // indices
- int ip; // index of the chosen violated constraint
- int iq; // current number of active constraints
- double psi; // current sum of constraint violations
- double c1; // Hessian trace
- double c2; // Hessian Chowlesky factor trace
- double ss; // largest constraint violation (negative for violation)
- double R_norm; // norm of matrix R
- const double inf = std::numeric_limits<double>::infinity();
- double t, t1, t2;
- /* t is the step length, which is the minimum of the partial step length t1
- * and the full step length t2 */
-
- iter = 0; // active-set iteration number
-
- /*
- * Preprocessing phase
- */
- /* compute the trace of the original matrix Hess */
- c1 = trace_sparse(Hess);
-
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
- Eigen::SimplicialLLT< SpMat, Eigen::Lower> cholsp_(Hess);
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
-
-
- /* initialize the matrix R */
- d.setZero(nVars);
- R.setZero(nVars, nVars);
- R_norm = 1.0;
-
- /* 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);
- m_J.setIdentity(nVars, nVars);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+
+ goto l2a;
+}
+
+double trace_sparse(const EiquadprogFast::SpMat& Hess) {
+ double sum = 0;
+ for (int k = 0; k < Hess.outerSize(); ++k) sum += Hess.coeff(k, k);
+ 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) {
+ 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);
+
+ assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
+ assert(g0.size() == m_nVars);
+ assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars);
+ assert(ce0.size() == m_nEqCon);
+ assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars);
+ assert(ci0.size() == m_nIneqCon);
+
+ int i, k, l; // indices
+ int ip; // index of the chosen violated constraint
+ int iq; // current number of active constraints
+ double psi; // current sum of constraint violations
+ double c1; // Hessian trace
+ double c2; // Hessian Chowlesky factor trace
+ double ss; // largest constraint violation (negative for violation)
+ double R_norm; // norm of matrix R
+ const double inf = std::numeric_limits<double>::infinity();
+ double t, t1, t2;
+ /* t is the step length, which is the minimum of the partial step length t1
+ * and the full step length t2 */
+
+ iter = 0; // active-set iteration number
+
+ /*
+ * Preprocessing phase
+ */
+ /* compute the trace of the original matrix Hess */
+ c1 = trace_sparse(Hess);
+
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
+ Eigen::SimplicialLLT<SpMat, Eigen::Lower> cholsp_(Hess);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION);
+
+ /* initialize the matrix R */
+ d.setZero(nVars);
+ R.setZero(nVars, nVars);
+ R_norm = 1.0;
+
+ /* 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);
+ m_J.setIdentity(nVars, nVars);
#ifdef OPTIMIZE_HESSIAN_INVERSE
- cholsp_.matrixU().solve(m_J);
+ cholsp_.matrixU().solve(m_J);
#else
- m_J = cholsp_.matrixU().solve(m_J);
+ m_J = cholsp_.matrixU().solve(m_J);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
+ }
- c2 = m_J.trace();
+ c2 = m_J.trace();
#ifdef _SOLVER
- print_matrix("m_J", m_J, nVars);
+ print_matrix("m_J", m_J, nVars);
#endif
- /* 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
- */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
- if(is_inverse_provided_)
- {
- x = m_J*(m_J.transpose()*g0);
- }
- else
- {
+ /* 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
+ */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
+ if (is_inverse_provided_) {
+ x = m_J * (m_J.transpose() * g0);
+ } else {
#ifdef OPTIMIZE_UNCONSTR_MINIM
- //x = -g0;
- x = cholsp_.solve(-g0);
- }
+ // x = -g0;
+ x = cholsp_.solve(-g0);
+ }
#else
- x = cholsp_.solve(g0);
- }
- x = -x;
+ x = cholsp_.solve(g0);
+ }
+ x = -x;
#endif
- /* and compute the current solution value */
- f_value = 0.5 * g0.dot(x);
+ /* and compute the current solution value */
+ f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
- std::cout << "Unconstrained solution: " << f_value << std::endl;
- print_vector("x", x, nVars);
+ std::cout << "Unconstrained solution: " << f_value << std::endl;
+ print_vector("x", x, nVars);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
- /* Add equality constraints to the working set A */
+ /* Add equality constraints to the working set A */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
- iq = 0;
- for (i = 0; i < nEqCon; i++)
- {
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
- np = CE.row(i);
- compute_d(d, m_J, np);
- update_z(z, m_J, d, iq);
- update_r(R, r, d, iq);
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
+ iq = 0;
+ for (i = 0; i < nEqCon; i++) {
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
+ np = CE.row(i);
+ compute_d(d, m_J, np);
+ update_z(z, m_J, d, iq);
+ update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
- print_matrix("R", R, iq);
- print_vector("z", z, nVars);
- print_vector("r", r, iq);
- print_vector("d", d, nVars);
+ print_matrix("R", R, iq);
+ print_vector("z", z, nVars);
+ print_vector("r", r, iq);
+ 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 */
- t2 = 0.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;
-
- /* set u = u+ */
- u(iq) = t2;
- u.head(iq) -= t2 * r.head(iq);
-
- /* compute the new solution value */
- f_value += 0.5 * (t2 * t2) * z.dot(np);
- A(i) = -i - 1;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
-
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- // Equality constraints are linearly dependent
- return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
- }
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
- }
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
+ /* 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
+ t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
+
+ x += t2 * z;
+
+ /* set u = u+ */
+ u(iq) = t2;
+ u.head(iq) -= t2 * r.head(iq);
+
+ /* compute the new solution value */
+ f_value += 0.5 * (t2 * t2) * z.dot(np);
+ A(i) = -i - 1;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
+
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ // Equality constraints are linearly dependent
+ return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
+ }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
+ }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
- /* set iai = K \ A */
- for (i = 0; i < nIneqCon; i++)
- iai(i) = i;
+ /* set iai = K \ A */
+ 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")
- for (i = nEqCon; i < q; i++)
- {
- iai(i-nEqCon) = -1;
- ip = A(i);
- np = CI.row(ip);
- compute_d(d, m_J, np);
- 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 */
- t2 = 0.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")
-
- x += t2 * z;
-
- /* set u = u+ */
- u(iq) = t2;
- u.head(iq) -= t2 * r.head(iq);
-
- /* compute the new solution value */
- f_value += 0.5 * (t2 * t2) * z.dot(np);
-
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- // constraints are linearly dependent
- std::cout << "[WARM START] Constraints are linearly dependent\n";
- return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
- }
- }
+ // 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);
+ np = CI.row(ip);
+ compute_d(d, m_J, np);
+ 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 */
+ t2 = 0.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")
+
+ x += t2 * z;
+
+ /* set u = u+ */
+ u(iq) = t2;
+ u.head(iq) -= t2 * r.head(iq);
+
+ /* compute the new solution value */
+ f_value += 0.5 * (t2 * t2) * z.dot(np);
+
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ // constraints are linearly dependent
+ std::cout << "[WARM START] Constraints are linearly dependent\n";
+ return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
+ }
+ }
#else
#endif
l1:
- iter++;
- if(iter>=m_maxIter)
- {
- q = iq;
- return EIQUADPROG_FAST_MAX_ITER_REACHED;
- }
+ iter++;
+ if (iter >= m_maxIter) {
+ q = iq;
+ return EIQUADPROG_FAST_MAX_ITER_REACHED;
+ }
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
#ifdef TRACE_SOLVER
- print_vector("x", x, nVars);
+ print_vector("x", x, nVars);
#endif
- /* step 1: choose a violated constraint */
- for (i = nEqCon; i < iq; i++)
- {
- ip = A(i);
- iai(ip) = -1;
- }
+ /* step 1: choose a violated constraint */
+ for (i = nEqCon; i < iq; i++) {
+ ip = A(i);
+ iai(ip) = -1;
+ }
- /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
- ss = 0.0;
- ip = 0; /* ip will be the index of the chosen violated constraint */
+ /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
+ ss = 0.0;
+ ip = 0; /* ip will be the index of the chosen violated constraint */
#ifdef OPTIMIZE_STEP_1_2
- s = ci0;
- s.noalias() += CI*x;
- iaexcl.setOnes();
- psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
+ s = ci0;
+ s.noalias() += CI * x;
+ iaexcl.setOnes();
+ psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
#else
- psi = 0.0; /* this value will contain the sum of all infeasibilities */
- for (i = 0; i < nIneqCon; i++)
- {
- iaexcl(i) = 1;
- s(i) = CI.row(i).dot(x) + ci0(i);
- psi += std::min(0.0, s(i));
- }
+ psi = 0.0; /* this value will contain the sum of all infeasibilities */
+ for (i = 0; i < nIneqCon; i++) {
+ iaexcl(i) = 1;
+ s(i) = CI.row(i).dot(x) + ci0(i);
+ psi += std::min(0.0, s(i));
+ }
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
#ifdef TRACE_SOLVER
- print_vector("s", s, nIneqCon);
+ print_vector("s", s, nIneqCon);
#endif
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
-
- 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")
- return EIQUADPROG_FAST_OPTIMAL;
- }
+ 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")
+ return EIQUADPROG_FAST_OPTIMAL;
+ }
- /* save old values for u, x and A */
- u_old.head(iq) = u.head(iq);
- A_old.head(iq) = A.head(iq);
- x_old = x;
+ /* save old values for u, x and A */
+ u_old.head(iq) = u.head(iq);
+ A_old.head(iq) = A.head(iq);
+ x_old = x;
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?)
- for (i = 0; i < nIneqCon; i++)
- {
- if (s(i) < ss && iai(i) != -1 && iaexcl(i))
- {
- ss = s(i);
- ip = i;
- }
- }
- if (ss >= 0.0)
- {
- q = iq;
- // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
- return EIQUADPROG_FAST_OPTIMAL;
- }
-
- /* set np = n(ip) */
- np = CI.row(ip);
- /* set u = (u 0)^T */
- u(iq) = 0.0;
- /* add ip to the active set A */
- A(iq) = ip;
-
- // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
+ // 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);
+ ip = i;
+ }
+ }
+ if (ss >= 0.0) {
+ q = iq;
+ // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
+ return EIQUADPROG_FAST_OPTIMAL;
+ }
+
+ /* set np = n(ip) */
+ np = CI.row(ip);
+ /* set u = (u 0)^T */
+ u(iq) = 0.0;
+ /* add ip to the active set A */
+ A(iq) = ip;
+
+ // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
#ifdef TRACE_SOLVER
- std::cout << "Trying with constraint " << ip << std::endl;
- print_vector("np", np, nVars);
+ std::cout << "Trying with constraint " << ip << std::endl;
+ print_vector("np", np, nVars);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
-
-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_d(d, m_J, np);
- // update_z(z, m_J, d, iq);
- if(iq >= nVars)
- {
- // throw std::runtime_error("iq >= m_J.cols()");
- z.setZero();
- }else{
- update_z(z, m_J, d, iq);
- }
- /* compute N* np (if q > 0): the negative of the step direction in the dual space */
- update_r(R, r, d, iq);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
+
+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_d(d, m_J, np);
+ // update_z(z, m_J, d, iq);
+ if (iq >= nVars) {
+ // throw std::runtime_error("iq >= m_J.cols()");
+ z.setZero();
+ } else {
+ update_z(z, m_J, d, iq);
+ }
+ /* 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;
- print_vector("z", z, nVars);
- print_vector("r", r, iq + 1);
- print_vector("u", u, iq + 1);
- print_vector("d", d, nVars);
- print_vector("A", A, iq + 1);
+ std::cout << "Step direction z" << std::endl;
+ print_vector("z", z, nVars);
+ print_vector("r", r, iq + 1);
+ print_vector("u", u, iq + 1);
+ print_vector("d", d, nVars);
+ print_vector("A", A, iq + 1);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
-
- /* 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 */
- t1 = inf; /* +inf */
- /* find the index l s.t. it reaches the minimum of u+(x) / r */
- // l: index of constraint to drop (maybe)
- for (k = nEqCon; k < iq; k++)
- {
- double tmp;
- if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
- {
- t1 = tmp;
- 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
- t2 = -s(ip) / z.dot(np);
- else
- t2 = inf; /* +inf */
-
- /* the step is chosen as the minimum of t1 and t2 */
- t = std::min(t1, t2);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
+
+ /* 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 */
+ t1 = inf; /* +inf */
+ /* find the index l s.t. it reaches the minimum of u+(x) / r */
+ // l: index of constraint to drop (maybe)
+ for (k = nEqCon; k < iq; k++) {
+ double tmp;
+ if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
+ t1 = tmp;
+ 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
+ t2 = -s(ip) / z.dot(np);
+ else
+ t2 = inf; /* +inf */
+
+ /* 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);
-
- /* Step 2c: determine new S-pair and take step: */
- START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- /* case (i): no step in primal or dual space */
- if (t >= inf)
- {
- /* QPP is infeasible */
- q = iq;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- return EIQUADPROG_FAST_UNBOUNDED;
- }
- /* case (ii): step in dual space */
- if (t2 >= inf)
- {
- /* set u = u + t * [-r 1) and drop constraint l from the active set A */
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
- iai(l) = l;
- delete_constraint(R, m_J, A, u, nEqCon, iq, l);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
+
+ /* Step 2c: determine new S-pair and take step: */
+ START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ /* case (i): no step in primal or dual space */
+ if (t >= inf) {
+ /* QPP is infeasible */
+ q = iq;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ return EIQUADPROG_FAST_UNBOUNDED;
+ }
+ /* case (ii): step in dual space */
+ if (t2 >= inf) {
+ /* set u = u + t * [-r 1) and drop constraint l from the active set A */
+ u.head(iq) -= t * r.head(iq);
+ u(iq) += t;
+ iai(l) = l;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
#ifdef TRACE_SOLVER
- std::cout << " in dual space: "<< f_value << std::endl;
- print_vector("x", x, nVars);
- print_vector("z", z, nVars);
- print_vector("A", A, iq + 1);
+ std::cout << " in dual space: " << f_value << std::endl;
+ print_vector("x", x, nVars);
+ print_vector("z", z, nVars);
+ print_vector("A", A, iq + 1);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l2a;
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l2a;
+ }
- /* case (iii): step in primal and dual space */
- x += t * z;
- /* update the solution value */
- f_value += t * z.dot(np) * (0.5 * t + u(iq));
+ /* case (iii): step in primal and dual space */
+ x += t * z;
+ /* update the solution value */
+ f_value += t * z.dot(np) * (0.5 * t + u(iq));
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
+ u.head(iq) -= t * r.head(iq);
+ u(iq) += t;
#ifdef TRACE_SOLVER
- std::cout << " in both spaces: "<< f_value << std::endl;
- print_vector("x", x, nVars);
- print_vector("u", u, iq + 1);
- print_vector("r", r, iq + 1);
- print_vector("A", A, iq + 1);
+ std::cout << " in both spaces: " << f_value << std::endl;
+ print_vector("x", x, nVars);
+ print_vector("u", u, iq + 1);
+ print_vector("r", r, iq + 1);
+ print_vector("A", A, iq + 1);
#endif
- if (t == t2)
- {
+ if (t == t2) {
#ifdef TRACE_SOLVER
- std::cout << "Full step has taken " << t << std::endl;
- print_vector("x", x, nVars);
+ std::cout << "Full step has taken " << t << std::endl;
+ print_vector("x", x, nVars);
#endif
- /* full step has taken */
- /* add constraint ip to the active set*/
- if (!add_constraint(R, m_J, d, iq, R_norm))
- {
- iaexcl(ip) = 0;
- delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
+ /* full step has taken */
+ /* add constraint ip to the active set*/
+ if (!add_constraint(R, m_J, d, iq, R_norm)) {
+ iaexcl(ip) = 0;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
#ifdef TRACE_SOLVER
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
#endif
- for (i = 0; i < nIneqCon; i++)
- iai(i) = i;
- for (i = 0; i < iq; i++)
- {
- A(i) = A_old(i);
- iai(A(i)) = -1;
- u(i) = u_old(i);
- }
- x = x_old;
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l2; /* go to step 2 */
- }
- else
- iai(ip) = -1;
+ for (i = 0; i < nIneqCon; i++) iai(i) = i;
+ for (i = 0; i < iq; i++) {
+ A(i) = A_old(i);
+ iai(A(i)) = -1;
+ u(i) = u_old(i);
+ }
+ x = x_old;
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l2; /* go to step 2 */
+ } else
+ iai(ip) = -1;
#ifdef TRACE_SOLVER
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l1;
- }
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ goto l1;
+ }
- /* a partial step has been taken => drop constraint l */
- iai(l) = l;
- delete_constraint(R, m_J, A, u, nEqCon, iq, l);
- s(ip) = CI.row(ip).dot(x) + ci0(ip);
+ /* a partial step has been taken => drop constraint l */
+ iai(l) = l;
+ delete_constraint(R, m_J, A, u, nEqCon, iq, l);
+ s(ip) = CI.row(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER
- std::cout << "Partial step has taken " << t << std::endl;
- print_vector("x", x, nVars);
- print_matrix("R", R, nVars);
- print_vector("A", A, iq);
- print_vector("s", s, nIneqCon);
+ std::cout << "Partial step has taken " << t << std::endl;
+ print_vector("x", x, nVars);
+ print_matrix("R", R, nVars);
+ print_vector("A", A, iq);
+ print_vector("s", s, nIneqCon);
#endif
- STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
+ STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
- goto l2a;
- }
+ goto l2a;
+}
- } /* namespace solvers */
+} /* namespace solvers */
} /* namespace tsid */
diff --git a/src/glpk-wrapper.cpp b/src/glpk-wrapper.cpp
index 46b2b152cff1f42dfc5e53b93889bb9c76145f3e..138d7ae02b1c102f13af7cc2810098748db31ab8 100644
--- a/src/glpk-wrapper.cpp
+++ b/src/glpk-wrapper.cpp
@@ -15,109 +15,90 @@
// <http://www.gnu.org/licenses/>.
//
-
#include "hpp/bezier-com-traj/solver/glpk-wrapper.hpp"
#include <glpk.h>
#include <iostream>
+namespace solvers {
- namespace solvers
- {
-
- int getType(const VectorXd & mib, const VectorXd & mab, const int i)
- {
- const double& mibV = mib(i);
- const double& mabV = mab(i);
- int type = GLP_FR;
- if(mibV > UNBOUNDED_DOWN && mabV < UNBOUNDED_UP)
- type = GLP_DB;
- else if(mibV > UNBOUNDED_DOWN)
- type = GLP_LO;
- else if(mabV < UNBOUNDED_UP)
- type = GLP_UP;
- return type;
- }
+int getType(const VectorXd& mib, const VectorXd& mab, const int i) {
+ const double& mibV = mib(i);
+ const double& mabV = mab(i);
+ int type = GLP_FR;
+ if (mibV > UNBOUNDED_DOWN && mabV < UNBOUNDED_UP)
+ type = GLP_DB;
+ else if (mibV > UNBOUNDED_DOWN)
+ type = GLP_LO;
+ else if (mabV < UNBOUNDED_UP)
+ type = GLP_UP;
+ 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)
- {
- 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 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
- //ROWS
- const int numEqConstraints = (int)(CE.rows());
- const int numIneqConstraints =(int)(CI.rows());
- const int num_constraints_total (numEqConstraints + numIneqConstraints);
- glp_add_rows(lp, num_constraints_total);
- int idrow = 1;//adds three rows to the problem object
- int idcol = 1;
- int idConsMat = 1;
- int xsize = (int)(x.size());
- for (int i =0; i < numIneqConstraints; ++i, ++idrow )
- {
- 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 */
- ++ idConsMat;
- }
- }
- idcol = 1;
+ // ROWS
+ const int numEqConstraints = (int)(CE.rows());
+ const int numIneqConstraints = (int)(CI.rows());
+ const int num_constraints_total(numEqConstraints + numIneqConstraints);
+ glp_add_rows(lp, num_constraints_total);
+ int idrow = 1; // adds three rows to the problem object
+ int idcol = 1;
+ int idConsMat = 1;
+ int xsize = (int)(x.size());
+ for (int i = 0; i < numIneqConstraints; ++i, ++idrow) {
+ 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 */
+ ++idConsMat;
}
- for (int i =0; i < numEqConstraints; ++i, ++idrow )
- {
- 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 */
- ++ idConsMat;
- }
- }
- idcol = 1;
+ }
+ idcol = 1;
+ }
+ for (int i = 0; i < numEqConstraints; ++i, ++idrow) {
+ 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 */
+ ++idConsMat;
}
+ }
+ idcol = 1;
+ }
- //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;
- 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));
- }
- glp_load_matrix(lp,idConsMat-1,ia,ja,ar);
+ // 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;
+ 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));
+ }
+ glp_load_matrix(lp, idConsMat - 1, ia, ja, ar);
- 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
- 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_free_env();
- return res;
+ 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
+ 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_free_env();
+ return res;
+}
- } /* namespace solvers */
+} /* namespace solvers */
diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp
index 8ff85004c24b9bb464d8c8f91a6b808a66112de4..c4afa0f0cd79267108cc9daabe33ed2481c4e55f 100644
--- a/src/solve_0_step.cpp
+++ b/src/solve_0_step.cpp
@@ -8,156 +8,141 @@
#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 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;
- w.second.head(3) = 6*alpha*(p0 - 2*p1);
- w.second.tail(3) =(-p0).cross(12*alpha*p1 + g);
- return w;
+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 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;
+ w.second.head(3) = 6 * alpha * (p0 - 2 * p1);
+ w.second.tail(3) = (-p0).cross(12 * alpha * p1 + g);
+ return w;
+}
+
+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();
+ w.first.block<3, 3>(3, 0) = skew(1.5 * (3 * p1 - p0)) * alpha;
+ w.second.head(3) = 1.5 * alpha * (3 * p0 - 5 * p1);
+ w.second.tail(3) = (3 * alpha * p0).cross(-p1) + 0.25 * (gX * (3 * p1 + p0));
+ return w;
+}
+
+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;
+ w.first.block<3, 3>(3, 0) = skew(0.5 * g - 3 * alpha * p0 + 3 * alpha * p1);
+ w.second.head(3) = 3 * alpha * (p0 - p1);
+ w.second.tail(3) = 0.5 * gX * p1;
+ return w;
+}
+
+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();
+ w.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (p1 + p0));
+ w.second.head(3) = 1.5 * alpha * (p1 + p0);
+ // w.second.tail(3) = 0;
+ return w;
+}
+
+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();
+ w.first.block<3, 3>(3, 0) = skew(g - 6 * alpha * p1);
+ w.second.head(3) = 6 * alpha * p1;
+ // w.second.tail(3) = 0;
+ return w;
+}
+
+waypoint6_t u0(point_t_tC l0, const double alpha) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ // w.first.block<3,3>(0,0) = 0;
+ w.first.block<3, 3>(3, 0) = 3 * alpha * Matrix3::Identity();
+ // w.second.head(3) = 0;
+ w.second.tail(3) = -3 * alpha * l0;
+ return w;
+}
+
+waypoint6_t u1(point_t_tC l0, const double alpha) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ // w.first.block<3,3>(0,0) = 0;
+ // w.first.block<3,3>(3,0) = 0;
+ // w.second.head(3) = 0;
+ w.second.tail(3) = -1.5 * alpha * l0;
+ return w;
+}
+
+waypoint6_t u2(point_t_tC l0, const double alpha) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ // w.first.block<3,3>(0,0) = 0;
+ w.first.block<3, 3>(3, 0) = -1.5 * alpha * Matrix3::Identity();
+ // w.second.head(3) = 0;
+ w.second.tail(3) = -l0 / 2. * alpha;
+ return w;
+}
+
+waypoint6_t u3(point_t_tC /*l0*/, const double alpha) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ w.first.block<3, 3>(3, 0) = -1.5 * alpha * Matrix3::Identity();
+ // w.second.head(3) = 0;
+ // w.second.tail(3) = 0.;
+ return w;
+}
+
+waypoint6_t u4(point_t_tC /*l0*/, const double /*alpha*/) {
+ waypoint6_t w = initwp<waypoint6_t>();
+ // w.first.block<3,3>(0,0) = 0;
+ // w.first.block<3,3>(3,0) = 0;
+ // w.second.head(3) = 0;
+ // w.second.tail(3) = 0.;
+ return w;
+}
+
+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,
+ const double timeStep) {
+ int numSteps = computeNumSteps(T, timeStep);
+ static const double n = 3.; // degree
+ point_t p1 = dc0 * T / n + p0;
+ Matrix3 p0X = skew(p0);
+ Matrix3 p1X = skew(p1);
+ Matrix3 gX = skew(g);
+ double alpha = 1. / (T * T);
+ std::vector<waypoint6_t> wps;
+ wps.push_back(w0(p0, p1, g, p0X, p1X, gX, alpha));
+ wps.push_back(w1(p0, p1, g, p0X, p1X, gX, alpha));
+ wps.push_back(w2(p0, p1, g, p0X, p1X, gX, alpha));
+ wps.push_back(w3(p0, p1, g, p0X, p1X, gX, alpha));
+ wps.push_back(w4(p0, p1, g, p0X, p1X, gX, alpha));
+ if (numSteps > 0) {
+ std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
+ wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
+ }
+ return wps;
+}
+
+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;
+ wps.push_back(u0(l0, alpha));
+ wps.push_back(u1(l0, alpha));
+ wps.push_back(u2(l0, alpha));
+ wps.push_back(u3(l0, alpha));
+ wps.push_back(u4(l0, alpha));
+ if (numSteps > 0) {
+ std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
+ wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
+ }
+ return wps;
}
-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();
- w.first.block<3,3>(3,0) = skew(1.5 * (3*p1 - p0))*alpha;
- w.second.head(3) = 1.5 *alpha* (3*p0 - 5*p1);
- w.second.tail(3) =(3*alpha*p0).cross(-p1) + 0.25 * (gX * (3*p1 + p0)) ;
- return w;
-}
-
-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;
- w.first.block<3,3>(3,0) = skew(0.5*g - 3*alpha* p0 + 3*alpha*p1);
- w.second.head(3) = 3*alpha*(p0 - p1);
- w.second.tail(3) = 0.5 * gX*p1;
- return w;
-}
-
-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();
- w.first.block<3,3>(3,0) = skew(g - 1.5 *alpha* (p1 + p0));
- w.second.head(3) = 1.5*alpha * (p1 + p0);
- //w.second.tail(3) = 0;
- return w;
-}
-
-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();
- w.first.block<3,3>(3,0) = skew(g - 6*alpha* p1);
- w.second.head(3) = 6*alpha*p1;
- //w.second.tail(3) = 0;
- return w;
-}
-
-waypoint6_t u0 (point_t_tC l0, const double alpha)
-{
- waypoint6_t w = initwp<waypoint6_t>();
- //w.first.block<3,3>(0,0) = 0;
- w.first.block<3,3>(3,0) = 3*alpha * Matrix3::Identity();
- //w.second.head(3) = 0;
- w.second.tail(3) = -3*alpha*l0;
- return w;
-}
-
-waypoint6_t u1 (point_t_tC l0, const double alpha)
-{
- waypoint6_t w = initwp<waypoint6_t>();
- //w.first.block<3,3>(0,0) = 0;
- //w.first.block<3,3>(3,0) = 0;
- //w.second.head(3) = 0;
- w.second.tail(3) = -1.5*alpha*l0;
- return w;
-}
-
-waypoint6_t u2 (point_t_tC l0, const double alpha)
-{
- waypoint6_t w = initwp<waypoint6_t>();
- //w.first.block<3,3>(0,0) = 0;
- w.first.block<3,3>(3,0) = -1.5*alpha * Matrix3::Identity();
- //w.second.head(3) = 0;
- w.second.tail(3) = -l0 / 2. * alpha;
- return w;
-}
-
-waypoint6_t u3 (point_t_tC /*l0*/, const double alpha)
-{
- waypoint6_t w = initwp<waypoint6_t>();
- w.first.block<3,3>(3,0) = -1.5*alpha * Matrix3::Identity();
- //w.second.head(3) = 0;
- //w.second.tail(3) = 0.;
- return w;
-}
-
-
-waypoint6_t u4 (point_t_tC /*l0*/, const double /*alpha*/)
-{
- waypoint6_t w = initwp<waypoint6_t>();
- //w.first.block<3,3>(0,0) = 0;
- //w.first.block<3,3>(3,0) = 0;
- //w.second.head(3) = 0;
- //w.second.tail(3) = 0.;
- return w;
-}
-
-
-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, const double timeStep)
-{
- int numSteps = computeNumSteps(T, timeStep);
- static const double n = 3.; //degree
- point_t p1 = dc0 * T / n + p0;
- Matrix3 p0X = skew(p0);
- Matrix3 p1X = skew(p1);
- Matrix3 gX = skew( g);
- double alpha = 1. / (T*T);
- std::vector<waypoint6_t> wps;
- wps.push_back(w0(p0, p1, g, p0X, p1X, gX, alpha));
- wps.push_back(w1(p0, p1, g, p0X, p1X, gX, alpha));
- wps.push_back(w2(p0, p1, g, p0X, p1X, gX, alpha));
- wps.push_back(w3(p0, p1, g, p0X, p1X, gX, alpha));
- wps.push_back(w4(p0, p1, g, p0X, p1X, gX, alpha));
- if (numSteps > 0)
- {
- std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
- wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
- }
- return wps;
-}
-
-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;
- wps.push_back(u0(l0, alpha));
- wps.push_back(u1(l0, alpha));
- wps.push_back(u2(l0, alpha));
- wps.push_back(u3(l0, alpha));
- wps.push_back(u4(l0, alpha));
- if (numSteps > 0)
- {
- std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
- wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
- }
- return wps;
-}
-
-
/* 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).
@@ -167,103 +152,90 @@ 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::vector<waypoint6_t> wps, wpL;
- wps = ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
- if (useAngMomentum)
- wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
- return compute6dControlPointInequalities(cData,wps,wpL, useAngMomentum, fail);
-}
-
-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);
- res.second = VectorX::Zero(dimPb);
- Ref_matrixXX H = res.first;
- Ref_vectorX g = res.second;
-
- //minimize distance to initial point
- double weightDist = useAngMomentum ? 0.0001 : 1.;
- H.block<3,3>(0,0) = Matrix3::Identity() * weightDist;
- g.head(3) = - p0 * weightDist;
-
- // now angular momentum integral minimization
- if(useAngMomentum)
- {
- H.block<3,3>(3,3) = Matrix3::Identity() * 6./5.;
- g.tail(3) = 0.5 *(-(9.* l0) / 5.);
- }
- return res;
-}
-
-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));
- }
- else
- resData.cost_ = (pData.c0_ - resData.x).norm();
-}
-
-void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res)
-{
+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);
+ if (useAngMomentum) wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
+ return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum, fail);
+}
+
+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);
+ res.second = VectorX::Zero(dimPb);
+ Ref_matrixXX H = res.first;
+ Ref_vectorX g = res.second;
+
+ // minimize distance to initial point
+ double weightDist = useAngMomentum ? 0.0001 : 1.;
+ H.block<3, 3>(0, 0) = Matrix3::Identity() * weightDist;
+ g.head(3) = -p0 * weightDist;
+
+ // now angular momentum integral minimization
+ if (useAngMomentum) {
+ H.block<3, 3>(3, 3) = Matrix3::Identity() * 6. / 5.;
+ g.tail(3) = 0.5 * (-(9. * l0) / 5.);
+ }
+ return res;
+}
+
+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));
+ } else
+ resData.cost_ = (pData.c0_ - resData.x).norm();
+}
+
+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_);
+ wps.push_back(res.x.head(3));
+ wps.push_back(res.x.head(3));
+ res.c_of_t_ = bezier_t(wps.begin(), wps.end(), Ts[0]);
+}
+
+void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) {
+ if (pData.useAngularMomentum_) {
std::vector<Vector3> wps;
- wps.push_back(pData.c0_);
- wps.push_back(pData.dc0_ * Ts[0] / 3 + pData.c0_);
- wps.push_back(res.x.head(3));
- wps.push_back(res.x.head(3));
- res.c_of_t_ = bezier_t (wps.begin(), wps.end(),Ts[0]);
-}
-
-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_));
- wps.push_back(3*(-res.x.tail(3)));
- wps.push_back(Vector3::Zero());
- res.dL_of_t_ = bezier_t(wps.begin(), wps.end(),Ts[0], 1./Ts[0]);
- }
- else
- res.dL_of_t_ = bezier_t::zero(Ts[0]);
+ wps.push_back(3 * (res.x.tail(3) - pData.l0_));
+ wps.push_back(3 * (-res.x.tail(3)));
+ wps.push_back(Vector3::Zero());
+ res.dL_of_t_ = bezier_t(wps.begin(), wps.end(), Ts[0], 1. / Ts[0]);
+ } else
+ res.dL_of_t_ = bezier_t::zero(Ts[0]);
}
// no angular momentum for now
-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);
- ResultDataCOMTraj res;
- if(fail)
- return res;
- 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_;
- if(dimPb > 3)
- init.tail(3) = pData.l0_;
- // rewriting 0.5 || Dx -d ||^2 as x'Hx + g'x
- ResultData resQp = solve(Ab,Hg, init);
- if(resQp.success_)
- {
- res.success_ = true;
- res.x = resQp.x;
- computeRealCost(pData, res);
- computeC_of_T (pData,Ts,res);
- computedL_of_T(pData,Ts,res);
- }
- return res;
-}
-
-
-
-} // namespace bezier_com_traj
+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);
+ ResultDataCOMTraj res;
+ if (fail) return res;
+ 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_;
+ if (dimPb > 3) init.tail(3) = pData.l0_;
+ // rewriting 0.5 || Dx -d ||^2 as x'Hx + g'x
+ ResultData resQp = solve(Ab, Hg, init);
+ if (resQp.success_) {
+ res.success_ = true;
+ res.x = resQp.x;
+ computeRealCost(pData, res);
+ computeC_of_T(pData, Ts, res);
+ computedL_of_T(pData, Ts, res);
+ }
+ return res;
+}
+
+} // namespace bezier_com_traj
diff --git a/src/solver-abstract.cpp b/src/solver-abstract.cpp
index 8e3ec7db81fa983cf70a6a27e0cc8f2e8b59b549..f3cb30db160663329900b06139a02c4571f68393 100644
--- a/src/solver-abstract.cpp
+++ b/src/solver-abstract.cpp
@@ -15,7 +15,6 @@
// <http://www.gnu.org/licenses/>.
//
-
#include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
#ifdef USE_GLPK_SOLVER
#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
@@ -26,123 +25,106 @@
#include <Eigen/Sparse>
#include <stdexcept>
-namespace solvers
-{
+namespace solvers {
-template<typename Derived>
-inline bool is_nan(const Eigen::MatrixBase<Derived>& x)
-{
- bool isnan = !((x.array()==x.array()).all());
- return isnan;
+template <typename Derived>
+inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
+ bool isnan = !((x.array() == x.array()).all());
+ return isnan;
}
-
typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::SparseVector<double> SpVec;
-typedef Eigen::SparseVector<int> SpVeci;
+typedef Eigen::SparseVector<int> SpVeci;
-namespace
-{
- 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.head(resv.size()) = resv;
- d.tail(res.cols()) = -minBounds;
- data.first = D;data.second = d;
- }
+namespace {
+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.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)
- {
- 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.head(resv.size()) = resv;
- d.tail(res.cols()) = maxBounds;
- data.first = D;data.second = d;
- }
+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.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> data;
- data.first = CI;
- data.second = ci0;
- addConstraintMinBoundQuadProg(minBounds, data);
- addConstraintMaxBoundQuadProg(maxBounds, data);
- return data;
- }
+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;
+ addConstraintMinBoundQuadProg(minBounds, data);
+ addConstraintMaxBoundQuadProg(maxBounds, data);
+ return data;
}
+} // 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,
- solvers::Cref_vectorX maxBounds,
- const SolverType solver)
-{
- assert (!(is_nan(A)));
- assert (!(is_nan(b)));
- assert (!(is_nan(D)));
- assert (!(is_nan(d)));
- assert (!(is_nan(initGuess)));
- ResultData res;
- res.x = initGuess;
- switch(solver)
- {
- /*
- * solves the problem
- * min. x' Hess x + 2 g0' x
- * s.t. CE x + ce0 = 0
- * CI x + ci0 >= 0
- * Thus CI = -A; ci0 = b
- * CI = D; ce0 = -d
- */
- case SOLVER_QUADPROG:
- //case SOLVER_QUADPROG_SPARSE:
- {
- assert (!(is_nan(Hess)));
- std::pair<MatrixXd,VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
- VectorXd ce0 = -d;
- 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);
- /* else
- {
- SpMat Hsp = Hess.sparseView();
- status = QPsolver.solve_quadprog_sparse(Hsp,g,D,ce0,CI,b,res.x);
- }*/
- res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
- if(res.success_)
- res.cost_ = QPsolver.getObjValue();
- return res;
- }
-#ifdef USE_GLPK_SOLVER
- case SOLVER_GLPK:
- {
- res.success_ = (solvers::solveglpk(g,D,d,A,b,minBounds, maxBounds,res.x,res.cost_) == GLP_OPT);
+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)));
+ assert(!(is_nan(D)));
+ assert(!(is_nan(d)));
+ assert(!(is_nan(initGuess)));
+ ResultData res;
+ res.x = initGuess;
+ switch (solver) {
+ /*
+ * solves the problem
+ * min. x' Hess x + 2 g0' x
+ * s.t. CE x + ce0 = 0
+ * CI x + ci0 >= 0
+ * Thus CI = -A; ci0 = b
+ * CI = D; ce0 = -d
+ */
+ case SOLVER_QUADPROG:
+ // case SOLVER_QUADPROG_SPARSE:
+ {
+ assert(!(is_nan(Hess)));
+ std::pair<MatrixXd, VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
+ VectorXd ce0 = -d;
+ 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);
+ /* else
+ {
+ SpMat Hsp = Hess.sparseView();
+ status = QPsolver.solve_quadprog_sparse(Hsp,g,D,ce0,CI,b,res.x);
+ }*/
+ res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL);
+ if (res.success_) res.cost_ = QPsolver.getObjValue();
return res;
+ }
+#ifdef USE_GLPK_SOLVER
+ case SOLVER_GLPK: {
+ res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds, res.x, res.cost_) == GLP_OPT);
+ return res;
}
#endif
default:
- throw std::runtime_error("Unknown solver type in solver-asbtract");
- }
+ throw std::runtime_error("Unknown solver type in solver-asbtract");
+ }
}
} /* namespace solvers */
diff --git a/src/utils.cpp b/src/utils.cpp
index 80912f643646bbbb7f74d7fc2c16d2bbde1a4e01..75f4e662f4fd9adfcba1a145ff94a3026cd7e034 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -4,141 +4,127 @@
*/
#include <hpp/bezier-com-traj/utils.hh>
-namespace bezier_com_traj
-{
+namespace bezier_com_traj {
-
-waypoint_t initwp(const size_t rows, const size_t cols){
- waypoint_t w;
- w.first = MatrixXX::Zero(rows,cols);
- w.second = VectorX::Zero(rows);
- return w;
-}
-
-
-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())
- 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 initwp(const size_t rows, const size_t cols) {
+ waypoint_t w;
+ w.first = MatrixXX::Zero(rows, cols);
+ w.second = VectorX::Zero(rows);
+ return w;
}
-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())
- 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())
+ 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 waypoint_t& w,const double k){
- return waypoint_t(k*w.first,k*w.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())
+ 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 waypoint_t& w, const double k) { return waypoint_t(k * w.first, k * w.second); }
-template<> waypoint9_t initwp<waypoint9_t>()
-{
- waypoint9_t w;
- w.first = Matrix39::Zero();
- w.second = point3_t::Zero();
- return w;
+template <>
+waypoint9_t initwp<waypoint9_t>() {
+ waypoint9_t w;
+ w.first = Matrix39::Zero();
+ w.second = point3_t::Zero();
+ return w;
}
-template<> waypoint6_t initwp<waypoint6_t>()
-{
- waypoint6_t w;
- w.first = matrix6_t::Zero();
- w.second = point6_t::Zero();
- return w;
+template <>
+waypoint6_t initwp<waypoint6_t>() {
+ waypoint6_t w;
+ w.first = matrix6_t::Zero();
+ w.second = point6_t::Zero();
+ return w;
}
-template<> waypoint3_t initwp<waypoint3_t>()
-{
- waypoint3_t w;
- w.first = matrix3_t::Zero();
- w.second = point3_t::Zero();
- return w;
+template <>
+waypoint3_t initwp<waypoint3_t>() {
+ waypoint3_t w;
+ w.first = matrix3_t::Zero();
+ w.second = point3_t::Zero();
+ return w;
}
-Matrix3 skew(point_t_tC x)
-{
- Matrix3 res = Matrix3::Zero();
- res(0,1) = - x(2); res(0,2) = x(1);
- res(1,0) = x(2); res(1,2) = - x(0);
- res(2,0) = - x(1); res(2,1) = x(0);
- return res;
+Matrix3 skew(point_t_tC x) {
+ Matrix3 res = Matrix3::Zero();
+ res(0, 1) = -x(2);
+ res(0, 2) = x(1);
+ res(1, 0) = x(2);
+ res(1, 2) = -x(0);
+ res(2, 0) = -x(1);
+ res(2, 1) = x(0);
+ return res;
}
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree)
-{
- std::vector<spline::Bern<double> > res;
- for (unsigned int i =0; i <= (unsigned int)degree; ++i)
- res.push_back(spline::Bern<double>(degree,i));
- return res;
+std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) {
+ std::vector<spline::Bern<double> > res;
+ for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(spline::Bern<double>(degree, i));
+ return res;
}
-
-T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase )
-{
- T_time timeArray;
- double t = 0;
- double t_total = phaseTimings.sum();
- timeArray.push_back(std::make_pair(0.,0));
- for(int i = 0 ; i < phaseTimings.size() ; ++i)
- {
- double step = (double) phaseTimings[i] / pointsPerPhase;
- for(int j = 0 ; j < pointsPerPhase ; ++j)
- {
- t += step;
- timeArray.push_back(std::make_pair(t,i));
- }
+T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase) {
+ T_time timeArray;
+ double t = 0;
+ double t_total = phaseTimings.sum();
+ timeArray.push_back(std::make_pair(0., 0));
+ for (int i = 0; i < phaseTimings.size(); ++i) {
+ double step = (double)phaseTimings[i] / pointsPerPhase;
+ for (int j = 0; j < pointsPerPhase; ++j) {
+ t += step;
+ timeArray.push_back(std::make_pair(t, i));
}
- timeArray.pop_back();
- timeArray.push_back(std::make_pair(t_total,phaseTimings.size()-1)); // avoid numerical errors
- return timeArray;
+ }
+ timeArray.pop_back();
+ 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 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");
- t = currentTiming;
- currentTiming += phaseTimings[i];
- while(t < currentTiming)
- {
- timeArray.push_back(std::make_pair(t,i));
- t += timeStep;
- }
- timeArray.push_back(std::make_pair(currentTiming,i));
+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");
+ t = currentTiming;
+ currentTiming += phaseTimings[i];
+ while (t < currentTiming) {
+ timeArray.push_back(std::make_pair(t, i));
+ t += timeStep;
}
- return timeArray;
+ timeArray.push_back(std::make_pair(currentTiming, i));
+ }
+ return timeArray;
}
-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<<"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;
- }
- if(clipZ){
- file<<"\t"<<0<<"\t"<<0<<"\t"<<1.<<"\t"<<-3.<<endl;
- file<<"\t"<<0<<"\t"<<0<<"\t"<<-1.<<"\t"<<-1.<<endl;
- }
- file.close();
+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 << "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;
+ }
+ if (clipZ) {
+ file << "\t" << 0 << "\t" << 0 << "\t" << 1. << "\t" << -3. << endl;
+ file << "\t" << 0 << "\t" << 0 << "\t" << -1. << "\t" << -1. << endl;
+ }
+ file.close();
}
-
-} // namespace bezier_com_traj
+} // namespace bezier_com_traj
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index f0fd555556a3ecc24744a69516a54384581b1db3..089961099057a57101131f5e7acdf28b5e22ecd6 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -21,422 +21,382 @@
#include "boost/assign.hpp"
-
-
-namespace bezier_com_traj{
+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){
- if(pData.constraints_.flag_ & FIVE_FREE_VAR)
- return 15;
- else if(pData.constraints_.flag_ & FOUR_FREE_VAR)
- return 12;
- else if(pData.constraints_.flag_ & THREE_FREE_VAR)
- return 9;
- else if(pData.constraints_.flag_ & TWO_FREE_VAR)
- return 6;
- else
- return 3;
+int dimVar(const ProblemData& pData) {
+ if (pData.constraints_.flag_ & FIVE_FREE_VAR)
+ return 15;
+ else if (pData.constraints_.flag_ & FOUR_FREE_VAR)
+ return 12;
+ else if (pData.constraints_.flag_ & THREE_FREE_VAR)
+ return 9;
+ else if (pData.constraints_.flag_ & TWO_FREE_VAR)
+ return 6;
+ else
+ return 3;
}
-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);
-
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
+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);
+
+/** @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
*/
// 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)
-{
- CIT_evalCurveAtTime cit = evalCurveAtTimes.find(pData.constraints_.flag_);
- if(cit != evalCurveAtTimes.end())
- return cit->second(pi,t);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+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);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-
-
-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)
- (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);
-
-
-/** @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
- */
- 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
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+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)(
+ 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);
+
+/** @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
+ */
+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 {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
+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);
- 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);
-
-
- /** @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
- */
- // 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_);
- if(cit != evalCurveWaypointAtTimes.end())
- return cit->second(pi,t);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- 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);
-
- /** @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
- */
- // 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_);
- if(cit != evalVelCurveWaypointAtTimes.end())
- return cit->second(pi,T,t);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+/** @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
+ */
+// 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_);
+ if (cit != evalCurveWaypointAtTimes.end())
+ return cit->second(pi, t);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ 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);
-
- /** @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
- */
- // 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_);
- if(cit != evalAccCurveWaypointAtTimes.end())
- return cit->second(pi,T,t);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- 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);
-
-
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
+}
+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)
* @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
*/
// 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,double t)
-{
- CIT_evalJerkCurveWaypointAtTime cit = evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
- if(cit != evalJerkCurveWaypointAtTimes.end())
- return cit->second(pi,T,t);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+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 {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ 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);
+
+/** @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
+ */
+// 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_);
+ if (cit != evalAccCurveWaypointAtTimes.end())
+ return cit->second(pi, T, t);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ 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);
+
+/** @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
+ */
+// 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,
+ double t) {
+ CIT_evalJerkCurveWaypointAtTime cit = evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
+ if (cit != evalJerkCurveWaypointAtTimes.end())
+ return cit->second(pi, T, t);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-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);
+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);
/**
- * @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)
-{
- CIT_compConsWp cit = compConsWps.find(pData.constraints_.flag_);
- if(cit != compConsWps.end())
- return cit->second(pData,T);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+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);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-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 ){
- waypoint_t w = initwp(DIM_POINT,DIM_VAR);
- if(pit->isZero()){
- w.first = MatrixXX::Identity(DIM_POINT,DIM_VAR);
- }else{
- w.second = *pit;
- }
- wps.push_back(w);
+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) {
+ waypoint_t w = initwp(DIM_POINT, DIM_VAR);
+ if (pit->isZero()) {
+ w.first = MatrixXX::Identity(DIM_POINT, DIM_VAR);
+ } else {
+ w.second = *pit;
}
- return wps;
+ wps.push_back(w);
+ }
+ return wps;
}
+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);
- 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);
-
-
- /**
- * @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)
- {
- CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
- if(cit != compVelWps.end())
- return cit->second(pData,T,pi);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
- }
-
-
- 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);
-
-
- /**
- * @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 )
- {
- CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
- if(cit != compAccWps.end())
- return cit->second(pData,T,pi);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+/**
+ * @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) {
+ CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
+ if (cit != compVelWps.end())
+ return cit->second(pData, T, pi);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
}
+}
-
-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);
-
-
+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);
/**
-* @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 )
-{
- CIT_compJerkWp cit = compJerkWps.find(pData.constraints_.flag_);
- if(cit != compJerkWps.end())
- return cit->second(pData,T,pi);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+ * @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) {
+ CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
+ if (cit != compAccWps.end())
+ return cit->second(pData, T, pi);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-
-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);
-
+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);
/**
- * @brief computeWwaypoints compute the constant waypoints of w(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
*/
- bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T)
-{
- CIT_compWp cit = compWps.find(pData.constraints_.flag_);
- if(cit != compWps.end())
- return cit->second(pData,T);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+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);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-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);
-
+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);
- coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T)
-{
- CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
- if(cit != compFinalVelPs.end())
- return cit->second(pData,T);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+/**
+ * @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final
+ * states
+ * @param pData
+ * @param T
+ * @return
+ */
+bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
+ CIT_compWp cit = compWps.find(pData.constraints_.flag_);
+ if (cit != compWps.end())
+ return cit->second(pData, T);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
+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);
+
+coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
+ CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
+ if (cit != compFinalVelPs.end())
+ return cit->second(pData, T);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}
-
-
-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);
-
-
+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);
/**
-* @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 )
-{
- CIT_compVelCost cit = compVelCosts.find(pData.constraints_.flag_);
- if(cit != compVelCosts.end())
- return cit->second(pData,T,pi);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
+ * @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) {
+ CIT_compVelCost cit = compVelCosts.find(pData.constraints_.flag_);
+ if (cit != compVelCosts.end())
+ return cit->second(pData, T, pi);
+ else {
+ std::cout << "Current constraints set are not implemented" << std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
-
-}
+} // namespace bezier_com_traj
#endif
diff --git a/tests/test-bezier-symbolic.cpp b/tests/test-bezier-symbolic.cpp
index d5ae824870ec29217f9b27cdf72a8156d35f4dc7..d06373f8e7ab8547a4735d93f01fb87948a9831e 100644
--- a/tests/test-bezier-symbolic.cpp
+++ b/tests/test-bezier-symbolic.cpp
@@ -16,8 +16,6 @@
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
-
-
#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
@@ -30,223 +28,205 @@
using namespace bezier_com_traj;
const double T = 1.5;
-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.);
- pData.dc0_ = Vector3::Zero();
- pData.dc1_ = Vector3::Zero();
- pData.ddc0_ = Vector3::Zero();
- pData.ddc1_ = Vector3::Zero();
- pData.constraints_.flag_ = INIT_POS | INIT_VEL | END_VEL | END_POS;
-
- MatrixX3 normals(2,3),positions(2,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- 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)));
-
-
- return pData;
-}
-
-std::vector<point_t> generate_wps(){
- return computeConstantWaypoints(buildPData(),T);
+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.);
+ pData.dc0_ = Vector3::Zero();
+ pData.dc1_ = Vector3::Zero();
+ pData.ddc0_ = Vector3::Zero();
+ pData.ddc1_ = Vector3::Zero();
+ pData.constraints_.flag_ = INIT_POS | INIT_VEL | END_VEL | END_POS;
+
+ MatrixX3 normals(2, 3), positions(2, 3);
+ normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
+ 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)));
+
+ return pData;
}
+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());
- BOOST_CHECK((a-b).norm() < EPS);
+void vectorEqual(const VectorX& a, const VectorX& b, const double EPS = 1e-14) {
+ BOOST_CHECK_EQUAL(a.size(), b.size());
+ BOOST_CHECK((a - b).norm() < EPS);
}
-BOOST_AUTO_TEST_SUITE( symbolic )
+BOOST_AUTO_TEST_SUITE(symbolic)
-BOOST_AUTO_TEST_CASE(symbolic_eval_c){
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
+BOOST_AUTO_TEST_CASE(symbolic_eval_c) {
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
- bezier_t c (pts.begin(),pts.end(),T);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
+ bezier_t c(pts.begin(), pts.end(), T);
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
- double t = 0.;
- while(t<T){
- vectorEqual(c(t),eval(c_sym(t),y));
- t += 0.01;
- }
+ double t = 0.;
+ while (t < T) {
+ vectorEqual(c(t), eval(c_sym(t), y));
+ t += 0.01;
+ }
}
-BOOST_AUTO_TEST_CASE(symbolic_eval_dc){
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
-
- bezier_t c (pts.begin(),pts.end(),T);
- bezier_t dc = c.compute_derivate(1);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
- bezier_wp_t dc_sym = c_sym.compute_derivate(1);
-
- double t = 0.;
- while(t<T){
- vectorEqual(dc(t),eval(dc_sym(t),y));
- t += 0.01;
- }
+BOOST_AUTO_TEST_CASE(symbolic_eval_dc) {
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
+
+ bezier_t c(pts.begin(), pts.end(), T);
+ bezier_t dc = c.compute_derivate(1);
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
+ bezier_wp_t dc_sym = c_sym.compute_derivate(1);
+
+ double t = 0.;
+ while (t < T) {
+ vectorEqual(dc(t), eval(dc_sym(t), y));
+ t += 0.01;
+ }
}
-BOOST_AUTO_TEST_CASE(symbolic_eval_ddc){
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
-
- bezier_t c (pts.begin(),pts.end(),T);
- bezier_t ddc = c.compute_derivate(2);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
- bezier_wp_t ddc_sym = c_sym.compute_derivate(2);
-
- double t = 0.;
- while(t<T){
- vectorEqual(ddc(t),eval(ddc_sym(t),y),1e-10);
- t += 0.01;
- }
+BOOST_AUTO_TEST_CASE(symbolic_eval_ddc) {
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
+
+ bezier_t c(pts.begin(), pts.end(), T);
+ bezier_t ddc = c.compute_derivate(2);
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
+ bezier_wp_t ddc_sym = c_sym.compute_derivate(2);
+
+ double t = 0.;
+ while (t < T) {
+ vectorEqual(ddc(t), eval(ddc_sym(t), y), 1e-10);
+ t += 0.01;
+ }
}
-
-BOOST_AUTO_TEST_CASE(symbolic_eval_jc){
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
-
- bezier_t c (pts.begin(),pts.end(),T);
- bezier_t jc = c.compute_derivate(3);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
- bezier_wp_t jc_sym = c_sym.compute_derivate(3);
-
-
- double t = 0.;
- while(t<T){
- vectorEqual(jc(t),eval(jc_sym(t),y),1e-10);
- t += 0.01;
- }
+BOOST_AUTO_TEST_CASE(symbolic_eval_jc) {
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
+
+ bezier_t c(pts.begin(), pts.end(), T);
+ bezier_t jc = c.compute_derivate(3);
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
+ bezier_wp_t jc_sym = c_sym.compute_derivate(3);
+
+ double t = 0.;
+ while (t < T) {
+ vectorEqual(jc(t), eval(jc_sym(t), y), 1e-10);
+ t += 0.01;
+ }
}
-BOOST_AUTO_TEST_CASE(symbolic_split_c){
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
-
- bezier_t c (pts.begin(),pts.end(),T);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
-
- double a,b,t,t1,t2;
- for(size_t i = 0 ; i < 100 ; ++i){
- a = (rand()/(double)RAND_MAX ) * T;
- b = (rand()/(double)RAND_MAX ) * T;
- t1 = std::min(a,b);
- t2 = std::max(a,b);
- // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
- bezier_t c_e = c.extract(t1,t2);
- bezier_wp_t c_sym_e = c_sym.extract(t1,t2);
- t = t1;
- while(t < t2){
- vectorEqual(c_e(t-t1),eval(c_sym_e(t-t1),y));
- vectorEqual(c(t),eval(c_sym_e(t-t1),y));
- t+= 0.01;
- }
+BOOST_AUTO_TEST_CASE(symbolic_split_c) {
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
+
+ bezier_t c(pts.begin(), pts.end(), T);
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
+
+ double a, b, t, t1, t2;
+ for (size_t i = 0; i < 100; ++i) {
+ a = (rand() / (double)RAND_MAX) * T;
+ b = (rand() / (double)RAND_MAX) * T;
+ t1 = std::min(a, b);
+ t2 = std::max(a, b);
+ // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
+ bezier_t c_e = c.extract(t1, t2);
+ bezier_wp_t c_sym_e = c_sym.extract(t1, t2);
+ t = t1;
+ while (t < t2) {
+ vectorEqual(c_e(t - t1), eval(c_sym_e(t - t1), y));
+ vectorEqual(c(t), eval(c_sym_e(t - t1), y));
+ t += 0.01;
}
+ }
}
+BOOST_AUTO_TEST_CASE(symbolic_split_c_bench) {
+ using namespace std;
-BOOST_AUTO_TEST_CASE(symbolic_split_c_bench){
- using namespace std;
+ std::vector<point_t> pts = generate_wps();
+ bezier_wp_t::t_point_t wps = generate_wps_symbolic();
+ point_t y(1, 0.2, 4.5);
+ pts[2] = y;
- std::vector<point_t> pts = generate_wps();
- bezier_wp_t::t_point_t wps = generate_wps_symbolic();
- point_t y(1,0.2,4.5);
- pts[2] = y;
+ bezier_wp_t c_sym(wps.begin(), wps.end(), T);
- bezier_wp_t c_sym (wps.begin(),wps.end(),T);
+ std::vector<double> values;
+ for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX);
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- values.push_back((double)rand()/RAND_MAX);
+ 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) {
+ splitted = c_sym.split(*cit);
+ }
+ e0 = clock();
- 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)
- {
- 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::endl;
+ std::cout << "Time required to split a c curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
+ << std::endl;
}
-
-BOOST_AUTO_TEST_CASE(symbolic_split_w){
- bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(),T);
- point_t y(1,0.2,4.5);
-
- bezier_wp_t w (wps.begin(),wps.end(),T);
-
- double a,b,t,t1,t2;
- for(size_t i = 0 ; i < 100 ; ++i){
- a = (rand()/(double)RAND_MAX ) * T;
- b = (rand()/(double)RAND_MAX ) * T;
- t1 = std::min(a,b);
- t2 = std::max(a,b);
- // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
- bezier_wp_t w_e = w.extract(t1,t2);
- t = t1;
- while(t < t2){
- vectorEqual(eval(w(t),y),eval(w_e(t-t1),y),1e-12);
- t+= 0.01;
- }
+BOOST_AUTO_TEST_CASE(symbolic_split_w) {
+ bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T);
+ point_t y(1, 0.2, 4.5);
+
+ bezier_wp_t w(wps.begin(), wps.end(), T);
+
+ double a, b, t, t1, t2;
+ for (size_t i = 0; i < 100; ++i) {
+ a = (rand() / (double)RAND_MAX) * T;
+ b = (rand() / (double)RAND_MAX) * T;
+ t1 = std::min(a, b);
+ t2 = std::max(a, b);
+ // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
+ bezier_wp_t w_e = w.extract(t1, t2);
+ t = t1;
+ while (t < t2) {
+ vectorEqual(eval(w(t), y), eval(w_e(t - t1), y), 1e-12);
+ t += 0.01;
}
+ }
}
+BOOST_AUTO_TEST_CASE(symbolic_split_w_bench) {
+ bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T);
+ point_t y(1, 0.2, 4.5);
-BOOST_AUTO_TEST_CASE(symbolic_split_w_bench){
- bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(),T);
- point_t y(1,0.2,4.5);
+ bezier_wp_t w(wps.begin(), wps.end(), T);
- bezier_wp_t w (wps.begin(),wps.end(),T);
+ std::vector<double> values;
+ for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX);
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- values.push_back((double)rand()/RAND_MAX);
-
- 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)
- {
- splitted = w.split(*cit);
- }
- e0 = clock();
-
- std::cout<<"Time required to split a w curve : "<<((double)(e0-s0)/CLOCKS_PER_SEC)/100.<<" ms "<<std::endl;
+ 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) {
+ splitted = w.split(*cit);
+ }
+ e0 = clock();
+ std::cout << "Time required to split a w curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
+ << std::endl;
}
-
-
-
BOOST_AUTO_TEST_SUITE_END()
diff --git a/tests/test-transition-quasi-static.cpp b/tests/test-transition-quasi-static.cpp
index cd7d948bcf42664a5fd73a2bc91e1b61141d4d85..159661b232126805ebcdb67922528af44ae8526e 100644
--- a/tests/test-transition-quasi-static.cpp
+++ b/tests/test-transition-quasi-static.cpp
@@ -16,134 +16,127 @@
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
-
-#define BOOST_TEST_MODULE transition-quasiStatic
+#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/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"
+BOOST_AUTO_TEST_SUITE(quasiStatic)
+
+BOOST_AUTO_TEST_CASE(single_support) {
+ // check if state valid with only one contact :
+ MatrixX3 normal(1, 3);
+ 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<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 :
+}
-
-
-BOOST_AUTO_TEST_SUITE( quasiStatic )
-
-BOOST_AUTO_TEST_CASE(single_support){
- // check if state valid with only one contact :
- MatrixX3 normal(1,3);
- 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<Matrix3,Vector3> Hg = computeCost();
+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);
+
+ for (size_t i = 0; i < 500; ++i) {
+ MatrixX3 normal(1, 3);
+ MatrixX3 position(1, 3);
+ // normal.block<1,3>(0,0) = firstLeg_n;
+ // position.block<1,3>(0,0) = firstLeg_p;
+ double x = fRandom(KIN_X_MIN, KIN_X_MAX);
+ double y = fRandom(KIN_Y_MIN, KIN_Y_MAX);
+ 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<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
+ 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 :
-
+ }
}
-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);
-
- for(size_t i = 0 ; i < 500 ; ++i){
- MatrixX3 normal(1,3);
- MatrixX3 position(1,3);
- //normal.block<1,3>(0,0) = firstLeg_n;
- //position.block<1,3>(0,0) = firstLeg_p;
- double x = fRandom(KIN_X_MIN,KIN_X_MAX);
- double y = fRandom(KIN_Y_MIN,KIN_Y_MAX);
- 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<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
- BOOST_CHECK(res.success_);
- }
-}
-
-
-BOOST_AUTO_TEST_CASE(quasiStatic_empty_upX){
- for(size_t i = 0 ; i < 500 ; ++i){
- MatrixX3 normal(1,3);
- MatrixX3 position(1,3);
- //normal.block<1,3>(0,0) = firstLeg_n;
- //position.block<1,3>(0,0) = firstLeg_p;
- double x = fRandom(KIN_X_MAX+(LX/2.)+0.001,10);
- double y = fRandom(KIN_Y_MIN,KIN_Y_MAX);
- 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<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
- BOOST_CHECK(! res.success_);
- }
+BOOST_AUTO_TEST_CASE(quasiStatic_empty_upX) {
+ for (size_t i = 0; i < 500; ++i) {
+ MatrixX3 normal(1, 3);
+ MatrixX3 position(1, 3);
+ // normal.block<1,3>(0,0) = firstLeg_n;
+ // position.block<1,3>(0,0) = firstLeg_p;
+ double x = fRandom(KIN_X_MAX + (LX / 2.) + 0.001, 10);
+ double y = fRandom(KIN_Y_MIN, KIN_Y_MAX);
+ 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<Matrix3, Vector3> Hg = computeCost();
+ Vector3 init = Vector3::Zero();
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
+ BOOST_CHECK(!res.success_);
+ }
}
-BOOST_AUTO_TEST_CASE(quasiStatic_empty_downX){
- for(size_t i = 0 ; i < 500 ; ++i){
- MatrixX3 normal(1,3);
- MatrixX3 position(1,3);
- //normal.block<1,3>(0,0) = firstLeg_n;
- //position.block<1,3>(0,0) = firstLeg_p;
- double x = fRandom(-10,KIN_X_MIN-(LX/2.)-0.001);
- double y = fRandom(KIN_Y_MIN,KIN_Y_MAX);
- 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<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
- BOOST_CHECK(! res.success_);
- }
+BOOST_AUTO_TEST_CASE(quasiStatic_empty_downX) {
+ for (size_t i = 0; i < 500; ++i) {
+ MatrixX3 normal(1, 3);
+ MatrixX3 position(1, 3);
+ // normal.block<1,3>(0,0) = firstLeg_n;
+ // position.block<1,3>(0,0) = firstLeg_p;
+ double x = fRandom(-10, KIN_X_MIN - (LX / 2.) - 0.001);
+ double y = fRandom(KIN_Y_MIN, KIN_Y_MAX);
+ 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<Matrix3, Vector3> Hg = computeCost();
+ Vector3 init = Vector3::Zero();
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
+ BOOST_CHECK(!res.success_);
+ }
}
-BOOST_AUTO_TEST_CASE(quasiStatic_empty_downY){
- for(size_t i = 0 ; i < 500 ; ++i){
- MatrixX3 normal(1,3);
- MatrixX3 position(1,3);
- //normal.block<1,3>(0,0) = firstLeg_n;
- //position.block<1,3>(0,0) = firstLeg_p;
- double x = fRandom(KIN_X_MIN,KIN_X_MAX);
- double y = fRandom(-10,KIN_Y_MIN-(LY/2.)-0.001);
- 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<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
- BOOST_CHECK(! res.success_);
- }
+BOOST_AUTO_TEST_CASE(quasiStatic_empty_downY) {
+ for (size_t i = 0; i < 500; ++i) {
+ MatrixX3 normal(1, 3);
+ MatrixX3 position(1, 3);
+ // normal.block<1,3>(0,0) = firstLeg_n;
+ // position.block<1,3>(0,0) = firstLeg_p;
+ double x = fRandom(KIN_X_MIN, KIN_X_MAX);
+ double y = fRandom(-10, KIN_Y_MIN - (LY / 2.) - 0.001);
+ 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<Matrix3, Vector3> Hg = computeCost();
+ Vector3 init = Vector3::Zero();
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
+ BOOST_CHECK(!res.success_);
+ }
}
-BOOST_AUTO_TEST_CASE(quasiStatic_empty_upY){
- for(size_t i = 0 ; i < 500 ; ++i){
- MatrixX3 normal(1,3);
- MatrixX3 position(1,3);
- //normal.block<1,3>(0,0) = firstLeg_n;
- //position.block<1,3>(0,0) = firstLeg_p;
- double x = fRandom(KIN_X_MIN,KIN_X_MAX);
- double y = fRandom(KIN_Y_MAX+(LY/2.)+0.001,10);
- 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<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
- bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
- BOOST_CHECK(! res.success_);
- }
+BOOST_AUTO_TEST_CASE(quasiStatic_empty_upY) {
+ for (size_t i = 0; i < 500; ++i) {
+ MatrixX3 normal(1, 3);
+ MatrixX3 position(1, 3);
+ // normal.block<1,3>(0,0) = firstLeg_n;
+ // position.block<1,3>(0,0) = firstLeg_p;
+ double x = fRandom(KIN_X_MIN, KIN_X_MAX);
+ double y = fRandom(KIN_Y_MAX + (LY / 2.) + 0.001, 10);
+ 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<Matrix3, Vector3> Hg = computeCost();
+ Vector3 init = Vector3::Zero();
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
+ BOOST_CHECK(!res.success_);
+ }
}
-
BOOST_AUTO_TEST_SUITE_END()
diff --git a/tests/test-transition.cpp b/tests/test-transition.cpp
index 0166a7f193a3fdc29f9d12ba0239f25089194b8f..66275181aa714f0d3ba34fad96899f086db8de40 100644
--- a/tests/test-transition.cpp
+++ b/tests/test-transition.cpp
@@ -16,7 +16,6 @@
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
-
#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
@@ -27,447 +26,424 @@
#define NOMINAL_COM_HEIGHT 0.795
-
-std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,const int pointsPerPhase ){
- std::vector<double> timeArray;
- double t = 0;
- double t_total = 0;
- for(long int i = 0 ; i < phaseTimings.size() ; ++i)
- t_total += phaseTimings[i];
-
- for(long int i = 0 ; i < phaseTimings.size() ; ++i){
- double step = (double) phaseTimings[i] / pointsPerPhase;
- for(int j = 0 ; j < pointsPerPhase ; ++j){
- t += step;
- timeArray.push_back(t);
- }
+std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings, const int pointsPerPhase) {
+ std::vector<double> timeArray;
+ double t = 0;
+ double t_total = 0;
+ for (long int i = 0; i < phaseTimings.size(); ++i) t_total += phaseTimings[i];
+
+ for (long int i = 0; i < phaseTimings.size(); ++i) {
+ double step = (double)phaseTimings[i] / pointsPerPhase;
+ for (int j = 0; j < pointsPerPhase; ++j) {
+ t += step;
+ timeArray.push_back(t);
}
- timeArray.pop_back();
- timeArray.push_back(t_total); // avoid numerical errors
- return timeArray;
+ }
+ timeArray.pop_back();
+ timeArray.push_back(t_total); // avoid numerical errors
+ 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));
- return true;
+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)
-{
- // check if timing is respected
- std::vector<double> timings = computeDiscretizedTime(Ts,pointsPerPhase);
- BOOST_CHECK_EQUAL(timings.back(),t_total);
-
- Vector3 c,dc,ddc;
- bezier_com_traj::bezier_t ct = res.c_of_t_;
- bezier_com_traj::bezier_t dct = ct.compute_derivate(1);
- bezier_com_traj::bezier_t ddct = dct.compute_derivate(1);
-
- BOOST_CHECK_EQUAL(ct.min(),0);
- BOOST_CHECK_EQUAL(dct.min(),0);
- BOOST_CHECK_EQUAL(ddct.min(),0);
- BOOST_CHECK_EQUAL(ct.max(),t_total);
- BOOST_CHECK_EQUAL(dct.max(),t_total);
- BOOST_CHECK_EQUAL(ddct.max(),t_total);
-
- // 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);
- std::size_t id_phase = 0;
- bezier_com_traj::ContactData phase = pData.contacts_[id_phase];
-
- for(size_t id_step = 0 ; id_step < timings.size() ; ++id_step){
- c = ct(timings[id_step]);
- dc = dct(timings[id_step]);
- ddc = ddct(timings[id_step]);
-
- // check i (c,dc,ddc) verify the constraints of current phase
- check_constraints(phase,c,dc,ddc);
-
- // check if we switch phases
- for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
- if(id_step == (std::size_t)stepIdForPhase[i]){
- id_phase=i+1;
- phase = pData.contacts_[id_phase];
- check_constraints(phase,c,dc,ddc);
- }
- }
+ 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);
+
+ Vector3 c, dc, ddc;
+ bezier_com_traj::bezier_t ct = res.c_of_t_;
+ bezier_com_traj::bezier_t dct = ct.compute_derivate(1);
+ bezier_com_traj::bezier_t ddct = dct.compute_derivate(1);
+
+ BOOST_CHECK_EQUAL(ct.min(), 0);
+ BOOST_CHECK_EQUAL(dct.min(), 0);
+ BOOST_CHECK_EQUAL(ddct.min(), 0);
+ BOOST_CHECK_EQUAL(ct.max(), t_total);
+ BOOST_CHECK_EQUAL(dct.max(), t_total);
+ BOOST_CHECK_EQUAL(ddct.max(), t_total);
+
+ // 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);
+ std::size_t id_phase = 0;
+ bezier_com_traj::ContactData phase = pData.contacts_[id_phase];
+
+ for (size_t id_step = 0; id_step < timings.size(); ++id_step) {
+ c = ct(timings[id_step]);
+ dc = dct(timings[id_step]);
+ ddc = ddct(timings[id_step]);
+
+ // check i (c,dc,ddc) verify the constraints of current phase
+ check_constraints(phase, c, dc, ddc);
+
+ // check if we switch phases
+ for (std::size_t i = 0; i < (stepIdForPhase.size() - 1); ++i) {
+ if (id_step == (std::size_t)stepIdForPhase[i]) {
+ id_phase = i + 1;
+ phase = pData.contacts_[id_phase];
+ check_constraints(phase, c, dc, ddc);
+ }
}
-}
-
-
-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;
- for(long int i = 0 ; i < Ts.size() ; ++i)
- t_total += Ts[i];
-
- int pointsPerPhase = 5;
-
- // check if transition is feasible (should be)
- bezier_com_traj::ResultDataCOMTraj res;
- if(continuous)
- {
- // testing all available solvers
- 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);
- BOOST_CHECK(res.success_ == res2.success_);
- if(res.success_)
- {
- 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);
- }*/
+ }
+}
+
+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;
+ for (long int i = 0; i < Ts.size(); ++i) t_total += Ts[i];
+
+ int pointsPerPhase = 5;
+
+ // check if transition is feasible (should be)
+ bezier_com_traj::ResultDataCOMTraj res;
+ if (continuous) {
+ // testing all available solvers
+ 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);
+ BOOST_CHECK(res.success_ == res2.success_);
+ if(res.success_)
+ {
+ 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);
+ }*/
#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);
- //e0 = clock();
- //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);
- }
+ // clock_t s0,e0;
+ // s0 = clock();
+ 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;
+ BOOST_CHECK(res.success_ == res3.success_);
+ if (res3.success_) {
+ discretized_check(pData, Ts, res3, pointsPerPhase, t_total);
+ }
#endif
- }
}
- else
- res = bezier_com_traj::computeCOMTrajFixedSize(pData,Ts,pointsPerPhase);
-
- if(shouldFail){
- BOOST_CHECK(!res.success_);
- if(test_continuous)
- {
- 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);
- BOOST_CHECK(!res.success_);
+ } else
+ res = bezier_com_traj::computeCOMTrajFixedSize(pData, Ts, pointsPerPhase);
+
+ if (shouldFail) {
+ BOOST_CHECK(!res.success_);
+ if (test_continuous) {
+ 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);
+ BOOST_CHECK(!res.success_);
#ifdef USE_GLPK_SOLVER
- res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_GLPK);
- BOOST_CHECK(!res.success_);
+ res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
+ BOOST_CHECK(!res.success_);
#endif
- }
- return;
}
-
- BOOST_CHECK(res.success_);
-
- if(res.success_)
- discretized_check(pData,Ts,res,pointsPerPhase,t_total);
- if(continuous && pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION)
- {
- pData.representation_ = bezier_com_traj::FORCE;
- check_transition(pData,Ts,shouldFail,true);
+ return;
+ }
+
+ BOOST_CHECK(res.success_);
+
+ if (res.success_) discretized_check(pData, Ts, res, pointsPerPhase, t_total);
+ 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)
+ check_transition(pData, Ts, shouldFail, true);
+ else {
+ for (size_t i = 0; i < pData.contacts_.size(); ++i) {
+ delete pData.contacts_[i].contactPhase_;
}
- else if(!continuous && test_continuous)
- check_transition(pData,Ts,shouldFail,true);
- else{
- for(size_t i = 0 ; i < pData.contacts_.size() ; ++i){
- delete pData.contacts_[i].contactPhase_;
- }
- }
-
-
+ }
}
-
-
-BOOST_AUTO_TEST_SUITE( flat_ground )
+BOOST_AUTO_TEST_SUITE(flat_ground)
// one step (left foot) : init pos of end effector : (0,0.1)(0,-0.1)
// end pos : (0.3,0.1)(0,-0.1)
// three phases : both feet in contact, only right feet, both feet
-bezier_com_traj::ContactData phase0_flat(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- 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));
+bezier_com_traj::ContactData phase0_flat() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
+ 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)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
+ 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;
+
+ return cData;
}
-bezier_com_traj::ContactData phase1_flat(){
- bezier_com_traj::ContactData cData;
- 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));
+bezier_com_traj::ContactData phase1_flat() {
+ bezier_com_traj::ContactData cData;
+ 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));
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,-0.1,0));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
+ ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
- return cData;
-}
-
-bezier_com_traj::ContactData phase2_flat(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- 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)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
-}
-
-
-bezier_com_traj::ProblemData gen_problem_data_flat(){
- bezier_com_traj::ProblemData pData;
- pData.c0_ = Vector3(0,0,NOMINAL_COM_HEIGHT);
- pData.c1_ = Vector3(0.15,0,NOMINAL_COM_HEIGHT);
- pData.dc0_ = Vector3::Zero();
- pData.dc1_ = Vector3::Zero();
- pData.ddc0_ = Vector3::Zero();
- pData.ddc1_ = Vector3::Zero();
-
- pData.contacts_.push_back(phase0_flat());
- pData.contacts_.push_back(phase1_flat());
- pData.contacts_.push_back(phase2_flat());
-
- return pData;
-}
-
-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)));
-
- bezier_com_traj::ContactData cDataMid = phase1_flat();
- ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
-
- std::pair<Matrix3,Vector3> Hg = computeCost();
- Vector3 init = Vector3::Zero();
-
- ConstraintsPair Ab_first = stackConstraints(kin0,stab);
- 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);
- BOOST_CHECK(res_second.success_);
- delete cDataMid.contactPhase_;
-}
-
-
-BOOST_AUTO_TEST_CASE(transition){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-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;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-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;
- VectorX Ts(1);
- Ts<<0.2;
- check_transition(pData,Ts,false,false,true);
-}
-
-BOOST_AUTO_TEST_CASE(transition_noDc1){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-BOOST_AUTO_TEST_CASE(transition_ddc0){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
- pData.constraints_.flag_ |= bezier_com_traj::END_ACC;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-BOOST_AUTO_TEST_CASE(transition_noAcc){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.constraintAcceleration_ = false;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-BOOST_AUTO_TEST_CASE(transition_noDc1_noAcc){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
- pData.constraints_.constraintAcceleration_ = false;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
- pData.constraints_.constraintAcceleration_ = false;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-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_.constraintAcceleration_ = false;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-
-BOOST_AUTO_TEST_CASE(transition_Acc1){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- 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
-}
-
-BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
- 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
-}
-BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
- 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
-}
-
-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_.maxAcceleration_=2.;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts);
-}
-
-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_.maxAcceleration_=0.5;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts,false,false,false); // fail with continuous formulation
-}
-
-BOOST_AUTO_TEST_CASE(transition_Acc05){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- 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
+bezier_com_traj::ContactData phase2_flat() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
+ 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)));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ProblemData gen_problem_data_flat() {
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = Vector3(0, 0, NOMINAL_COM_HEIGHT);
+ pData.c1_ = Vector3(0.15, 0, NOMINAL_COM_HEIGHT);
+ pData.dc0_ = Vector3::Zero();
+ pData.dc1_ = Vector3::Zero();
+ pData.ddc0_ = Vector3::Zero();
+ pData.ddc1_ = Vector3::Zero();
+
+ pData.contacts_.push_back(phase0_flat());
+ pData.contacts_.push_back(phase1_flat());
+ pData.contacts_.push_back(phase2_flat());
+
+ return pData;
+}
+
+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)));
+
+ bezier_com_traj::ContactData cDataMid = phase1_flat();
+ ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
+
+ std::pair<Matrix3, Vector3> Hg = computeCost();
+ Vector3 init = Vector3::Zero();
+
+ ConstraintsPair Ab_first = stackConstraints(kin0, stab);
+ 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);
+ BOOST_CHECK(res_second.success_);
+ delete cDataMid.contactPhase_;
+}
+
+BOOST_AUTO_TEST_CASE(transition) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+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;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+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;
+ VectorX Ts(1);
+ Ts << 0.2;
+ check_transition(pData, Ts, false, false, true);
+}
+
+BOOST_AUTO_TEST_CASE(transition_noDc1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+BOOST_AUTO_TEST_CASE(transition_ddc0) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ pData.constraints_.flag_ |= bezier_com_traj::END_ACC;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+BOOST_AUTO_TEST_CASE(transition_noAcc) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.constraintAcceleration_ = false;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+BOOST_AUTO_TEST_CASE(transition_noDc1_noAcc) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
+ pData.constraints_.constraintAcceleration_ = false;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ pData.constraints_.constraintAcceleration_ = false;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+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_.constraintAcceleration_ = false;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+BOOST_AUTO_TEST_CASE(transition_Acc1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ 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
+}
+
+BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
+ 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
+}
+BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ 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
+}
+
+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_.maxAcceleration_ = 2.;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts);
+}
+
+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_.maxAcceleration_ = 0.5;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts, false, false, false); // fail with continuous formulation
+}
+
+BOOST_AUTO_TEST_CASE(transition_Acc05) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ 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
}
// constraints that should fails :
-BOOST_AUTO_TEST_CASE(transition_Acc02){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.maxAcceleration_=0.2;
- pData.constraints_.constraintAcceleration_=true;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts,true);
-}
-
-BOOST_AUTO_TEST_CASE(transition_noDc1_Acc05){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
- pData.constraints_.maxAcceleration_=0.5;
- pData.constraints_.constraintAcceleration_=true;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts,true);
-}
-
-
-BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1){
- bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
- pData.constraints_.maxAcceleration_=1.;
- pData.constraints_.constraintAcceleration_=true;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts,true);
-}
-
-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_.maxAcceleration_=0.2;
- pData.constraints_.constraintAcceleration_=true;
- VectorX Ts(3);
- Ts<<0.6,0.6,0.6;
- check_transition(pData,Ts,true);
+BOOST_AUTO_TEST_CASE(transition_Acc02) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.maxAcceleration_ = 0.2;
+ pData.constraints_.constraintAcceleration_ = true;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts, true);
+}
+
+BOOST_AUTO_TEST_CASE(transition_noDc1_Acc05) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
+ pData.constraints_.maxAcceleration_ = 0.5;
+ pData.constraints_.constraintAcceleration_ = true;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts, true);
+}
+
+BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ pData.constraints_.maxAcceleration_ = 1.;
+ pData.constraints_.constraintAcceleration_ = true;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts, true);
+}
+
+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_.maxAcceleration_ = 0.2;
+ pData.constraints_.constraintAcceleration_ = true;
+ VectorX Ts(3);
+ Ts << 0.6, 0.6, 0.6;
+ check_transition(pData, Ts, true);
}
BOOST_AUTO_TEST_SUITE_END()
-
-
-BOOST_AUTO_TEST_SUITE( platform )
+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)
@@ -476,291 +452,287 @@ BOOST_AUTO_TEST_SUITE( platform )
// (0.775, 0.23, -0.02);(0.35,-0.1,0) -> (0.775, 0.23, -0.02);(1.15,-0.1,0)
// unfeasible in quasi-static
-//third step :
+// third step :
//(0.775, 0.23, -0.02);(1.15,-0.1,0) -> (1.15,0.1,0);(1.15,-0.1,0)
-bezier_com_traj::ContactData phase0_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- 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));
+bezier_com_traj::ContactData phase0_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
+ 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));
+
+ ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
+ ConstraintsPair kin =
+ stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, 0.1, 0)));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase1_platform() {
+ bezier_com_traj::ContactData cData;
+ 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));
+
+ ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase2_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ 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);
+ 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));
+ Matrix3 rot = quat.normalized().toRotationMatrix();
+ ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase3_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(1, 3), positions(1, 3);
+ 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));
+
+ 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));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase4_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ 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);
+ 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));
+ Matrix3 rot = quat.normalized().toRotationMatrix();
+ ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase5_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(1, 3), positions(1, 3);
+ 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));
+
+ ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
+
+bezier_com_traj::ContactData phase6_platform() {
+ bezier_com_traj::ContactData cData;
+ MatrixX3 normals(2, 3), positions(2, 3);
+ normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
+ positions.block<1, 3>(0, 0) = Vector3(1.15, -0.1, 0);
+ 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));
+ ConstraintsPair kin =
+ stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, 0.1, 0)));
+ cData.Kin_ = kin.first;
+ cData.kin_ = kin.second;
+
+ return cData;
+}
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,-0.1,0));
- ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,0.1,0)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
-}
+bezier_com_traj::ProblemData gen_problem_data_Step0() {
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = Vector3(0.35, 0, NOMINAL_COM_HEIGHT);
+ pData.c1_ = Vector3(0.56, 0.04, NOMINAL_COM_HEIGHT);
+ pData.dc0_ = Vector3::Zero();
+ pData.dc1_ = Vector3::Zero();
+ pData.ddc0_ = Vector3::Zero();
+ pData.ddc1_ = Vector3::Zero();
+
+ pData.contacts_.push_back(phase0_platform());
+ pData.contacts_.push_back(phase1_platform());
+ pData.contacts_.push_back(phase2_platform());
+
+ return pData;
+}
+
+bezier_com_traj::ProblemData gen_problem_data_Step1() {
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = Vector3(0.56, 0.04, 0.765);
+ pData.c1_ = Vector3(0.98, 0.02, 0.77);
+ pData.dc0_ = Vector3::Zero();
+ pData.dc1_ = Vector3::Zero();
+ pData.ddc0_ = Vector3::Zero();
+ pData.ddc1_ = Vector3::Zero();
+
+ pData.contacts_.push_back(phase2_platform());
+ pData.contacts_.push_back(phase3_platform());
+ pData.contacts_.push_back(phase4_platform());
+
+ return pData;
+}
+
+bezier_com_traj::ProblemData gen_problem_data_Step2() {
+ bezier_com_traj::ProblemData pData;
+ pData.c0_ = Vector3(0.98, 0.04, NOMINAL_COM_HEIGHT);
+ pData.c1_ = Vector3(1.15, 0, NOMINAL_COM_HEIGHT);
+ pData.dc0_ = Vector3::Zero();
+ pData.dc1_ = Vector3::Zero();
+ pData.ddc0_ = Vector3::Zero();
+ pData.ddc1_ = Vector3::Zero();
-bezier_com_traj::ContactData phase1_platform(){
- bezier_com_traj::ContactData cData;
- 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));
-
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,-0.1,0));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
+ pData.contacts_.push_back(phase4_platform());
+ pData.contacts_.push_back(phase5_platform());
+ pData.contacts_.push_back(phase6_platform());
+
+ return pData;
}
-bezier_com_traj::ContactData phase2_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- 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);
- 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));
+BOOST_AUTO_TEST_CASE(quasi_static_0) {
+ // should be successfull in quasiStatic
+ // compute kinematic constraints for the right foot :
+ bezier_com_traj::ContactData cDataFirst = phase0_platform();
+ bezier_com_traj::ContactData cDataMid = phase1_platform();
+ bezier_com_traj::ContactData cDataSecond = phase2_platform();
- 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)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
-}
+ ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.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();
-bezier_com_traj::ContactData phase3_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(1,3),positions(1,3);
- 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);
+ ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ BOOST_CHECK(res_first.success_);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));
+ ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ BOOST_CHECK(res_second.success_);
- 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));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
+ delete cDataFirst.contactPhase_;
+ delete cDataMid.contactPhase_;
+ delete cDataSecond.contactPhase_;
}
-bezier_com_traj::ContactData phase4_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- 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);
- 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));
- Matrix3 rot = quat.normalized().toRotationMatrix();
- ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(rot,Vector3(0.775, 0.23, -0.02)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
+BOOST_AUTO_TEST_CASE(quasi_static_1) {
+ // should NOT be successfull in quasiStatic
+ // compute kinematic constraints for the right foot :
+ bezier_com_traj::ContactData cDataFirst = phase2_platform();
+ bezier_com_traj::ContactData cDataMid = phase3_platform();
+ bezier_com_traj::ContactData cDataSecond = phase4_platform();
- return cData;
-}
+ ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.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();
-bezier_com_traj::ContactData phase5_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(1,3),positions(1,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- positions.block<1,3>(0,0)=Vector3(1.15,-0.1,0);
+ ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
+ BOOST_CHECK(!res_first.success_);
- std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
- cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));
+ ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
+ BOOST_CHECK(!res_second.success_);
- ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,-0.1,0));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
+ delete cDataFirst.contactPhase_;
+ delete cDataMid.contactPhase_;
+ delete cDataSecond.contactPhase_;
}
-bezier_com_traj::ContactData phase6_platform(){
- bezier_com_traj::ContactData cData;
- MatrixX3 normals(2,3),positions(2,3);
- normals.block<1,3>(0,0)=Vector3(0,0,1);
- positions.block<1,3>(0,0)=Vector3(1.15,-0.1,0);
- 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));
+BOOST_AUTO_TEST_CASE(quasi_static_2) {
+ // should be successfull in quasiStatic
+ // compute kinematic constraints for the right foot :
+ bezier_com_traj::ContactData cDataFirst = phase4_platform();
+ bezier_com_traj::ContactData cDataMid = phase5_platform();
+ bezier_com_traj::ContactData cDataSecond = phase6_platform();
- ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,-0.1,0));
- ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,0.1,0)));
- cData.Kin_ = kin.first;
- cData.kin_ = kin.second;
-
- return cData;
-}
+ ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.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);
+ 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);
+ BOOST_CHECK(res_second.success_);
-bezier_com_traj::ProblemData gen_problem_data_Step0(){
- bezier_com_traj::ProblemData pData;
- pData.c0_ = Vector3(0.35,0,NOMINAL_COM_HEIGHT);
- pData.c1_ = Vector3(0.56,0.04,NOMINAL_COM_HEIGHT);
- pData.dc0_ = Vector3::Zero();
- pData.dc1_ = Vector3::Zero();
- pData.ddc0_ = Vector3::Zero();
- pData.ddc1_ = Vector3::Zero();
-
- pData.contacts_.push_back(phase0_platform());
- pData.contacts_.push_back(phase1_platform());
- pData.contacts_.push_back(phase2_platform());
-
- return pData;
+ delete cDataFirst.contactPhase_;
+ delete cDataMid.contactPhase_;
+ delete cDataSecond.contactPhase_;
}
-
-bezier_com_traj::ProblemData gen_problem_data_Step1(){
- bezier_com_traj::ProblemData pData;
- pData.c0_ = Vector3(0.56,0.04,0.765);
- pData.c1_ = Vector3(0.98,0.02,0.77);
- pData.dc0_ = Vector3::Zero();
- pData.dc1_ = Vector3::Zero();
- pData.ddc0_ = Vector3::Zero();
- pData.ddc1_ = Vector3::Zero();
-
- pData.contacts_.push_back(phase2_platform());
- pData.contacts_.push_back(phase3_platform());
- pData.contacts_.push_back(phase4_platform());
-
- return pData;
+BOOST_AUTO_TEST_CASE(transition_0) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_Step0();
+ VectorX Ts(3);
+ Ts << 0.8, 0.6, 0.8;
+ check_transition(pData, Ts);
}
+BOOST_AUTO_TEST_CASE(transition_1) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_Step1();
+ VectorX Ts(3);
+ Ts << 0.4, 0.2, 0.4;
-bezier_com_traj::ProblemData gen_problem_data_Step2(){
- bezier_com_traj::ProblemData pData;
- pData.c0_ = Vector3(0.98,0.04,NOMINAL_COM_HEIGHT);
- pData.c1_ = Vector3(1.15,0,NOMINAL_COM_HEIGHT);
- pData.dc0_ = Vector3::Zero();
- pData.dc1_ = Vector3::Zero();
- pData.ddc0_ = Vector3::Zero();
- pData.ddc1_ = Vector3::Zero();
-
- pData.contacts_.push_back(phase4_platform());
- pData.contacts_.push_back(phase5_platform());
- pData.contacts_.push_back(phase6_platform());
-
- return pData;
+ check_transition(pData, Ts);
}
-BOOST_AUTO_TEST_CASE(quasi_static_0){
- // should be successfull in quasiStatic
-// compute kinematic constraints for the right foot :
- bezier_com_traj::ContactData cDataFirst = phase0_platform();
- bezier_com_traj::ContactData cDataMid = phase1_platform();
- 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 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);
- 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);
- BOOST_CHECK(res_second.success_);
-
- delete cDataFirst.contactPhase_;
- delete cDataMid.contactPhase_;
- delete cDataSecond.contactPhase_;
-}
-
-BOOST_AUTO_TEST_CASE(quasi_static_1){
- // should NOT be successfull in quasiStatic
-// compute kinematic constraints for the right foot :
- bezier_com_traj::ContactData cDataFirst = phase2_platform();
- bezier_com_traj::ContactData cDataMid = phase3_platform();
- 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 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);
- 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);
- BOOST_CHECK(! res_second.success_);
-
- delete cDataFirst.contactPhase_;
- delete cDataMid.contactPhase_;
- delete cDataSecond.contactPhase_;
-}
-
-BOOST_AUTO_TEST_CASE(quasi_static_2){
- // should be successfull in quasiStatic
-// compute kinematic constraints for the right foot :
- bezier_com_traj::ContactData cDataFirst = phase4_platform();
- bezier_com_traj::ContactData cDataMid = phase5_platform();
- 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 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);
- 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);
- BOOST_CHECK(res_second.success_);
-
-
- delete cDataFirst.contactPhase_;
- delete cDataMid.contactPhase_;
- delete cDataSecond.contactPhase_;
-}
-
-
-BOOST_AUTO_TEST_CASE(transition_0){
- bezier_com_traj::ProblemData pData = gen_problem_data_Step0();
- VectorX Ts(3);
- Ts<<0.8,0.6,0.8;
- check_transition(pData,Ts);
-
-}
-
-BOOST_AUTO_TEST_CASE(transition_1){
- bezier_com_traj::ProblemData pData = gen_problem_data_Step1();
- VectorX Ts(3);
- Ts<<0.4,0.2,0.4;
-
- check_transition(pData,Ts);
-
-}
-
-BOOST_AUTO_TEST_CASE(transition_2){
- bezier_com_traj::ProblemData pData = gen_problem_data_Step2();
- VectorX Ts(3);
- Ts<<0.8,0.6,0.8;
-
- check_transition(pData,Ts);
+BOOST_AUTO_TEST_CASE(transition_2) {
+ bezier_com_traj::ProblemData pData = gen_problem_data_Step2();
+ VectorX Ts(3);
+ Ts << 0.8, 0.6, 0.8;
+ check_transition(pData, Ts);
}
BOOST_AUTO_TEST_SUITE_END()
diff --git a/tests/test-zero-step-capturability.cpp b/tests/test-zero-step-capturability.cpp
index 42c01c19a2a8e10de1cc8d6495b8d8770c5242ab..82dc62cbd67cb2b0b04fe3638ec1d8c7d0a9a019 100644
--- a/tests/test-zero-step-capturability.cpp
+++ b/tests/test-zero-step-capturability.cpp
@@ -25,129 +25,114 @@ 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)
-{
+ 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
- N.setZero(4*N_CONTACTS,3); // contact normals
+ p.setZero(4 * N_CONTACTS, 3); // contact points
+ N.setZero(4 * N_CONTACTS, 3); // contact normals
// Generate contact positions and orientations
bool collision;
- for(unsigned int i=0; i<N_CONTACTS; i++)
- {
- while(true) // generate contact position
+ 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));
- if(i==0)
- break;
+ 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(collision==false)
- break;
+ 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 (collision == false) break;
}
-// generate contact orientation
+ // 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));
-// }
+ // 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));
+ // }
}
-double fRandom(double fMin, double fMax)
-{
- double f = (double)std::rand() / RAND_MAX;
- return fMin + f * (fMax - fMin);
+double fRandom(double fMin, double fMax) {
+ double f = (double)std::rand() / RAND_MAX;
+ return fMin + f * (fMax - fMin);
}
-bool findStaticEqCOMPos(Equilibrium * eq, const RVector3& com_LB, const RVector3& com_UB, Vector3& c0)
-{
- int trials = 0;
- while (trials < 1000)
- {
- ++trials;
- double x = fRandom(com_LB(0), com_UB(0));
- double y = fRandom(com_LB(1), com_UB(1));
- double z = fRandom(com_LB(2), com_UB(2));
- c0 << x , y, z;
- bool isStable(false);
- eq->checkRobustEquilibrium(c0,isStable);
- if(isStable)
- {
- return true;
- }
+bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB, const RVector3& com_UB, Vector3& c0) {
+ int trials = 0;
+ while (trials < 1000) {
+ ++trials;
+ double x = fRandom(com_LB(0), com_UB(0));
+ double y = fRandom(com_LB(1), com_UB(1));
+ double z = fRandom(com_LB(2), com_UB(2));
+ c0 << x, y, z;
+ bool isStable(false);
+ eq->checkRobustEquilibrium(c0, isStable);
+ if (isStable) {
+ return true;
}
- return false;
+ }
+ return false;
}
-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);
- w.head(3) = w1;
- w.tail(3) = c.cross(w1) + dL;
- return w;
+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);
+ w.head(3) = w1;
+ w.tail(3) = c.cross(w1) + dL;
+ 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)
-{
- // retrieve H
- centroidal_dynamics::MatrixXX Hrow; VectorX h;
- eq->getPolytopeInequalities(Hrow,h);
- MatrixXX H = -Hrow;
- H.rowwise().normalize();
- const bezier_com_traj::bezier_t c_of_t = resData.c_of_t_;
- const bezier_com_traj::bezier_t dL_of_t = resData.dL_of_t_;
- bezier_com_traj::bezier_t ddc_of_t = c_of_t.compute_derivate(2);
- for (int i = 1; i < num_steps; ++i)
- {
- double dt = double(i) / double(num_steps) * T;
- c_of_t(dt);
- dL_of_t(dt);
- Vector6 w = computew(eq, c_of_t(dt),ddc_of_t(dt),dL_of_t(dt));
- 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 << "c0 " << c0.transpose() << std::endl;
- std::cout << "H row " << H.row(j) << std::endl;
- std::cout << "w " << w << std::endl;
- std::cout << "j " << j << std::endl;
- //throw;
- std::cout << "mult ddc " << ddc_of_t.mult_T_ << std::endl;
- std::cout << "mult c " << c_of_t.mult_T_ << std::endl;
- std::cout << "T c" << c_of_t.T_ << std::endl;
- std::cout << "T " << T << std::endl;
- std::cout << "T ddc " << ddc_of_t.T_ << std::endl;
- std::cout << "1 / T * T " << 1/ (T * T) << std::endl;*/
- return false;
- }
- }
- return true;
+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;
+ eq->getPolytopeInequalities(Hrow, h);
+ MatrixXX H = -Hrow;
+ H.rowwise().normalize();
+ const bezier_com_traj::bezier_t c_of_t = resData.c_of_t_;
+ const bezier_com_traj::bezier_t dL_of_t = resData.dL_of_t_;
+ bezier_com_traj::bezier_t ddc_of_t = c_of_t.compute_derivate(2);
+ for (int i = 1; i < num_steps; ++i) {
+ double dt = double(i) / double(num_steps) * T;
+ c_of_t(dt);
+ dL_of_t(dt);
+ Vector6 w = computew(eq, c_of_t(dt), ddc_of_t(dt), dL_of_t(dt));
+ 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 << "c0 " << c0.transpose() << std::endl;
+ std::cout << "H row " << H.row(j) << std::endl;
+ std::cout << "w " << w << std::endl;
+ std::cout << "j " << j << std::endl;
+ //throw;
+ std::cout << "mult ddc " << ddc_of_t.mult_T_ << std::endl;
+ std::cout << "mult c " << c_of_t.mult_T_ << std::endl;
+ std::cout << "T c" << c_of_t.T_ << std::endl;
+ std::cout << "T " << T << std::endl;
+ std::cout << "T ddc " << ddc_of_t.T_ << std::endl;
+ std::cout << "1 / T * T " << 1/ (T * T) << std::endl;*/
+ return false;
+ }
+ }
+ return true;
}
-int main()
-{
+int main() {
srand((unsigned int)time(NULL));
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
@@ -159,175 +144,165 @@ int main()
unsigned int generatorsPerContact = 4;
unsigned int N_CONTACTS = 2;
double MIN_CONTACT_DISTANCE = 0.3;
- double LX = 0.5*0.2172; // half contact surface size in x direction
- double LY = 0.5*0.138; // half contact surface size in y direction
- CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
- CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
- double gamma = atan(mu); // half friction cone angle
- RPY_LOWER_BOUNDS << -2*gamma, -2*gamma, -M_PI;
- RPY_UPPER_BOUNDS << +2*gamma, +2*gamma, +M_PI;
+ double LX = 0.5 * 0.2172; // half contact surface size in x direction
+ double LY = 0.5 * 0.138; // half contact surface size in y direction
+ CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
+ CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
+ double gamma = atan(mu); // half friction cone angle
+ RPY_LOWER_BOUNDS << -2 * gamma, -2 * gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +2 * gamma, +2 * gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
/************************************ 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,
+ 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,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
// compute upper and lower bounds of com positions to test
- com_LB(0) = p.col(0).minCoeff()-X_MARG;
- com_UB(0) = p.col(0).maxCoeff()+X_MARG;
- com_LB(1) = p.col(1).minCoeff()-Y_MARG;
- com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
- com_LB(2) = p.col(2).minCoeff()+0.3;
- com_UB(2) = p.col(2).maxCoeff()+1.5;
+ com_LB(0) = p.col(0).minCoeff() - X_MARG;
+ com_UB(0) = p.col(0).maxCoeff() + X_MARG;
+ com_LB(1) = p.col(1).minCoeff() - Y_MARG;
+ com_UB(1) = p.col(1).maxCoeff() + Y_MARG;
+ com_LB(2) = p.col(2).minCoeff() + 0.3;
+ com_UB(2) = p.col(2).maxCoeff() + 1.5;
Vector3 c0;
- solver_PP.setNewContacts(p,N,mu,EQUILIBRIUM_ALGORITHM_PP);
+ solver_PP.setNewContacts(p, N, mu, EQUILIBRIUM_ALGORITHM_PP);
const double DISCRETIZATION_STEP = 0.05;
- if(findStaticEqCOMPos(&solver_PP, com_LB, com_UB,c0))
- {
- numSol +=1;
- for(int j = 0; j < 100; ++j)
+ if (findStaticEqCOMPos(&solver_PP, com_LB, com_UB, c0)) {
+ numSol += 1;
+ for (int j = 0; j < 100; ++j) {
+ 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.c0_ = c0;
+ pData.dc0_ << fRandom(-1., 1.), fRandom(-1., 1.), fRandom(-1., 1.);
+ // pData.dc0_ *= 1;
+ pData.l0_ = Vector3(0., 0., 0.);
+ pData.contacts_.push_back(data);
+ pData.costFunction_ = bezier_com_traj::DISTANCE_TRAVELED;
+ double T;
+ for (int k = -1; k < 2; ++k)
+ // for (int k = 0; k < 1; ++k)
{
- 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.c0_ = c0;
- pData.dc0_ << fRandom(-1.,1.) , fRandom(-1.,1.) , fRandom(-1.,1.);
- //pData.dc0_ *= 1;
- pData.l0_ = Vector3(0.,0.,0.);
- pData.contacts_.push_back(data);
- pData.costFunction_ = bezier_com_traj::DISTANCE_TRAVELED;
- double T;
- for (int k = -1; k < 2; ++k)
- //for (int k = 0; k < 1; ++k)
- {
- 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;
- T = 1. + 0.3 * k;
- std::vector<double> Ts;
- Ts.push_back(T);
- 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;
- succContinuous += 1;
- std::string solverName("continuous");
- checkTrajectory(c0, solverName, &solver_PP,rData, T);
- }
- else
- {
- succCont = false;
- }
- // try discretize
- VectorX t2(1); t2 << Ts[0];
- 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)));
- 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);
- 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));
- }
- else
- {
- VectorX t2(1); t2 << Ts[0];
- bezier_com_traj::ResultDataCOMTraj rDatatest = 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::endl;
- succDisc = false;
- }
+ 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;
+ T = 1. + 0.3 * k;
+ std::vector<double> Ts;
+ Ts.push_back(T);
+ 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;
+ succContinuous += 1;
+ std::string solverName("continuous");
+ checkTrajectory(c0, solverName, &solver_PP, rData, T);
+ } else {
+ succCont = false;
+ }
+ // try discretize
+ VectorX t2(1);
+ t2 << Ts[0];
+ 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)));
+ 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);
+ 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));
+ } else {
+ VectorX t2(1);
+ t2 << Ts[0];
+ bezier_com_traj::ResultDataCOMTraj rDatatest =
+ 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::endl;
+ succDisc = false;
+ }
- pData.useAngularMomentum_ = true;
- 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;
- succdL += 1;
- std::string solverName("continuous AngularMomentum");
- 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::endl;
- succDisc = false;
- }
+ pData.useAngularMomentum_ = true;
+ 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;
+ succdL += 1;
+ std::string solverName("continuous AngularMomentum");
+ 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::endl;
+ succDisc = false;
+ }
- 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 ));
- }
- else
- {
- if(succCont || succDisc ||succdLbool)
- 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);
- pData.contacts_.front().Ang_ = Eigen::Matrix3d::Identity();
- pData.contacts_.front().ang_ = Vector3::Constant(3,0.1);
- if(rData3.success_)
- {
- assert ((rData3.c_of_t_(0.) - c0).norm() < 0.0001);
- succdLKinBool = true;
- succdLKin += 1;
- std::string solverName("kinematic and angular constraint");
- checkTrajectory(c0, solverName, &solver_PP,rData3,T);
- }
+ 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));
+ } else {
+ if (succCont || succDisc || succdLbool)
+ 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);
+ pData.contacts_.front().Ang_ = Eigen::Matrix3d::Identity();
+ pData.contacts_.front().ang_ = Vector3::Constant(3, 0.1);
+ if (rData3.success_) {
+ assert((rData3.c_of_t_(0.) - c0).norm() < 0.0001);
+ succdLKinBool = true;
+ succdLKin += 1;
+ std::string solverName("kinematic and angular constraint");
+ checkTrajectory(c0, solverName, &solver_PP, rData3, T);
+ }
- pData.contacts_.front().kin_ = VectorX::Zero(0);
- 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;
- succdLAng += 1;
- std::string solverName("angular constraint");
- checkTrajectory(c0, solverName, &solver_PP,rData31,T);
- }
+ pData.contacts_.front().kin_ = VectorX::Zero(0);
+ 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;
+ succdLAng += 1;
+ std::string solverName("angular constraint");
+ checkTrajectory(c0, solverName, &solver_PP, rData31, T);
+ }
- 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);
- if(rData4.success_)
- {
- assert ((rData4.c_of_t_(0.) - c0).norm() < 0.0001);
- succKinBool = true;
- succKin += 1;
- std::string solverName("kinematic constraint");
- checkTrajectory(c0, solverName, &solver_PP,rData4,T);
- }
- }
+ 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);
+ if (rData4.success_) {
+ assert((rData4.c_of_t_(0.) - c0).norm() < 0.0001);
+ succKinBool = true;
+ succKin += 1;
+ std::string solverName("kinematic constraint");
+ checkTrajectory(c0, solverName, &solver_PP, rData4, T);
+ }
}
+ }
}
}
@@ -337,10 +312,10 @@ int main()
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 continunous with angular momentum and kinematic and angular constraints" << succdLKin
+ << 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 a99820f280b3023f640b015a15efc896b24c49c4..443c15ace41440e08a5f6bf11a2fd7a8a9a25d2c 100644
--- a/tests/test_helper.hh
+++ b/tests/test_helper.hh
@@ -6,217 +6,213 @@
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <boost/test/included/unit_test.hpp>
-using bezier_com_traj::MatrixXX;
-using bezier_com_traj::MatrixX3;
using bezier_com_traj::Matrix3;
-using bezier_com_traj::VectorX;
+using bezier_com_traj::MatrixX3;
+using bezier_com_traj::MatrixXX;
using bezier_com_traj::Vector3;
+using bezier_com_traj::VectorX;
#define MASS 50.
#define MU 0.5
-#define LX 0.2172 // contact surface size in x direction
-#define LY 0.138 // contact surface size in y direction
+#define LX 0.2172 // contact surface size in x direction
+#define LY 0.138 // contact surface size in y direction
#define KIN_X_MIN -0.7
-#define KIN_X_MAX 0.7
+#define KIN_X_MAX 0.7
#define KIN_Y_MIN -0.4
-#define KIN_Y_MAX 0.4
-#define KIN_Z_MIN 0
-#define KIN_Z_MAX 0.9
-#define EPSILON 1e-6
-
-typedef std::pair<MatrixX3,VectorX> ConstraintsPair;
-
-std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints(){
- // generate a simple polytgone : faces aligned along x,y,z axis
- // size : x [-0.5,0.5] ; y = [-0.3,1] ; z [0,0.8]
- MatrixX3 N(6,3);
- MatrixX3 V(6,3);
-
- N.block<1,3>(0,0) = Vector3(-1,0,0);
- V.block<1,3>(0,0) = Vector3(KIN_X_MIN,KIN_Y_MAX,0);
- N.block<1,3>(1,0) = Vector3(0,1,0);
- V.block<1,3>(1,0) = Vector3(KIN_X_MIN,KIN_Y_MAX,0);
- N.block<1,3>(2,0) = Vector3(1,0,0);
- V.block<1,3>(2,0) = Vector3(KIN_X_MAX,KIN_Y_MIN,0);
- N.block<1,3>(3,0) = Vector3(0,-1,0);
- V.block<1,3>(3,0) = Vector3(KIN_X_MAX,KIN_Y_MIN,0);
- N.block<1,3>(4,0) = Vector3(0,0,-1);
- V.block<1,3>(4,0) = Vector3(0,0,KIN_Z_MIN);
- N.block<1,3>(5,0) = Vector3(0,0,1);
- V.block<1,3>(5,0) = Vector3(0,0,KIN_Z_MAX);
-
- return std::make_pair(N,V);
+#define KIN_Y_MAX 0.4
+#define KIN_Z_MIN 0
+#define KIN_Z_MAX 0.9
+#define EPSILON 1e-6
+
+typedef std::pair<MatrixX3, VectorX> ConstraintsPair;
+
+std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints() {
+ // generate a simple polytgone : faces aligned along x,y,z axis
+ // size : x [-0.5,0.5] ; y = [-0.3,1] ; z [0,0.8]
+ MatrixX3 N(6, 3);
+ MatrixX3 V(6, 3);
+
+ N.block<1, 3>(0, 0) = Vector3(-1, 0, 0);
+ V.block<1, 3>(0, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
+ N.block<1, 3>(1, 0) = Vector3(0, 1, 0);
+ V.block<1, 3>(1, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
+ N.block<1, 3>(2, 0) = Vector3(1, 0, 0);
+ V.block<1, 3>(2, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
+ N.block<1, 3>(3, 0) = Vector3(0, -1, 0);
+ V.block<1, 3>(3, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
+ N.block<1, 3>(4, 0) = Vector3(0, 0, -1);
+ V.block<1, 3>(4, 0) = Vector3(0, 0, KIN_Z_MIN);
+ N.block<1, 3>(5, 0) = Vector3(0, 0, 1);
+ V.block<1, 3>(5, 0) = Vector3(0, 0, KIN_Z_MAX);
+
+ return std::make_pair(N, V);
}
-
-std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotation, Vector3 endEffTranslation){
-
- std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
- MatrixX3 N = NV.first;
- MatrixX3 V = NV.second;
- size_t numFaces = N.rows();
- MatrixX3 A(numFaces,3);
- VectorX b(numFaces);
- VectorX n,v;
-
- for(size_t i = 0 ; i < numFaces ; ++i){
- n = endEffRotation * (N.block<1,3>(i,0).transpose());
- v = endEffRotation * (V.block<1,3>(i,0).transpose()) + endEffTranslation;
- A.block<1,3>(i,0) = n;
- b[i] = v.dot(n);
- }
-
- return std::make_pair(A,b);
+std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotation, Vector3 endEffTranslation) {
+ std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
+ MatrixX3 N = NV.first;
+ MatrixX3 V = NV.second;
+ size_t numFaces = N.rows();
+ MatrixX3 A(numFaces, 3);
+ VectorX b(numFaces);
+ VectorX n, v;
+
+ for (size_t i = 0; i < numFaces; ++i) {
+ n = endEffRotation * (N.block<1, 3>(i, 0).transpose());
+ v = endEffRotation * (V.block<1, 3>(i, 0).transpose()) + endEffTranslation;
+ A.block<1, 3>(i, 0) = n;
+ b[i] = v.dot(n);
+ }
+
+ return std::make_pair(A, b);
}
-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 ?)
+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 ?)
- BOOST_CHECK(normals.rows() == positions.rows());
- MatrixX3 rec_normals(normals.rows()*4,3);
- MatrixX3 rec_positions(normals.rows()*4,3);
+ BOOST_CHECK(normals.rows() == positions.rows());
+ MatrixX3 rec_normals(normals.rows() * 4, 3);
+ MatrixX3 rec_positions(normals.rows() * 4, 3);
- double lx = size_X/2.;
- double ly = size_Y/2.;
- MatrixX3 p(4,3);
- p << lx, ly, 0,
- lx, -ly, 0,
- -lx, -ly, 0,
- -lx, ly, 0;
+ double lx = size_X / 2.;
+ double ly = size_Y / 2.;
+ MatrixX3 p(4, 3);
+ p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0;
-
- 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);
- }
+ 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);
}
- return std::make_pair(rec_normals,rec_positions);
+ }
+ return std::make_pair(rec_normals, rec_positions);
}
-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;
- contactCone.setNewContacts(positions,normals,MU,algo);
- return contactCone;
+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;
+ contactCone.setNewContacts(positions, normals, MU, algo);
+ return contactCone;
}
-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);
- // compute GIWC
- centroidal_dynamics::MatrixXX Hrow;
- VectorX h;
- contactPhase.getPolytopeInequalities(Hrow,h);
- MatrixXX H = -Hrow;
- H.rowwise().normalize();
- int dimH = (int)(H.rows());
- MatrixXX mH = contactPhase.m_mass * H;
- // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
- // A = mH g^
- // b = h + mHg
- MatrixX3 A = mH.block(0,3,dimH,3) * (gSkew - accSkew);
- VectorX b = h+mH.block(0,0,dimH,3)*(g - acc);
- return std::make_pair(A,b);
+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);
+ // compute GIWC
+ centroidal_dynamics::MatrixXX Hrow;
+ VectorX h;
+ contactPhase.getPolytopeInequalities(Hrow, h);
+ MatrixXX H = -Hrow;
+ H.rowwise().normalize();
+ int dimH = (int)(H.rows());
+ MatrixXX mH = contactPhase.m_mass * H;
+ // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
+ // A = mH g^
+ // b = h + mHg
+ MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
+ VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);
+ return std::make_pair(A, b);
}
-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);
- return generateStabilityConstraints(contactPhase,acc);
+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);
+ return generateStabilityConstraints(contactPhase, acc);
}
-std::pair<Matrix3, Vector3> computeCost(){
- Matrix3 H = Matrix3::Identity();
- Vector3 g = Vector3::Zero();
- return std::make_pair(H,g);
+std::pair<Matrix3, Vector3> computeCost() {
+ Matrix3 H = Matrix3::Identity();
+ Vector3 g = Vector3::Zero();
+ return std::make_pair(H, g);
}
-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);
- size_t numIneq = Ab.first.rows() + Cd.first.rows();
- MatrixXX M(numIneq,3);
- VectorX n(numIneq);
- M.block(0,0,Ab.first.rows(),3) = Ab.first;
- M.block(Ab.first.rows(),0,Cd.first.rows(),3) = Cd.first;
- n.segment(0,Ab.first.rows()) = Ab.second;
- n.segment(Ab.first.rows(),Cd.first.rows()) = Cd.second;
- return std::make_pair(M,n);
+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);
+ size_t numIneq = Ab.first.rows() + Cd.first.rows();
+ MatrixXX M(numIneq, 3);
+ VectorX n(numIneq);
+ M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
+ M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
+ n.segment(0, Ab.first.rows()) = Ab.second;
+ n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
+ return std::make_pair(M, n);
}
-
-double fRandom(double fMin, double fMax)
-{
- double f = (double)std::rand() / RAND_MAX;
- return fMin + f * (fMax - fMin);
+double fRandom(double fMin, double fMax) {
+ double f = (double)std::rand() / RAND_MAX;
+ return fMin + f * (fMax - fMin);
}
-
-
-ConstraintsPair stackConstraints(const ConstraintsPair& Ab,const ConstraintsPair& Cd){
- size_t numIneq = Ab.first.rows() + Cd.first.rows();
- MatrixX3 M(numIneq,3);
- VectorX n(numIneq);
- M.block(0,0,Ab.first.rows(),3) = Ab.first;
- M.block(Ab.first.rows(),0,Cd.first.rows(),3) = Cd.first;
- n.segment(0,Ab.first.rows()) = Ab.second;
- n.segment(Ab.first.rows(),Cd.first.rows()) = Cd.second;
- return std::make_pair(M,n);
+ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPair& Cd) {
+ size_t numIneq = Ab.first.rows() + Cd.first.rows();
+ MatrixX3 M(numIneq, 3);
+ VectorX n(numIneq);
+ M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
+ M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
+ n.segment(0, Ab.first.rows()) = Ab.second;
+ n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
+ return std::make_pair(M, n);
}
-
-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;
- }
+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;
}
- return true;
+ }
+ return true;
}
-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);
- if(success)
- success = res>=-EPSILON;
- if(!success)
- std::cout << "fail level " << res << std::endl;
- return success;
+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);
+ 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){
- // compute inequalities :
- const Vector3& g = contactPhase.m_gravity;
- const Matrix3 gSkew = bezier_com_traj::skew(g);
- const Matrix3 accSkew = bezier_com_traj::skew(acc);
- // compute GIWC
- centroidal_dynamics::MatrixXX Hrow;
- VectorX h;
- contactPhase.getPolytopeInequalities(Hrow,h);
- MatrixXX H = -Hrow;
- H.rowwise().normalize();
- int dimH = (int)(H.rows());
- MatrixXX mH = contactPhase.m_mass * H;
- // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
- // A = mH g^
- // b = h + mHg
- MatrixX3 A = mH.block(0,3,dimH,3) * (gSkew - accSkew);
- VectorX b = h+mH.block(0,0,dimH,3)*(g - acc);
-
- // verify inequalities with c :
- for(long int i = 0 ; i < b.size() ; ++i){
- if(A.block<1,3>(i,0).dot(c) -EPSILON > b[i] ){
- return false;
- }
+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);
+ const Matrix3 accSkew = bezier_com_traj::skew(acc);
+ // compute GIWC
+ centroidal_dynamics::MatrixXX Hrow;
+ VectorX h;
+ contactPhase.getPolytopeInequalities(Hrow, h);
+ MatrixXX H = -Hrow;
+ H.rowwise().normalize();
+ int dimH = (int)(H.rows());
+ MatrixXX mH = contactPhase.m_mass * H;
+ // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
+ // A = mH g^
+ // b = h + mHg
+ MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
+ VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);
+
+ // verify inequalities with c :
+ for (long int i = 0; i < b.size(); ++i) {
+ if (A.block<1, 3>(i, 0).dot(c) - EPSILON > b[i]) {
+ return false;
}
- return true;
+ }
+ return true;
}
-
-#endif // TEST_HELPER_HH
+#endif // TEST_HELPER_HH