Merge pull request #32 from Gepetto/topic/mnaveau/fix_devel_unit_tests

[fix] Fix the unit-tests on debug in ubuntu20.04

include/aig/biped_ig.hpp

@@ -132,16 +132,6 @@ class BipedIG {
   Eigen::Vector3d com_from_waist_;
   int lleg_idx_qs_;  // Indexes in the configuration vector.
   int rleg_idx_qs_;  // Indexes in the configuration vector.
-  double mass_;
-  Eigen::Matrix2d S_;
-  Eigen::Vector3d gravity_;
-  // Eigen::Vector3d com_;
-  // Eigen::Vector3d vcom_;
-  Eigen::Vector3d acom_;
-  Eigen::Vector2d cop_;
-  Eigen::Vector3d dL_;
-  Eigen::Vector3d L_;
-  Eigen::Vector2d n_;
 
   // variables used in the waist-com vector correction:
   Eigen::Vector3d error_, com_temp_;
@@ -165,10 +155,6 @@ class BipedIG {
 
   void configureLegs();
 
-  // Internal computation variables
-  // on computeDynamics
-  Eigen::Vector3d groundForce_, groundCoMTorque_, nonCoPTorque_, weight_;
-
   // Public methods.
  public:
   BipedIG();
@@ -190,15 +176,20 @@ class BipedIG {
 
   /// @brief Get the Angular Momentum variation. Please call computeDynamics
   /// first. Deprecate it, AIG is not made for dynamics
-  const Eigen::Vector3d &getAMVariation() { return dL_; }
-
-  /// @brief Get the Angular Momentum. Please call computeDynamics first.
-  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 &getAMVariation() { return dynamics_.getAMVariation(); }
+
+  /// @brief Get the CoM position. Please call computeDynamics first.
+  const Eigen::Vector3d &getCoM() { return dynamics_.getCoM(); }
+  /// @brief Get the CoM velocity. Please call computeDynamics first.
+  const Eigen::Vector3d &getVCoM() { return dynamics_.getVCoM(); }
+  /// @brief Get the CoM acceleration. Please call computeDynamics first.
+  const Eigen::Vector3d &getACoM() { return dynamics_.getACoM(); }
+  /// @brief Get the angular momentum. Please call computeDynamics first.
+  const Eigen::Vector3d &getAM() { return dynamics_.getAM(); }
+  /// @brief Get the CoP Position. Please call computeDynamics first.
+  const Eigen::Vector2d &getCoP() { return dynamics_.getCoP(); }
+  /// @brief Get the nonlinear effect. Please call computeDynamics first.
+  const Eigen::Vector2d &getNL() { return dynamics_.getNL(); }
 
   void checkCompatibility();  // TODO
 

package.xml

@@ -3,7 +3,7 @@
   <name>aig</name>
   <version>1.1.0</version>
   <description>
-    Analitcical inverse geometry for 6 links kinematic chains
+    Analytical inverse geometry for 6 links kinematic chains
   </description>
   <maintainer email="[email protected]">Guilhem saurel</maintainer>
   <license>BSD</license>

src/biped_ig.cpp

@@ -87,14 +87,6 @@ void BipedIG::initialize(const BipedIGSettings &settings) {
   // Build pinocchio cache.
   data_ = pinocchio::Data(model_);
 
-  gravity_ = model_.gravity981;
-  mass_ = 0.0;
-  for (size_t k = 0; k < model_.inertias.size(); ++k) {
-    mass_ += model_.inertias[k].mass();
-  }
-  weight_ = mass_ * gravity_;
-  S_ << 0, -1, 1, 0;
-
   // Extract the CoM to Waist level arm.
   if (srdf_file_exists) {
     pinocchio::srdf::loadReferenceConfigurations(model_, settings_.srdf, false);
@@ -403,12 +395,6 @@ void BipedIG::computeDynamics(const Eigen::VectorXd &posture,
                               bool flatHorizontalGround) {
   dynamics_.computeDynamics(posture, velocity, acceleration, externalWrench,
                             flatHorizontalGround);
-  acom_ = dynamics_.getACoM();
-  dL_ = dynamics_.getAMVariation();
-  L_ = dynamics_.getAM();
-  groundForce_ = dynamics_.getGroundCoMForce();
-  groundCoMTorque_ = dynamics_.getGroundCoMTorque();
-  cop_ = dynamics_.getCoP();
 }
 
 void BipedIG::computeNL(const double &w, const Eigen::VectorXd &posture,
@@ -418,7 +404,6 @@ void BipedIG::computeNL(const double &w, const Eigen::VectorXd &posture,
                         bool flatHorizontalGround) {
   dynamics_.computeNL(w, posture, velocity, acceleration, externalWrench,
                       flatHorizontalGround);
-  n_ = dynamics_.getNL();
 }
 
 void BipedIG::computeNL(const double &w) {
@@ -427,7 +412,6 @@ void BipedIG::computeNL(const double &w) {
    * before.
    */
   dynamics_.computeNL(w);
-  n_ = dynamics_.getNL();
 }
 
 }  // namespace aig

src/dyna_com.cpp

@@ -153,7 +153,7 @@ void DynaCoM::computeNL(const double &w, const Eigen::VectorXd &posture,
 
 void DynaCoM::computeNL(const double &w) {
   /**
-   * In this function form, computeDynamics is suposed to have been called
+   * In this function form, computeDynamics is supposed to have been called
    * before.
    */
   n_ = acom_.head<2>() / (w * w) - data_.com[0].head<2>() + cop_;