biped_ig.hpp

Go to the documentation of this file.

#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 {

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;
 std::string urdf;
 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);

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

 const Eigen::Vector3d &getAMVariation() { return dynamics_.getAMVariation(); }

 const Eigen::Vector3d &getCoM() { return dynamics_.getCoM(); }
 const Eigen::Vector3d &getVCoM() { return dynamics_.getVCoM(); }
 const Eigen::Vector3d &getACoM() { return dynamics_.getACoM(); }
 const Eigen::Vector3d &getAM() { return dynamics_.getAM(); }
 const Eigen::Vector2d &getCoP() { return dynamics_.getCoP(); }
 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