Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/aig/biped_ig.hpp	Lines:	32	52	61.5 %
Date:	2023-12-03 14:15:28	Branches:	24	48	50.0 %


/**
 * @file
 * @authors Nahuel Villa, Maximilien Naveau
 * @copyright Copyright (c) 2022 LAAS-CNRS, TOWARD, PAL_ROBOTICS
 *            License BSD-2
 *
 * @brief Class to perform inverse geometry on a biped robot.
 */

#ifndef AIG_BIPED_IG
#define AIG_BIPED_IG

// 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 "aig/arm_ig.hpp"
#include "aig/leg_ig.hpp"
#include "aig/dyna_com.hpp"
// clang-format on

namespace aig {
/**
 * @brief @todo Describe BipedIGSettings
 */

struct BipedIGSettings {
 public:
  std::string left_hip_joint_name;
  std::string left_knee_joint_name;
  std::string left_ankle_joint_name;
  std::string left_foot_frame_name;
  std::string right_hip_joint_name;
  std::string right_knee_joint_name;
  std::string right_ankle_joint_name;
  std::string right_foot_frame_name;
  /**
   * @brief This must contain either a valid path to the urdf file or the
   * content of this file in a string.
   */
  std::string urdf;
  /**
   * @brief This must contain either a valid path to the srdf file or the
   * content of this file in a string.
   */
  std::string srdf;

  BipedIGSettings()
      : left_hip_joint_name(""),
        left_knee_joint_name(""),
        left_ankle_joint_name(""),
        left_foot_frame_name(""),
        right_hip_joint_name(""),
        right_knee_joint_name(""),
        right_ankle_joint_name(""),
        right_foot_frame_name(""),
        urdf(""),
        srdf("") {}

  BipedIGSettings(const std::string &_left_hip_joint_name,
                  const std::string &_left_knee_joint_name,
                  const std::string &_left_ankle_joint_name,
                  const std::string &_left_foot_frame_name,
                  const std::string &_right_hip_joint_name,
                  const std::string &_right_knee_joint_name,
                  const std::string &_right_ankle_joint_name,
                  const std::string &_right_foot_frame_name,
                  const std::string &_urdf, const std::string &_srdf)
      : left_hip_joint_name(_left_hip_joint_name),
        left_knee_joint_name(_left_knee_joint_name),
        left_ankle_joint_name(_left_ankle_joint_name),
        left_foot_frame_name(_left_foot_frame_name),
        right_hip_joint_name(_right_hip_joint_name),
        right_knee_joint_name(_right_knee_joint_name),
        right_ankle_joint_name(_right_ankle_joint_name),
        right_foot_frame_name(_right_foot_frame_name),
        urdf(_urdf),
        srdf(_srdf) {}

  friend std::ostream &operator<<(std::ostream &out,
                                  const BipedIGSettings &obj) {
    out << "BipedIGSettings:\n";
    out << "    left_hip_joint_name: " << obj.left_hip_joint_name << "\n";
    out << "    left_knee_joint_name: " << obj.left_knee_joint_name << "\n";
    out << "    left_ankle_joint_name: " << obj.left_ankle_joint_name << "\n";
    out << "    left_foot_frame_name: " << obj.left_foot_frame_name << "\n";
    out << "    right_hip_joint_name: " << obj.right_hip_joint_name << "\n";
    out << "    right_knee_joint_name: " << obj.right_knee_joint_name << "\n";
    out << "    right_ankle_joint_name: " << obj.right_ankle_joint_name << "\n";
    out << "    right_foot_frame_name: " << obj.right_foot_frame_name << "\n";
    out << "    urdf: " << obj.urdf << "\n";
    out << "    srdf: " << obj.srdf << std::endl;
    return out;
  }

  friend bool operator==(const BipedIGSettings &lhs,
                         const BipedIGSettings &rhs) {
    bool test = true;
    test &= lhs.left_hip_joint_name == rhs.left_hip_joint_name;
    test &= lhs.left_knee_joint_name == rhs.left_knee_joint_name;
    test &= lhs.left_ankle_joint_name == rhs.left_ankle_joint_name;
    test &= lhs.left_foot_frame_name == rhs.left_foot_frame_name;
    test &= lhs.right_hip_joint_name == rhs.right_hip_joint_name;
    test &= lhs.right_knee_joint_name == rhs.right_knee_joint_name;
    test &= lhs.right_ankle_joint_name == rhs.right_ankle_joint_name;
    test &= lhs.right_foot_frame_name == rhs.right_foot_frame_name;
    test &= lhs.urdf == rhs.urdf;
    test &= lhs.srdf == rhs.srdf;
    return test;
  }
};

BipedIGSettings makeSettingsFor(const std::string &path_to_robots,
                                const std::string &robot_name);

/**
 * @brief @todo Describe BipedIG
 *
 */
class BipedIG {
  // Private attributes.
 private:
  pinocchio::Model model_;
  pinocchio::Data data_;
  BipedIGSettings settings_;
  LegIG left_leg_, right_leg_;
  // ArmIG left_arm_, right_arm_;
  Eigen::VectorXd q0_;  // q0_ is a reference configuration used to take all not
                        // computed joints (such as head and arms)
  Eigen::Vector3d com_from_waist_;
  int lleg_idx_qs_;  // Indexes in the configuration vector.
  int rleg_idx_qs_;  // Indexes in the configuration vector.

  // variables used in the waist-com vector correction:
  Eigen::Vector3d error_, com_temp_;
  Eigen::VectorXd posture_temp_;
  Eigen::Matrix3d baseRotation_temp_;

  aig::DynaCoM dynamics_;

  // Private methods.
 private:
  void derivatives(const Eigen::VectorXd &q1, const Eigen::VectorXd &q3,
                   Eigen::VectorXd &posture, Eigen::VectorXd &velocity,
                   Eigen::VectorXd &acceleration, const double &dt);

  pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
                             const pinocchio::SE3 &leftFoot,
                             const pinocchio::SE3 &rightFoot);

  pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
                             const Eigen::Matrix3d &baseRotation);

  void configureLegs();

  // Public methods.
 public:
  BipedIG();

  BipedIG(const BipedIGSettings &settings);

  void initialize(const BipedIGSettings &settings);

  const BipedIGSettings &get_settings() { return settings_; };
  const LegIGSettings &get_left_leg_settings() {
    return left_leg_.get_settings();
  };
  const LegIGSettings &get_right_leg_settings() {
    return right_leg_.get_settings();
  };

  const Eigen::VectorXd &getQ0() { return q0_; }
  void setQ0(const Eigen::VectorXd q0) { q0_ = q0; }

  /// @brief Get the Angular Momentum variation. Please call computeDynamics
  /// first. Deprecate it, AIG is not made for dynamics
  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

  void solve(const Eigen::Vector3d &com, const pinocchio::SE3 &leftFoot,
             const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0,
             Eigen::VectorXd &posture, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const Eigen::Vector3d &com, const Eigen::Isometry3d &leftFeet,
             const Eigen::Isometry3d &rightFeet, const Eigen::VectorXd &q0,
             Eigen::VectorXd &posture, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const Eigen::Vector3d &com, const Eigen::Matrix3d &baseRotation,
             const pinocchio::SE3 &leftFoot, const pinocchio::SE3 &rightFoot,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             const double &tolerance = 1e-10, const int &max_iterations = 0);

  void solve(const Eigen::Vector3d &com, const Eigen::Matrix3d &baseRotation,
             const Eigen::Isometry3d &leftFoot,
             const Eigen::Isometry3d &rightFoot, const Eigen::VectorXd &q0,
             Eigen::VectorXd &posture, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const pinocchio::SE3 &base, const pinocchio::SE3 &leftFoot,
             const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0,
             Eigen::VectorXd &posture);

  void solve(const Eigen::Isometry3d &base, const Eigen::Isometry3d &leftFoot,
             const Eigen::Isometry3d &rightFoot, const Eigen::VectorXd &q0,
             Eigen::VectorXd &posture);

  void solve(const std::array<Eigen::Vector3d, 3> &coms,
             const std::array<pinocchio::SE3, 3> &leftFeet,
             const std::array<pinocchio::SE3, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const std::array<Eigen::Vector3d, 3> &coms,
             const std::array<Eigen::Isometry3d, 3> &leftFeet,
             const std::array<Eigen::Isometry3d, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const std::array<Eigen::Vector3d, 3> &coms,
             const std::array<Eigen::Matrix3d, 3> &baseRotations,
             const std::array<pinocchio::SE3, 3> &leftFeet,
             const std::array<pinocchio::SE3, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const std::array<Eigen::Vector3d, 3> &coms,
             const std::array<Eigen::Matrix3d, 3> &baseRotations,
             const std::array<Eigen::Isometry3d, 3> &leftFeet,
             const std::array<Eigen::Isometry3d, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt, const double &tolerance = 1e-10,
             const int &max_iterations = 0);

  void solve(const std::array<pinocchio::SE3, 3> &bases,
             const std::array<pinocchio::SE3, 3> &leftFeet,
             const std::array<pinocchio::SE3, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt);

  void solve(const std::array<Eigen::Isometry3d, 3> &bases,
             const std::array<Eigen::Isometry3d, 3> &leftFeet,
             const std::array<Eigen::Isometry3d, 3> &rightFeet,
             const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
             Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
             const double &dt);

  void set_com_from_waist(const Eigen::Vector3d &com_from_waist);

  void set_com_from_waist(const Eigen::VectorXd &q);

  void correctCoMfromWaist(const Eigen::Vector3d &com,
                           const pinocchio::SE3 &leftFoot,
                           const pinocchio::SE3 &rightFoot,
                           const Eigen::VectorXd &q0,
                           const double &tolerance = 1e-10,
                           const int &max_iterations = 20);

  void correctCoMfromWaist(const Eigen::Vector3d &com,
                           const Eigen::Isometry3d &leftFoot,
                           const Eigen::Isometry3d &rightFoot,
                           const Eigen::VectorXd &q0,
                           const double &tolerance = 1e-10,
                           const int &max_iterations = 20);

  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);

  pinocchio::Model &get_model() { return model_; }
  pinocchio::Data &get_data() { return data_; }
  Eigen::Vector3d &get_com_from_waist() { return com_from_waist_; }
};
}  // namespace aig
#endif  // AIG_BIPED_IG


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/**
2			* @file
3			* @authors Nahuel Villa, Maximilien Naveau
4			* @copyright Copyright (c) 2022 LAAS-CNRS, TOWARD, PAL_ROBOTICS
5			* License BSD-2
6			*
7			* @brief Class to perform inverse geometry on a biped robot.
8			*/
9
10			#ifndef AIG_BIPED_IG
11			#define AIG_BIPED_IG
12
13			// clang-format off
14			#include <Eigen/Dense>
15			#include "pinocchio/fwd.hpp"
16			#include "pinocchio/multibody/data.hpp"
17			#include "pinocchio/multibody/model.hpp"
18			#include "pinocchio/spatial/se3.hpp"
19			#include "aig/arm_ig.hpp"
20			#include "aig/leg_ig.hpp"
21			#include "aig/dyna_com.hpp"
22			// clang-format on
23
24			namespace aig {
25			/**
26			* @brief @todo Describe BipedIGSettings
27			*/
28
29			struct BipedIGSettings {
30			public:
31			std::string left_hip_joint_name;
32			std::string left_knee_joint_name;
33			std::string left_ankle_joint_name;
34			std::string left_foot_frame_name;
35			std::string right_hip_joint_name;
36			std::string right_knee_joint_name;
37			std::string right_ankle_joint_name;
38			std::string right_foot_frame_name;
39			/**
40			* @brief This must contain either a valid path to the urdf file or the
41			* content of this file in a string.
42			*/
43			std::string urdf;
44			/**
45			* @brief This must contain either a valid path to the srdf file or the
46			* content of this file in a string.
47			*/
48			std::string srdf;
49
50		15	BipedIGSettings()
51		15	: left_hip_joint_name(""),
52			left_knee_joint_name(""),
53			left_ankle_joint_name(""),
54			left_foot_frame_name(""),
55			right_hip_joint_name(""),
56			right_knee_joint_name(""),
57			right_ankle_joint_name(""),
58			right_foot_frame_name(""),
59			urdf(""),
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	15	srdf("") {}
61
62		4	BipedIGSettings(const std::string &_left_hip_joint_name,
63			const std::string &_left_knee_joint_name,
64			const std::string &_left_ankle_joint_name,
65			const std::string &_left_foot_frame_name,
66			const std::string &_right_hip_joint_name,
67			const std::string &_right_knee_joint_name,
68			const std::string &_right_ankle_joint_name,
69			const std::string &_right_foot_frame_name,
70			const std::string &_urdf, const std::string &_srdf)
71		4	: left_hip_joint_name(_left_hip_joint_name),
72			left_knee_joint_name(_left_knee_joint_name),
73			left_ankle_joint_name(_left_ankle_joint_name),
74			left_foot_frame_name(_left_foot_frame_name),
75			right_hip_joint_name(_right_hip_joint_name),
76			right_knee_joint_name(_right_knee_joint_name),
77			right_ankle_joint_name(_right_ankle_joint_name),
78			right_foot_frame_name(_right_foot_frame_name),
79			urdf(_urdf),
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	srdf(_srdf) {}
81
82			friend std::ostream &operator<<(std::ostream &out,
83			const BipedIGSettings &obj) {
84			out << "BipedIGSettings:\n";
85			out << " left_hip_joint_name: " << obj.left_hip_joint_name << "\n";
86			out << " left_knee_joint_name: " << obj.left_knee_joint_name << "\n";
87			out << " left_ankle_joint_name: " << obj.left_ankle_joint_name << "\n";
88			out << " left_foot_frame_name: " << obj.left_foot_frame_name << "\n";
89			out << " right_hip_joint_name: " << obj.right_hip_joint_name << "\n";
90			out << " right_knee_joint_name: " << obj.right_knee_joint_name << "\n";
91			out << " right_ankle_joint_name: " << obj.right_ankle_joint_name << "\n";
92			out << " right_foot_frame_name: " << obj.right_foot_frame_name << "\n";
93			out << " urdf: " << obj.urdf << "\n";
94			out << " srdf: " << obj.srdf << std::endl;
95			return out;
96			}
97
98		3	friend bool operator==(const BipedIGSettings &lhs,
99			const BipedIGSettings &rhs) {
100		3	bool test = true;
101		3	test &= lhs.left_hip_joint_name == rhs.left_hip_joint_name;
102		3	test &= lhs.left_knee_joint_name == rhs.left_knee_joint_name;
103		3	test &= lhs.left_ankle_joint_name == rhs.left_ankle_joint_name;
104		3	test &= lhs.left_foot_frame_name == rhs.left_foot_frame_name;
105		3	test &= lhs.right_hip_joint_name == rhs.right_hip_joint_name;
106		3	test &= lhs.right_knee_joint_name == rhs.right_knee_joint_name;
107		3	test &= lhs.right_ankle_joint_name == rhs.right_ankle_joint_name;
108		3	test &= lhs.right_foot_frame_name == rhs.right_foot_frame_name;
109		3	test &= lhs.urdf == rhs.urdf;
110		3	test &= lhs.srdf == rhs.srdf;
111		3	return test;
112			}
113			};
114
115			BipedIGSettings makeSettingsFor(const std::string &path_to_robots,
116			const std::string &robot_name);
117
118			/**
119			* @brief @todo Describe BipedIG
120			*
121			*/
122			class BipedIG {
123			// Private attributes.
124			private:
125			pinocchio::Model model_;
126			pinocchio::Data data_;
127			BipedIGSettings settings_;
128			LegIG left_leg_, right_leg_;
129			// ArmIG left_arm_, right_arm_;
130			Eigen::VectorXd q0_; // q0_ is a reference configuration used to take all not
131			// computed joints (such as head and arms)
132			Eigen::Vector3d com_from_waist_;
133			int lleg_idx_qs_; // Indexes in the configuration vector.
134			int rleg_idx_qs_; // Indexes in the configuration vector.
135
136			// variables used in the waist-com vector correction:
137			Eigen::Vector3d error_, com_temp_;
138			Eigen::VectorXd posture_temp_;
139			Eigen::Matrix3d baseRotation_temp_;
140
141			aig::DynaCoM dynamics_;
142
143			// Private methods.
144			private:
145			void derivatives(const Eigen::VectorXd &q1, const Eigen::VectorXd &q3,
146			Eigen::VectorXd &posture, Eigen::VectorXd &velocity,
147			Eigen::VectorXd &acceleration, const double &dt);
148
149			pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
150			const pinocchio::SE3 &leftFoot,
151			const pinocchio::SE3 &rightFoot);
152
153			pinocchio::SE3 computeBase(const Eigen::Vector3d &com,
154			const Eigen::Matrix3d &baseRotation);
155
156			void configureLegs();
157
158			// Public methods.
159			public:
160			BipedIG();
161
162			BipedIG(const BipedIGSettings &settings);
163
164			void initialize(const BipedIGSettings &settings);
165
166		3	const BipedIGSettings &get_settings() { return settings_; };
167		1	const LegIGSettings &get_left_leg_settings() {
168		1	return left_leg_.get_settings();
169			};
170		1	const LegIGSettings &get_right_leg_settings() {
171		1	return right_leg_.get_settings();
172			};
173
174			const Eigen::VectorXd &getQ0() { return q0_; }
175			void setQ0(const Eigen::VectorXd q0) { q0_ = q0; }
176
177			/// @brief Get the Angular Momentum variation. Please call computeDynamics
178			/// first. Deprecate it, AIG is not made for dynamics
179		1	const Eigen::Vector3d &getAMVariation() { return dynamics_.getAMVariation(); }
180
181			/// @brief Get the CoM position. Please call computeDynamics first.
182			const Eigen::Vector3d &getCoM() { return dynamics_.getCoM(); }
183			/// @brief Get the CoM velocity. Please call computeDynamics first.
184			const Eigen::Vector3d &getVCoM() { return dynamics_.getVCoM(); }
185			/// @brief Get the CoM acceleration. Please call computeDynamics first.
186			const Eigen::Vector3d &getACoM() { return dynamics_.getACoM(); }
187			/// @brief Get the angular momentum. Please call computeDynamics first.
188		1	const Eigen::Vector3d &getAM() { return dynamics_.getAM(); }
189			/// @brief Get the CoP Position. Please call computeDynamics first.
190			const Eigen::Vector2d &getCoP() { return dynamics_.getCoP(); }
191			/// @brief Get the nonlinear effect. Please call computeDynamics first.
192		1	const Eigen::Vector2d &getNL() { return dynamics_.getNL(); }
193
194			void checkCompatibility(); // TODO
195
196			void solve(const Eigen::Vector3d &com, const pinocchio::SE3 &leftFoot,
197			const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0,
198			Eigen::VectorXd &posture, const double &tolerance = 1e-10,
199	✓✗	3	const int &max_iterations = 0);
200
201			void solve(const Eigen::Vector3d &com, const Eigen::Isometry3d &leftFeet,
202			const Eigen::Isometry3d &rightFeet, const Eigen::VectorXd &q0,
203			Eigen::VectorXd &posture, const double &tolerance = 1e-10,
204			const int &max_iterations = 0);
205
206			void solve(const Eigen::Vector3d &com, const Eigen::Matrix3d &baseRotation,
207			const pinocchio::SE3 &leftFoot, const pinocchio::SE3 &rightFoot,
208			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
209	✓✗	6	const double &tolerance = 1e-10, const int &max_iterations = 0);
210
211			void solve(const Eigen::Vector3d &com, const Eigen::Matrix3d &baseRotation,
212			const Eigen::Isometry3d &leftFoot,
213			const Eigen::Isometry3d &rightFoot, const Eigen::VectorXd &q0,
214			Eigen::VectorXd &posture, const double &tolerance = 1e-10,
215			const int &max_iterations = 0);
216
217			void solve(const pinocchio::SE3 &base, const pinocchio::SE3 &leftFoot,
218			const pinocchio::SE3 &rightFoot, const Eigen::VectorXd &q0,
219			Eigen::VectorXd &posture);
220
221			void solve(const Eigen::Isometry3d &base, const Eigen::Isometry3d &leftFoot,
222			const Eigen::Isometry3d &rightFoot, const Eigen::VectorXd &q0,
223			Eigen::VectorXd &posture);
224
225			void solve(const std::array<Eigen::Vector3d, 3> &coms,
226			const std::array<pinocchio::SE3, 3> &leftFeet,
227			const std::array<pinocchio::SE3, 3> &rightFeet,
228			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
229			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
230			const double &dt, const double &tolerance = 1e-10,
231	✓✗✓✗	2	const int &max_iterations = 0);
232
233			void solve(const std::array<Eigen::Vector3d, 3> &coms,
234			const std::array<Eigen::Isometry3d, 3> &leftFeet,
235			const std::array<Eigen::Isometry3d, 3> &rightFeet,
236			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
237			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
238			const double &dt, const double &tolerance = 1e-10,
239	✓✗	1	const int &max_iterations = 0);
240
241			void solve(const std::array<Eigen::Vector3d, 3> &coms,
242			const std::array<Eigen::Matrix3d, 3> &baseRotations,
243			const std::array<pinocchio::SE3, 3> &leftFeet,
244			const std::array<pinocchio::SE3, 3> &rightFeet,
245			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
246			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
247			const double &dt, const double &tolerance = 1e-10,
248			const int &max_iterations = 0);
249
250			void solve(const std::array<Eigen::Vector3d, 3> &coms,
251			const std::array<Eigen::Matrix3d, 3> &baseRotations,
252			const std::array<Eigen::Isometry3d, 3> &leftFeet,
253			const std::array<Eigen::Isometry3d, 3> &rightFeet,
254			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
255			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
256			const double &dt, const double &tolerance = 1e-10,
257			const int &max_iterations = 0);
258
259			void solve(const std::array<pinocchio::SE3, 3> &bases,
260			const std::array<pinocchio::SE3, 3> &leftFeet,
261			const std::array<pinocchio::SE3, 3> &rightFeet,
262			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
263			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
264			const double &dt);
265
266			void solve(const std::array<Eigen::Isometry3d, 3> &bases,
267			const std::array<Eigen::Isometry3d, 3> &leftFeet,
268			const std::array<Eigen::Isometry3d, 3> &rightFeet,
269			const Eigen::VectorXd &q0, Eigen::VectorXd &posture,
270			Eigen::VectorXd &velocity, Eigen::VectorXd &acceleration,
271			const double &dt);
272
273			void set_com_from_waist(const Eigen::Vector3d &com_from_waist);
274
275			void set_com_from_waist(const Eigen::VectorXd &q);
276
277			void correctCoMfromWaist(const Eigen::Vector3d &com,
278			const pinocchio::SE3 &leftFoot,
279			const pinocchio::SE3 &rightFoot,
280			const Eigen::VectorXd &q0,
281			const double &tolerance = 1e-10,
282			const int &max_iterations = 20);
283
284			void correctCoMfromWaist(const Eigen::Vector3d &com,
285			const Eigen::Isometry3d &leftFoot,
286			const Eigen::Isometry3d &rightFoot,
287			const Eigen::VectorXd &q0,
288			const double &tolerance = 1e-10,
289			const int &max_iterations = 20);
290
291			void computeDynamics(const Eigen::VectorXd &posture,
292			const Eigen::VectorXd &velocity,
293			const Eigen::VectorXd &acceleration,
294			const Eigen::Matrix<double, 6, 1> &externalWrench =
295			Eigen::Matrix<double, 6, 1>::Zero(),
296			bool flatHorizontalGround = true);
297
298			void computeNL(const double &w, const Eigen::VectorXd &posture,
299			const Eigen::VectorXd &velocity,
300			const Eigen::VectorXd &acceleration,
301			const Eigen::Matrix<double, 6, 1> &externalWrench =
302			Eigen::Matrix<double, 6, 1>::Zero(),
303			bool flatHorizontalGround = true);
304
305			void computeNL(const double &w);
306
307		4	pinocchio::Model &get_model() { return model_; }
308			pinocchio::Data &get_data() { return data_; }
309			Eigen::Vector3d &get_com_from_waist() { return com_from_waist_; }
310			};
311			} // namespace aig
312			#endif // AIG_BIPED_IG