From 03adb392e5b72dc61b11cbb7a61f1bfd7f006875 Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Mon, 19 Mar 2018 14:44:11 +0100
Subject: [PATCH] cost functions called by flags
---
CMakeLists.txt | 11 +++
.../bezier-com-traj/common_solve_methods.hh | 4 +-
.../cost/costfunction_definition.hh | 53 +++++++++++
include/bezier-com-traj/data.hh | 89 ++-----------------
include/bezier-com-traj/definitions.hh | 58 ++++++++++++
include/bezier-com-traj/flags.hh | 52 +++++++++++
include/bezier-com-traj/solve.hh | 4 +-
.../waypoints/waypoints_c0_dc0_c1.hh | 6 ++
.../waypoints/waypoints_c0_dc0_dc1_c1.hh | 5 ++
.../waypoints/waypoints_c0_dc0_ddc0_c1.hh | 6 ++
.../waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh | 6 ++
.../waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh | 6 ++
.../waypoints/waypoints_definition.hh | 4 +
src/CMakeLists.txt | 3 +
src/solve_transition.cpp | 31 ++++++-
tests/test-zero-step-capturability.cpp | 6 +-
16 files changed, 252 insertions(+), 92 deletions(-)
create mode 100644 include/bezier-com-traj/cost/costfunction_definition.hh
create mode 100644 include/bezier-com-traj/definitions.hh
create mode 100644 include/bezier-com-traj/flags.hh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed1c885..ff96906 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,11 +52,22 @@ find_package (Spline REQUIRED)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/data.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/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_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/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 ac5270f..538e3ec 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.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_COMMON_SOLVE_H
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 0000000..319432a
--- /dev/null
+++ b/include/bezier-com-traj/cost/costfunction_definition.hh
@@ -0,0 +1,53 @@
+/*
+ * 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 {
+
+void computeCostMinAcceleration(const ProblemData& pData,const VectorX& Ts,
+ const int pointsPerPhase, MatrixXX& H, VectorX& g);
+
+
+typedef void (*costCompute) (const ProblemData&, const VectorX&, const int, MatrixXX&, VectorX&);
+typedef std::map<CostFunction,costCompute > T_costCompute;
+static const T_costCompute costs = boost::assign::map_list_of
+ (ACCELERATION, computeCostMinAcceleration);
+
+/** @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 int pointsPerPhase, MatrixXX& H, VectorX& g)
+{
+ T_costCompute::const_iterator cit = costs.find(pData.costFunction_);
+ if(cit != costs.end())
+ return cit->second(pData, Ts, pointsPerPhase, H, g);
+ else
+ {
+ std::cout<<"Unknown cost function"<<std::endl;
+ throw std::runtime_error("Unknown cost function");
+ }
+}
+
+
+} // namespace cost
+} // namespace bezier_com_traj
+
+#endif
diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index b9a7187..02f2ffb 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -1,54 +1,23 @@
/*
- * 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 <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
- */
- 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()
@@ -66,46 +35,8 @@ namespace bezier_com_traj
VectorX ang_;
};
- enum BEZIER_COM_TRAJ_DLLAPI CostFunction{
- ACCELERATION = 0x00001,
- DISTANCE_TRAVELED = 0x00002,
- UNKNOWN_COST = 0x00004
- };
-
- 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));}
-
struct BEZIER_COM_TRAJ_DLLAPI Constraints
{
-
Constraints()
: flag_(INIT_POS | INIT_VEL | END_VEL | END_POS)
, constraintAcceleration_(true)
@@ -124,7 +55,6 @@ namespace bezier_com_traj
bool constraintAcceleration_;
double maxAcceleration_;
double reduce_h_;
-
};
@@ -148,10 +78,6 @@ namespace bezier_com_traj
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;
struct BEZIER_COM_TRAJ_DLLAPI ResultData
{
ResultData():
@@ -177,7 +103,6 @@ namespace bezier_com_traj
x = (other.x);
return *this;
}
-
bool success_;
double cost_;
VectorX x;
diff --git a/include/bezier-com-traj/definitions.hh b/include/bezier-com-traj/definitions.hh
new file mode 100644
index 0000000..e1a8077
--- /dev/null
+++ b/include/bezier-com-traj/definitions.hh
@@ -0,0 +1,58 @@
+/*
+ * 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;
+
+/**
+* @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 0000000..ba56d66
--- /dev/null
+++ b/include/bezier-com-traj/flags.hh
@@ -0,0 +1,52 @@
+/*
+ * 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,
+ UNKNOWN_COST = 0x00004
+ };
+
+ 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 173adca..cf5a98d 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
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 46119de..9e6b20a 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{
@@ -112,3 +116,5 @@ coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
}
}
+
+#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 bb06d23..9ea80ae 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{
@@ -129,3 +132,5 @@ coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
}
}
+
+#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 5c9c264..fea48f0 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{
@@ -129,3 +133,5 @@ coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
}
}
+
+#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 a394cdc..abf3d58 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{
@@ -147,3 +151,5 @@ coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
}
}
+
+#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 868bf75..fdb96e1 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{
@@ -159,3 +163,5 @@ coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
}
}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index 96a27d6..e2d5675 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -3,6 +3,8 @@
* 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>
@@ -176,3 +178,5 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T)
}
}
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e484832..cadf7b6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -12,9 +12,12 @@ 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/flags.hh
${INCLUDE_DIR}/bezier-com-traj/solve.hh
${INCLUDE_DIR}/bezier-com-traj/common_solve_methods.hh
+ ${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
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 0a45a95..c3c8d82 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -5,9 +5,12 @@
#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 <bezier-com-traj/waypoints/waypoints_definition.hh>
#ifndef QHULL
#define QHULL 1
@@ -438,10 +441,11 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
ResultDataCOMTraj res;
VectorX constraint_equivalence;
std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,pointsPerPhase,constraint_equivalence);
- if(pData.constraints_.flag_ & END_VEL)
+ /*if(pData.constraints_.flag_ & END_VEL)
computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
else
- computeCostEndVelocity(pData,T,H,g);
+ computeCostEndVelocity(pData,T,H,g);*/
+ cost::genCostFunction(pData,Ts,pointsPerPhase,H,g);
#if USE_SLACK
addSlackInCost(H,g);
@@ -474,5 +478,26 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
return res;
}
+namespace cost {
+
+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;
+ }
+}
+}
+
} // namespace
diff --git a/tests/test-zero-step-capturability.cpp b/tests/test-zero-step-capturability.cpp
index 0639dda..fc3d141 100644
--- a/tests/test-zero-step-capturability.cpp
+++ b/tests/test-zero-step-capturability.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright 2015, LAAS-CNRS
- * Author: Andrea Del Prete
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
*/
#include <vector>
@@ -147,7 +147,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;
--
GitLab