1 |
|
|
/** |
2 |
|
|
* @file |
3 |
|
|
* @copyright Copyright (c) 2022, LAAS-CNRS, Toward, PalRobotics |
4 |
|
|
* @brief |
5 |
|
|
*/ |
6 |
|
|
|
7 |
|
|
#include "aig/leg_ig.hpp" |
8 |
|
|
|
9 |
|
|
#include <iostream> |
10 |
|
|
|
11 |
|
|
namespace aig { |
12 |
|
|
|
13 |
|
|
LegIG::LegIG() { reset_internals(); } |
14 |
|
|
|
15 |
|
|
LegIG::LegIG(const LegIGSettings &settings) { initialize(settings); } |
16 |
|
|
|
17 |
|
|
void LegIG::initialize(const LegIGSettings &settings) { |
18 |
|
|
settings_ = settings; |
19 |
|
|
reset_internals(); |
20 |
|
|
} |
21 |
|
|
|
22 |
|
|
void LegIG::reset_internals() { |
23 |
|
|
// Small quantity to prevent numerical issues. |
24 |
|
|
epsilon_ = 1.0e-6; |
25 |
|
|
hip_ = ankle_ = hip_from_ankle_ = Eigen::Vector3d::Zero(); |
26 |
|
|
c5_ = 0.0; |
27 |
|
|
q2_ = q3_ = q4_ = q5_ = q6_ = q7_ = 0.0; |
28 |
|
|
sign_hip_from_ankle_z = 0.0; |
29 |
|
|
a_ = b_ = c_ = 0.0; |
30 |
|
|
Rint_ = Rext_ = R_ = Eigen::Matrix3d::Zero(); |
31 |
|
|
output_ = LegJoints::Zero(); |
32 |
|
|
} |
33 |
|
|
|
34 |
|
|
LegJoints LegIG::solve(const pinocchio::SE3 &base, |
35 |
|
|
const pinocchio::SE3 &endEffector) { |
36 |
|
|
reset_internals(); |
37 |
|
|
|
38 |
|
|
// First we compute the position of the hip with respect to the ankle. |
39 |
|
|
hip_ = base.translation() + base.rotation() * settings_.hip_from_waist; |
40 |
|
|
ankle_ = endEffector.translation() + |
41 |
|
|
endEffector.rotation() * settings_.ankle_from_foot; |
42 |
|
|
hip_from_ankle_ = endEffector.rotation().transpose() * (hip_ - ankle_); |
43 |
|
|
|
44 |
|
|
// Compute the cos(q5) |
45 |
|
|
const Eigen::Vector3d &knee_from_hip = settings_.knee_from_hip; |
46 |
|
|
const Eigen::Vector3d &ankle_from_knee = settings_.ankle_from_knee; |
47 |
|
|
a_ = abs(knee_from_hip(2)); // Femur Height. |
48 |
|
|
b_ = abs(ankle_from_knee(2)); // Tibia Height. |
49 |
|
|
c_ = hip_from_ankle_.norm(); |
50 |
|
|
c5_ = 0.5 * (c_ * c_ - a_ * a_ - b_ * b_) / (a_ * b_); |
51 |
|
|
|
52 |
|
|
// Compute q5 (the knee). |
53 |
|
|
if (c5_ > 1.0 - epsilon_) |
54 |
|
|
q5_ = 0.0; |
55 |
|
|
else if (c5_ < -1.0 + epsilon_) |
56 |
|
|
q5_ = M_PI; |
57 |
|
|
else |
58 |
|
|
q5_ = acos(c5_); |
59 |
|
|
|
60 |
|
|
// Compute the orientation of the ankle. |
61 |
|
|
sign_hip_from_ankle_z = hip_from_ankle_(2) > 0 ? 1.0 : -1.0; |
62 |
|
|
q6_ = -atan2(hip_from_ankle_(0), |
63 |
|
|
sign_hip_from_ankle_z * hip_from_ankle_.tail<2>().norm()) - |
64 |
|
|
asin(a_ * sin(M_PI - q5_) / c_); |
65 |
|
|
|
66 |
|
|
q7_ = atan2(hip_from_ankle_(1), hip_from_ankle_(2)); |
67 |
|
|
|
68 |
|
|
if (q7_ > M_PI_2) |
69 |
|
|
q7_ -= M_PI; |
70 |
|
|
else if (q7_ < -M_PI_2) |
71 |
|
|
q7_ += M_PI; |
72 |
|
|
|
73 |
|
|
Rext_ = base.rotation().transpose() * endEffector.rotation(); |
74 |
|
|
Rint_ = Eigen::AngleAxisd(-q7_, Eigen::Vector3d(1, 0, 0)) * |
75 |
|
|
Eigen::AngleAxisd(-q5_ - q6_, Eigen::Vector3d(0, 1, 0)); |
76 |
|
|
R_ = Rext_ * Rint_; |
77 |
|
|
q2_ = atan2(-R_(0, 1), R_(1, 1)); |
78 |
|
|
q3_ = atan2(R_(2, 1), -R_(0, 1) * sin(q2_) + R_(1, 1) * cos(q2_)); |
79 |
|
|
q4_ = atan2(-R_(2, 0), R_(2, 2)); |
80 |
|
|
|
81 |
|
|
output_ << q2_, q3_, q4_, q5_, q6_, q7_; |
82 |
|
|
return output_; |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
} // namespace aig |