leg_ig.hpp
Go to the documentation of this file.
1 
10 #ifndef AIG_LEG_IG
11 #define AIG_LEG_IG
12 
13 // clang-format off
14 #include <Eigen/Dense>
15 
16 #include "pinocchio/fwd.hpp"
17 #include "pinocchio/spatial/se3.hpp"
18 // clang-format on
19 
20 namespace aig {
21 
22 typedef Eigen::Matrix<double, 6, 1> LegJoints;
23 
27 struct LegIGSettings {
28  public:
29  Eigen::Vector3d hip_from_waist = Eigen::Vector3d::Zero();
30  Eigen::Vector3d knee_from_hip = Eigen::Vector3d::Zero();
31  Eigen::Vector3d ankle_from_knee = Eigen::Vector3d::Zero();
32  Eigen::Vector3d ankle_from_foot = Eigen::Vector3d::Zero();
33 
34  friend std::ostream &operator<<(std::ostream &out, const LegIGSettings &obj) {
35  out << "LegIGSettings:\n"
36  << " hip_from_waist: " << obj.hip_from_waist.transpose() << "\n"
37  << " knee_from_hip: " << obj.knee_from_hip.transpose() << "\n"
38  << " ankle_from_knee: " << obj.ankle_from_knee.transpose() << "\n"
39  << " ankle_from_foot: " << obj.ankle_from_foot.transpose()
40  << std::endl;
41  return out;
42  }
43 
44  friend bool operator==(const LegIGSettings &lhs, const LegIGSettings &rhs) {
45  bool test = true;
46  test &= lhs.hip_from_waist == rhs.hip_from_waist;
47  test &= lhs.knee_from_hip == rhs.knee_from_hip;
48  test &= lhs.ankle_from_knee == rhs.ankle_from_knee;
49  test &= lhs.ankle_from_foot == rhs.ankle_from_foot;
50  return test;
51  }
52 };
53 
57 class LegIG {
58  private:
59  LegIGSettings settings_;
60 
61  // internals
62  Eigen::Vector3d hip_, ankle_, hip_from_ankle_;
63  double epsilon_, c5_;
64  double q2_, q3_, q4_, q5_, q6_, q7_;
65  double opp_sign_hip_from_waist_y_, sign_hip_from_ankle_z;
66  double a_, b_, c_;
67  Eigen::Matrix3d Rint_, Rext_, R_;
68  LegJoints output_;
69 
70  public:
71  LegIG();
72  LegIG(const LegIGSettings &settings);
73  void reset_internals();
74  const LegIGSettings &get_settings() { return settings_; }
75  void initialize(const LegIGSettings &settings);
76  LegJoints solve(const pinocchio::SE3 &base,
77  const pinocchio::SE3 &endEffector);
78 };
79 } // namespace aig
80 
81 #endif // AIG_LEG_IG
Definition: leg_ig.hpp:57
const LegIGSettings & get_settings()
Definition: leg_ig.hpp:74
LegJoints solve(const pinocchio::SE3 &base, const pinocchio::SE3 &endEffector)
Definition: leg_ig.cpp:32
void reset_internals()
Definition: leg_ig.cpp:20
LegIG()
Definition: leg_ig.cpp:11
void initialize(const LegIGSettings &settings)
Definition: leg_ig.cpp:15
Definition: arm_ig.hpp:18
Eigen::Matrix< double, 6, 1 > LegJoints
Definition: leg_ig.hpp:22
Definition: leg_ig.hpp:27
Eigen::Vector3d ankle_from_foot
Definition: leg_ig.hpp:32
friend std::ostream & operator<<(std::ostream &out, const LegIGSettings &obj)
Definition: leg_ig.hpp:34
Eigen::Vector3d knee_from_hip
Definition: leg_ig.hpp:30
Eigen::Vector3d ankle_from_knee
Definition: leg_ig.hpp:31
friend bool operator==(const LegIGSettings &lhs, const LegIGSettings &rhs)
Definition: leg_ig.hpp:44
Eigen::Vector3d hip_from_waist
Definition: leg_ig.hpp:29