add StaticEquilibrium::findMaximumAcceleration

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -234,6 +234,27 @@ public:
    * contact has been defined, will return LP_STATUS_INFEASIBLE
    */
   LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
+
+  /**
+   * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
+          find          b, alpha0
+          minimize      -alpha0
+          subject to    -h <= [-G  (Hv)] [b a0]^T   <= -h
+                        0       <= [b a0]^T <= Inf
+
+
+          b         are the coefficient of the contact force generators (f = V b)
+          v         is the vector3 defining the direction of the motion
+          alpha0    is the maximal amplitude of the acceleration, for the given direction v
+          c         is the CoM position
+          G         is the matrix whose columns are the gravito-inertial wrench generators
+          A         is [-G  (Hv)]
+   * @param A
+   * @param h
+   * @param alpha0
+   * @return The status of the LP solver.
+   */
+  LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
 };
 
 } // end namespace robust_equilibrium

include/robust-equilibrium-lib/util.hh

@@ -56,6 +56,7 @@ namespace robust_equilibrium
 
   typedef const Eigen::Ref<const Vector2>     & Cref_vector2;
   typedef const Eigen::Ref<const Vector3>     & Cref_vector3;
+  typedef const Eigen::Ref<const Vector6>     & Cref_vector6;
   typedef const Eigen::Ref<const VectorX>     & Cref_vectorX;
   typedef const Eigen::Ref<const Rotation>    & Cref_rotation;
   typedef const Eigen::Ref<const MatrixX3>    & Cref_matrixX3;

src/static_equilibrium.cpp

@@ -562,4 +562,30 @@ double StaticEquilibrium::convert_emax_to_b0(double emax)
   return (emax/m_b0_to_emax_coefficient);
 }
 
+
+LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
+  int m = (int)A.cols() -1 ; // 4* number of contacts
+  VectorX b_a0(m+1);
+  VectorX c = VectorX::Zero(m+1);
+  c(m) = -1.0;  // because we search max alpha0
+  VectorX lb = VectorX::Zero(m+1);
+  VectorX ub = VectorX::Ones(m+1)*1e10; // Inf
+  VectorX Alb = -h;
+  VectorX Aub = -h;
+
+
+  LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
+  if(lpStatus==LP_STATUS_OPTIMAL)
+  {
+    alpha0 = -1.0 * m_solver->getObjectiveValue();
+    return lpStatus;
+  }
+  alpha0 = 0.0;
+  SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+  return lpStatus;
+
+}
+
+
+
 } // end namespace robust_equilibrium