From a22ddc4e135f24b76ed31dd591cdb16972ce9e02 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Fri, 12 May 2017 14:18:33 +0200
Subject: [PATCH] grasping
---
.../centroidal_dynamics.hh | 14 +-
include/centroidal-dynamics-lib/util.hh | 4 +-
python/centroidal_dynamics_python.cpp | 27 +++-
python/test/binding_tests.py | 25 +++
src/centroidal_dynamics.cpp | 145 ++++++------------
src/util.cpp | 42 +++--
6 files changed, 140 insertions(+), 117 deletions(-)
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 997fd73..cbc5274 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -66,9 +66,9 @@ private:
/** Coefficient used for converting the robustness measure in Newtons */
double m_b0_to_emax_coefficient;
- bool computePolytopeProjection(Cref_matrix6X v);
+ bool computePolytopeProjection(Cref_matrix6X v, const int graspIndex = -1);
bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
- double frictionCoefficient, const bool perturbate = false);
+ double frictionCoefficient, const int graspIndex, const double maxGraspForce, const bool perturbate = false);
/**
* @brief Given the smallest coefficient of the contact force generators it computes
@@ -130,10 +130,13 @@ public:
* @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
* @param frictionCoefficient The contact friction coefficient.
* @param alg Algorithm to use for testing equilibrium.
+ * @param graspIndex if different than -1, specify starting where the contact points are grasping points
+ * WARNING graspIndexOnly works for PP algorithm
+ * @param maxGraspForce maximum grasping force applicable at each contact point
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg);
+ const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex = -1, const double maxGraspForce = 50.);
/**
* @brief Specify a new set of contacts.
@@ -144,10 +147,13 @@ public:
* @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
* @param frictionCoefficient The contact friction coefficient.
* @param alg Algorithm to use for testing equilibrium.
+ * @param graspIndex if different than -1, specify starting where the contact points are grasping points
+ * WARNING graspIndexOnly works for PP algorithm
+ * @param maxGraspForce maximum grasping force applicable at each contact point
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg);
+ const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex = -1, const double maxGraspForce = 50.);
void setG(Cref_matrix6X G){m_G_centr = G;}
diff --git a/include/centroidal-dynamics-lib/util.hh b/include/centroidal-dynamics-lib/util.hh
index 66e2231..1c7ba0d 100644
--- a/include/centroidal-dynamics-lib/util.hh
+++ b/include/centroidal-dynamics-lib/util.hh
@@ -115,7 +115,7 @@ namespace centroidal_dynamics
* @return The mX(n+1) output cdd matrix, which contains an additional column,
* the first one, with all zeros.
*/
- dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false);
+ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false, const int graspIndex = -1);
/**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
@@ -191,8 +191,6 @@ namespace centroidal_dynamics
int running_i = 0;
int running_j = 0;
Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
- /* const std::function<void(int,int)> doCombs =
- [&running,&N,&doCombs,&running_i,&running_j,&U,&V](int offset, int k)*/;
doCombs(running, running_i, running_j, U, V,0,k);
}
} //namespace centroidal_dynamics
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 82f1b37..60256c8 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -26,6 +26,25 @@ boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const V
return boost::python::make_tuple(status, robustness);
}
+boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com)
+{
+ bool robustness;
+ LP_status status = self.checkRobustEquilibrium(com, robustness);
+ return boost::python::make_tuple(status, robustness);
+}
+
+bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient, const EquilibriumAlgorithm alg)
+{
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+}
+
+bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
+{
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg, graspIndex, maxGraspForce);
+}
+
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
{
MatrixXX H;
@@ -86,17 +105,19 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
/** END enum types **/
- bool (Equilibrium::*setNewContacts)
- (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
+ //bool (Equilibrium::*setNewContacts)
+ // (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
.def("useWarmStart", &Equilibrium::useWarmStart)
.def("setUseWarmStart", &Equilibrium::setUseWarmStart)
.def("getName", &Equilibrium::getName)
.def("getAlgorithm", &Equilibrium::getAlgorithm)
- .def("setNewContacts", setNewContacts)
+ .def("setNewContacts", wrapSetNewContacts)
+ .def("setNewContacts", wrapSetNewContactsFull)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
+ .def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities)
;
}
diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py
index 14db32b..1de874d 100644
--- a/python/test/binding_tests.py
+++ b/python/test/binding_tests.py
@@ -45,3 +45,28 @@ assert (robustness < 0), "first test should NOT be in equilibrirum"
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
H,h = eq.getPolytopeInequalities()
+
+#now, use polytope projection algorithm with grasping
+eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP,4,50.)
+H,h = eq.getPolytopeInequalities()
+print P
+c= asmatrix(array([0.,0.,1.]))
+status, stable = eq.checkRobustEquilibrium(c)
+#~ assert (status == LP_STATUS_OPTIMAL), "checkRobustEquilibrium should not fail"
+#~ assert (stable), "lat test should be in equilibrirum"
+
+
+from numpy import array, vstack, zeros, sqrt, cross, matrix, asmatrix
+from numpy.linalg import norm
+import numpy as np
+
+def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
+ w1 = m * (ddc - g_vec)
+ return array(w1.tolist() + (cross(c, w1) + dL).tolist())
+
+
+def is_stable(H,c=array([0.,0.,1.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
+ w = compute_w(c, ddc, dL, m, g_vec)
+ return (H.dot(-w)<=-robustness).all()
+
+assert(is_stable(H))
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index c8b3b6b..4046c19 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -55,9 +55,13 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
}
bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
- double frictionCoefficient, const bool perturbate)
+ double frictionCoefficient, const int graspIndex, const double maxGraspForce, const bool perturbate)
{
- long int c = contactPoints.rows();
+ if(graspIndex != -1)
+ {
+ assert(graspIndex < contactPoints.rows());
+ }
+ long int c = graspIndex > -1 ? graspIndex : contactPoints.rows();
unsigned int &cg = m_generatorsPerContact;
double theta, delta_theta=2*M_PI/cg;
const value_type pi_4 = value_type(M_PI_4);
@@ -118,6 +122,30 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// project generators in 6d centroidal space
m_G_centr.block(0,cg*i,6,cg) = A * G;
}
+ if(graspIndex > -1)
+ {
+ Matrix3X cube(3, 8);
+ cube.col(0)= Vector3(-maxGraspForce,-maxGraspForce,-maxGraspForce);
+ cube.col(1)= Vector3( maxGraspForce,-maxGraspForce,-maxGraspForce);
+ cube.col(2)= Vector3( maxGraspForce, maxGraspForce,-maxGraspForce);
+ cube.col(3)= Vector3(-maxGraspForce, maxGraspForce,-maxGraspForce);
+ cube.col(4)= Vector3(-maxGraspForce,-maxGraspForce, maxGraspForce);
+ cube.col(5)= Vector3( maxGraspForce,-maxGraspForce, maxGraspForce);
+ cube.col(6)= Vector3( maxGraspForce, maxGraspForce, maxGraspForce);
+ cube.col(7)= Vector3(-maxGraspForce, maxGraspForce, maxGraspForce);
+ //first compute the cube constraining the grasping force.
+ for(long int i=graspIndex; i<contactPoints.rows(); i++)
+ {
+ // compute tangent directions
+ //N = contactNormals.row(i);
+ P = contactPoints.row(i);
+ // compute matrix mapping contact forces to gravito-inertial wrench
+ A.bottomRows<3>() = crossMatrix(-1.0*P);
+ // project generators in 6d centroidal space
+ m_G_centr.block(0,graspIndex*cg + 8*(i-graspIndex),6,8) = A * cube;
+ }
+ }
+
// Compute the coefficient to convert b0 to e_max
Vector3 f0 = Vector3::Zero();
for(int j=0; j<cg; j++)
@@ -138,18 +166,22 @@ void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
}
bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
+ const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
{
MatrixX3 _contactPoints = contactPoints;
MatrixX3 _contactNormals = contactNormals;
- return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg);
+ return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg, graspIndex, maxGraspForce);
}
bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
+ const double frictionCoefficient, const EquilibriumAlgorithm alg, const int graspIndex, const double maxGraspForce)
{
assert(contactPoints.rows()==contactNormals.rows());
-
+ if(alg!=EQUILIBRIUM_ALGORITHM_PP && graspIndex != -1)
+ {
+ SEND_ERROR_MSG("GRASPING not implemented yet expect for EQUILIBRIUM_ALGORITHM_PP");
+ return false;
+ }
if(alg==EQUILIBRIUM_ALGORITHM_IP)
{
SEND_ERROR_MSG("Algorithm IP not implemented yet");
@@ -164,9 +196,12 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
m_algorithm = alg;
// Lists of contact generators (3 X generatorsPerContact)
- m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
+ if(graspIndex == -1)
+ m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
+ else
+ m_G_centr.resize(6,graspIndex*m_generatorsPerContact+(contactPoints.rows()-graspIndex)*8);
- if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient,false))
+ if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, graspIndex, maxGraspForce,false))
{
return false;
}
@@ -174,9 +209,9 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
if(m_algorithm==EQUILIBRIUM_ALGORITHM_PP)
{
unsigned int attempts = max_num_cdd_trials;
- while(!computePolytopeProjection(m_G_centr) && attempts>0)
+ while(!computePolytopeProjection(m_G_centr, graspIndex) && attempts>0)
{
- computeGenerators(contactPoints,contactNormals,frictionCoefficient,true);
+ computeGenerators(contactPoints,contactNormals,frictionCoefficient, graspIndex, maxGraspForce,true);
attempts--;
}
// resetting random because obviously libcdd always resets to 0
@@ -521,55 +556,7 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
-///
-/// \brief Computes factorial of a number
-///
-int fact(const int n)
-{
- assert(n>=0);
- int res = 1;
- for (int i=2 ; i <= n ; ++i)
- res *= i;
- return res;
-}
-
-///
-/// \brief Computes a binomal coefficient
-///
-int choosenk(const int n, const int k)
-{
- return fact(n) / (fact(k) * fact(n - k));
-}
-
-VectorX computeCombVector(const int order)
-{
- VectorX res = VectorX::Zero(order);
- for (int i = 0; i< order; ++i)
- res[i] = i;
- return res;
-}
-#include <algorithm>
-#include <set>
-#include <iterator>
-
-std::set<int> vec_to_set(Cref_vectorX A)
-{
- std::set<int> res;
- for(int i = 0; i < A.size(); ++i)
- res.insert(A[i]);
- return res;
-}
-
-//elements of A not in B
-std::set<int> setDiff(Cref_vectorX A, Cref_vectorX B)
-{
- std::set<int> s1 = vec_to_set(A), s2 = vec_to_set(B);
- // Fill in s1 and s2 with values
- std::set<int> result;
- std::set_difference(s1.begin(), s1.end(), s2.begin(), s2.end(),
- std::inserter(result, result.end()));
-}
-bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
+bool Equilibrium::computePolytopeProjection(Cref_matrix6X v, const int graspIndex)
{
// todo: for the moment using ad hoc upper bound = 500 N
@@ -580,41 +567,8 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
return false;
}
- MatrixXX I;
- VectorX comb = computeCombVector(m);
- nchoosek(comb,n-1,I);
- int nbcomb = I.rows();
- int sizemat = (int)(2*nchoosek(m,n-1));
- MatrixXX C = MatrixXX::Zero(sizemat,n);
- VectorX d = VectorX::Zero(sizemat);
- MatrixXX V = MatrixXX::Zero(n,m);
- for(int i = 0; i< nbcomb; ++i)
- {
- const Eigen::Ref<VectorX>& chosen_comb = I.row(i);
- for(int j = 0; j < chosen_comb.size(); ++j)
- V.col(j) = v.col((int)(chosen_comb[j]));
- Eigen::FullPivLU<MatrixXX> lu(V);
- MatrixXX c = lu.kernel();
- std::cout << "c " << c.cols() << std::endl;
- std::cout << "V " << V.cols() << std::endl;
- if(c.cols()==1) // The n-1 lines of V are lineraly independent
- {
- c.normalize();
- C.row(2*i-1) = c;
- C.row(2*i) = c;
- std::set<int> J = setDiff(comb,chosen_comb);
- std::set<int>::const_iterator setit = J.begin();
- MatrixXX VV = MatrixXX::Zero(n,J.size());
- for(int j = 0; j < chosen_comb.size(); ++j, ++setit)
- VV.col(j) = v.col(*setit);
- MatrixXX scalar=VV*c(0,0);
- std::cout << "scalar " << scalar << std::endl;
- }
- }
-
- //int I = I=nchoosek(1:m,n-1);*/
// getProfiler().start("eigen_to_cdd");
- /* dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
+ dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix,graspIndex);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
@@ -665,7 +619,6 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
m_h(rowsize + i) = -m_h((int)(*cit));
m_H(rowsize + i) = -m_H((int)(*cit));
}
-<<<<<<< HEAD
// getProfiler().stop("cdd to eigen");
if(m_h.rows() < n )
{
@@ -674,10 +627,6 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
return false;
}
-=======
-// getProfiler().stop("cdd to eigen");*/
->>>>>>> ongoing
-
return true;
}
diff --git a/src/util.cpp b/src/util.cpp
index a0e1fcd..d714f87 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -12,7 +12,7 @@
namespace centroidal_dynamics
{
-dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize)
+dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize, const int graspIndex)
{
dd_debug = false;
dd_MatrixPtr M=NULL;
@@ -28,17 +28,42 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
M=dd_CreateMatrix(m_input, d_input);
M->representation=rep;
M->numbtype=NT;
-
- for (i = 0; i < input.rows(); i++)
+ if(graspIndex > 0)
{
- dd_set_d(value, 0);
- dd_set(M->matrix[i][0],value);
- for (j = 1; j < d_input; j++)
+ for (i = 0; i < input.rows(); i++)
+ {
+ dd_set_d(value, 1);
+ dd_set(M->matrix[i][0],value);
+ for (j = graspIndex+1; j < d_input; j++)
+ {
+ dd_set_d(value, input(i,j-1));
+ dd_set(M->matrix[i][j],value);
+ }
+ }
+ for (i = 0; i < input.rows(); i++)
{
- dd_set_d(value, input(i,j-1));
- dd_set(M->matrix[i][j],value);
+ dd_set_d(value, 0);
+ dd_set(M->matrix[i][0],value);
+ for (j = 1; j < graspIndex+1; j++)
+ {
+ dd_set_d(value, input(i,j-1));
+ dd_set(M->matrix[i][j],value);
+ }
}
}
+ else
+ {
+ for (i = 0; i < input.rows(); i++)
+ {
+ dd_set_d(value, 0);
+ dd_set(M->matrix[i][0],value);
+ for (j = 1; j < d_input; j++)
+ {
+ dd_set_d(value, input(i,j-1));
+ dd_set(M->matrix[i][j],value);
+ }
+ }
+ }
dd_clear(value);
if(canonicalize)
{
@@ -50,7 +75,6 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
set_free(impl_linset);
free(newpos);
}
-
return M;
}
--
GitLab