Loading...
Searching...
No Matches
flexi-hips.hpp
Go to the documentation of this file.
1#ifndef _FLEXIBILITY_COMPENSATION_
2#define _FLEXIBILITY_COMPENSATION_
3
4// #include "flex-joints/fwd.hpp"
5#include <deque>
6
7#include "flex-joints/fwd.hpp"
8
9namespace flex {
10
12 public:
13 eVector2 left_stiffness = eVector2(15000, 15000); // (y, x) [Nm/rad]
14 eVector2 left_damping = 2 * left_stiffness.cwiseSqrt(); // (y, x) [Nm/rad]
15 eVector2 right_stiffness = eVector2(15000, 15000); // (y, x) [Nm/rad]
16 eVector2 right_damping = 2 * right_stiffness.cwiseSqrt(); // (y, x) [Nm/rad]
17 eVector3 flexToJoint = eVector3(0, 0, -0.09); // (x, y, z) [m]
18 Eigen::Array3i left_hip_indices = Eigen::Array3i::Zero();
19 Eigen::Array3i right_hip_indices = Eigen::Array3i::Zero();
20
21 double dt = 0.002, MA_duration = 0.01; // [s]
22 bool filtered = false;
23
24 friend std::ostream &operator<<(std::ostream &out, const FlexSettings &obj) {
25 out << "FlexSettings:\n";
26 out << " left_stiffness: " << obj.left_stiffness << "\n";
27 out << " left_damping: " << obj.left_damping << "\n";
28 out << " right_stiffness: " << obj.right_stiffness << "\n";
29 out << " left_damping: " << obj.right_damping << "\n";
30 out << " flexToJoint: " << obj.flexToJoint << "\n";
31 out << " left_hip_indices: " << obj.left_hip_indices << "\n";
32 out << " right_hip_indices: " << obj.right_hip_indices << "\n";
33 out << " filtered: " << obj.filtered << "\n";
34 out << " MA_duration: " << obj.MA_duration << "\n";
35 out << " dt: " << obj.dt << "\n" << std::endl;
36 return out;
37 }
38
39 friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs) {
40 bool test = true;
41 test &= lhs.left_stiffness == rhs.left_stiffness;
42 test &= lhs.left_damping == rhs.left_damping;
43 test &= lhs.right_stiffness == rhs.right_stiffness;
44 test &= lhs.right_damping == rhs.right_damping;
45 test &= lhs.flexToJoint == rhs.flexToJoint;
46 test &= lhs.dt == rhs.dt;
47 test &= lhs.left_hip_indices.matrix() == rhs.left_hip_indices.matrix();
48 test &= lhs.right_hip_indices.matrix() == rhs.right_hip_indices.matrix();
49 test &= lhs.MA_duration == rhs.MA_duration;
50 return test;
51 }
52};
53
54class Flex {
55 private:
56 FlexSettings settings_;
57 unsigned long MA_samples_;
58 const eMatrix2 xy_to_yx = (eMatrix2() << 0, 1, 1, 0).finished();
59
60 // memory pre-allocations:
61 std::deque<eArray2> queue_LH_, queue_RH_;
62 eVector3 resulting_angles_;
63 eVector2 computed_deflection_;
64 eArray2 temp_damping_, temp_actuation_, temp_full_torque_, temp_stiff_;
65 eArray2 temp_equiv_stiff_, temp_compliance_;
66 eArray2 average_;
67 eArray2 summation_LH_ = eArray2::Zero();
68 eArray2 summation_RH_ = eArray2::Zero();
69 unsigned long queueSize_;
70
71 // equivalentAngles:
72 double qz_;
73
74 // correctDeflections:
75 eVector2 leftFlex0_ = eVector2::Zero();
76 eVector2 rightFlex0_ = eVector2::Zero();
77 eVector2 leftFlex_, rightFlex_;
78 eVector2 leftFlexRate_, rightFlexRate_;
79
80 // estimateFlexingTorque:
81 eMatrix2 flexRotation_;
82 eMatrix2 getFlexing_;
83 eVector2 flexingTorque_;
84 eMatrixRot deformRotation_;
85 eVector3 currentFlexToJoint_;
86 eVector3 r_x_F_;
87
88 // correctEstimatedDeflections:
89 eVector2 flexingLeftTorque_, flexingRightTorque_;
90
91 // correctHip:
92 eMatrixRot rotationA_;
93 eMatrixRot rotationB_;
94 eMatrixRot rotationC_;
95 eMatrixRot rotationD_;
96 eMatrixRot rotationE_;
97 eVector3 flexRateY_, flexRateX_;
98 eVector3 dqZ_, dqX_, dqY_;
99 eVector3 legAngularVelocity_;
100 eMatrixRot rigidRotC_, rigidRotD_, M_;
101
102 const eVector3 &equivalentAngles(const eMatrixRot &fullRotation);
103 void correctHip(const eVector2 &delta, const eVector2 &deltaDot, eVectorX &q,
104 eVectorX &dq, const Eigen::Array3i &hipIndices);
105
106 const eArray2 &movingAverage(const eArray2 &x, std::deque<eArray2> &queue,
107 eArray2 &summation);
108
109 public:
110 Flex();
111
112 Flex(const FlexSettings &settings);
113
114 void initialize(const FlexSettings &settings);
115
116 const eVector2 &computeDeflection(const eArray2 &torques,
117 const eArray2 &delta0,
118 const eArray2 &stiffness,
119 const eArray2 &damping, const double dt);
120
121 const eVector3 &currentFlexToJoint(const eVector2 &delta);
122
123 const eVector2 &estimateFlexingTorque(const eVector3 &hipPos,
124 const eVector3 &jointTorque);
125
126 const eVector2 &estimateFlexingTorque(const eVector3 &hipPos,
127 const eVector3 &jointTorque,
128 const eVector2 &delta0,
129 const eVector3 &jointForce);
130
131 void correctDeflections(const eVector2 &leftFlexingTorque,
132 const eVector2 &rightFlexingTorque, eVectorX &q,
133 eVectorX &dq);
134
135 void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q,
136 eVectorX &dq, const eVector3 &leftForce,
137 const eVector3 &rightForce);
138
139 void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q,
140 eVectorX &dq);
141
142 const FlexSettings &getSettings() { return settings_; }
143
144 void setLeftStiffness(const eVector2 &stiffness);
145 void setRightStiffness(const eVector2 &stiffness);
146 void setLeftDamping(const eVector2 &damping);
147 void setRightDamping(const eVector2 &damping);
148
149 void reset();
150
151 void setLeftFlex0(const eVector2 &delta0) { leftFlex0_ = delta0; }
152 const eVector2 &getLeftFlex0(void) { return leftFlex0_; }
153
154 void setRightFlex0(const eVector2 &delta0) { rightFlex0_ = delta0; }
155 const eVector2 &getRightFlex0(void) { return rightFlex0_; }
156
157 const eVector2 getLeftFlex(void) { return leftFlex_; }
158 const eVector2 getRightFlex(void) { return rightFlex_; }
159 const eVector2 getLeftFlexRate(void) { return leftFlexRate_; }
160 const eVector2 getRightFlexRate(void) { return rightFlexRate_; }
161
162 const eArray2 &get_summation_LH(void) { return summation_LH_; }
163 const eArray2 &get_summation_RH(void) { return summation_RH_; }
164 const std::deque<eArray2> &get_queue_LH(void) { return queue_LH_; }
165 const std::deque<eArray2> &get_queue_RH(void) { return queue_RH_; }
166};
167} // namespace flex
168
169#endif
Definition flexi-hips.hpp:54
void setLeftDamping(const eVector2 &damping)
Definition flexi-hips.cpp:233
const eVector2 getLeftFlexRate(void)
Definition flexi-hips.hpp:159
void setRightDamping(const eVector2 &damping)
Definition flexi-hips.cpp:237
void setLeftStiffness(const eVector2 &stiffness)
Definition flexi-hips.cpp:225
const eVector2 & getLeftFlex0(void)
Definition flexi-hips.hpp:152
void setRightStiffness(const eVector2 &stiffness)
Definition flexi-hips.cpp:229
void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q, eVectorX &dq, const eVector3 &leftForce, const eVector3 &rightForce)
Definition flexi-hips.cpp:148
const std::deque< eArray2 > & get_queue_LH(void)
Definition flexi-hips.hpp:164
const eVector3 & currentFlexToJoint(const eVector2 &delta)
Definition flexi-hips.cpp:129
const eVector2 & getRightFlex0(void)
Definition flexi-hips.hpp:155
const eVector2 getRightFlexRate(void)
Definition flexi-hips.hpp:160
const eVector2 & estimateFlexingTorque(const eVector3 &hipPos, const eVector3 &jointTorque)
Definition flexi-hips.cpp:119
void setRightFlex0(const eVector2 &delta0)
Definition flexi-hips.hpp:154
const std::deque< eArray2 > & get_queue_RH(void)
Definition flexi-hips.hpp:165
void setLeftFlex0(const eVector2 &delta0)
Definition flexi-hips.hpp:151
const eArray2 & get_summation_RH(void)
Definition flexi-hips.hpp:163
const FlexSettings & getSettings()
Definition flexi-hips.hpp:142
const eVector2 & computeDeflection(const eArray2 &torques, const eArray2 &delta0, const eArray2 &stiffness, const eArray2 &damping, const double dt)
Definition flexi-hips.cpp:18
const eArray2 & get_summation_LH(void)
Definition flexi-hips.hpp:162
void correctDeflections(const eVector2 &leftFlexingTorque, const eVector2 &rightFlexingTorque, eVectorX &q, eVectorX &dq)
Definition flexi-hips.cpp:83
void reset()
Definition flexi-hips.cpp:216
Flex()
Definition flexi-hips.cpp:7
const eVector2 getLeftFlex(void)
Definition flexi-hips.hpp:157
void initialize(const FlexSettings &settings)
Definition flexi-hips.cpp:13
const eVector2 getRightFlex(void)
Definition flexi-hips.hpp:158
Definition flexi-hips.hpp:9
Eigen::Vector3d eVector3
Definition fwd.hpp:11
Eigen::Vector2d eVector2
Definition fwd.hpp:8
Eigen::Matrix2d eMatrix2
Definition fwd.hpp:13
Eigen::VectorXd eVectorX
Definition fwd.hpp:12
Eigen::Array2d eArray2
Definition fwd.hpp:9
Eigen::Matrix3d eMatrixRot
Definition fwd.hpp:10
Definition flexi-hips.hpp:11
friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs)
Definition flexi-hips.hpp:39
eVector2 left_stiffness
Definition flexi-hips.hpp:13
Eigen::Array3i left_hip_indices
Definition flexi-hips.hpp:18
eVector3 flexToJoint
Definition flexi-hips.hpp:17
eVector2 right_stiffness
Definition flexi-hips.hpp:15
double MA_duration
Definition flexi-hips.hpp:21
Eigen::Array3i right_hip_indices
Definition flexi-hips.hpp:19
double dt
Definition flexi-hips.hpp:21
friend std::ostream & operator<<(std::ostream &out, const FlexSettings &obj)
Definition flexi-hips.hpp:24
eVector2 left_damping
Definition flexi-hips.hpp:14
eVector2 right_damping
Definition flexi-hips.hpp:16
bool filtered
Definition flexi-hips.hpp:22