Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • jchemin/hpp-bezier-com-traj
  • pfernbac/hpp-bezier-com-traj
  • gsaurel/hpp-bezier-com-traj
  • humanoid-path-planner/hpp-bezier-com-traj
4 results
Show changes
Hide whitespace changes
Inline Side-by-side
src/utils.cpp
View file @ 2ef67540
......@@ -14,20 +14,26 @@ waypoint_t initwp(const size_t rows, const size_t cols) {
}
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2) {
if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
if (w1.second.rows() != w2.second.rows() ||
w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first + w2.first, w1.second + w2.second);
}
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2) {
if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
if (w1.second.rows() != w2.second.rows() ||
w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first - w2.first, w1.second - w2.second);
}
waypoint_t operator*(const double k, const waypoint_t& w) { return waypoint_t(k * w.first, k * w.second); }
waypoint_t operator*(const double k, const waypoint_t& w) {
return waypoint_t(k * w.first, k * w.second);
}
waypoint_t operator*(const waypoint_t& w, const double k) { return waypoint_t(k * w.first, k * w.second); }
waypoint_t operator*(const waypoint_t& w, const double k) {
return waypoint_t(k * w.first, k * w.second);
}
template <>
waypoint9_t initwp<waypoint9_t>() {
......@@ -64,13 +70,16 @@ Matrix3 skew(point_t_tC x) {
return res;
}
std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) {
std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(
const unsigned int degree) {
std::vector<ndcurves::Bern<double> > res;
for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(ndcurves::Bern<double>(degree, i));
for (unsigned int i = 0; i <= (unsigned int)degree; ++i)
res.push_back(ndcurves::Bern<double>(degree, i));
return res;
}
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase) {
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
const unsigned int pointsPerPhase) {
T_time timeArray;
double t = 0;
double t_total = phaseTimings.sum();
......@@ -83,17 +92,20 @@ T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned i
}
}
timeArray.pop_back();
timeArray.push_back(std::make_pair(t_total, phaseTimings.size() - 1)); // avoid numerical errors
timeArray.push_back(std::make_pair(
t_total, phaseTimings.size() - 1)); // avoid numerical errors
return timeArray;
}
T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep) {
T_time computeDiscretizedTime(const VectorX& phaseTimings,
const double timeStep) {
T_time timeArray;
double t = 0;
double currentTiming = 0.;
for (int i = 0; i < phaseTimings.size(); ++i) {
assert(timeStep * 2 <= phaseTimings[i] &&
"Time step too high: should allow to contain at least 2 points per phase");
"Time step too high: should allow to contain at least 2 points per "
"phase");
t = currentTiming;
currentTiming += phaseTimings[i];
while (t < currentTiming) {
......@@ -105,20 +117,21 @@ T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep
return timeArray;
}
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint, const std::string& fileName,
bool clipZ) {
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
const std::string& fileName, bool clipZ) {
std::ofstream file;
using std::endl;
std::string path("/local/fernbac/bench_iros18/constraints_obj/");
path.append(fileName);
file.open(path.c_str(), std::ios::out | std::ios::trunc);
file << "3 1" << endl;
file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2] << endl;
file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2]
<< endl;
file << "4" << endl;
clipZ ? file << Ab.first.rows() + 2 << endl : file << Ab.first.rows() << endl;
for (int i = 0; i < Ab.first.rows(); ++i) {
file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t" << Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001
<< endl;
file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t"
<< Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001 << endl;
}
if (clipZ) {
file << "\t" << 0 << "\t" << 0 << "\t" << 1. << "\t" << -3. << endl;
......
src/waypoints_definition.cpp
View file @ 2ef67540
......@@ -7,7 +7,6 @@
#define BEZIER_COM_TRAJ_WP_DEF_H
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
......@@ -18,13 +17,14 @@
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include "boost/assign.hpp"
namespace bezier_com_traj {
/**
* This file is used to choose the correct expressions of the curves waypoints, depending on the options set in
* ProblemData.constraints
* This file is used to choose the correct expressions of the curves waypoints,
* depending on the options set in ProblemData.constraints
*/
int dimVar(const ProblemData& pData) {
......@@ -44,22 +44,28 @@ typedef std::pair<double, point3_t> coefs_t;
typedef coefs_t (*evalCurveAtTime)(const std::vector<point_t>& pi, double t);
typedef std::map<ConstraintFlag, evalCurveAtTime> T_evalCurveAtTime;
typedef T_evalCurveAtTime::const_iterator CIT_evalCurveAtTime;
static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of(
c0_dc0_c1::flag, c0_dc0_c1::evaluateCurveAtTime)(c0_dc0_dc1::flag, c0_dc0_dc1::evaluateCurveAtTime)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateCurveAtTime)(c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateCurveAtTime)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateCurveAtTime)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)(c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
static const T_evalCurveAtTime evalCurveAtTimes =
boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::evaluateCurveAtTime)(
c0_dc0_dc1::flag, c0_dc0_dc1::evaluateCurveAtTime)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateCurveAtTime)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateCurveAtTime)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateCurveAtTime)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)(
c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
* one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODOin C++ 10, all these methods could be just one function :)
coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t) {
coefs_t evaluateCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi, double t) {
CIT_evalCurveAtTime cit = evalCurveAtTimes.find(pData.constraints_.flag_);
if (cit != evalCurveAtTimes.end())
return cit->second(pi, t);
......@@ -69,27 +75,36 @@ coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>
}
}
typedef coefs_t (*evalAccCurveAtTime)(const std::vector<point_t>& pi, double T, double t);
typedef coefs_t (*evalAccCurveAtTime)(const std::vector<point_t>& pi, double T,
double t);
typedef std::map<ConstraintFlag, evalAccCurveAtTime> T_evalAccCurveAtTime;
typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
static const T_evalAccCurveAtTime evalAccCurveAtTimes =
boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_dc1::flag, c0_dc0_dc1::evaluateAccelerationCurveAtTime)(c0_dc0_dc1_c1::flag,
c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)(
boost::assign::map_list_of(c0_dc0_c1::flag,
c0_dc0_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_dc1::flag, c0_dc0_dc1::evaluateAccelerationCurveAtTime)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
c0_dc0_ddc0_dc1_c1::flag,
c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the
* waypoint pi and one free waypoint (x)
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
* on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t) {
CIT_evalAccCurveAtTime cit = evalAccCurveAtTimes.find(pData.constraints_.flag_);
coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi,
double T, double t) {
CIT_evalAccCurveAtTime cit =
evalAccCurveAtTimes.find(pData.constraints_.flag_);
if (cit != evalAccCurveAtTimes.end())
return cit->second(pi, T, t);
else {
......@@ -98,23 +113,33 @@ coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vec
}
}
typedef waypoint_t (*evalCurveWaypointAtTime)(const std::vector<point_t>& pi, double t);
typedef std::map<ConstraintFlag, evalCurveWaypointAtTime> T_evalCurveWaypointAtTime;
typedef waypoint_t (*evalCurveWaypointAtTime)(const std::vector<point_t>& pi,
double t);
typedef std::map<ConstraintFlag, evalCurveWaypointAtTime>
T_evalCurveWaypointAtTime;
typedef T_evalCurveWaypointAtTime::const_iterator CIT_evalCurveWaypointAtTime;
static const T_evalCurveWaypointAtTime evalCurveWaypointAtTimes = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime);
static const T_evalCurveWaypointAtTime evalCurveWaypointAtTimes =
boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
* one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODOin C++ 10, all these methods could be just one function :)
waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t) {
CIT_evalCurveWaypointAtTime cit = evalCurveWaypointAtTimes.find(pData.constraints_.flag_);
waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData,
const std::vector<point_t>& pi,
double t) {
CIT_evalCurveWaypointAtTime cit =
evalCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalCurveWaypointAtTimes.end())
return cit->second(pi, t);
else {
......@@ -122,24 +147,35 @@ waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vect
throw std::runtime_error("Current constraints set are not implemented");
}
}
typedef waypoint_t (*evalVelCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime> T_evalVelCurveWaypointAtTime;
typedef T_evalVelCurveWaypointAtTime::const_iterator CIT_evalVelCurveWaypointAtTime;
static const T_evalVelCurveWaypointAtTime evalVelCurveWaypointAtTimes = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime);
typedef waypoint_t (*evalVelCurveWaypointAtTime)(const std::vector<point_t>& pi,
const double T, double t);
typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime>
T_evalVelCurveWaypointAtTime;
typedef T_evalVelCurveWaypointAtTime::const_iterator
CIT_evalVelCurveWaypointAtTime;
static const T_evalVelCurveWaypointAtTime evalVelCurveWaypointAtTimes =
boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
* one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODOin C++ 10, all these methods could be just one function :)
waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const double T,
const std::vector<point_t>& pi, double t) {
CIT_evalVelCurveWaypointAtTime cit = evalVelCurveWaypointAtTimes.find(pData.constraints_.flag_);
waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData,
const double T,
const std::vector<point_t>& pi,
double t) {
CIT_evalVelCurveWaypointAtTime cit =
evalVelCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalVelCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
......@@ -147,24 +183,36 @@ waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const d
throw std::runtime_error("Current constraints set are not implemented");
}
}
typedef waypoint_t (*evalAccCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime> T_evalAccCurveWaypointAtTime;
typedef T_evalAccCurveWaypointAtTime::const_iterator CIT_evalAccCurveWaypointAtTime;
static const T_evalAccCurveWaypointAtTime evalAccCurveWaypointAtTimes = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime);
typedef waypoint_t (*evalAccCurveWaypointAtTime)(const std::vector<point_t>& pi,
const double T, double t);
typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime>
T_evalAccCurveWaypointAtTime;
typedef T_evalAccCurveWaypointAtTime::const_iterator
CIT_evalAccCurveWaypointAtTime;
static const T_evalAccCurveWaypointAtTime evalAccCurveWaypointAtTimes =
boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::
evaluateAccelerationCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::
evaluateAccelerationCurveWaypointAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
* one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODOin C++ 10, all these methods could be just one function :)
waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, const double T,
const std::vector<point_t>& pi, double t) {
CIT_evalAccCurveWaypointAtTime cit = evalAccCurveWaypointAtTimes.find(pData.constraints_.flag_);
waypoint_t evaluateAccelerationCurveWaypointAtTime(
const ProblemData& pData, const double T, const std::vector<point_t>& pi,
double t) {
CIT_evalAccCurveWaypointAtTime cit =
evalAccCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalAccCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
......@@ -172,24 +220,35 @@ waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, con
throw std::runtime_error("Current constraints set are not implemented");
}
}
typedef waypoint_t (*evalJerkCurveWaypointAtTime)(const std::vector<point_t>& pi, const double T, double t);
typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime> T_evalJerkCurveWaypointAtTime;
typedef T_evalJerkCurveWaypointAtTime::const_iterator CIT_evalJerkCurveWaypointAtTime;
static const T_evalJerkCurveWaypointAtTime evalJerkCurveWaypointAtTimes = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime);
typedef waypoint_t (*evalJerkCurveWaypointAtTime)(
const std::vector<point_t>& pi, const double T, double t);
typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime>
T_evalJerkCurveWaypointAtTime;
typedef T_evalJerkCurveWaypointAtTime::const_iterator
CIT_evalJerkCurveWaypointAtTime;
static const T_evalJerkCurveWaypointAtTime evalJerkCurveWaypointAtTimes =
boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and
* one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODOin C++ 10, all these methods could be just one function :)
waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const double T, const std::vector<point_t>& pi,
waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData,
const double T,
const std::vector<point_t>& pi,
double t) {
CIT_evalJerkCurveWaypointAtTime cit = evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
CIT_evalJerkCurveWaypointAtTime cit =
evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
if (cit != evalJerkCurveWaypointAtTimes.end())
return cit->second(pi, T, t);
else {
......@@ -202,24 +261,30 @@ typedef std::vector<point_t> (*compConsWp)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compConsWp> T_compConsWp;
typedef T_compConsWp::const_iterator CIT_compConsWp;
static const T_compConsWp compConsWps = boost::assign::map_list_of(
c0_dc0_c1::flag, c0_dc0_c1::computeConstantWaypoints)(c0_dc0_dc1::flag, c0_dc0_dc1::computeConstantWaypoints)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeConstantWaypoints)(c0_dc0_ddc0::flag,
c0_dc0_ddc0::computeConstantWaypoints)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeConstantWaypoints)(c0_dc0_ddc0_dc1_c1::flag,
c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeConstantWaypoints);
c0_dc0_c1::flag, c0_dc0_c1::computeConstantWaypoints)(
c0_dc0_dc1::flag, c0_dc0_dc1::computeConstantWaypoints)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::computeConstantWaypoints)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeConstantWaypoints)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeConstantWaypoints);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
* final states
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
* defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
CIT_compConsWp cit = compConsWps.find(pData.constraints_.flag_);
if (cit != compConsWps.end())
return cit->second(pData, T);
......@@ -229,12 +294,14 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T
}
}
bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData, double T) {
bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(
const ProblemData& pData, double T) {
const int DIM_POINT = 3; // FIXME : always true ??
const int DIM_VAR = dimVar(pData);
std::vector<point_t> pts = computeConstantWaypoints(pData, T);
bezier_wp_t::t_point_t wps;
for (std::vector<point_t>::const_iterator pit = pts.begin(); pit != pts.end(); ++pit) {
for (std::vector<point_t>::const_iterator pit = pts.begin(); pit != pts.end();
++pit) {
waypoint_t w = initwp(DIM_POINT, DIM_VAR);
if (pit->isZero()) {
w.first = MatrixXX::Identity(DIM_POINT, DIM_VAR);
......@@ -246,23 +313,27 @@ bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData
return wps;
}
typedef std::vector<waypoint_t> (*compVelWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::vector<waypoint_t> (*compVelWp)(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compVelWp> T_compVelWp;
typedef T_compVelWp::const_iterator CIT_compVelWp;
static const T_compVelWp compVelWps = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityWaypoints);
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityWaypoints);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
* final states
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
* defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi) {
std::vector<waypoint_t> computeVelocityWaypoints(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
if (cit != compVelWps.end())
return cit->second(pData, T, pi);
......@@ -272,23 +343,27 @@ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, doubl
}
}
typedef std::vector<waypoint_t> (*compAccWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::vector<waypoint_t> (*compAccWp)(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compAccWp> T_compAccWp;
typedef T_compAccWp::const_iterator CIT_compAccWp;
static const T_compAccWp compAccWps = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
* final states
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
* defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi) {
std::vector<waypoint_t> computeAccelerationWaypoints(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
if (cit != compAccWps.end())
return cit->second(pData, T, pi);
......@@ -298,22 +373,27 @@ std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData, d
}
}
typedef std::vector<waypoint_t> (*compJerkWp)(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::vector<waypoint_t> (*compJerkWp)(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compJerkWp> T_compJerkWp;
typedef T_compJerkWp::const_iterator CIT_compJerkWp;
static const T_compJerkWp compJerkWps = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeJerkWaypoints);
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeJerkWaypoints);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and
* final states
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
* defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
std::vector<waypoint_t> computeJerkWaypoints(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compJerkWp cit = compJerkWps.find(pData.constraints_.flag_);
if (cit != compJerkWps.end())
return cit->second(pData, T, pi);
......@@ -326,16 +406,20 @@ std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, double T,
typedef bezier_wp_t::t_point_t (*compWp)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compWp> T_compWp;
typedef T_compWp::const_iterator CIT_compWp;
static const T_compWp compWps = boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::computeWwaypoints)(
c0_dc0_dc1::flag, c0_dc0_dc1::computeWwaypoints)(c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeWwaypoints)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::computeWwaypoints)(c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeWwaypoints)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeWwaypoints)(c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
static const T_compWp compWps = boost::assign::map_list_of(
c0_dc0_c1::flag, c0_dc0_c1::computeWwaypoints)(
c0_dc0_dc1::flag, c0_dc0_dc1::computeWwaypoints)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeWwaypoints)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::computeWwaypoints)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeWwaypoints)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeWwaypoints)(
c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
/**
* @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final
* states
* @brief computeWwaypoints compute the constant waypoints of w(t) defined by
* the constraints on initial and final states
* @param pData
* @param T
* @return
......@@ -354,12 +438,14 @@ typedef coefs_t (*compFinalVelP)(const ProblemData& pData, double T);
typedef std::map<ConstraintFlag, compFinalVelP> T_compFinalVelP;
typedef T_compFinalVelP::const_iterator CIT_compFinalVelP;
static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of(
c0_dc0_c1::flag, c0_dc0_c1::computeFinalVelocityPoint)(c0_dc0_dc1::flag, c0_dc0_dc1::computeFinalVelocityPoint)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeFinalVelocityPoint)(c0_dc0_ddc0::flag,
c0_dc0_ddc0::computeFinalVelocityPoint)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeFinalVelocityPoint)(c0_dc0_ddc0_dc1_c1::flag,
c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)(
c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
c0_dc0_c1::flag, c0_dc0_c1::computeFinalVelocityPoint)(
c0_dc0_dc1::flag, c0_dc0_dc1::computeFinalVelocityPoint)(
c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeFinalVelocityPoint)(
c0_dc0_ddc0::flag, c0_dc0_ddc0::computeFinalVelocityPoint)(
c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeFinalVelocityPoint)(
c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)(
c0_dc0_ddc0_ddc1_dc1_c1::flag,
c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
......@@ -371,23 +457,27 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
}
typedef std::pair<MatrixXX, VectorX> (*compVelCost)(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi);
typedef std::pair<MatrixXX, VectorX> (*compVelCost)(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag, compVelCost> T_compVelCost;
typedef T_compVelCost::const_iterator CIT_compVelCost;
static const T_compVelCost compVelCosts = boost::assign::map_list_of(
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityCost)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityCost)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag, c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityCost);
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityCost)(
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityCost)(
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityCost);
/**
* @brief computeVelocityCost the matrices H and g defining a cost that minimise the integral of the squared velocity
* @brief computeVelocityCost the matrices H and g defining a cost that minimise
* the integral of the squared velocity
* @param pData
* @param T
* @return
*/
std::pair<MatrixXX, VectorX> computeVelocityCost(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi) {
std::pair<MatrixXX, VectorX> computeVelocityCost(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
CIT_compVelCost cit = compVelCosts.find(pData.constraints_.flag_);
if (cit != compVelCosts.end())
return cit->second(pData, T, pi);
......
tests/test-bezier-symbolic.cpp
View file @ 2ef67540
......@@ -17,19 +17,21 @@
// <http://www.gnu.org/licenses/>.
#define BOOST_TEST_MODULE transition
#include <ndcurves/bezier_curve.h>
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"
#include <ndcurves/bezier_curve.h>
using namespace bezier_com_traj;
const double T = 1.5;
ProblemData buildPData(
const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
ProblemData buildPData(const centroidal_dynamics::EquilibriumAlgorithm algo =
centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
ProblemData pData;
pData.c0_ = Vector3(0, 0.5, 5.);
pData.c1_ = Vector3(2, -0.5, 5.);
......@@ -44,18 +46,25 @@ ProblemData buildPData(
positions.block<1, 3>(0, 0) = Vector3(0, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
pData.contacts_.push_back(
new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second, algo)));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
pData.contacts_.push_back(new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second, algo)));
return pData;
}
std::vector<point_t> generate_wps() { return computeConstantWaypoints(buildPData(), T); }
std::vector<point_t> generate_wps() {
return computeConstantWaypoints(buildPData(), T);
}
bezier_wp_t::t_point_t generate_wps_symbolic() { return computeConstantWaypointsSymbolic(buildPData(), T); }
bezier_wp_t::t_point_t generate_wps_symbolic() {
return computeConstantWaypointsSymbolic(buildPData(), T);
}
VectorX eval(const waypoint_t& w, const point_t& x) { return w.first * x + w.second; }
VectorX eval(const waypoint_t& w, const point_t& x) {
return w.first * x + w.second;
}
void vectorEqual(const VectorX& a, const VectorX& b, const double EPS = 1e-14) {
BOOST_CHECK_EQUAL(a.size(), b.size());
......@@ -177,12 +186,14 @@ BOOST_AUTO_TEST_CASE(symbolic_split_c_bench) {
clock_t s0, e0;
std::pair<bezier_wp_t, bezier_wp_t> splitted = c_sym.split(0.5);
s0 = clock();
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
for (std::vector<double>::const_iterator cit = values.begin();
cit != values.end(); ++cit) {
splitted = c_sym.split(*cit);
}
e0 = clock();
std::cout << "Time required to split a c curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
std::cout << "Time required to split a c curve : "
<< ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
<< std::endl;
}
......@@ -220,12 +231,14 @@ BOOST_AUTO_TEST_CASE(symbolic_split_w_bench) {
clock_t s0, e0;
std::pair<bezier_wp_t, bezier_wp_t> splitted = w.split(0.5);
s0 = clock();
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
for (std::vector<double>::const_iterator cit = values.begin();
cit != values.end(); ++cit) {
splitted = w.split(*cit);
}
e0 = clock();
std::cout << "Time required to split a w curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
std::cout << "Time required to split a w curve : "
<< ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
<< std::endl;
}
......
tests/test-transition-quasi-static.cpp
View file @ 2ef67540
......@@ -18,9 +18,10 @@
#define BOOST_TEST_MODULE transition - quasiStatic
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"
BOOST_AUTO_TEST_SUITE(quasiStatic)
......@@ -31,19 +32,20 @@ BOOST_AUTO_TEST_CASE(single_support) {
normal << 0, 0, 1;
MatrixX3 position(1, 3);
position << 0, 0, 0;
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
BOOST_CHECK(res.success_);
// sample positions for second foot inside kinematics constraints and check for feasibility :
// sample positions for second foot inside kinematics constraints and check
// for feasibility :
}
BOOST_AUTO_TEST_CASE(quasiStatic_exist) {
// sample positions for second foot inside kinematics constraints and check for feasibility :
// Vector3 firstLeg_n(0,0,1);
// Vector3 firstLeg_p(0,0,0);
// sample positions for second foot inside kinematics constraints and check
// for feasibility : Vector3 firstLeg_n(0,0,1); Vector3 firstLeg_p(0,0,0);
for (size_t i = 0; i < 500; ++i) {
MatrixX3 normal(1, 3);
......@@ -55,7 +57,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_exist) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
......@@ -74,7 +77,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upX) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
......@@ -93,7 +97,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downX) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
......@@ -112,7 +117,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_downY) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
......@@ -131,7 +137,8 @@ BOOST_AUTO_TEST_CASE(quasiStatic_empty_upY) {
normal.block<1, 3>(0, 0) = Vector3(0, 0, 1);
position.block<1, 3>(0, 0) = Vector3(x, y, 0.);
std::pair<MatrixX3, VectorX> Ab = generateConstraints(normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<MatrixX3, VectorX> Ab = generateConstraints(
normal, position, Matrix3::Identity(), Vector3::Zero());
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
bezier_com_traj::ResultData res = bezier_com_traj::solve(Ab, Hg, init);
......
tests/test-transition.cpp
View file @ 2ef67540
......@@ -18,15 +18,17 @@
#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"
#define NOMINAL_COM_HEIGHT 0.795
std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings, const int pointsPerPhase) {
std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,
const int pointsPerPhase) {
std::vector<double> timeArray;
double t = 0;
double t_total = 0;
......@@ -44,15 +46,21 @@ std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings, const in
return timeArray;
}
bool check_constraints(const bezier_com_traj::ContactData& contactPhase, Vector3 c, Vector3 dc, Vector3 ddc) {
BOOST_CHECK(verifyKinematicConstraints(std::make_pair(contactPhase.Kin_, contactPhase.kin_), c));
BOOST_CHECK(verifyStabilityConstraintsDLP(*contactPhase.contactPhase_, c, dc, ddc));
BOOST_CHECK(verifyStabilityConstraintsPP(*contactPhase.contactPhase_, c, dc, ddc));
bool check_constraints(const bezier_com_traj::ContactData& contactPhase,
Vector3 c, Vector3 dc, Vector3 ddc) {
BOOST_CHECK(verifyKinematicConstraints(
std::make_pair(contactPhase.Kin_, contactPhase.kin_), c));
BOOST_CHECK(
verifyStabilityConstraintsDLP(*contactPhase.contactPhase_, c, dc, ddc));
BOOST_CHECK(
verifyStabilityConstraintsPP(*contactPhase.contactPhase_, c, dc, ddc));
return true;
}
void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX& Ts,
const bezier_com_traj::ResultDataCOMTraj& res, const int pointsPerPhase, const double t_total) {
void discretized_check(const bezier_com_traj::ProblemData& pData,
const VectorX& Ts,
const bezier_com_traj::ResultDataCOMTraj& res,
const int pointsPerPhase, const double t_total) {
// check if timing is respected
std::vector<double> timings = computeDiscretizedTime(Ts, pointsPerPhase);
BOOST_CHECK_EQUAL(timings.back(), t_total);
......@@ -72,8 +80,10 @@ void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX&
// check if each discretized point is feasible :
std::vector<int>
stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
for (int i = 0; i < Ts.size(); ++i) stepIdForPhase.push_back(pointsPerPhase * (i + 1) - 1);
stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase
// i / first step of phase i+1 (overlap)
for (int i = 0; i < Ts.size(); ++i)
stepIdForPhase.push_back(pointsPerPhase * (i + 1) - 1);
std::size_t id_phase = 0;
bezier_com_traj::ContactData phase = pData.contacts_[id_phase];
......@@ -96,8 +106,9 @@ void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX&
}
}
void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shouldFail = false,
bool continuous = false, bool test_continuous = true) {
void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,
bool shouldFail = false, bool continuous = false,
bool test_continuous = true) {
BOOST_CHECK_EQUAL(pData.contacts_.size(), Ts.size());
double t_total = 0;
......@@ -109,7 +120,8 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
bezier_com_traj::ResultDataCOMTraj res;
if (continuous) {
// testing all available solvers
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
solvers::SOLVER_QUADPROG);
if (pData.representation_ == bezier_com_traj::FORCE) {
/*bezier_com_traj::ResultDataCOMTraj res2 =
bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG_SPARSE);
......@@ -119,14 +131,17 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
std::cout << " x " << res.cost_ << std::endl;
std::cout << " x2 " << res2.cost_ << std::endl;
discretized_check(pData,Ts,res2,pointsPerPhase,t_total);
BOOST_CHECK(!res.success_ || (res.x.head<3>() - res2.x.head<3>()).norm() < EPSILON);
BOOST_CHECK(!res.success_ || (res.x.head<3>() -
res2.x.head<3>()).norm() < EPSILON);
}*/
#ifdef USE_GLPK_SOLVER
// clock_t s0,e0;
// s0 = clock();
bezier_com_traj::ResultDataCOMTraj res3 = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
bezier_com_traj::ResultDataCOMTraj res3 =
bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
// e0 = clock();
// std::cout<<"Time to perform full lp : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
// std::cout<<"Time to perform full lp :
// "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
BOOST_CHECK(res.success_ == res3.success_);
if (res3.success_) {
discretized_check(pData, Ts, res3, pointsPerPhase, t_total);
......@@ -139,13 +154,16 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
if (shouldFail) {
BOOST_CHECK(!res.success_);
if (test_continuous) {
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
solvers::SOLVER_QUADPROG);
BOOST_CHECK(!res.success_);
pData.representation_ = bezier_com_traj::FORCE;
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_QUADPROG);
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1,
solvers::SOLVER_QUADPROG);
BOOST_CHECK(!res.success_);
#ifdef USE_GLPK_SOLVER
res = bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
res =
bezier_com_traj::computeCOMTraj(pData, Ts, -1, solvers::SOLVER_GLPK);
BOOST_CHECK(!res.success_);
#endif
}
......@@ -155,7 +173,8 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts, bool shou
BOOST_CHECK(res.success_);
if (res.success_) discretized_check(pData, Ts, res, pointsPerPhase, t_total);
if (continuous && pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION) {
if (continuous &&
pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION) {
pData.representation_ = bezier_com_traj::FORCE;
check_transition(pData, Ts, shouldFail, true);
} else if (!continuous && test_continuous)
......@@ -180,11 +199,16 @@ bezier_com_traj::ContactData phase0_flat() {
positions.block<1, 3>(0, 0) = Vector3(0, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 =
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin = stackConstraints(
kin1,
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -196,10 +220,13 @@ bezier_com_traj::ContactData phase1_flat() {
MatrixX3 normals(1, 3), positions(1, 3);
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(0, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin =
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -213,12 +240,16 @@ bezier_com_traj::ContactData phase2_flat() {
positions.block<1, 3>(0, 0) = Vector3(0.3, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin =
stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 =
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin = stackConstraints(
kin1,
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -243,11 +274,14 @@ bezier_com_traj::ProblemData gen_problem_data_flat() {
BOOST_AUTO_TEST_CASE(quasi_static) {
// compute kinematic constraints for the right foot :
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin0 =
stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
ConstraintsPair kin2 =
stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
ConstraintsPair kin1 =
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, -0.1, 0));
ConstraintsPair kin0 = stackConstraints(
kin1,
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0, 0.1, 0)));
ConstraintsPair kin2 = stackConstraints(
kin1,
generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.3, 0.1, 0)));
bezier_com_traj::ContactData cDataMid = phase1_flat();
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
......@@ -256,11 +290,13 @@ BOOST_AUTO_TEST_CASE(quasi_static) {
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin0, stab);
bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
bezier_com_traj::ResultData res_first =
bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin2, stab);
bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
bezier_com_traj::ResultData res_second =
bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataMid.contactPhase_;
}
......@@ -274,7 +310,9 @@ BOOST_AUTO_TEST_CASE(transition) {
BOOST_AUTO_TEST_CASE(transition_noc1) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
bezier_com_traj::INIT_VEL |
bezier_com_traj::END_VEL;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts);
......@@ -284,7 +322,9 @@ BOOST_AUTO_TEST_CASE(transition_no_terminal_constraints) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.contacts_.pop_back();
pData.contacts_.pop_back();
pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::INIT_ACC;
pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
bezier_com_traj::INIT_VEL |
bezier_com_traj::INIT_ACC;
VectorX Ts(1);
Ts << 0.2;
check_transition(pData, Ts, false, false, true);
......@@ -342,7 +382,8 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_noAcc) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.flag_ |=
bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.constraintAcceleration_ = false;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
......@@ -354,7 +395,8 @@ BOOST_AUTO_TEST_CASE(transition_Acc1) {
pData.constraints_.maxAcceleration_ = 1.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts, false, false, false); // fail with continuous formulation
check_transition(pData, Ts, false, false,
false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1) {
......@@ -363,7 +405,8 @@ BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1) {
pData.constraints_.maxAcceleration_ = 1.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts, false, false, false); // fail with continuous formulation
check_transition(pData, Ts, false, false,
false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
......@@ -371,12 +414,14 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2) {
pData.constraints_.maxAcceleration_ = 2.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts, false, false, false); // fail with continuous formulation
check_transition(pData, Ts, false, false,
false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.flag_ |=
bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 2.;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
......@@ -385,11 +430,13 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc05) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.flag_ |=
bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 0.5;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts, false, false, false); // fail with continuous formulation
check_transition(pData, Ts, false, false,
false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_Acc05) {
......@@ -397,7 +444,8 @@ BOOST_AUTO_TEST_CASE(transition_Acc05) {
pData.constraints_.maxAcceleration_ = 0.5;
VectorX Ts(3);
Ts << 0.6, 0.6, 0.6;
check_transition(pData, Ts, false, false, false); // fail with continuous formulation
check_transition(pData, Ts, false, false,
false); // fail with continuous formulation
}
// constraints that should fails :
......@@ -433,7 +481,8 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1) {
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc02) {
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.flag_ |=
bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC;
pData.constraints_.maxAcceleration_ = 0.2;
pData.constraints_.constraintAcceleration_ = true;
VectorX Ts(3);
......@@ -446,7 +495,8 @@ BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE(platform)
// platform : first step :
// (0.35,0.1,0) ; (0.35,-0.1,0) -> (0.775, 0.23, -0.02);(0.35,-0.1,0) (normal : 0.0, -0.423, 0.906)
// (0.35,0.1,0) ; (0.35,-0.1,0) -> (0.775, 0.23, -0.02);(0.35,-0.1,0) (normal :
// 0.0, -0.423, 0.906)
// second step :
// (0.775, 0.23, -0.02);(0.35,-0.1,0) -> (0.775, 0.23, -0.02);(1.15,-0.1,0)
......@@ -462,12 +512,16 @@ bezier_com_traj::ContactData phase0_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.35, 0.1, 0);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0.35, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(0.35, -0.1, 0));
ConstraintsPair kin =
stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, 0.1, 0)));
stackConstraints(kin1, generateKinematicsConstraints(
Matrix3::Identity(), Vector3(0.35, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -479,10 +533,13 @@ bezier_com_traj::ContactData phase1_platform() {
MatrixX3 normals(1, 3), positions(1, 3);
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(0.35, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(0.35, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -496,14 +553,18 @@ bezier_com_traj::ContactData phase2_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(0.35, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(0.35, -0.1, 0));
Eigen::Quaterniond quat =
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(0.35, -0.1, 0));
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
ConstraintsPair kin = stackConstraints(
kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -516,13 +577,16 @@ bezier_com_traj::ContactData phase3_platform() {
normals.block<1, 3>(0, 0) = Vector3(0.0, -0.423, 0.906).normalized();
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
Eigen::Quaterniond quat =
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
ConstraintsPair kin = generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02));
ConstraintsPair kin =
generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -536,14 +600,18 @@ bezier_com_traj::ContactData phase4_platform() {
positions.block<1, 3>(0, 0) = Vector3(0.775, 0.23, -0.02);
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(1.15, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
Eigen::Quaterniond quat =
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(1.15, -0.1, 0));
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(
Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0.0, -0.423, 0.906));
Matrix3 rot = quat.normalized().toRotationMatrix();
ConstraintsPair kin = stackConstraints(kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
ConstraintsPair kin = stackConstraints(
kin1, generateKinematicsConstraints(rot, Vector3(0.775, 0.23, -0.02)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -556,10 +624,13 @@ bezier_com_traj::ContactData phase5_platform() {
normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(0, 0) = Vector3(1.15, -0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(1.15, -0.1, 0));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -574,12 +645,16 @@ bezier_com_traj::ContactData phase6_platform() {
normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
positions.block<1, 3>(1, 0) = Vector3(1.15, 0.1, 0);
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second));
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
cData.contactPhase_ = new centroidal_dynamics::Equilibrium(
ComputeContactCone(contacts.first, contacts.second));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, -0.1, 0));
ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),
Vector3(1.15, -0.1, 0));
ConstraintsPair kin =
stackConstraints(kin1, generateKinematicsConstraints(Matrix3::Identity(), Vector3(1.15, 0.1, 0)));
stackConstraints(kin1, generateKinematicsConstraints(
Matrix3::Identity(), Vector3(1.15, 0.1, 0)));
cData.Kin_ = kin.first;
cData.kin_ = kin.second;
......@@ -642,17 +717,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_0) {
bezier_com_traj::ContactData cDataSecond = phase2_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair kin_second =
std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
bezier_com_traj::ResultData res_first =
bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
bezier_com_traj::ResultData res_second =
bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataFirst.contactPhase_;
......@@ -668,17 +746,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_1) {
bezier_com_traj::ContactData cDataSecond = phase4_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair kin_second =
std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
bezier_com_traj::ResultData res_first =
bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(!res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
bezier_com_traj::ResultData res_second =
bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(!res_second.success_);
delete cDataFirst.contactPhase_;
......@@ -694,17 +775,20 @@ BOOST_AUTO_TEST_CASE(quasi_static_2) {
bezier_com_traj::ContactData cDataSecond = phase6_platform();
ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_, cDataFirst.kin_);
ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair kin_second =
std::make_pair(cDataSecond.Kin_, cDataSecond.kin_);
ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
std::pair<Matrix3, Vector3> Hg = computeCost();
Vector3 init = Vector3::Zero();
ConstraintsPair Ab_first = stackConstraints(kin_first, stab);
bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first, Hg, init);
bezier_com_traj::ResultData res_first =
bezier_com_traj::solve(Ab_first, Hg, init);
BOOST_CHECK(res_first.success_);
ConstraintsPair Ab_second = stackConstraints(kin_second, stab);
bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second, Hg, init);
bezier_com_traj::ResultData res_second =
bezier_com_traj::solve(Ab_second, Hg, init);
BOOST_CHECK(res_second.success_);
delete cDataFirst.contactPhase_;
......
tests/test-zero-step-capturability.cpp
View file @ 2ef67540
......@@ -3,13 +3,14 @@
* Author: Steve Tonneau
*/
#include <vector>
#include <iostream>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <math.h>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <iostream>
#include <vector>
using namespace centroidal_dynamics;
using namespace Eigen;
using namespace std;
......@@ -25,9 +26,12 @@ using namespace std;
#define EPS 1e-3 // required precision
void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, double LX, double LY,
RVector3& CONTACT_POINT_LOWER_BOUNDS, RVector3& CONTACT_POINT_UPPER_BOUNDS,
RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS, MatrixX3& p, MatrixX3& N) {
void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE,
double LX, double LY,
RVector3& CONTACT_POINT_LOWER_BOUNDS,
RVector3& CONTACT_POINT_UPPER_BOUNDS,
RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS,
MatrixX3& p, MatrixX3& N) {
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
p.setZero(4 * N_CONTACTS, 3); // contact points
......@@ -38,27 +42,31 @@ void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, doub
for (unsigned int i = 0; i < N_CONTACTS; i++) {
while (true) // generate contact position
{
uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
contact_pos.row(i));
if (i == 0) break;
collision = false;
for (unsigned int j = 0; j < i - 1; j++)
if ((contact_pos.row(i) - contact_pos.row(j)).norm() < MIN_CONTACT_DISTANCE) collision = true;
if ((contact_pos.row(i) - contact_pos.row(j)).norm() <
MIN_CONTACT_DISTANCE)
collision = true;
if (collision == false) break;
}
// generate contact orientation
uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i), p.middleRows<4>(i * 4),
N.middleRows<4>(i * 4));
// printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1),
// contact_pos(i,2)); printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1),
// contact_rpy(i,2));
generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
p.middleRows<4>(i * 4), N.middleRows<4>(i * 4));
// printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i,
// contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
// printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0),
// contact_rpy(i,1), contact_rpy(i,2));
}
// for(int i=0; i<p.rows(); i++)
// {
// printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
// printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
// printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1),
// p(i,2)); printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
// }
}
......@@ -67,7 +75,8 @@ double fRandom(double fMin, double fMax) {
return fMin + f * (fMax - fMin);
}
bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB, const RVector3& com_UB, Vector3& c0) {
bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB,
const RVector3& com_UB, Vector3& c0) {
int trials = 0;
while (trials < 1000) {
++trials;
......@@ -84,7 +93,8 @@ bool findStaticEqCOMPos(Equilibrium* eq, const RVector3& com_LB, const RVector3&
return false;
}
Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c, const Vector3 ddc, const Vector3 dL) {
Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c,
const Vector3 ddc, const Vector3 dL) {
Vector6 w;
Vector3 w1;
w1 = eq->m_mass * (ddc - eq->m_gravity);
......@@ -93,8 +103,10 @@ Vector6 computew(const Equilibrium* eq, const bezier_com_traj::Vector3 c, const
return w;
}
bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilibrium* eq,
const bezier_com_traj::ResultDataCOMTraj& resData, const double T, const int num_steps = 100) {
bool checkTrajectory(const Vector3& c0, const std::string& solver,
const Equilibrium* eq,
const bezier_com_traj::ResultDataCOMTraj& resData,
const double T, const int num_steps = 100) {
// retrieve H
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
......@@ -112,9 +124,11 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
VectorX res = H * w;
for (long j = 0; j < res.size(); j++)
if (res(j) > 0.0001) {
std::cout << "check trajectory failed for solver " << solver << " " << res(j) << "; iteration " << dt
<< "; numsteps " << num_steps << " c_of_t(dt) " << (c_of_t)(dt).transpose() << " dc_of_t(dt) "
<< c_of_t.derivate(dt, 1).transpose() << " dL_of_t(dt) " << (dL_of_t)(dt).transpose() << std::endl;
std::cout << "check trajectory failed for solver " << solver << " "
<< res(j) << "; iteration " << dt << "; numsteps "
<< num_steps << " c_of_t(dt) " << (c_of_t)(dt).transpose()
<< " dc_of_t(dt) " << c_of_t.derivate(dt, 1).transpose()
<< " dL_of_t(dt) " << (dL_of_t)(dt).transpose() << std::endl;
/*std::cout << "c0 " << c0.transpose() << std::endl;
std::cout << "H row " << H.row(j) << std::endl;
std::cout << "w " << w << std::endl;
......@@ -137,7 +151,8 @@ int main() {
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
/************************************** USER PARAMETERS *******************************/
/************************************** USER PARAMETERS
* *******************************/
unsigned int N_TESTS = 500;
double mass = 55.0;
double mu = 0.3; // friction coefficient
......@@ -153,16 +168,19 @@ int main() {
RPY_UPPER_BOUNDS << +2 * gamma, +2 * gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
/************************************ END USER PARAMETERS *****************************/
/************************************ END USER PARAMETERS
* *****************************/
MatrixX3 p, N;
RVector3 com_LB, com_UB;
Equilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES, false, 10, false);
int succContinuous = 0, succDiscretize = 0, succdL = 0, succDiscretizedL = 0, succKin = 0, succdLKin = 0,
succdLAng = 0;
Equilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES,
false, 10, false);
int succContinuous = 0, succDiscretize = 0, succdL = 0, succDiscretizedL = 0,
succKin = 0, succdLKin = 0, succdLAng = 0;
int numSol = 0;
for (unsigned n_test = 0; n_test < N_TESTS; n_test++) {
generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY,
CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
// compute upper and lower bounds of com positions to test
......@@ -182,7 +200,9 @@ int main() {
bezier_com_traj::ContactData data;
data.contactPhase_ = &solver_PP;
bezier_com_traj::ProblemData pData;
pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
pData.constraints_.flag_ = bezier_com_traj::INIT_POS |
bezier_com_traj::INIT_VEL |
bezier_com_traj::END_VEL;
pData.c0_ = c0;
pData.dc0_ << fRandom(-1., 1.), fRandom(-1., 1.), fRandom(-1., 1.);
// pData.dc0_ *= 1;
......@@ -195,12 +215,14 @@ int main() {
{
pData.useAngularMomentum_ = false;
pData.contacts_.front().kin_ = VectorX::Zero(0);
bool succCont = false, succDisc = false, succdLbool = false, succDiscdL = false, succKinBool = false,
succdLKinBool = false, succdLAngBool = false;
bool succCont = false, succDisc = false, succdLbool = false,
succDiscdL = false, succKinBool = false, succdLKinBool = false,
succdLAngBool = false;
T = 1. + 0.3 * k;
std::vector<double> Ts;
Ts.push_back(T);
bezier_com_traj::ResultDataCOMTraj rData = bezier_com_traj::solve0step(pData, Ts);
bezier_com_traj::ResultDataCOMTraj rData =
bezier_com_traj::solve0step(pData, Ts);
if (rData.success_) {
assert((rData.c_of_t_(0.) - pData.c0_).norm() < 0.0001);
succCont = true;
......@@ -213,19 +235,26 @@ int main() {
// try discretize
VectorX t2(1);
t2 << Ts[0];
bezier_com_traj::ResultDataCOMTraj rData0 = bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
bezier_com_traj::ResultDataCOMTraj rData0 =
bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
if (rData0.success_) {
bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
// bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::computeCOMTraj(pData,t2,tg,(int)(round(T
// / DISCRETIZATION_STEP)));
bezier_com_traj::ResultDataCOMTraj rDatatest =
bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
// bezier_com_traj::ResultDataCOMTraj rDatatest =
// bezier_com_traj::computeCOMTraj(pData,t2,tg,(int)(round(T /
// DISCRETIZATION_STEP)));
assert(rDatatest.success_);
assert((rData0.c_of_t_(0.) - c0).norm() < 0.0001);
assert((rData0.c_of_t_.compute_derivate(1)(0.) - pData.dc0_).norm() < 0.0001);
assert(
(rData0.c_of_t_.compute_derivate(1)(0.) - pData.dc0_).norm() <
0.0001);
succDisc = true;
succDiscretize += 1;
std::string solverName("discretize");
checkTrajectory(c0, solverName, &solver_PP, rData0, T, (int)(round(T / DISCRETIZATION_STEP)));
// checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T / DISCRETIZATION_STEP));
checkTrajectory(c0, solverName, &solver_PP, rData0, T,
(int)(round(T / DISCRETIZATION_STEP)));
// checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T /
// DISCRETIZATION_STEP));
} else {
VectorX t2(1);
t2 << Ts[0];
......@@ -233,13 +262,15 @@ int main() {
bezier_com_traj::computeCOMTraj(pData, t2, DISCRETIZATION_STEP);
assert(!rDatatest.success_);
if (succCont)
std::cout << "error: Solver discretize failed while a solution was found for the continuous case"
std::cout << "error: Solver discretize failed while a solution "
"was found for the continuous case"
<< std::endl;
succDisc = false;
}
pData.useAngularMomentum_ = true;
bezier_com_traj::ResultDataCOMTraj rData1 = bezier_com_traj::solve0step(pData, Ts);
bezier_com_traj::ResultDataCOMTraj rData1 =
bezier_com_traj::solve0step(pData, Ts);
if (rData1.success_) {
assert((rData1.c_of_t_(0.) - c0).norm() < 0.0001);
succdLbool = true;
......@@ -248,28 +279,33 @@ int main() {
checkTrajectory(c0, solverName, &solver_PP, rData1, T);
} else {
// if(succCont)
// std::cout << "error: Solver Ang momentum failed while a solution was found without angular momentum"
// std::cout << "error: Solver Ang momentum failed while a
// solution was found without angular momentum"
// << std::endl;
succDisc = false;
}
bezier_com_traj::ResultDataCOMTraj rData2 = bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
bezier_com_traj::ResultDataCOMTraj rData2 =
bezier_com_traj::solve0step(pData, Ts, DISCRETIZATION_STEP);
if (rData2.success_) {
assert((rData2.c_of_t_(0.) - c0).norm() < 0.0001);
succDiscdL = true;
succDiscretizedL += 1;
std::string solverName("discretize AngularMomentum");
checkTrajectory(c0, solverName, &solver_PP, rData2, T, int(T / DISCRETIZATION_STEP));
checkTrajectory(c0, solverName, &solver_PP, rData2, T,
int(T / DISCRETIZATION_STEP));
} else {
if (succCont || succDisc || succdLbool)
std::cout << "error: Solver discretize with angular momentum failed while a solution was found for "
std::cout << "error: Solver discretize with angular momentum "
"failed while a solution was found for "
"another case"
<< std::endl;
succDiscdL = false;
}
pData.contacts_.front().Kin_ = Eigen::Matrix3d::Identity();
pData.contacts_.front().kin_ = Vector3::Constant(3, 0.5);
bezier_com_traj::ResultDataCOMTraj rData3 = bezier_com_traj::solve0step(pData, Ts);
bezier_com_traj::ResultDataCOMTraj rData3 =
bezier_com_traj::solve0step(pData, Ts);
pData.contacts_.front().Ang_ = Eigen::Matrix3d::Identity();
pData.contacts_.front().ang_ = Vector3::Constant(3, 0.1);
if (rData3.success_) {
......@@ -281,7 +317,8 @@ int main() {
}
pData.contacts_.front().kin_ = VectorX::Zero(0);
bezier_com_traj::ResultDataCOMTraj rData31 = bezier_com_traj::solve0step(pData, Ts);
bezier_com_traj::ResultDataCOMTraj rData31 =
bezier_com_traj::solve0step(pData, Ts);
if (rData31.success_) {
assert((rData31.c_of_t_(0.) - c0).norm() < 0.0001);
succdLAngBool = true;
......@@ -293,7 +330,8 @@ int main() {
pData.useAngularMomentum_ = false;
pData.contacts_.front().ang_ = VectorX::Zero(0);
pData.contacts_.front().kin_ = Vector3::Constant(3, 0.5);
bezier_com_traj::ResultDataCOMTraj rData4 = bezier_com_traj::solve0step(pData, Ts);
bezier_com_traj::ResultDataCOMTraj rData4 =
bezier_com_traj::solve0step(pData, Ts);
if (rData4.success_) {
assert((rData4.c_of_t_(0.) - c0).norm() < 0.0001);
succKinBool = true;
......@@ -307,14 +345,21 @@ int main() {
}
std::cout << "sucesses continunous" << succContinuous << std::endl;
std::cout << "sucesses continuous with Kinematic constraints " << succKin << std::endl;
std::cout << "sucesses continuous with Kinematic constraints " << succKin
<< std::endl;
std::cout << "sucesses discretize " << succDiscretize << std::endl;
std::cout << "sucesses continunous with angular momentum" << succdL << std::endl;
std::cout << "sucesses discretize with angular momentum" << succDiscretizedL << std::endl;
std::cout << "sucesses continunous with angular momentum and angular constraints" << succdLAng << std::endl;
std::cout << "sucesses continunous with angular momentum and kinematic and angular constraints" << succdLKin
std::cout << "sucesses continunous with angular momentum" << succdL
<< std::endl;
std::cout << "sucesses discretize with angular momentum" << succDiscretizedL
<< std::endl;
std::cout
<< "sucesses continunous with angular momentum and angular constraints"
<< succdLAng << std::endl;
std::cout << "sucesses continunous with angular momentum and kinematic and "
"angular constraints"
<< succdLKin << std::endl;
std::cout << "sucesses discretize with angular momentum" << succDiscretizedL
<< std::endl;
std::cout << "sucesses discretize with angular momentum" << succDiscretizedL << std::endl;
std::cout << "eq static point found " << numSol << std::endl;
return 0;
......
tests/test_helper.hh
View file @ 2ef67540
#ifndef TEST_HELPER_HH
#define TEST_HELPER_HH
#include <hpp/bezier-com-traj/solve.hh>
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <boost/test/included/unit_test.hpp>
using bezier_com_traj::Matrix3;
using bezier_com_traj::MatrixX3;
......@@ -48,7 +48,8 @@ std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints() {
return std::make_pair(N, V);
}
std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotation, Vector3 endEffTranslation) {
std::pair<MatrixXX, VectorX> generateKinematicsConstraints(
Matrix3 endEffRotation, Vector3 endEffTranslation) {
std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
MatrixX3 N = NV.first;
MatrixX3 V = NV.second;
......@@ -67,9 +68,12 @@ std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotatio
return std::make_pair(A, b);
}
std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, MatrixX3 positions, double size_X,
std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals,
MatrixX3 positions,
double size_X,
double size_Y) {
// TODO : consider normal != z (see code in rbprm :: stability.cc (or add it as dependency ?)
// TODO : consider normal != z (see code in rbprm :: stability.cc (or add it
// as dependency ?)
BOOST_CHECK(normals.rows() == positions.rows());
MatrixX3 rec_normals(normals.rows() * 4, 3);
......@@ -83,7 +87,8 @@ std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, Matri
for (long int ic = 0; ic < normals.rows(); ++ic) {
for (long int i = 0; i < 4; ++i) {
rec_normals.block<1, 3>(ic * 4 + i, 0) = normals.block<1, 3>(ic, 0);
rec_positions.block<1, 3>(ic * 4 + i, 0) = positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
rec_positions.block<1, 3>(ic * 4 + i, 0) =
positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
}
}
return std::make_pair(rec_normals, rec_positions);
......@@ -91,16 +96,20 @@ std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, Matri
centroidal_dynamics::Equilibrium ComputeContactCone(
MatrixX3 normals, MatrixX3 positions,
const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
centroidal_dynamics::Equilibrium contactCone("test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES,
true, 10, false);
// centroidal_dynamics::EquilibriumAlgorithm alg = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
const centroidal_dynamics::EquilibriumAlgorithm algo =
centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
centroidal_dynamics::Equilibrium contactCone(
"test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES, true,
10, false);
// centroidal_dynamics::EquilibriumAlgorithm alg =
// centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
contactCone.setNewContacts(positions, normals, MU, algo);
return contactCone;
}
std::pair<MatrixXX, VectorX> generateStabilityConstraints(centroidal_dynamics::Equilibrium contactPhase,
Vector3 acc = Vector3::Zero()) {
std::pair<MatrixXX, VectorX> generateStabilityConstraints(
centroidal_dynamics::Equilibrium contactPhase,
Vector3 acc = Vector3::Zero()) {
const Vector3& g = contactPhase.m_gravity;
const Matrix3 gSkew = bezier_com_traj::skew(g);
const Matrix3 accSkew = bezier_com_traj::skew(acc);
......@@ -122,9 +131,12 @@ std::pair<MatrixXX, VectorX> generateStabilityConstraints(centroidal_dynamics::E
std::pair<MatrixXX, VectorX> generateStabilityConstraints(
MatrixX3 normals, MatrixX3 positions, Vector3 acc = Vector3::Zero(),
const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
centroidal_dynamics::Equilibrium contactPhase = ComputeContactCone(contacts.first, contacts.second, algo);
const centroidal_dynamics::EquilibriumAlgorithm algo =
centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
std::pair<MatrixX3, MatrixX3> contacts =
computeRectangularContacts(normals, positions, LX, LY);
centroidal_dynamics::Equilibrium contactPhase =
ComputeContactCone(contacts.first, contacts.second, algo);
return generateStabilityConstraints(contactPhase, acc);
}
......@@ -134,10 +146,14 @@ std::pair<Matrix3, Vector3> computeCost() {
return std::make_pair(H, g);
}
std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals, MatrixX3 positions, Matrix3 endEffRotation,
std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals,
MatrixX3 positions,
Matrix3 endEffRotation,
Vector3 endEffTranslation) {
std::pair<MatrixX3, VectorX> Ab = generateKinematicsConstraints(endEffRotation, endEffTranslation);
std::pair<MatrixX3, VectorX> Cd = generateStabilityConstraints(normals, positions);
std::pair<MatrixX3, VectorX> Ab =
generateKinematicsConstraints(endEffRotation, endEffTranslation);
std::pair<MatrixX3, VectorX> Cd =
generateStabilityConstraints(normals, positions);
size_t numIneq = Ab.first.rows() + Cd.first.rows();
MatrixXX M(numIneq, 3);
VectorX n(numIneq);
......@@ -153,7 +169,8 @@ double fRandom(double fMin, double fMax) {
return fMin + f * (fMax - fMin);
}
ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPair& Cd) {
ConstraintsPair stackConstraints(const ConstraintsPair& Ab,
const ConstraintsPair& Cd) {
size_t numIneq = Ab.first.rows() + Cd.first.rows();
MatrixX3 M(numIneq, 3);
VectorX n(numIneq);
......@@ -164,7 +181,8 @@ ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPai
return std::make_pair(M, n);
}
bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3& point) {
bool verifyKinematicConstraints(const ConstraintsPair& Ab,
const Vector3& point) {
for (long int i = 0; i < Ab.second.size(); ++i) {
if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
return false;
......@@ -173,21 +191,24 @@ bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3& point)
return true;
}
bool verifyStabilityConstraintsDLP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
Vector3 ddc) {
bool verifyStabilityConstraintsDLP(
centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
Vector3 ddc) {
bool success(false);
double res;
centroidal_dynamics::Equilibrium contactPhaseDLP(contactPhase);
contactPhaseDLP.setAlgorithm(centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
centroidal_dynamics::LP_status status = contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL || status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
centroidal_dynamics::LP_status status =
contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL ||
status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
if (success) success = res >= -EPSILON;
if (!success) std::cout << "fail level " << res << std::endl;
return success;
}
bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
Vector3 acc) {
bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase,
Vector3 c, Vector3 /*dc*/, Vector3 acc) {
// compute inequalities :
const Vector3& g = contactPhase.m_gravity;
const Matrix3 gSkew = bezier_com_traj::skew(g);
......