leg_ig.hpp

Go to the documentation of this file.

 
#ifndef AIG_LEG_IG
#define AIG_LEG_IG
 
// clang-format off
#include <Eigen/Dense>
 
#include "pinocchio/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
// clang-format on
 
namespace aig {
 
typedef Eigen::Matrix<double, 6, 1> LegJoints;
 
struct LegIGSettings {
 public:
  Eigen::Vector3d hip_from_waist;
  Eigen::Vector3d knee_from_hip;
  Eigen::Vector3d ankle_from_knee;
  Eigen::Vector3d ankle_from_foot;
 
  LegIGSettings()
      : hip_from_waist(Eigen::Vector3d::Zero()),
        knee_from_hip(Eigen::Vector3d::Zero()),
        ankle_from_knee(Eigen::Vector3d::Zero()),
        ankle_from_foot(Eigen::Vector3d::Zero()) {}
 
  LegIGSettings(const Eigen::Vector3d &_hip_from_waist,
                const Eigen::Vector3d &_knee_from_hip,
                const Eigen::Vector3d &_ankle_from_knee,
                const Eigen::Vector3d &_ankle_from_foot)
      : hip_from_waist(_hip_from_waist),
        knee_from_hip(_knee_from_hip),
        ankle_from_knee(_ankle_from_knee),
        ankle_from_foot(_ankle_from_foot) {}
 
  friend std::ostream &operator<<(std::ostream &out, const LegIGSettings &obj) {
    out << "LegIGSettings:\n"
        << "    hip_from_waist: " << obj.hip_from_waist.transpose() << "\n"
        << "    knee_from_hip: " << obj.knee_from_hip.transpose() << "\n"
        << "    ankle_from_knee: " << obj.ankle_from_knee.transpose() << "\n"
        << "    ankle_from_foot: " << obj.ankle_from_foot.transpose()
        << std::endl;
    return out;
  }
 
  friend bool operator==(const LegIGSettings &lhs, const LegIGSettings &rhs) {
    bool test = true;
    test &= lhs.hip_from_waist == rhs.hip_from_waist;
    test &= lhs.knee_from_hip == rhs.knee_from_hip;
    test &= lhs.ankle_from_knee == rhs.ankle_from_knee;
    test &= lhs.ankle_from_foot == rhs.ankle_from_foot;
    return test;
  }
};
 
class LegIG {
 private:
  LegIGSettings settings_;
 
  // internals
  Eigen::Vector3d hip_, ankle_, hip_from_ankle_;
  double epsilon_, c5_;
  double q2_, q3_, q4_, q5_, q6_, q7_;
  double sign_hip_from_ankle_z;
  double a_, b_, c_;
  Eigen::Matrix3d Rint_, Rext_, R_;
  LegJoints output_;
 
 public:
  LegIG();
  LegIG(const LegIGSettings &settings);
  void reset_internals();
  const LegIGSettings &get_settings() { return settings_; }
  void initialize(const LegIGSettings &settings);
  LegJoints solve(const pinocchio::SE3 &base,
                  const pinocchio::SE3 &endEffector);
};
}  // namespace aig
 
#endif  // AIG_LEG_IG