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 
9 namespace flex {
10 
11 struct FlexSettings {
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 
54 class 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
flex::Flex::getSettings
const FlexSettings & getSettings()
Definition: flexi-hips.hpp:142
flex::Flex::currentFlexToJoint
const eVector3 & currentFlexToJoint(const eVector2 &delta)
Definition: flexi-hips.cpp:129
flex::Flex::getLeftFlex
const eVector2 getLeftFlex(void)
Definition: flexi-hips.hpp:157
flex::FlexSettings::right_stiffness
eVector2 right_stiffness
Definition: flexi-hips.hpp:15
flex::FlexSettings::filtered
bool filtered
Definition: flexi-hips.hpp:22
flex::Flex::get_queue_LH
const std::deque< eArray2 > & get_queue_LH(void)
Definition: flexi-hips.hpp:164
fwd.hpp
flex
Definition: flexi-hips.hpp:9
flex::Flex::getRightFlex
const eVector2 getRightFlex(void)
Definition: flexi-hips.hpp:158
flex::Flex::getLeftFlexRate
const eVector2 getLeftFlexRate(void)
Definition: flexi-hips.hpp:159
flex::FlexSettings::operator<<
friend std::ostream & operator<<(std::ostream &out, const FlexSettings &obj)
Definition: flexi-hips.hpp:24
flex::FlexSettings::right_hip_indices
Eigen::Array3i right_hip_indices
Definition: flexi-hips.hpp:19
flex::Flex::getRightFlexRate
const eVector2 getRightFlexRate(void)
Definition: flexi-hips.hpp:160
flex::Flex::setRightDamping
void setRightDamping(const eVector2 &damping)
Definition: flexi-hips.cpp:237
flex::Flex::get_summation_LH
const eArray2 & get_summation_LH(void)
Definition: flexi-hips.hpp:162
flex::eMatrix2
Eigen::Matrix2d eMatrix2
Definition: fwd.hpp:13
flex::eVector2
Eigen::Vector2d eVector2
Definition: fwd.hpp:8
flex::Flex::get_summation_RH
const eArray2 & get_summation_RH(void)
Definition: flexi-hips.hpp:163
flex::FlexSettings::left_hip_indices
Eigen::Array3i left_hip_indices
Definition: flexi-hips.hpp:18
flex::Flex::estimateFlexingTorque
const eVector2 & estimateFlexingTorque(const eVector3 &hipPos, const eVector3 &jointTorque)
Definition: flexi-hips.cpp:119
flex::FlexSettings::left_damping
eVector2 left_damping
Definition: flexi-hips.hpp:14
flex::Flex::setLeftStiffness
void setLeftStiffness(const eVector2 &stiffness)
Definition: flexi-hips.cpp:225
flex::FlexSettings
Definition: flexi-hips.hpp:11
flex::eVector3
Eigen::Vector3d eVector3
Definition: fwd.hpp:11
flex::Flex::get_queue_RH
const std::deque< eArray2 > & get_queue_RH(void)
Definition: flexi-hips.hpp:165
flex::Flex::correctEstimatedDeflections
void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q, eVectorX &dq, const eVector3 &leftForce, const eVector3 &rightForce)
Definition: flexi-hips.cpp:148
flex::Flex::setLeftDamping
void setLeftDamping(const eVector2 &damping)
Definition: flexi-hips.cpp:233
flex::Flex
Definition: flexi-hips.hpp:54
flex::Flex::reset
void reset()
Definition: flexi-hips.cpp:216
flex::Flex::getRightFlex0
const eVector2 & getRightFlex0(void)
Definition: flexi-hips.hpp:155
flex::eMatrixRot
Eigen::Matrix3d eMatrixRot
Definition: fwd.hpp:10
flex::FlexSettings::operator==
friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs)
Definition: flexi-hips.hpp:39
flex::Flex::setRightStiffness
void setRightStiffness(const eVector2 &stiffness)
Definition: flexi-hips.cpp:229
flex::eVectorX
Eigen::VectorXd eVectorX
Definition: fwd.hpp:12
flex::FlexSettings::dt
double dt
Definition: flexi-hips.hpp:21
flex::Flex::Flex
Flex()
Definition: flexi-hips.cpp:7
flex::Flex::initialize
void initialize(const FlexSettings &settings)
Definition: flexi-hips.cpp:13
flex::Flex::setRightFlex0
void setRightFlex0(const eVector2 &delta0)
Definition: flexi-hips.hpp:154
flex::FlexSettings::MA_duration
double MA_duration
Definition: flexi-hips.hpp:21
flex::Flex::correctDeflections
void correctDeflections(const eVector2 &leftFlexingTorque, const eVector2 &rightFlexingTorque, eVectorX &q, eVectorX &dq)
Definition: flexi-hips.cpp:83
flex::eArray2
Eigen::Array2d eArray2
Definition: fwd.hpp:9
flex::FlexSettings::right_damping
eVector2 right_damping
Definition: flexi-hips.hpp:16
flex::FlexSettings::left_stiffness
eVector2 left_stiffness
Definition: flexi-hips.hpp:13
flex::Flex::getLeftFlex0
const eVector2 & getLeftFlex0(void)
Definition: flexi-hips.hpp:152
flex::FlexSettings::flexToJoint
eVector3 flexToJoint
Definition: flexi-hips.hpp:17
flex::Flex::computeDeflection
const eVector2 & computeDeflection(const eArray2 &torques, const eArray2 &delta0, const eArray2 &stiffness, const eArray2 &damping, const double dt)
Definition: flexi-hips.cpp:18
flex::Flex::setLeftFlex0
void setLeftFlex0(const eVector2 &delta0)
Definition: flexi-hips.hpp:151