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 
89  // correctEstimatedDeflections:
90  eVector2 flexingLeftTorque_, flexingRightTorque_;
91 
92  // correctHip:
93  eMatrixRot rotationA_;
94  eMatrixRot rotationB_;
95  eMatrixRot rotationC_;
96  eMatrixRot rotationD_;
97  eMatrixRot rotationE_;
98  eVector3 flexRateY_, flexRateX_;
99  eVector3 dqZ_, dqX_, dqY_;
100  eVector3 legAngularVelocity_;
101  eMatrixRot rigidRotC_, rigidRotD_, M_;
102 
103  const eVector3 &equivalentAngles(const eMatrixRot &fullRotation);
104  void correctHip(const eVector2 &delta, const eVector2 &deltaDot, eVectorX &q,
105  eVectorX &dq, const Eigen::Array3i &hipIndices);
106 
107  const eArray2 &movingAverage(const eArray2 &x, std::deque<eArray2> &queue,
108  eArray2 &summation);
109 
110  public:
111  Flex();
112 
113  Flex(const FlexSettings &settings);
114 
115  void initialize(const FlexSettings &settings);
116 
117  const eVector2 &computeDeflection(const eArray2 &torques,
118  const eArray2 &delta0,
119  const eArray2 &stiffness,
120  const eArray2 &damping, const double dt);
121 
122  const eVector3 &currentFlexToJoint(const eVector2 &delta);
123 
124  const eVector2 &estimateFlexingTorque(const eVector3 &hipPos,
125  const eVector3 &jointTorque);
126 
127  const eVector2 &estimateFlexingTorque(const eVector3 &hipPos,
128  const eVector3 &jointTorque,
129  const eVector2 &delta0,
130  const eVector3 &jointForce);
131 
132  void correctDeflections(const eVector2 &leftFlexingTorque,
133  const eVector2 &rightFlexingTorque, eVectorX &q,
134  eVectorX &dq);
135 
136  void correctEstimatedDeflections(const eVectorX &desiredTorque,
137  eVectorX &q, eVectorX &dq,
138  const eVector3 &leftForce,
139  const eVector3 &rightForce);
140 
141  void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q,
142  eVectorX &dq);
143 
144  const FlexSettings &getSettings() { return settings_; }
145 
146  void reset();
147 
148  void setLeftFlex0(const eVector2 &delta0) { leftFlex0_ = delta0; }
149  const eVector2 &getLeftFlex0(void) { return leftFlex0_; }
150 
151  void setRightFlex0(const eVector2 &delta0) { rightFlex0_ = delta0; }
152  const eVector2 &getRightFlex0(void) { return rightFlex0_; }
153 
154  const eArray2 &get_summation_LH(void) { return summation_LH_; }
155  const eArray2 &get_summation_RH(void) { return summation_RH_; }
156  const std::deque<eArray2> &get_queue_LH(void) { return queue_LH_; }
157  const std::deque<eArray2> &get_queue_RH(void) { return queue_RH_; }
158 };
159 } // namespace flex
160 
161 #endif
Eigen::Matrix3d eMatrixRot
Definition: fwd.hpp:10
Eigen::Matrix2d eMatrix2
Definition: fwd.hpp:13
eVector3 flexToJoint
Definition: flexi-hips.hpp:17
eVector2 right_stiffness
Definition: flexi-hips.hpp:15
eVector2 left_damping
Definition: flexi-hips.hpp:14
Definition: flexi-hips.hpp:9
Eigen::Array2d eArray2
Definition: fwd.hpp:9
const std::deque< eArray2 > & get_queue_RH(void)
Definition: flexi-hips.hpp:157
bool filtered
Definition: flexi-hips.hpp:22
Definition: flexi-hips.hpp:54
const eVector2 & getLeftFlex0(void)
Definition: flexi-hips.hpp:149
const eArray2 & get_summation_RH(void)
Definition: flexi-hips.hpp:155
const std::deque< eArray2 > & get_queue_LH(void)
Definition: flexi-hips.hpp:156
const FlexSettings & getSettings()
Definition: flexi-hips.hpp:144
const eVector2 & computeDeflection(const eArray2 &torques, const eArray2 &delta0, const eArray2 &stiffness, const eArray2 &damping, const double dt)
Definition: flexi-hips.cpp:18
Eigen::VectorXd eVectorX
Definition: fwd.hpp:12
const eVector2 & estimateFlexingTorque(const eVector3 &hipPos, const eVector3 &jointTorque)
Definition: flexi-hips.cpp:119
Flex()
Definition: flexi-hips.cpp:7
void reset()
Definition: flexi-hips.cpp:214
Definition: flexi-hips.hpp:11
eVector2 left_stiffness
Definition: flexi-hips.hpp:13
void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q, eVectorX &dq, const eVector3 &leftForce, const eVector3 &rightForce)
Definition: flexi-hips.cpp:149
Eigen::Array3i left_hip_indices
Definition: flexi-hips.hpp:18
const eVector2 & getRightFlex0(void)
Definition: flexi-hips.hpp:152
friend std::ostream & operator<<(std::ostream &out, const FlexSettings &obj)
Definition: flexi-hips.hpp:24
void setLeftFlex0(const eVector2 &delta0)
Definition: flexi-hips.hpp:148
friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs)
Definition: flexi-hips.hpp:39
double dt
Definition: flexi-hips.hpp:21
Eigen::Vector2d eVector2
Definition: fwd.hpp:8
Eigen::Array3i right_hip_indices
Definition: flexi-hips.hpp:19
Eigen::Vector3d eVector3
Definition: fwd.hpp:11
const eVector3 & currentFlexToJoint(const eVector2 &delta)
Definition: flexi-hips.cpp:130
void setRightFlex0(const eVector2 &delta0)
Definition: flexi-hips.hpp:151
void correctDeflections(const eVector2 &leftFlexingTorque, const eVector2 &rightFlexingTorque, eVectorX &q, eVectorX &dq)
Definition: flexi-hips.cpp:83
double MA_duration
Definition: flexi-hips.hpp:21
const eArray2 & get_summation_LH(void)
Definition: flexi-hips.hpp:154
eVector2 right_damping
Definition: flexi-hips.hpp:16
void initialize(const FlexSettings &settings)
Definition: flexi-hips.cpp:13