Skip to content
GitLab
Commit dff4d63d authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent 645cf409
Expand all Hide whitespace changes
Inline Side-by-side
LICENSE
View file @ dff4d63d
......@@ -337,4 +337,3 @@ proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.
include/hpp/centroidal-dynamics/Stdafx.hh
View file @ dff4d63d
......@@ -29,9 +29,9 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#pragma once
#include <ctime>
#include <iostream>
#include <map>
#include <ctime>
#include <sstream>
#endif
include/hpp/centroidal-dynamics/logger.hh
View file @ dff4d63d
......@@ -10,10 +10,11 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <hpp/centroidal-dynamics/local_config.hh>
#include <sstream>
#include <Eigen/Dense>
#include <hpp/centroidal-dynamics/local_config.hh>
#include <map>
#include <sstream>
#include "boost/assign.hpp"
namespace centroidal_dynamics {
......@@ -43,7 +44,8 @@ namespace centroidal_dynamics {
#define SEND_WARNING_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING)
#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
#define SEND_DEBUG_STREAM_MSG(msg)
#define SEND_INFO_STREAM_MSG(msg) #define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
#define SEND_INFO_STREAM_MSG(msg) \
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
#endif
......@@ -90,7 +92,8 @@ std::string toString(const T& v) {
}
template <typename T>
std::string toString(const std::vector<T>& v, const std::string separator = ", ") {
std::string toString(const std::vector<T>& v,
const std::string separator = ", ") {
std::stringstream ss;
for (int i = 0; i < v.size() - 1; i++) ss << v[i] << separator;
ss << v[v.size() - 1];
......@@ -98,7 +101,8 @@ std::string toString(const std::vector<T>& v, const std::string separator = ", "
}
template <typename T, int n>
std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator = ", ") {
std::string toString(const Eigen::MatrixBase<T>& v,
const std::string separator = ", ") {
if (v.rows() > v.cols()) return toString(v.transpose(), separator);
std::stringstream ss;
ss << v;
......@@ -132,7 +136,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Logger {
* the point where sendMsg is called so that streaming messages are
* printed only every streamPrintPeriod iterations.
*/
void sendMsg(std::string msg, MsgType type, const char* file = "", int line = 0);
void sendMsg(std::string msg, MsgType type, const char* file = "",
int line = 0);
/** Set the sampling time at which the method countdown()
* is going to be called. */
......@@ -145,26 +150,36 @@ class CENTROIDAL_DYNAMICS_DLLAPI Logger {
void setVerbosity(LoggerVerbosity lv);
protected:
LoggerVerbosity m_lv; /// verbosity of the logger
double m_timeSample; /// specify the period of call of the countdown method
LoggerVerbosity m_lv; /// verbosity of the logger
double m_timeSample; /// specify the period of call of the countdown method
double m_streamPrintPeriod; /// specify the time period of the stream prints
double m_printCountdown; /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
double m_printCountdown; /// every time this is < 0 (i.e. every
/// _streamPrintPeriod sec) print stuff
/** Pointer to the dynamic structure which holds the collection of streaming messages */
/** Pointer to the dynamic structure which holds the collection of streaming
* messages */
std::map<std::string, double> m_stream_msg_counters;
bool isStreamMsg(MsgType m) {
return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_INFO_STREAM ||
m == MSG_TYPE_WARNING_STREAM;
return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_DEBUG_STREAM ||
m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_WARNING_STREAM;
}
bool isDebugMsg(MsgType m) { return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG; }
bool isDebugMsg(MsgType m) {
return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG;
}
bool isInfoMsg(MsgType m) { return m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_INFO; }
bool isInfoMsg(MsgType m) {
return m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_INFO;
}
bool isWarningMsg(MsgType m) { return m == MSG_TYPE_WARNING_STREAM || m == MSG_TYPE_WARNING; }
bool isWarningMsg(MsgType m) {
return m == MSG_TYPE_WARNING_STREAM || m == MSG_TYPE_WARNING;
}
bool isErrorMsg(MsgType m) { return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_ERROR; }
bool isErrorMsg(MsgType m) {
return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_ERROR;
}
};
/** Method to get the logger (singleton). */
......
include/hpp/centroidal-dynamics/solver_LP_abstract.hh
View file @ dff4d63d
......@@ -65,8 +65,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
Cref_vectorX Aub, Ref_vectorX sol) = 0;
virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
Ref_vectorX sol) = 0;
/**
* @brief Solve the linear program described in the specified file.
......@@ -90,13 +91,13 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
virtual bool writeLpToFile(const std::string &filename, Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub);
virtual bool writeLpToFile(const std::string &filename, Cref_vectorX c,
Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A,
Cref_vectorX Alb, Cref_vectorX Aub);
/**
* @brief Read the data describing a Linear Program from the specified binary file.
* The vectors and matrices are resized inside the method.
* minimize c' x
* @brief Read the data describing a Linear Program from the specified binary
* file. The vectors and matrices are resized inside the method. minimize c' x
* subject to Alb <= A x <= Aub
* lb <= x <= ub
* @param filename
......@@ -108,7 +109,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
virtual bool readLpFromFile(const std::string &filename, VectorX &c, VectorX &lb, VectorX &ub, MatrixXX &A,
virtual bool readLpFromFile(const std::string &filename, VectorX &c,
VectorX &lb, VectorX &ub, MatrixXX &A,
VectorX &Alb, VectorX &Aub);
/** Get the status of the solver. */
......@@ -123,7 +125,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
/** Return true if the solver is allowed to warm start, false otherwise. */
virtual bool getUseWarmStart() { return m_useWarmStart; }
/** Specify whether the solver is allowed to use warm-start techniques. */
virtual void setUseWarmStart(bool useWarmStart) { m_useWarmStart = useWarmStart; }
virtual void setUseWarmStart(bool useWarmStart) {
m_useWarmStart = useWarmStart;
}
/** Get the current maximum number of iterations performed by the solver. */
virtual unsigned int getMaximumIterations() { return m_maxIter; }
......
include/hpp/centroidal-dynamics/solver_LP_clp.hh
View file @ dff4d63d
......@@ -9,8 +9,9 @@
#define CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_CLP_HH
#include <hpp/centroidal-dynamics/local_config.hh>
#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include <hpp/centroidal-dynamics/util.hh>
#include "ClpSimplex.hpp"
namespace centroidal_dynamics {
......@@ -27,8 +28,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp : public Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
Cref_vectorX Aub, Ref_vectorX sol);
virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
Ref_vectorX sol);
/** Get the status of the solver. */
virtual LP_status getStatus();
......
include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
View file @ dff4d63d
......@@ -7,8 +7,8 @@
#define HPP_CENTROIDAL_DYNAMICS_SOLVER_LP_QPOASES_HH
#include <hpp/centroidal-dynamics/local_config.hh>
#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include <hpp/centroidal-dynamics/util.hh>
#include <qpOASES.hpp>
namespace centroidal_dynamics {
......@@ -18,8 +18,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
qpOASES::Options m_options; // solver options
qpOASES::SQProblem m_solver; // qpoases solver
MatrixXX m_H; // Hessian matrix
bool m_init_succeeded; // true if solver has been successfully initialized
MatrixXX m_H; // Hessian matrix
bool m_init_succeeded; // true if solver has been successfully initialized
qpOASES::returnValue m_status; // status code returned by the solver
public:
......@@ -32,8 +32,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
Cref_vectorX Aub, Ref_vectorX sol);
LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
Ref_vectorX sol);
/** Get the status of the solver. */
virtual LP_status getStatus();
......@@ -41,7 +42,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
/** Get the objective value of the last solved problem. */
virtual double getObjectiveValue() { return m_solver.getObjVal(); }
virtual void getDualSolution(Ref_vectorX res) { m_solver.getDualSolution(res.data()); }
virtual void getDualSolution(Ref_vectorX res) {
m_solver.getDualSolution(res.data());
}
};
} // end namespace centroidal_dynamics
......
include/hpp/centroidal-dynamics/stop-watch.hh
View file @ dff4d63d
......@@ -169,7 +169,8 @@ class Stopwatch {
void reset_all();
/** Dump the data of a certain performance record */
void report(std::string perf_name, int precision = 2, std::ostream& output = std::cout);
void report(std::string perf_name, int precision = 2,
std::ostream& output = std::cout);
/** Dump the data of all the performance records */
void report_all(int precision = 2, std::ostream& output = std::cout);
......@@ -207,7 +208,13 @@ class Stopwatch {
/** Struct to hold the performance data */
struct PerformanceData {
PerformanceData()
: clock_start(0), total_time(0), min_time(0), max_time(0), last_time(0), paused(false), stops(0) {}
: clock_start(0),
total_time(0),
min_time(0),
max_time(0),
last_time(0),
paused(false),
stops(0) {}
/** Start time */
long double clock_start;
......
include/hpp/centroidal-dynamics/util.hh
View file @ dff4d63d
......@@ -5,18 +5,18 @@
#ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
#define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
#include <iostream>
#include <fstream>
#include <cmath>
#include <cassert>
#include <Eigen/src/Core/util/Macros.h>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/Macros.h>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include "cdd.h"
#include "cddmp.h"
#include "setoper.h"
#include "cddtypes.h"
#include "cdd.h"
#include "setoper.h"
namespace centroidal_dynamics {
......@@ -44,7 +44,9 @@ typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
typedef Eigen::Matrix<value_type, 6, 2, Eigen::RowMajor> Matrix62;
typedef Eigen::Matrix<value_type, 6, 3, Eigen::RowMajor> Matrix63;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, 6, Eigen::RowMajor> MatrixX6;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>
MatrixXX;
typedef Eigen::Ref<Vector2> Ref_vector2;
typedef Eigen::Ref<Vector3> Ref_vector3;
......@@ -67,8 +69,11 @@ typedef const Eigen::Ref<const Matrix63>& Cref_matrix63;
typedef const Eigen::Ref<const MatrixXX>& Cref_matrixXX;
/**Column major definitions for compatibility with classical eigen use**/
typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor> MatrixX3ColMajor;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixXXColMajor;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
MatrixX3ColMajor;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
Eigen::ColMajor>
MatrixXXColMajor;
typedef const Eigen::Ref<const MatrixX3ColMajor>& Cref_matrixX3ColMajor;
typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
......@@ -77,12 +82,14 @@ typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
*/
template <class Matrix>
bool writeMatrixToFile(const std::string& filename, const Matrix& matrix) {
std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
std::ofstream out(filename.c_str(),
std::ios::out | std::ios::binary | std::ios::trunc);
if (!out.is_open()) return false;
typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
out.write((char*)(&rows), sizeof(typename Matrix::Index));
out.write((char*)(&cols), sizeof(typename Matrix::Index));
out.write((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
out.write((char*)matrix.data(),
rows * cols * sizeof(typename Matrix::Scalar));
out.close();
return true;
}
......@@ -110,10 +117,12 @@ bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
* @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);
/**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
* Compute the cross-product skew-symmetric matrix associated to the specified
* vector.
*/
Rotation crossMatrix(Cref_vector3 x);
......@@ -121,13 +130,17 @@ void init_cdd_library();
void release_cdd_library();
// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not compile
void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out);
void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not
// compile
void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds,
Ref_vector3 out);
void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds,
Ref_matrixXX out);
void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p,
bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos,
Cref_vector3 rpy, Ref_matrix43 p,
Ref_matrix43 N);
std::string getDateAndTimeAsString();
......@@ -139,8 +152,10 @@ std::string getDateAndTimeAsString();
value_type nchoosek(const int n, const int k);
template <typename DerivedV, typename DerivedU>
void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running, int& running_i, int& running_j,
Eigen::PlainObjectBase<DerivedU>& U, const Eigen::MatrixBase<DerivedV>& V, int offset, int k) {
void doCombs(
Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running,
int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU>& U,
const Eigen::MatrixBase<DerivedV>& V, int offset, int k) {
int N = (int)(V.size());
if (k == 0) {
U.row(running_i) = running;
......@@ -156,8 +171,9 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
}
/**
* Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
* Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
* Computes a matrix C containing all possible combinations of the elements of
* vector v taken k at a time. Matrix C has k columns and n!/((n–k)! k!) rows,
* where n is length(v).
* @param V n-long vector of elements
* @param k size of sub-set to consider
* @param U result matrix
......@@ -165,7 +181,8 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
* the first one, with all zeros.
*/
template <typename DerivedV, typename DerivedU>
void nchoosek(const Eigen::MatrixBase<DerivedV>& V, const int k, Eigen::PlainObjectBase<DerivedU>& U) {
void nchoosek(const Eigen::MatrixBase<DerivedV>& V, const int k,
Eigen::PlainObjectBase<DerivedU>& U) {
using namespace Eigen;
if (V.size() == 0) {
U.resize(0, k);
......
python/centroidal_dynamics_python.cpp
View file @ dff4d63d
#include "hpp/centroidal-dynamics/centroidal_dynamics.hh"
#include "hpp/centroidal-dynamics/util.hh"
#include <eigenpy/memory.hpp>
#include <boost/python.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include <boost/python.hpp>
#include "hpp/centroidal-dynamics/centroidal_dynamics.hh"
#include "hpp/centroidal-dynamics/util.hh"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(centroidal_dynamics::Equilibrium)
namespace centroidal_dynamics {
using namespace boost::python;
boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com) {
boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self,
const Vector3& com) {
double robustness;
LP_status status = self.computeEquilibriumRobustness(com, robustness);
return boost::python::make_tuple(status, robustness);
}
boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc) {
boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self,
const Vector3& com,
const Vector3& acc) {
double robustness;
LP_status status = self.computeEquilibriumRobustness(com, acc, robustness);
return boost::python::make_tuple(status, robustness);
}
boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com) {
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,
bool wrapSetNewContacts(Equilibrium& self,
const MatrixX3ColMajor& contactPoints,
const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient,
const EquilibriumAlgorithm alg) {
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient,
alg);
}
bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints,
const MatrixX3ColMajor& contactNormals, const double frictionCoefficient,
bool wrapSetNewContactsFull(Equilibrium& self,
const MatrixX3ColMajor& contactPoints,
const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient,
const EquilibriumAlgorithm alg) {
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient,
alg);
}
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self) {
......@@ -71,7 +80,9 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
.value("SOLVER_LP_CLP", SOLVER_LP_CLP)
.export_values();
#else
enum_<SolverLP>("SolverLP").value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES).export_values();
enum_<SolverLP>("SolverLP")
.value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
.export_values();
#endif
enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
......@@ -95,19 +106,22 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
/** END enum types **/
// bool (Equilibrium::*setNewContacts)
// (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int
// graspIndex, const double maxGraspForce) = &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> >())
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", wrapSetNewContacts)
.def("setNewContacts", wrapSetNewContactsFull)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness",
wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
.def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities);
......
python/test/binding_tests.py
View file @ dff4d63d
......@@ -2,18 +2,23 @@ import unittest
from numpy import array, asmatrix, cross
from hpp_centroidal_dynamics import LP_STATUS_OPTIMAL, Equilibrium, EquilibriumAlgorithm, SolverLP
from hpp_centroidal_dynamics import (
LP_STATUS_OPTIMAL,
Equilibrium,
EquilibriumAlgorithm,
SolverLP,
)
class TestCentroidalDynamics(unittest.TestCase):
def test_all(self):
# testing constructors
eq = Equilibrium("test", 54., 4)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
eq = Equilibrium("test", 54.0, 4)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
# whether useWarmStart is enable (True by default)
previous = eq.useWarmStart()
......@@ -24,52 +29,64 @@ class TestCentroidalDynamics(unittest.TestCase):
# access solver name
self.assertEqual(eq.getName(), "test")
z = array([0., 0., 1.])
P = asmatrix(array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]]))
z = array([0.0, 0.0, 1.0])
P = asmatrix(
array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]])
)
N = asmatrix(array([z for _ in range(4)]))
# setting contact positions and normals, as well as friction coefficients
eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
eq.setNewContacts(
asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP
)
c = asmatrix(array([0., 0., 1.])).T
c = asmatrix(array([0.0, 0.0, 1.0])).T
# computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
# computing robustness of a given configuration,
# first with no argument (0 acceleration, static equilibrium)
status, robustness = eq.computeEquilibriumRobustness(c)
self.assertEqual(status, LP_STATUS_OPTIMAL, "LP should not fail")
self.assertGreater(robustness, 0, "first test should be in equilibrirum")
# computing robustness of a given configuration with non zero acceleration
ddc = asmatrix(array([1000., 0., 0.])).T
ddc = asmatrix(array([1000.0, 0.0, 0.0])).T
status, robustness = eq.computeEquilibriumRobustness(c, ddc)
self.assertEqual(status, LP_STATUS_OPTIMAL, "LP should not fail")
self.assertLess(robustness, 0, "first test should NOT be in equilibrirum")
# now, use polytope projection algorithm
eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
eq.setNewContacts(
asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
)
H, h = eq.getPolytopeInequalities()
# c= asmatrix(array([0.,0.,1.])).T
status, stable = eq.checkRobustEquilibrium(c)
# TODO: This works well unless we add a --coverage in CXXFLAGS
# self.assertEqual(status, LP_STATUS_OPTIMAL, "checkRobustEquilibrium should not fail")
# self.assertEqual(status, LP_STATUS_OPTIMAL,
# "checkRobustEquilibrium should not fail")
self.assertTrue(stable, "lat test should be in equilibrirum")
def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
def compute_w(
c, ddc, dL=array([0.0, 0.0, 0.0]), m=54.0, g_vec=array([0.0, 0.0, -9.81])
):
w1 = m * (ddc - g_vec)
return array(w1.tolist() + (cross(c, w1) + dL).tolist())