From d5b83916b625b62b4b564d2dfdf4b938b2992b91 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Sat, 24 Feb 2018 17:56:52 +0100
Subject: [PATCH] add a - epsilon to h to do a ~ robustness treshold
---
src/solve_transition.cpp | 14 +++++++++++---
1 file changed, 11 insertions(+), 3 deletions(-)
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 0b05c8b..c98556e 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -10,7 +10,7 @@
#ifndef QHULL
-#define QHULL 0
+#define QHULL 1
#endif
#ifndef DDC0_CONSTRAINT
#define DDC0_CONSTRAINT 1
@@ -32,11 +32,15 @@
#endif
-
#ifndef MAX_ACC
#define MAX_ACC 1
#endif
+
+#ifndef REDUCE_h
+#define REDUCE_h 1e-3
+#endif
+
namespace bezier_com_traj
{
typedef waypoint3_t waypoint_t;
@@ -50,7 +54,7 @@ ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::p
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,const std::string& fileName,bool clipZ = false){
std::ofstream file;
using std::endl;
- std::string path("/home/pfernbac/Documents/com_ineq_test/");
+ 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;
@@ -917,6 +921,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
H.rowwise().normalize();
int dimH = (int)(H.rows());
mH = phase.contactPhase_->m_mass * H;
+ if(REDUCE_h > 0)
+ h -= VectorX::Ones(h.rows())*REDUCE_h;
// assign the Stability constraints for each discretized waypoints :
// we don't consider the first point, because it's independant of x.
@@ -940,6 +946,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
H.rowwise().normalize();
dimH = (int)(H.rows());
mH = phase.contactPhase_->m_mass * H;
+ if(REDUCE_h > 0)
+ h -= VectorX::Ones(h.rows())*REDUCE_h;
// the current waypoint must have the constraints of both phases. So we add it again :
// TODO : filter for redunbdant constraints ...
// add stability constraints :
--
GitLab