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 robot leg. |
8 |
|
|
*/ |
9 |
|
|
|
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 |
|
|
|
24 |
|
|
/** |
25 |
|
|
* @brief |
26 |
|
|
*/ |
27 |
|
|
struct LegIGSettings { |
28 |
|
|
public: |
29 |
|
|
Eigen::Vector3d hip_from_waist; |
30 |
|
|
Eigen::Vector3d knee_from_hip; |
31 |
|
|
Eigen::Vector3d ankle_from_knee; |
32 |
|
|
Eigen::Vector3d ankle_from_foot; |
33 |
|
|
|
34 |
|
16 |
LegIGSettings() |
35 |
|
|
: hip_from_waist(Eigen::Vector3d::Zero()), |
36 |
|
|
knee_from_hip(Eigen::Vector3d::Zero()), |
37 |
|
|
ankle_from_knee(Eigen::Vector3d::Zero()), |
38 |
✓✗✓✗
✓✗✓✗
|
16 |
ankle_from_foot(Eigen::Vector3d::Zero()) {} |
39 |
|
|
|
40 |
|
8 |
LegIGSettings(const Eigen::Vector3d &_hip_from_waist, |
41 |
|
|
const Eigen::Vector3d &_knee_from_hip, |
42 |
|
|
const Eigen::Vector3d &_ankle_from_knee, |
43 |
|
|
const Eigen::Vector3d &_ankle_from_foot) |
44 |
|
8 |
: hip_from_waist(_hip_from_waist), |
45 |
|
|
knee_from_hip(_knee_from_hip), |
46 |
|
|
ankle_from_knee(_ankle_from_knee), |
47 |
|
8 |
ankle_from_foot(_ankle_from_foot) {} |
48 |
|
|
|
49 |
|
|
friend std::ostream &operator<<(std::ostream &out, const LegIGSettings &obj) { |
50 |
|
|
out << "LegIGSettings:\n" |
51 |
|
|
<< " hip_from_waist: " << obj.hip_from_waist.transpose() << "\n" |
52 |
|
|
<< " knee_from_hip: " << obj.knee_from_hip.transpose() << "\n" |
53 |
|
|
<< " ankle_from_knee: " << obj.ankle_from_knee.transpose() << "\n" |
54 |
|
|
<< " ankle_from_foot: " << obj.ankle_from_foot.transpose() |
55 |
|
|
<< std::endl; |
56 |
|
|
return out; |
57 |
|
|
} |
58 |
|
|
|
59 |
|
4 |
friend bool operator==(const LegIGSettings &lhs, const LegIGSettings &rhs) { |
60 |
|
4 |
bool test = true; |
61 |
|
4 |
test &= lhs.hip_from_waist == rhs.hip_from_waist; |
62 |
|
4 |
test &= lhs.knee_from_hip == rhs.knee_from_hip; |
63 |
|
4 |
test &= lhs.ankle_from_knee == rhs.ankle_from_knee; |
64 |
|
4 |
test &= lhs.ankle_from_foot == rhs.ankle_from_foot; |
65 |
|
4 |
return test; |
66 |
|
|
} |
67 |
|
|
}; |
68 |
|
|
|
69 |
|
|
/** |
70 |
|
|
* @brief @todo |
71 |
|
|
*/ |
72 |
|
|
class LegIG { |
73 |
|
|
private: |
74 |
|
|
LegIGSettings settings_; |
75 |
|
|
|
76 |
|
|
// internals |
77 |
|
|
Eigen::Vector3d hip_, ankle_, hip_from_ankle_; |
78 |
|
|
double epsilon_, c5_; |
79 |
|
|
double q2_, q3_, q4_, q5_, q6_, q7_; |
80 |
|
|
double sign_hip_from_ankle_z; |
81 |
|
|
double a_, b_, c_; |
82 |
|
|
Eigen::Matrix3d Rint_, Rext_, R_; |
83 |
|
|
LegJoints output_; |
84 |
|
|
|
85 |
|
|
public: |
86 |
|
|
LegIG(); |
87 |
|
|
LegIG(const LegIGSettings &settings); |
88 |
|
|
void reset_internals(); |
89 |
|
4 |
const LegIGSettings &get_settings() { return settings_; } |
90 |
|
|
void initialize(const LegIGSettings &settings); |
91 |
|
|
LegJoints solve(const pinocchio::SE3 &base, |
92 |
|
|
const pinocchio::SE3 &endEffector); |
93 |
|
|
}; |
94 |
|
|
} // namespace aig |
95 |
|
|
|
96 |
|
|
#endif // AIG_LEG_IG |