dyna_com.hpp

Go to the documentation of this file.

 
#ifndef DYNACOM_DYNACOM
#define DYNACOM_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 "dynacom/contact6d.hpp"
// clang-format on
 
// @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 dynacom {
 
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<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 dynacom
 
#endif  // DYNACOM_DYNACOM