dyna_com.hpp

Go to the documentation of this file.

#ifndef AIG_DYNACOM
#define AIG_DYNACOM

// clang-format off
#include <Eigen/Dense>
#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/spatial/se3.hpp"
#include <map>
#include <memory>
#include "aig/contact6d.hpp"
// clang-format on

// @TODO: Separate the DynaCom in a new repository.
// @TODO: add abstract class contact and new implementation of it contactPoint.
// @TODO: implement method copy of contacts to be able to duplicate them easily.
// @TODO: In the getters uni_A, fria_A, uni_b, fri_b return only the active
// block.
// @TODO: change internaly the system of names by ids.

namespace aig {

struct DynaCoMSettings {
public:
 std::string urdf = "";

 friend std::ostream &operator<<(std::ostream &out,
 const DynaCoMSettings &obj) {
 out << "DynaCoMSettings:\n";
 out << " urdf: " << obj.urdf << std::endl;
 return out;
 }

 friend bool operator==(const DynaCoMSettings &lhs,
 const DynaCoMSettings &rhs) {
 bool test = true;
 test &= lhs.urdf == rhs.urdf;
 return test;
 }
};

class DynaCoM {
public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
 DynaCoMSettings settings_;
 Eigen::Matrix2d S_;
 pinocchio::Model model_;
 pinocchio::Data data_;
 double mass_;
 double inf_ = std::numeric_limits<double>::infinity();

 // Lists of contacts
 // std::vector< std::shared_ptr<Contact6D> > known_contact6ds_;
 std::map<std::string, std::shared_ptr<Contact6D>> known_contact6ds_;
 std::vector<std::string> active_contact6ds_;

 // QP Matrices
 Eigen::MatrixXd unilaterality_A_;
 Eigen::VectorXd unilaterality_b_;
 Eigen::MatrixXd friction_A_;
 Eigen::VectorXd friction_b_;
 Eigen::VectorXd regularization_A_;
 Eigen::VectorXd regularization_b_;
 Eigen::Matrix<double, 6, -1> newton_euler_A_;
 Eigen::Matrix<double, 6, 1> newton_euler_b_;

 // variables for QP problem with proxsuite formulation
 // Eigen::MatrixXd H_, , A_;
 // Eigen::VectorXd g_, u_, l_;
 // Eigen::Matrix<double, 6, 1> b_;

 // variables for QP problem with eiquadprog formulation
 Eigen::MatrixXd G_; // matrix for quadratic cost
 Eigen::VectorXd g0_; // linear part of the cost function
 Eigen::MatrixXd CE_, CI_, C_; // constraints matrix
 Eigen::VectorXd ce0_, ci0_; // constraints vector

 // variables for QP problem
 Eigen::VectorXd F_;
 Eigen::VectorXi activeSet_;
 size_t activeSetSize_;

 size_t cols_, uni_rows_, fri_rows_;
 // active sizes:
 size_t uni_i_, fri_i_, j_;

 // Internal variables:
 Eigen::Vector3d groundCoMForce_, groundCoMTorque_, CoPTorque_, weight_;
 Eigen::Vector3d acom_;
 Eigen::Vector2d cop_;
 Eigen::Vector3d dL_;
 Eigen::Vector3d L_;
 Eigen::Vector2d n_;
 std::vector<std::string>::iterator activeID_;
 std::map<std::string, std::shared_ptr<aig::Contact6D>>::iterator knownID_;

 // for the CoP wrench
 pinocchio::SE3 oMso_, soMs_;
 Eigen::Matrix<double, 6, 6> Sz_, oXso_, soXs_;

 void addSizes(const std::shared_ptr<Contact6D> &contact);
 void removeSizes(const std::shared_ptr<Contact6D> &contact);
 void resizeMatrices();
 void buildMatrices(const Eigen::Vector3d &groundCoMForce,
 const Eigen::Vector3d &groundCoMTorque,
 const Eigen::Vector3d &CoM);
 void solveQP();
 void distribute();

 const Eigen::Matrix<double, 6, 6> toWorldCoPWrench(pinocchio::SE3 pose);

public:
 DynaCoM();
 DynaCoM(const DynaCoMSettings settings);
 void initialize(const DynaCoMSettings settings);

 void computeDynamics(const Eigen::VectorXd &posture,
 const Eigen::VectorXd &velocity,
 const Eigen::VectorXd &acceleration,
 const Eigen::Matrix<double, 6, 1> &externalWrench =
 Eigen::Matrix<double, 6, 1>::Zero(),
 bool flatHorizontalGround = true);

 void computeNL(const double &w, const Eigen::VectorXd &posture,
 const Eigen::VectorXd &velocity,
 const Eigen::VectorXd &acceleration,
 const Eigen::Matrix<double, 6, 1> &externalWrench =
 Eigen::Matrix<double, 6, 1>::Zero(),
 bool flatHorizontalGround = true);

 void computeNL(const double &w);

 void addContact6d(const std::shared_ptr<Contact6D> &contact,
 const std::string &name, const bool active = true);
 void removeContact6d(const std::string &name);

 void activateContact6d(const std::string &name);
 void deactivateContact6d(const std::string &name);

 void distributeForce(const Eigen::Vector3d &groundCoMForce,
 const Eigen::Vector3d &groundCoMTorque,
 const Eigen::Vector3d &CoM);

 // GETTERS
 const Eigen::Vector3d &getAMVariation() { return dL_; }
 const Eigen::Vector3d &getCoM() { return data_.com[0]; }
 const Eigen::Vector3d &getVCoM() { return data_.vcom[0]; }
 const Eigen::Vector3d &getACoM() { return acom_; }
 const Eigen::Vector3d &getAM() { return L_; }
 const Eigen::Vector2d &getCoP() { return cop_; }
 const Eigen::Vector2d &getNL() { return n_; }
 const Eigen::Vector3d &getGroundCoMForce() { return groundCoMForce_; }
 const Eigen::Vector3d &getGroundCoMTorque() { return groundCoMTorque_; }
 const std::vector<std::string> &getActiveContacts() {
 return active_contact6ds_;
 }
 const std::shared_ptr<Contact6D> &getContact(std::string name) {
 return known_contact6ds_[name];
 }
 const DynaCoMSettings &getSettings() { return settings_; }
 const pinocchio::Model &getModel() { return model_; }
 const pinocchio::Data &getData() { return data_; }

 const Eigen::MatrixXd uni_A() {
 return unilaterality_A_.block(0, 0, uni_i_, j_);
 }
 const Eigen::VectorXd uni_b() { return unilaterality_b_.segment(0, j_); }
 const Eigen::MatrixXd fri_A() { return friction_A_.block(0, 0, fri_i_, j_); }
 const Eigen::VectorXd fri_b() { return friction_b_.segment(0, j_); }
 const Eigen::VectorXd reg_A() { return regularization_A_.segment(0, j_); }
 const Eigen::VectorXd reg_b() { return regularization_b_.segment(0, j_); }
 const Eigen::Matrix<double, 6, -1> NE_A() {
 return newton_euler_A_.block(0, 0, 6, j_);
 }
 const Eigen::Matrix<double, 6, 1> &NE_b() { return newton_euler_b_; }
 const Eigen::VectorXd &allForces() { return F_; }
};

} // namespace aig

#endif // AIG_DYNACOM