diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed1c88566cbef83a6c059a0baf013adb3d14ffee..0da4284b0fc26fde92006835a3f18c1b0b4fc208 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,11 +52,24 @@ find_package (Spline REQUIRED)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/data.hh
+ include/bezier-com-traj/utils.hh
+ include/bezier-com-traj/flags.hh
+ include/bezier-com-traj/definitions.hh
include/bezier-com-traj/config.hh
include/bezier-com-traj/solve.hh
include/bezier-com-traj/solve_end_effector.hh
include/bezier-com-traj/common_solve_methods.hh
+ include/bezier-com-traj/common_solve_methods.inl
+ include/bezier-com-traj/cost/costfunction_definition.hh
+ include/bezier-com-traj/waypoints/waypoints_definition.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
include/solver/eiquadprog-fast.hpp
+
)
find_package(Eigen3)
diff --git a/include/bezier-com-traj/common_solve_methods.hh b/include/bezier-com-traj/common_solve_methods.hh
index ac5270f60e706ea384a3f1569740ebae16e893f1..0d78540f1dbd4ddb7a8269f935383481da2682f8 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.hh
@@ -1,25 +1,20 @@
/*
- * Copyright 2015, LAAS-CNRS
- * Author: Andrea Del Prete
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
-#include <Eigen/Dense>
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/data.hh>
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <Eigen/Dense>
namespace bezier_com_traj
{
-
-BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
-template<typename T> T initwp();
-int Normalize(Ref_matrixXX A, Ref_vectorX b);
-
-
/**
* @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
* computes a discretization of the curve
@@ -28,7 +23,8 @@ int Normalize(Ref_matrixXX A, Ref_vectorX b);
* @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,
@@ -40,8 +36,9 @@ BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(con
* @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 solve x' h x + 2 g' x, subject to A*x <= b using quadprog
@@ -51,7 +48,8 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequa
* @param g cost Vector
* @return
*/
-BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess);
+BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
+ Cref_vectorX g, Cref_vectorX initGuess);
/**
@@ -60,15 +58,21 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_ma
* @param Hg Cost matrix and vector
* @return
*/
-BEZIER_COM_TRAJ_DLLAPI ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init);
+BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
+ const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init);
-/**
- * @brief Compute the Bernstein polynoms for a given degree
- * @param degree required degree
- * @return
- */
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
+
+template <typename Point>
+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);
} // end namespace bezier_com_traj
+#include "common_solve_methods.inl"
+
+
#endif
diff --git a/include/bezier-com-traj/common_solve_methods.inl b/include/bezier-com-traj/common_solve_methods.inl
new file mode 100644
index 0000000000000000000000000000000000000000..5717f26b50b962cd3337132f9a971434ea2fb08e
--- /dev/null
+++ b/include/bezier-com-traj/common_solve_methods.inl
@@ -0,0 +1,40 @@
+
+namespace bezier_com_traj
+{
+
+template <typename Point>
+std::vector< std::pair<double,Point> > computeDiscretizedWaypoints
+ (const ProblemData& pData, double T,const T_time& timeArray)
+{
+ typedef std::pair<double,Point> coefs_t;
+ std::vector<coefs_t> wps;
+ std::vector<Point> pi = computeConstantWaypoints(pData,T);
+ // evaluate curve work with normalized time !
+ for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
+ {
+ double t = std::min(cit->first / T, 1.);
+ wps.push_back(evaluateCurveAtTime(pData,pi,t));
+ }
+ return wps;
+}
+
+
+template <typename Point>
+std::vector< std::pair<double,Point> > computeDiscretizedAccelerationWaypoints
+ (const ProblemData& pData, double T,const T_time& timeArray)
+{
+ typedef std::pair<double,Point> coefs_t;
+ std::vector<coefs_t> wps;
+ std::vector<Point> pi = computeConstantWaypoints(pData,T);
+ // evaluate curve work with normalized time !
+ for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
+ {
+ double t = std::min(cit->first / T, 1.);
+ wps.push_back(evaluateAccelerationCurveAtTime(pData,pi,T,t));
+ }
+ return wps;
+}
+
+} // end namespace bezier_com_traj
+
+
diff --git a/include/bezier-com-traj/cost/costfunction_definition.hh b/include/bezier-com-traj/cost/costfunction_definition.hh
new file mode 100644
index 0000000000000000000000000000000000000000..90bbbac326acc41cb28d70e218dea8a72e0950a6
--- /dev/null
+++ b/include/bezier-com-traj/cost/costfunction_definition.hh
@@ -0,0 +1,35 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#ifndef BEZIER_COM_COST_WP_DEF_H
+#define BEZIER_COM_COST_WP_DEF_H
+
+#include <bezier-com-traj/data.hh>
+#include "boost/assign.hpp"
+
+
+
+namespace bezier_com_traj{
+/**
+* This file contains definitions for the different cost functions used in qp minimization
+*/
+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.
+ * @param Ts times per phase
+ * @param pointsPerPhase TODO replace
+ * @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);
+
+} // namespace cost
+} // namespace bezier_com_traj
+
+#endif
diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index 5c2ac0f2564cfa228faa568356c3c2f2d9f7e185..d9c2bfcd8956cd798a1b0cb3f42e53aa03a9bfdc 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -1,54 +1,30 @@
/*
- * Copyright 2015, LAAS-CNRS
- * Author: Andrea Del Prete
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_LIB_DATA_H
#define BEZIER_COM_TRAJ_LIB_DATA_H
-#include <Eigen/Dense>
#include <bezier-com-traj/config.hh>
+#include <bezier-com-traj/flags.hh>
+#include <bezier-com-traj/definitions.hh>
+#include <bezier-com-traj/utils.hh>
+
#include <spline/bezier_curve.h>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
+#include <Eigen/Dense>
+
#include <vector>
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, 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 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;
- typedef Matrix3 matrix3_t;
- typedef Vector3 point3_t;
/**
- * @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 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.
*/
- typedef std::pair<matrix6_t, point6_t> waypoint6_t;
- typedef std::pair<matrix3_t, point3_t> waypoint3_t;
- typedef std::pair<double,point3_t> coefs_t;
-
-
struct BEZIER_COM_TRAJ_DLLAPI ContactData
{
ContactData()
@@ -60,66 +36,30 @@ namespace bezier_com_traj
~ContactData(){}
centroidal_dynamics::Equilibrium* contactPhase_;
- MatrixX3 Kin_;
- VectorX kin_;
- MatrixX3 Ang_;
- VectorX ang_;
+ MatrixX3 Kin_; // inequality kinematic constraints
+ VectorX kin_;
+ MatrixX3 Ang_; // inequality angular momentum constraints
+ VectorX ang_;
};
- enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag{
- INIT_POS = 0x00001,
- INIT_VEL = 0x00002,
- INIT_ACC = 0x00004,
- END_POS = 0x00008,
- END_VEL = 0x00010,
- END_ACC = 0x00020
- };
-
- 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 (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));}
-
+ /**
+ * @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_(true)
, maxAcceleration_(5.)
- ,reduce_h_(1e-4) {}
+ , reduce_h_(1e-4) {}
Constraints(ConstraintFlag flag)
: flag_(flag)
, constraintAcceleration_(true)
, maxAcceleration_(5.)
- ,reduce_h_(1e-4)
- {
- /*if(dc0_)
- assert(c0_ && "You cannot constraint init velocity if init position is not constrained.");
- if(ddc0_)
- assert(dc0_ && "You cannot constraint init acceleration if init velocity is not constrained.");
- if(dc1_)
- assert(c1_ && "You cannot constraint final velocity if final position is not constrained.");
- if(ddc1_)
- assert(dc1_ && "You cannot constraint final acceleration if final velocity is not constrained.");*/
- }
+ , reduce_h_(1e-4) {}
~Constraints(){}
@@ -127,32 +67,42 @@ namespace bezier_com_traj
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_(Vector3::Zero())
- ,dc0_(Vector3::Zero())
- ,ddc0_(Vector3::Zero())
- , c1_(Vector3::Zero())
- ,dc1_(Vector3::Zero())
- ,ddc1_(Vector3::Zero())
- ,useAngularMomentum_(false) {}
+ : c0_ (point_t::Zero())
+ ,dc0_ (point_t::Zero())
+ ,ddc0_(point_t::Zero())
+ , c1_ (point_t::Zero())
+ ,dc1_ (point_t::Zero())
+ ,ddc1_(point_t::Zero())
+ ,useAngularMomentum_(false)
+ ,costFunction_(ACCELERATION) {}
std::vector<ContactData> contacts_;
- Vector3 c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
- Vector3 l0_;
+ point_t c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
+ point_t l0_;
bool useAngularMomentum_;
Constraints constraints_;
+ CostFunction costFunction_;
};
- typedef Eigen::Vector3d point_t;
- typedef const Eigen::Ref<const point_t>& point_t_tC;
- typedef spline::bezier_curve <double, double, 3, true, point_t > bezier_t;
- typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
+
+ /**
+ * @brief Struct used to return the results of the trajectory generation
+ * problem.
+ */
struct BEZIER_COM_TRAJ_DLLAPI ResultData
{
ResultData():
@@ -178,12 +128,16 @@ namespace bezier_com_traj
x = (other.x);
return *this;
}
-
- bool success_;
- double cost_;
- VectorX x;
+ bool success_; // whether the optimization was successful
+ double cost_; // cost evaluation for the solved control point
+ VectorX x; //control point
};
+
+ /**
+ * @brief Specialized ResultData that computes the Bezier curves
+ * corresponding to the computed trajectory
+ */
struct BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj : public ResultData
{
ResultDataCOMTraj():
@@ -195,10 +149,10 @@ namespace bezier_com_traj
~ResultDataCOMTraj(){}
- bezier_t c_of_t_;
- bezier_t dL_of_t_;
- point_t dc1_;
- point_t ddc1_;
+ 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
diff --git a/include/bezier-com-traj/definitions.hh b/include/bezier-com-traj/definitions.hh
new file mode 100644
index 0000000000000000000000000000000000000000..5ad093e9c04126c989a70ec6491bff668e57e7bf
--- /dev/null
+++ b/include/bezier-com-traj/definitions.hh
@@ -0,0 +1,61 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#ifndef BEZIER_COM_TRAJ_DEFINITIONS_H
+#define BEZIER_COM_TRAJ_DEFINITIONS_H
+
+#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
+#include <spline/bezier_curve.h>
+#include <Eigen/Dense>
+
+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, 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 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;
+typedef Matrix3 matrix3_t;
+typedef Vector3 point3_t;
+
+typedef Eigen::Vector3d point_t;
+typedef const Eigen::Ref<const point_t>& point_t_tC;
+
+typedef spline::bezier_curve <double, double, 3, true, point_t > bezier_t;
+typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
+
+typedef std::vector< std::pair<double, int> > T_time;
+typedef T_time::const_iterator CIT_time;
+
+/**
+* @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<double,point3_t> coefs_t;
+
+} // end namespace bezier_com_traj
+
+#endif
diff --git a/include/bezier-com-traj/flags.hh b/include/bezier-com-traj/flags.hh
new file mode 100644
index 0000000000000000000000000000000000000000..995085afe3d7f207582678113cb8d2f267004d1a
--- /dev/null
+++ b/include/bezier-com-traj/flags.hh
@@ -0,0 +1,53 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#ifndef BEZIER_COM_TRAJ_FLAGS_H
+#define BEZIER_COM_TRAJ_FLAGS_H
+
+#include <bezier-com-traj/config.hh>
+
+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,
+ UNKNOWN = 0x00040
+ };
+
+ 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 (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
+
+#endif
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index 88ace9b4838f1ee89c172ac8107803f240026fb6..ecc5bb0a49ead00da4c35cf01fb449449dac29d0 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -1,6 +1,6 @@
/*
- * Copyright 2015, LAAS-CNRS
- * Author: Andrea Del Prete
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_LIB_SOLVE_H
@@ -23,23 +23,33 @@ namespace bezier_com_traj
* @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);
+ BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts,
+ const double timeStep = -1);
/**
- * @brief solveOnestep Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity,
- * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * @brief computeCOMTraj Tries to solve the one step problem : Given two or three contact phases,
+ * an initial and final com position and velocity,
+ * try to compute the CoM trajectory (as a Bezier curve) that connect them
* @param pData problem Data. Should contain only two contact phases.
* @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 solveOnestep(const ProblemData& pData, const VectorX& Ts, const Vector3& init_guess,const int pointsPerPhase = 3);
-
-
- void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,const std::string& fileName,bool clipZ = false);
-
-
-
+ BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
+ const Vector3& init_guess,const int pointsPerPhase = 3,
+ const double feasability_treshold = 0.);
+
+
+ /**
+ * @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. Should contain only two contact phases.
+ * @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 computeCOMTraj(const ProblemData& pData, const VectorX& Ts, const double timeStep = -1);
} // end namespace bezier_com_traj
diff --git a/include/bezier-com-traj/utils.hh b/include/bezier-com-traj/utils.hh
new file mode 100644
index 0000000000000000000000000000000000000000..45494f686dfdb65a05eb6e1cf7eb21b4b1fe1e2a
--- /dev/null
+++ b/include/bezier-com-traj/utils.hh
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
+#define BEZIER_COM_TRAJ_LIB_UTILS_H
+
+#include <bezier-com-traj/config.hh>
+#include <bezier-com-traj/definitions.hh>
+#include <bezier-com-traj/flags.hh>
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace bezier_com_traj
+{
+template<typename T> T initwp();
+
+/**
+ * @brief Compute the Bernstein polynoms for a given degree
+ * @param degree required degree
+ * @return the bernstein polynoms
+ */
+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
+ * @param pData problem data
+ * @param T total trajectory time
+ * @param pis list of waypoints
+ * @return the bezier curve
+ */
+template<typename Bezier, typename Point>
+BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag, const double T,
+ const std::vector<Point>& pi, const Point& x);
+
+/**
+ * @brief computeDiscretizedTime build an array of discretized points in time,
+ * such that there is the same number of point in each phase. Doesn't contain t=0,
+ * is of size pointsPerPhase*phaseTimings.size()
+ * @param phaseTimings
+ * @param pointsPerPhase
+ * @return
+ */
+T_time computeDiscretizedTime(const VectorX& phaseTimings, const int pointsPerPhase );
+
+/**
+ * @brief computeDiscretizedTime build an array of discretized points in time,
+ * given the timestep. Doesn't contain t=0,
+ * is of size pointsPerPhase*phaseTimings.size()
+ * @param phaseTimings
+ * @param timeStep
+ * @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);
+
+/**
+ * @brief skew symmetric matrix
+ */
+BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
+
+/**
+ * @brief normalize inequality constraints
+ */
+int Normalize(Ref_matrixXX A, Ref_vectorX b);
+
+} // end namespace bezier_com_traj
+
+template<typename Bezier, typename Point>
+Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const double T,
+ const std::vector<Point>& pi, 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++;
+ }
+ }
+ }
+ 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++;
+ }
+ if(flag & END_POS){
+ wps.push_back(pi[i]);
+ i++;
+ }
+ }
+ return Bezier (wps.begin(), wps.end(),T);
+}
+
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index 46119de45e934cc5f6f870bf383ad3386e73e780..cef126d64a5702b2b5b55a2816a3acb4117d9f44 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -3,6 +3,10 @@
* Author: Pierre Fernbach
*/
+
+#ifndef BEZIER_COM_TRAJ_C0DC0C1_H
+#define BEZIER_COM_TRAJ_C0DC0C1_H
+
#include <bezier-com-traj/data.hh>
namespace bezier_com_traj{
@@ -16,31 +20,27 @@ static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS;
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+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;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
coefs_t wp;
double alpha = 1./(T*T);
// equation found with sympy
wp.first = (-18.0*t + 6.0)*alpha;
wp.second = (-6.0*pi[0]*t + 6.0*pi[0] + 18.0*pi[1]*t - 12.0*pi[1] + 6.0*pi[3]*t)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+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;
@@ -53,7 +53,7 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
return pi;
}
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
@@ -92,7 +92,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
}
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
coefs_t v;
// equation found with sympy
v.first = -3./T;
@@ -100,15 +100,8 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = -12./(T*T);
- v.second = (-6.*pi[1] + 6.*pi[3])/ (T*T);
- return v;
-}
-
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c4e2e2843bc6857f8ce0896a0c3f24530f9e3ca8
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
@@ -0,0 +1,136 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#ifndef BEZIER_COM_TRAJ_C0DC0D1_H
+#define BEZIER_COM_TRAJ_C0DC0D1_H
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_dc1{
+
+static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_VEL;
+
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE = 4)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one free waypoint (x)
+ * @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 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<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+ std::vector<waypoint6_t> wps;
+ 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 ...
+ /*waypoint6_t w0 = initwp<waypoint6_t>();
+ 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);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 2.0*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = 1.0*(-4.0*Cpi[0] + 6.0*Cpi[1])*alpha;
+ w1.second.head<3>() = 1.0*(4.0*pi[0] - 6.0*pi[1])*alpha;
+ w1.second.tail<3>() = 1.0*Cg*pi[1];
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = -2.0*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 2.0*Cpi[0])*alpha;
+ w2.second.head<3>() = 2.0*pi[0]*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -6*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 6.0*Cpi[1])*alpha;
+ w3.second.head<3>() = 6*pi[1]*alpha;
+ wps.push_back(w3);
+ return wps;*/
+
+ waypoint6_t w0 = initwp<waypoint6_t>();
+ 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);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ 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);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ 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);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ 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);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ 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;
+}
+
+
+}
+}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
index bb06d23df0ec82867ef62029abc51f729363de38..c67f383bbbc988caa9f87dfb44f34ab5853c1b45 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -3,6 +3,9 @@
* Author: Pierre Fernbach
*/
+#ifndef BEZIER_COM_TRAJ_C0DC0D1C1_H
+#define BEZIER_COM_TRAJ_C0DC0D1C1_H
+
#include <bezier-com-traj/data.hh>
namespace bezier_com_traj{
@@ -16,7 +19,7 @@ static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS | END_VEL;
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
coefs_t wp;
double t2 = t*t;
double t3 = t2*t;
@@ -24,24 +27,20 @@ coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double 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;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
coefs_t wp;
double alpha = 1./(T*T);
// equation found with sympy
wp.first = (72.0*t*t - 72.0*t + 12.0)*alpha;
wp.second = (12.0*pi[0]*t*t - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t*t + 72.0*pi[1]*t - 24.0*pi[1] - 48.0*pi[3]*t*t + 24.0*pi[3]*t + 12.0*pi[4]*t*t)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+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;
@@ -51,13 +50,10 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
pi.push_back(point_t::Zero()); // p2 = x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
pi.push_back(pData.c1_); // p4
- /* for(int i = 0 ; i < pi.size() ; ++i){
- std::cout<<" p"<<i<<" = "<<pi[i].transpose()<<std::endl;
- }*/
return pi;
}
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
@@ -108,7 +104,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
return wps;
}
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
coefs_t v;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
// equation found with sympy
@@ -117,15 +113,8 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 12./(T*T);
- v.second = (-24.0*pi[3] + 12.*pi[4])/ (T*T);
- return v;
-}
-
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index 5c9c264383bd97efdfe28324439855796e8fb648..13b914eb318016fee6db210f6293716dedcc6c36 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -3,6 +3,10 @@
* Author: Pierre Fernbach
*/
+
+#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
+#define BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
+
#include <bezier-com-traj/data.hh>
namespace bezier_com_traj{
@@ -17,7 +21,7 @@ static const ConstraintFlag flag =INIT_POS | INIT_VEL | INIT_ACC | END_POS;
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
coefs_t wp;
double t2 = t*t;
double t3 = t2*t;
@@ -25,25 +29,21 @@ coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double 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;
- //std::cout<<"wp at t = "<<t<<std::endl;
- //std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
coefs_t wp;
double alpha = 1./(T*T);
double t2 = t*t;
// equation found with sympy
wp.first = (-48.0*t2 + 24.0*t)*alpha;
wp.second = (12.0*pi[0]*t2 - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t2 + 72.0*pi[1]*t - 24.0*pi[1] + 72.0*pi[2]*t2 - 72.0*pi[2]*t + 12.0*pi[2] + 12.0*pi[4]*t2)*alpha;
- //std::cout<<"acc_wp at t = "<<t<<std::endl;
- //std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+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.;
@@ -53,14 +53,10 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
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
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
@@ -109,7 +105,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
return wps;
}
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
coefs_t v;
// equation found with sympy
v.first = -4./T;
@@ -117,15 +113,8 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = -24./(T*T);
- v.second = (12.0*pi[2] + 12.*pi[4])/ (T*T);
- return v;
-}
-
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index a394cdc5e8f9799fc8e8b4f10b6369d2e0f9a599..b86bb91697ce854f86bed7ced122b8f9c3d9c80b 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -3,6 +3,10 @@
* Author: Pierre Fernbach
*/
+
+#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
+#define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
+
#include <bezier-com-traj/data.hh>
namespace bezier_com_traj{
@@ -18,7 +22,7 @@ static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_VEL | EN
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t){
coefs_t wp;
double t2 = t*t;
double t3 = t2*t;
@@ -27,12 +31,10 @@ coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double 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;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
coefs_t wp;
double alpha = 1./(T*T);
double t2 = t*t;
@@ -40,13 +42,11 @@ coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double
// 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;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+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.;
@@ -57,14 +57,10 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
pi.push_back(point_t::Zero()); // x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
@@ -126,7 +122,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
return wps;
}
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
coefs_t v;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
// equation found with sympy
@@ -135,15 +131,7 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 20./(T*T);
- v.second = (-40.0*pi[4] + 20.*pi[5])/ (T*T);
- return v;
-}
-
-
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index 868bf758528bf5e86d4062d855dd04baf6fc98ad..31c11e8d8b5207559c86d465e4c6305b973654db 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -3,6 +3,10 @@
* 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 <bezier-com-traj/data.hh>
namespace bezier_com_traj{
@@ -18,7 +22,7 @@ static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | EN
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
coefs_t wp;
double t2 = t*t;
double t3 = t2*t;
@@ -28,12 +32,10 @@ coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double 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;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
coefs_t wp;
double alpha = 1./(T*T);
double t2 = t*t;
@@ -42,13 +44,11 @@ coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double
// 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;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+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.;
@@ -60,14 +60,10 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
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
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
@@ -139,7 +135,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
return wps;
}
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
coefs_t v;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
// equation found with sympy
@@ -148,14 +144,8 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-coefs_t computeFinalAccelerationPoint(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 = (-30.0*pi[4] - 60.*pi[5] + 30.*pi[6])/ (T*T);
- return v;
-}
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index 805d6a156efe032b57a4fd749f94fa87cd102aca..614a14206e6dac3e6970448f61e6b18c8349f92d 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -3,195 +3,55 @@
* Author: Pierre Fernbach
*/
+#ifndef BEZIER_COM_TRAJ_WP_DEF_H
+#define BEZIER_COM_TRAJ_WP_DEF_H
#include <bezier-com-traj/data.hh>
-#include <bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
-#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
-#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh>
-#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
-#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh>
-#include "boost/assign.hpp"
namespace bezier_com_traj{
/**
- * This file is used to choose the correct expressions of the curves waypoints, depending on the options set in ProblemData.constraints
- */
+* This file is used to choose the correct expressions of the curves waypoints,
+* depending on the options set in ProblemData.constraints
+*/
-
-typedef std::pair<double,point3_t> coefs_t;
-typedef coefs_t (*evalCurveAtTime) (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_c1::flag , c0_dc0_dc1_c1::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);
-
-/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
+/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t,
+ * defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-// TODOin C++ 10, all these methods could be just one function :)
-coefs_t evaluateCurveAtTime(const ProblemData& pData, 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);
-typedef coefs_t (*evalAccCurveAtTime) (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_c1::flag , c0_dc0_dc1_c1::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);
-
-/** @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, 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");
- }
-}
+/** @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);
-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_c1::flag , c0_dc0_dc1_c1::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);
/**
- * @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");
- }
-}
-
-typedef std::vector<waypoint6_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_c1::flag , c0_dc0_dc1_c1::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);
+std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T);
/**
- * @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
+ * @brief computeWwaypoints compute the constant waypoints of w(t)
+ * defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<waypoint6_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<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T);
-typedef coefs_t (*compFinalAccP) (const ProblemData& pData,double T);
-typedef std::map<ConstraintFlag,compFinalAccP > T_compFinalAccP;
-typedef T_compFinalAccP::const_iterator CIT_compFinalAccP;
-static const T_compFinalAccP compFinalAccPs = boost::assign::map_list_of
- (c0_dc0_c1::flag , c0_dc0_c1::computeFinalAccelerationPoint)
- (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalAccelerationPoint);
+coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T);
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T)
-{
- CIT_compFinalAccP cit = compFinalAccPs.find(pData.constraints_.flag_);
- if(cit != compFinalAccPs.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_c1::flag , c0_dc0_dc1_c1::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");
- }
-}
-
-void computeFinalAcceleration(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- if(pData.constraints_.flag_& END_ACC){
- res.ddc1_ = pData.ddc1_;
- }else{
- coefs_t a = computeFinalAccelerationPoint(pData,T);
- res.ddc1_ = a.first*res.x + a.second;
- }
-}
-
-void computeFinalVelocity(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- if(pData.constraints_.flag_& END_VEL)
- res.dc1_ = pData.dc1_;
- else{
- coefs_t v = computeFinalVelocityPoint(pData,T);
- res.dc1_ = v.first*res.x + v.second;
- }
-}
-
-
-}
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e48483216e9650dc85ecaa9b9463c22c73617788..2103d671bc31e1ea5c1e0e54209caecdb5598be7 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -12,21 +12,29 @@ SET(LIBRARY_NAME ${PROJECT_NAME})
SET(${LIBRARY_NAME}_SOURCES
${INCLUDE_DIR}/bezier-com-traj/config.hh
+ ${INCLUDE_DIR}/bezier-com-traj/definitions.hh
${INCLUDE_DIR}/bezier-com-traj/data.hh
+ ${INCLUDE_DIR}/bezier-com-traj/utils.hh
+ ${INCLUDE_DIR}/bezier-com-traj/flags.hh
${INCLUDE_DIR}/bezier-com-traj/solve.hh
${INCLUDE_DIR}/bezier-com-traj/common_solve_methods.hh
+ ${INCLUDE_DIR}/bezier-com-traj/common_solve_methods.inl
+ ${INCLUDE_DIR}/bezier-com-traj/cost/costfunction_definition.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_definition.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
- ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
${INCLUDE_DIR}/solver/eiquadprog-fast.hpp
common_solve_methods.cpp
+ costfunction_definition.cpp
eiquadprog-fast.cpp
- solve_transition.cpp
+ computeCOMTraj.cpp
solve_0_step.cpp
+ utils.cpp
+ waypoints_definition.cpp
)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index bc8d663932c221ab5d086bc9fa60191053e74fdc..e030a3510ac0bde629694e310560eb8e8b65c841 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -1,38 +1,10 @@
#include <bezier-com-traj/common_solve_methods.hh>
#include <solver/eiquadprog-fast.hpp>
-
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
namespace bezier_com_traj
{
-
-
-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;
-}
-
-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;
-}
-
-
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);
@@ -197,7 +169,6 @@ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX H, Cref_vector
ResultData res;
res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
res.x = x;
- //std::cout<<"quad_prog status : "<<status<<std::endl;
if(res.success_)
{
assert (!(is_nan(x)));
@@ -207,12 +178,9 @@ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX H, Cref_vector
}
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree)
+ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init)
{
- std::vector<spline::Bern<double> > res;
- for (unsigned int i =0; i <= degree; ++i)
- res.push_back(spline::Bern<double>(degree,i));
- return res;
+ return solve(Ab.first,Ab.second,Hg.first,Hg.second, init);
}
} // namespace bezier_com_traj
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e6f2480fa684b45e6a8d4f4ea86ab2c74d6f69c
--- /dev/null
+++ b/src/computeCOMTraj.cpp
@@ -0,0 +1,339 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/solve.hh>
+#include <bezier-com-traj/common_solve_methods.hh>
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <bezier-com-traj/cost/costfunction_definition.hh>
+
+#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
+
+#include <limits>
+#include <algorithm>
+
+#ifndef QHULL
+#define QHULL 1
+#endif
+
+const int numCol = 3;
+
+
+namespace bezier_com_traj
+{
+typedef waypoint3_t waypoint_t;
+typedef std::pair<double,point3_t> coefs_t;
+
+std::vector<waypoint6_t> computeDiscretizedWwaypoints(const ProblemData& pData,double T, const T_time& timeArray)
+{
+ std::vector<waypoint6_t> wps = computeWwaypoints(pData,T);
+ std::vector<waypoint6_t> res;
+ std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size()-1);
+ double t, b;
+ for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
+ {
+ waypoint6_t w = initwp<waypoint6_t>();
+ 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);
+ }
+ 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<MatrixXX,VectorX>(A,b);
+}
+
+std::pair<MatrixXX,VectorX> dynamicStabilityConstraints(const MatrixXX& mH,const VectorX& h,const Vector3& g,const waypoint6_t& w){
+ int dimH = (int)(mH.rows());
+ MatrixXX A(dimH,numCol);
+ 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<MatrixXX,VectorX>(A,b);
+}
+
+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);
+ }
+ }
+ 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;
+}
+
+void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH, VectorX& h, int& dimH)
+{
+ VectorX hrow;
+ centroidal_dynamics::MatrixXX Hrow;
+ phase.contactPhase_->getPolytopeInequalities(Hrow,hrow);
+ 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 waypoint6_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,numCol) = Ab_stab.first;
+ b.segment(id_rows,dimH) = Ab_stab.second;
+ id_rows += dimH ;
+}
+
+void switchContactPhase(const ProblemData& pData,
+ MatrixXX& A, VectorX& b,
+ MatrixXX& mH, VectorX& h,
+ const waypoint6_t& wp_w, ContactData& phase,
+ const long int id_phase, long int& id_rows, int& dimH)
+{
+ phase = pData.contacts_[id_phase];
+ 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;
+ std::vector<waypoint6_t> wps_w = computeDiscretizedWwaypoints(pData,t_total,timeArray);
+
+ std::size_t id_phase = 0;
+ ContactData phase = pData.contacts_[id_phase];
+ const Vector3& g = phase.contactPhase_->m_gravity;
+
+ //reference to current stability matrix
+ MatrixXX mH; VectorX h; int dimH;
+ updateH(pData, 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], phase, id_phase, id_rows, dimH);
+ }
+ }
+ }
+ return id_rows;
+}
+
+void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
+ const double t_total, const std::vector<int>& stepIdForPhase, long int& id_rows)
+{
+ std::size_t id_phase = 0;
+ std::vector<coefs_t> wps_c = computeDiscretizedWaypoints<point_t>(pData,t_total,timeArray);
+ ContactData phase = pData.contacts_[id_phase];
+ long int current_size;
+ id_phase = 0;
+ phase = pData.contacts_[id_phase];
+ // 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)phase.kin_.rows();
+ if(current_size > 0)
+ {
+ A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
+ b.segment(id_rows,current_size) = phase.kin_ - (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;
+ phase = pData.contacts_[id_phase];
+ // the current waypoint must have the constraints of both phases. So we add it again :
+ // TODO : filter for redunbdant constraints ...
+ current_size = phase.kin_.rows();
+ if(current_size > 0)
+ {
+ A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
+ b.segment(id_rows,current_size) = phase.kin_ - (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;
+ }
+ }
+}
+
+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,numCol); 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 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)
+{
+ MatrixXX H(numCol,numCol);
+ VectorX g(numCol);
+ 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>();
+ 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 computeCOMTraj(const ProblemData& pData, const VectorX& Ts,const Vector3& init_guess,
+ const int pointsPerPhase, const double /*feasability_treshold*/)
+{
+ assert(Ts.size() == pData.contacts_.size());
+ double T = Ts.sum();
+ T_time timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
+ std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,T,timeArray);
+ std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData,Ts,T,timeArray);
+ VectorX x = VectorX::Zero(numCol); x.head<3>() = init_guess;
+ ResultData resQp = solve(Ab,Hg, x);
+#if QHULL
+ if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
+#endif
+ return genTraj(resQp, pData, T);
+}
+
+ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
+ const double timeStep)
+{
+ assert(Ts.size() == pData.contacts_.size());
+ double T = Ts.sum();
+ T_time timeArray = computeDiscretizedTime(Ts,timeStep);
+ std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,T,timeArray);
+ std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData,Ts,T,timeArray);
+ VectorX x = VectorX::Zero(numCol);
+ ResultData resQp = solve(Ab,Hg, x);
+#if QHULL
+ if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
+#endif
+ return genTraj(resQp, pData, T);
+}
+
+
+} // namespace
diff --git a/src/costfunction_definition.cpp b/src/costfunction_definition.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57e0d70c29878a0e39f7e19aa81833dd4619faee
--- /dev/null
+++ b/src/costfunction_definition.cpp
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2017, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#include <bezier-com-traj/cost/costfunction_definition.hh>
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <bezier-com-traj/common_solve_methods.hh>
+
+namespace bezier_com_traj
+{
+namespace cost {
+//cost : min distance between x and midPoint :
+void computeCostMidPoint(const ProblemData& pData,const VectorX& /*Ts*/, const double /*T*/,
+ const T_time& /*timeArray*/, MatrixXX& H, VectorX& g)
+{
+ // 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_);
+}
+
+// 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;
+ }
+}
+
+
+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
diff --git a/src/eiquadprog-fast.cpp b/src/eiquadprog-fast.cpp
index 4bf7ec4749bbff7b0f7687eacdd7f15b36af4d5b..e17a7e5d4af3e7df190f830bc642b7503775b88b 100644
--- a/src/eiquadprog-fast.cpp
+++ b/src/eiquadprog-fast.cpp
@@ -89,11 +89,11 @@ namespace tsid
int& iq,
double& R_norm)
{
- int nVars = J.rows();
+ long int nVars = J.rows();
#ifdef TRACE_SOLVER
std::cout << "Add constraint " << iq << '/';
#endif
- int j, k;
+ long int j, k;
double cc, ss, h, t1, t2, xny;
#ifdef OPTIMIZE_ADD_CONSTRAINT
@@ -176,7 +176,7 @@ namespace tsid
int l)
{
- int nVars = R.rows();
+ const long int nVars = R.rows();
#ifdef TRACE_SOLVER
std::cout << "Delete constraint " << l << ' ' << iq;
#endif
@@ -270,9 +270,9 @@ namespace tsid
const VectorXd & ci0,
VectorXd & x)
{
- const int nVars = g0.size();
- const int nEqCon = ce0.size();
- const int nIneqCon = ci0.size();
+ 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);
diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp
index 569bd8d69faf159dc88b34123fece29f77340430..73af740c740db860390f267ec9af2a61279f507d 100644
--- a/src/solve_0_step.cpp
+++ b/src/solve_0_step.cpp
@@ -4,6 +4,7 @@
*/
#include <bezier-com-traj/solve.hh>
+#include <bezier-com-traj/utils.hh>
#include <bezier-com-traj/common_solve_methods.hh>
using namespace bezier_com_traj;
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
deleted file mode 100644
index c30e0b74a6df51891c48ea4a64279b4543df3db5..0000000000000000000000000000000000000000
--- a/src/solve_transition.cpp
+++ /dev/null
@@ -1,520 +0,0 @@
-/*
- * Copyright 2018, LAAS-CNRS
- * Author: Pierre Fernbach
- */
-
-#include <bezier-com-traj/solve.hh>
-#include <bezier-com-traj/common_solve_methods.hh>
-#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
-#include <limits>
-#include <bezier-com-traj/waypoints/waypoints_definition.hh>
-
-#ifndef QHULL
-#define QHULL 1
-#endif
-
-#ifndef USE_SLACK
-#define USE_SLACK 0
-#endif
-
-
-namespace bezier_com_traj
-{
-typedef waypoint3_t waypoint_t;
-typedef std::pair<double,point3_t> coefs_t;
-
-ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init)
-{
- return solve(Ab.first,Ab.second,Hg.first,Hg.second, init);
-}
-
-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();
-}
-
-
-
-
-
-
-
-/**
- * @brief computeDiscretizedTime build an array of discretized points in time, such that there is the same number of point in each phase. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size()
- * @param phaseTimings
- * @param pointsPerPhase
- * @return
- */
-std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,const int pointsPerPhase ){
- std::vector<double> timeArray;
- double t = 0;
- double t_total = 0;
- for(int i = 0 ; i < phaseTimings.size() ; ++i)
- t_total += phaseTimings[i];
-
- 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(t);
- }
- }
- timeArray.pop_back();
- timeArray.push_back(t_total); // avoid numerical errors
- return timeArray;
-}
-
-std::vector<coefs_t> computeDiscretizedWaypoints(const ProblemData& pData,double T,const std::vector<double>& timeArray){
- //int numStep = int(T / timeStep);
- std::vector<coefs_t> wps;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // evaluate curve work with normalized time !
- double t;
- for (std::size_t i = 0 ; i<timeArray.size() ; ++i ){
- t = timeArray[i] / T;
- if(t>1)
- t=1.;
- wps.push_back(evaluateCurveAtTime(pData,pi,t));
- }
- return wps;
-}
-
-
-std::vector<waypoint6_t> computeDiscretizedWwaypoints(const ProblemData& pData,double T,const std::vector<double>& timeArray){
- std::vector<waypoint6_t> wps = computeWwaypoints(pData,T);
- std::vector<waypoint6_t> res;
- std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size()-1);
- double t;
- double b;
- for(std::size_t i = 0 ; i < timeArray.size() ; ++i){
- waypoint6_t w = initwp<waypoint6_t>();
- for (std::size_t j = 0 ; j < wps.size() ; ++j){
- t = timeArray[i]/T;
- if(t>1.)
- t=1.;
- b = berns[j](t);
- w.first +=b*(wps[j].first );
- w.second+=b*(wps[j].second);
- }
- res.push_back(w);
- }
- return res;
-}
-
-
-
-std::vector<coefs_t> computeDiscretizedAccelerationWaypoints(const ProblemData& pData,double T,const std::vector<double>& timeArray){
- //int numStep = int(T / timeStep);
- std::vector<coefs_t> wps;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- double t;
- for (std::size_t i = 0 ; i<timeArray.size() ; ++i ){
- t = timeArray[i] / T;
- if(t>1)
- t=1.;
- wps.push_back(evaluateAccelerationCurveAtTime(pData,pi,T,t));
- }
- return wps;
-}
-
- 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<MatrixXX,VectorX>(A,b);
-}
-
-std::pair<MatrixXX,VectorX> dynamicStabilityConstraints(const MatrixXX& mH,const VectorX& h,const Vector3& g,const waypoint6_t& w){
- int dimH = (int)(mH.rows());
- int numCol;
- #if USE_SLACK
- numCol = 4;
- #else
- numCol = 3;
- #endif
- MatrixXX A(dimH,numCol);
- 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);
- #if USE_SLACK
- A.block(0,3,dimH,1) = VectorX::Ones(dimH); // slack variable, after normalization !!
- #endif
- return std::make_pair<MatrixXX,VectorX>(A,b);
-}
-
-std::pair<MatrixXX,VectorX> staticStabilityConstraints(const MatrixXX& mH,const VectorX& h, const Vector3& g,const coefs_t& c){
- int dimH = (int)(mH.rows());
- MatrixXX A(dimH,4);
- VectorX b(dimH);
- A.block(0,0,dimH,3) = mH.block(0,3,dimH,3) * c.first * skew(g);
- b = h + mH.block(0,0,dimH,3)*g - mH.block(0,3,dimH,3)*g.cross(c.second);
- // add 1 for the slack variable :
- A.block(0,3,dimH,1) = VectorX::Ones(dimH);
- // Normalize(A,b);
- return std::make_pair<MatrixXX,VectorX>(A,b);
-}
-
-void compareStabilityMethods(const MatrixXX& mH,const VectorX& h,const Vector3& g,const coefs_t& c,const coefs_t& ddc,const waypoint6_t& w){
- std::pair<MatrixXX,VectorX> Ab_cross,Ab_w;
- /*
- Vector3 wd;
- wd = c.cross(ddc) + g.cross(c);
- std::cout<<"wu cross : "<<ddc.first<<std::endl;
- std::cout<<"wu : "<<w.first.block<3,3>(0,0)<<std::endl;
- error_wu = ddc.first - w.first(0,0);
- */
-
- Ab_cross = dynamicStabilityConstraints_cross(mH,h,g,c,ddc);
- Ab_w = dynamicStabilityConstraints(mH,h,g,w);
- Normalize(Ab_cross.first,Ab_cross.second);
- Normalize(Ab_w.first,Ab_w.second);
-
-
- MatrixXX A_error = Ab_cross.first - Ab_w.first;
- VectorX b_error = Ab_cross.second - Ab_w.second;
- double A_error_norm = A_error.lpNorm<Eigen::Infinity>();
- double b_error_norm = b_error.lpNorm<Eigen::Infinity>();
- std::cout<<" max a error : "<<A_error_norm<<" ; b : "<<b_error_norm<<std::endl;
- std::cout<<"A error : "<<std::endl<<A_error<<std::endl;
- std::cout<<"b error : "<<std::endl<<b_error<<std::endl;
-
- assert(A_error_norm < 1e-4 && b_error_norm < 1e-4 && "Both method didn't find the same results.");
-}
-
-
-std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,const VectorX& Ts,const int pointsPerPhase,VectorX& /*constraints_equivalence*/){
- // compute the list of discretized waypoint :
- double t_total = 0.;
- for(int i = 0 ; i < Ts.size() ; ++i)
- t_total+=Ts[i];
- // Compute all the discretized wayPoint
- //std::cout<<"total time : "<<t_total<<std::endl;
- std::vector<double> timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
- std::vector<coefs_t> wps_c = computeDiscretizedWaypoints(pData,t_total,timeArray);
- std::vector<waypoint6_t> wps_w = computeDiscretizedWwaypoints(pData,t_total,timeArray);
- //std::cout<<" number of discretized waypoints c: "<<wps_c.size()<<std::endl;
- //std::cout<<" number of discretized waypoints w: "<<wps_w.size()<<std::endl;
- assert(/*wps_c.size() == wps_ddc.size() &&*/ wps_w.size() == wps_c.size());
- 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);
-
- assert(stepIdForPhase.back() == (wps_c.size()-1)); // -1 because the first one is the index (start at 0) and the second is the size
- // compute the total number of inequalities (to initialise A and b)
- int numCol;
- #if USE_SLACK
- numCol = 4;
- #else
- numCol = 3;
- #endif
- long int num_ineq = 0;
- long int num_stab_ineq = 0;
- long int num_kin_ineq = 0;
- int numStepForPhase;
- centroidal_dynamics::MatrixXX Hrow;
- VectorX h;
- MatrixXX H,mH;
- for(int i = 0 ; i < Ts.size() ; ++i){
- pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow,h);
- numStepForPhase = pointsPerPhase;
- if(i > 0 )
- ++numStepForPhase; // because at the switch point between phases we add the constraints of both phases.
- //std::cout<<"constraint size : Kin = "<<pData.contacts_[i].kin_.rows()<<" ; stab : "<<Hrow.rows()<<" times "<<numStepForPhase<<" steps"<<std::endl;
- num_stab_ineq += Hrow.rows() * numStepForPhase;
- if(i == Ts.size()-1)
- --numStepForPhase; // we don't consider kinematics constraints for the last point (because it's independant of x)
- num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
- }
- num_ineq = num_stab_ineq + num_kin_ineq;
- if(pData.constraints_.constraintAcceleration_){
- num_ineq += 2*3 *(wps_c.size()) ; // upper and lower bound on acceleration for each discretized waypoint (exept the first one)
- }
- //std::cout<<"total of inequalities : "<<num_ineq<<std::endl;
- // init constraints matrix :
- MatrixXX A = MatrixXX::Zero(num_ineq,numCol); // 3 + 1 : because of the slack constraints
- VectorX b(num_ineq);
- std::pair<MatrixXX,VectorX> Ab_stab;
-
- long int id_rows = 0;
- long int current_size;
-
- std::size_t id_phase = 0;
- ContactData phase = pData.contacts_[id_phase];
- // compute some constant matrice for the current phase :
- const Vector3& g = phase.contactPhase_->m_gravity;
- //std::cout<<"g = "<<g.transpose()<<std::endl;
- //std::cout<<"mass = "<<phase.contactPhase_->m_mass<<std::endl;
- //const Matrix3 gSkew = bezier_com_traj::skew(g);
- phase.contactPhase_->getPolytopeInequalities(Hrow,h);
- H = -Hrow;
- H.rowwise().normalize();
- int dimH = (int)(H.rows());
- mH = phase.contactPhase_->m_mass * H;
- if(pData.constraints_.reduce_h_ > 0 )
- h -= VectorX::Ones(h.rows())*pData.constraints_.reduce_h_;
-
- // 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 :
-
- Ab_stab = dynamicStabilityConstraints(mH,h,g,wps_w[id_step]);
- //compareStabilityMethods(mH,h,g,wps_c[id_step],wps_ddc[id_step],wps_w[id_step]);
- A.block(id_rows,0,dimH,numCol) = Ab_stab.first;
- b.segment(id_rows,dimH) = Ab_stab.second;
- id_rows += dimH ;
-
- // check if we are going to switch phases :
- for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
- if((int)id_step == stepIdForPhase[i]){
- // switch to phase i
- id_phase=i+1;
- phase = pData.contacts_[id_phase];
- phase.contactPhase_->getPolytopeInequalities(Hrow,h);
- H = -Hrow;
- H.rowwise().normalize();
- dimH = (int)(H.rows());
- mH = phase.contactPhase_->m_mass * H;
- if(pData.constraints_.reduce_h_ > 0 )
- h -= VectorX::Ones(h.rows())*pData.constraints_.reduce_h_;
- // the current waypoint must have the constraints of both phases. So we add it again :
- // TODO : filter for redunbdant constraints ...
- // add stability constraints :
- Ab_stab = dynamicStabilityConstraints(mH,h,g,wps_w[id_step]);
- //compareStabilityMethods(mH,h,g,wps_c[id_step],wps_ddc[id_step],wps_w[id_step]);
- A.block(id_rows,0,dimH,numCol) = Ab_stab.first;
- b.segment(id_rows,dimH) = Ab_stab.second;
- id_rows += dimH ;
- }
- }
- }
-
- id_phase = 0;
- phase = pData.contacts_[id_phase];
- // 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
- if(id_step != timeArray.size()-1){ // we don't consider kinematics constraints for the last point (because it's independant of x)
- current_size = (int)phase.kin_.rows();
- A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
- b.segment(id_rows,current_size) = phase.kin_ - (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;
- phase = pData.contacts_[id_phase];
- // the current waypoint must have the constraints of both phases. So we add it again :
- // TODO : filter for redunbdant constraints ...
- current_size = phase.kin_.rows();
- A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
- b.segment(id_rows,current_size) = phase.kin_ - (phase.Kin_*wps_c[id_step].second);
- id_rows += current_size;
- }
- }
- }
-
- if(pData.constraints_.constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
- std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints(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::cout<<"id rows : "<<id_rows<<" ; total rows : "<<A.rows()<<std::endl;
- assert(id_rows == (A.rows()) && "The constraints matrices were not fully filled.");
- return std::make_pair(A,b);
-}
-
-void addSlackInCost( MatrixXX& H, VectorX& g){
- H(3,3) = 1e9;
- g[3] = 0;
-}
-
-//cost : min distance between x and midPoint :
-void computeCostMidPoint(const ProblemData& pData, 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;
-}
-
-void computeCostMinAcceleration(const ProblemData& pData,const VectorX& Ts, const int pointsPerPhase, MatrixXX& H, VectorX& g){
- double t_total = 0.;
- for(int i = 0 ; i < Ts.size() ; ++i)
- t_total+=Ts[i];
- std::vector<double> timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
- std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints(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;
- }
-}
-
-//cost : min distance between end velocity and the one computed by planning
-void computeCostEndVelocity(const ProblemData& pData,const double T, MatrixXX& H, VectorX& g){
- 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_);
-}
-
-
-void computeBezierCurve(const ProblemData& pData, const double T, ResultDataCOMTraj& res)
-{
- std::vector<Vector3> wps;
-
- std::vector<Vector3> pi = computeConstantWaypoints(pData,T);
- size_t i = 0;
- if(pData.constraints_.flag_ & INIT_POS ){
- wps.push_back(pi[i]);
- i++;
- if(pData.constraints_.flag_ & INIT_VEL){
- wps.push_back(pi[i]);
- i++;
- if(pData.constraints_.flag_ & INIT_ACC){
- wps.push_back(pi[i]);
- i++;
- }
- }
- }
- wps.push_back(res.x);
- i++;
- if(pData.constraints_.flag_ & END_ACC){
- assert(pData.constraints_.flag_ & END_VEL && "You cannot constraint final acceleration if final velocity is not constrained.");
- wps.push_back(pi[i]);
- i++;
- }
- if(pData.constraints_.flag_ & END_VEL){
- assert(pData.constraints_.flag_ & END_POS && "You cannot constraint final velocity if final position is not constrained.");
- wps.push_back(pi[i]);
- i++;
- }
- if(pData.constraints_.flag_ & END_POS){
- wps.push_back(pi[i]);
- i++;
- }
-
- res.c_of_t_ = bezier_t (wps.begin(), wps.end(),T);
-}
-
-double analyseSlack(const VectorX& slack,const VectorX& constraint_equivalence ){
- //TODO
- assert(slack.size() == constraint_equivalence.size() && "slack variables and constraints equivalence should have the same size." );
- //std::cout<<"slack : "<<slack<<std::endl;
- std::cout<<"list of violated constraints : "<<std::endl;
- double previous_id = -1;
- for(long int i = 0 ; i < slack.size() ; ++i){
- if((slack[i]*slack[i]) > std::numeric_limits<double>::epsilon()){
- if(constraint_equivalence[i] != previous_id){
- std::cout<<"step "<<constraint_equivalence[i]<<std::endl;
- previous_id = constraint_equivalence[i];
- }
- }
- }
- //return (slack.squaredNorm())/(slack.size());
- return slack.lpNorm<Eigen::Infinity>();
-}
-
-ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const Vector3& init_guess,const int pointsPerPhase){
- assert(pData.contacts_.size() ==2 || pData.contacts_.size() ==3);
- assert(Ts.size() == pData.contacts_.size());
- double T = 0;
- for(int i = 0 ; i < Ts.size() ; ++i)
- T+=Ts[i];
- // bool fail = true;
- int sizeX;
- #if USE_SLACK
- sizeX = 4;
- #else
- sizeX = 3;
- #endif
- MatrixXX H(sizeX,sizeX);
- VectorX g(sizeX);
- ResultDataCOMTraj res;
- VectorX constraint_equivalence;
- std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,pointsPerPhase,constraint_equivalence);
- //computeCostMidPoint(pData,H,g);
- if(pData.constraints_.flag_ & END_VEL)
- computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
- else
- computeCostEndVelocity(pData,T,H,g);
-
- #if USE_SLACK
- addSlackInCost(H,g);
- #endif
-
- //std::cout<<"Init = "<<std::endl<<init_guess.transpose()<<std::endl;
-
- VectorX x = VectorX::Zero(sizeX); // 3 + slack
- x.head<3>() = init_guess;
-
- // rewriting 0.5 || Dx -d ||^2 as x'Hx + g'x
- ResultData resQp = solve(Ab.first,Ab.second,H,g, x);
- bool success;
- #if USE_SLACK
- double feasability = fabs(resQp.x[3]);
- //std::cout<<"feasability : "<<feasability<<" treshold = "<<feasability_treshold<<std::endl;
- success = feasability<=feasability_treshold;
- #else
- success = resQp.success_;
- #endif
-
- if(success)
- {
- res.success_ = true;
- res.x = resQp.x.head<3>();
- computeBezierCurve (pData,T,res);
- computeFinalVelocity(pData,T,res);
- computeFinalAcceleration(pData,T,res);
- //std::cout<<"Solved, success "<<" x = ["<<res.x[0]<<","<<res.x[1]<<","<<res.x[2]<<"]"<<std::endl;
- #if QHULL
- printQHullFile(Ab,resQp.x,"bezier_wp.txt");
- #endif
- }else{
- //std::cout<<"Over treshold, x = ["<<resQp.x[0]<<","<<resQp.x[1]<<","<<resQp.x[2]<<"]"<<std::endl;
- }
-
- //std::cout<<"Final cost : "<<resQp.cost_<<std::endl;
- return res;
-}
-
-
-} // namespace
diff --git a/src/utils.cpp b/src/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6bb2b453d42a7d5a677fab6ba81a2d415415f6e
--- /dev/null
+++ b/src/utils.cpp
@@ -0,0 +1,106 @@
+/*
+ * Copyright 2017, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#include <bezier-com-traj/utils.hh>
+namespace bezier_com_traj
+{
+
+
+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;
+}
+
+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;
+}
+
+
+T_time computeDiscretizedTime(const VectorX& phaseTimings, const 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;
+}
+
+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));
+ }
+ 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();
+}
+
+
+} // namespace bezier_com_traj
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dfa681b495c8449d91e7db354a13545d31aa4046
--- /dev/null
+++ b/src/waypoints_definition.cpp
@@ -0,0 +1,167 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#ifndef BEZIER_COM_TRAJ_WP_DEF_H
+#define BEZIER_COM_TRAJ_WP_DEF_H
+
+#include <bezier-com-traj/data.hh>
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh>
+#include "boost/assign.hpp"
+
+
+
+namespace bezier_com_traj{
+/**
+ * This file is used to choose the correct expressions of the curves waypoints, depending on the options set in ProblemData.constraints
+ */
+
+
+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_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);
+
+/** @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");
+ }
+}
+
+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_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);
+
+/** @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 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_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);
+/**
+ * @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");
+ }
+}
+
+typedef std::vector<waypoint6_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_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);
+
+/**
+ * @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
+ * @param pData
+ * @param T
+ * @return
+ */
+ std::vector<waypoint6_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_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");
+ }
+}
+
+}
+
+#endif
diff --git a/tests/test-transition-quasiStatic.cc b/tests/test-transition-quasiStatic.cc
index bd763c58f0c9ce3f8de941d0334aa181ebe46765..8534cc567b28d8a4d71d406f937544b2108a1f72 100644
--- a/tests/test-transition-quasiStatic.cc
+++ b/tests/test-transition-quasiStatic.cc
@@ -38,7 +38,7 @@ BOOST_AUTO_TEST_CASE(single_support){
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::solveIntersection(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 :
@@ -63,7 +63,7 @@ BOOST_AUTO_TEST_CASE(quasiStatic_exist){
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::solveIntersection(Ab,Hg,init);
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
BOOST_CHECK(res.success_);
}
}
@@ -83,7 +83,7 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upX){
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::solveIntersection(Ab,Hg,init);
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
BOOST_CHECK(! res.success_);
}
}
@@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downX){
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::solveIntersection(Ab,Hg,init);
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
BOOST_CHECK(! res.success_);
}
}
@@ -121,7 +121,7 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downY){
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::solveIntersection(Ab,Hg,init);
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
BOOST_CHECK(! res.success_);
}
}
@@ -140,7 +140,7 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upY){
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::solveIntersection(Ab,Hg,init);
+ bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab,Hg,init);
BOOST_CHECK(! res.success_);
}
}
diff --git a/tests/test-transition.cc b/tests/test-transition.cc
index 342daee237fe70ee86e66abf17998f47f41776dc..90fed234299bcd7c937cd51cc2588b7092c4682a 100644
--- a/tests/test-transition.cc
+++ b/tests/test-transition.cc
@@ -67,7 +67,7 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
int pointsPerPhase = 5;
// check if transition is feasible (should be)
- bezier_com_traj::ResultDataCOMTraj res = bezier_com_traj::solveOnestep(pData,Ts,init,pointsPerPhase);
+ bezier_com_traj::ResultDataCOMTraj res = bezier_com_traj::computeCOMTraj(pData,Ts,init,pointsPerPhase);
if(shouldFail){
BOOST_CHECK(!res.success_);
return;
@@ -213,11 +213,11 @@ BOOST_AUTO_TEST_CASE(quasi_static){
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin0,stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solveIntersection(Ab_first,Hg,init);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin2,stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solveIntersection(Ab_second,Hg,init);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
BOOST_CHECK(res_second.success_);
delete cDataMid.contactPhase_;
}
@@ -230,6 +230,13 @@ BOOST_AUTO_TEST_CASE(transition){
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_noDc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
@@ -588,11 +595,11 @@ BOOST_AUTO_TEST_CASE(quasi_static_0){
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solveIntersection(Ab_first,Hg,init);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solveIntersection(Ab_second,Hg,init);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
BOOST_CHECK(res_second.success_);
delete cDataFirst.contactPhase_;
@@ -614,11 +621,11 @@ BOOST_AUTO_TEST_CASE(quasi_static_1){
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solveIntersection(Ab_first,Hg,init);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
BOOST_CHECK(! res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solveIntersection(Ab_second,Hg,init);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
BOOST_CHECK(! res_second.success_);
delete cDataFirst.contactPhase_;
@@ -640,11 +647,11 @@ BOOST_AUTO_TEST_CASE(quasi_static_2){
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
- bezier_com_traj::ResultData res_first = bezier_com_traj::solveIntersection(Ab_first,Hg,init);
+ bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
- bezier_com_traj::ResultData res_second = bezier_com_traj::solveIntersection(Ab_second,Hg,init);
+ bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
BOOST_CHECK(res_second.success_);
diff --git a/tests/test-zero-step-capturability.cpp b/tests/test-zero-step-capturability.cpp
index 0639dda9eeb35438ca3133ab6e2707d5a2723811..f05e5b63ca768a23e944b0c23103c3e3603ed79a 100644
--- a/tests/test-zero-step-capturability.cpp
+++ b/tests/test-zero-step-capturability.cpp
@@ -1,12 +1,13 @@
/*
- * Copyright 2015, LAAS-CNRS
- * Author: Andrea Del Prete
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
*/
#include <vector>
#include <iostream>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <bezier-com-traj/solve.hh>
+#include <bezier-com-traj/data.hh>
#include <math.h>
using namespace centroidal_dynamics;
@@ -117,9 +118,9 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
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 = 0; i < num_steps; ++i)
+ for (int i = 1; i < num_steps; ++i)
{
- double dt = double(i) / float(num_steps) * T;
+ 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));
@@ -128,7 +129,7 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
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 << "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;
@@ -138,7 +139,7 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
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;
+ std::cout << "1 / T * T " << 1/ (T * T) << std::endl;*/
return false;
}
}
@@ -147,7 +148,7 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
int main()
{
- srand(time(NULL));
+ srand((unsigned int)time(NULL));
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
@@ -198,12 +199,14 @@ int main()
{
bezier_com_traj::ContactData data;
data.contactPhase_ = &solver_PP;
- bezier_com_traj::ProblemData pData;
+ 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.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)
@@ -229,17 +232,26 @@ int main()
succCont = false;
}
// try discretize
- bezier_com_traj::ResultDataCOMTraj rData0 = bezier_com_traj::solve0step(pData,Ts,DISCRETIZATION_STEP);
+ 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(T / DISCRETIZATION_STEP));
+ checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(round(T / DISCRETIZATION_STEP)));
+ //checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T / DISCRETIZATION_STEP));
}
else
{
+ VectorX t2(1); t2 << Ts[0];
+ 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;
@@ -273,8 +285,8 @@ int main()
}
else
{
- //if(succCont || succDisc ||succdLbool)
- // std::cout << "error: Solver discretize with angular momentum failed while a solution was found for another case" << std::endl;
+ 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();