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  Eigen::Array3i left_hip_indices = Eigen::Array3i::Zero();
18  Eigen::Array3i right_hip_indices = Eigen::Array3i::Zero();
19 
20  double dt = 0.002, MA_duration = 0.01; // [s]
21  bool filtered = false;
22 
23  friend std::ostream &operator<<(std::ostream &out, const FlexSettings &obj) {
24  out << "FlexSettings:\n";
25  out << " left_stiffness: " << obj.left_stiffness << "\n";
26  out << " left_damping: " << obj.left_damping << "\n";
27  out << " right_stiffness: " << obj.right_stiffness << "\n";
28  out << " left_damping: " << obj.right_damping << "\n";
29  out << " left_hip_indices: " << obj.left_hip_indices << "\n";
30  out << " right_hip_indices: " << obj.right_hip_indices << "\n";
31  out << " filtered: " << obj.filtered << "\n";
32  out << " MA_duration: " << obj.MA_duration << "\n";
33  out << " dt: " << obj.dt << "\n" << std::endl;
34  return out;
35  }
36 
37  friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs) {
38  bool test = true;
39  test &= lhs.left_stiffness == rhs.left_stiffness;
40  test &= lhs.left_damping == rhs.left_damping;
41  test &= lhs.right_stiffness == rhs.right_stiffness;
42  test &= lhs.right_damping == rhs.right_damping;
43  test &= lhs.dt == rhs.dt;
44  test &= lhs.left_hip_indices.matrix() == rhs.left_hip_indices.matrix();
45  test &= lhs.right_hip_indices.matrix() == rhs.right_hip_indices.matrix();
46  test &= lhs.MA_duration == rhs.MA_duration;
47  return test;
48  }
49 };
50 
51 class Flex {
52  private:
53  FlexSettings settings_;
54  unsigned long MA_samples_;
55  const eMatrix2 xy_to_yx = (eMatrix2() << 0, 1, 1, 0).finished();
56 
57  // memory pre-allocations:
58  std::deque<eArray2> queue_LH_, queue_RH_;
59  eVector3 resulting_angles_;
60  eVector2 computed_deflection_;
61  eArray2 temp_damping_, temp_actuation_, temp_full_torque_, temp_stiff_;
62  eArray2 temp_equiv_stiff_, temp_compliance_;
63  eArray2 average_;
64  eArray2 summation_LH_ = eArray2::Zero();
65  eArray2 summation_RH_ = eArray2::Zero();
66  unsigned long queueSize_;
67 
68  // equivalentAngles:
69  double qz_;
70 
71  // correctDeflections:
72  eVector2 leftFlex0_ = eVector2::Zero();
73  eVector2 rightFlex0_ = eVector2::Zero();
74  eVector2 leftFlex_, rightFlex_;
75  eVector2 leftFlexRate_, rightFlexRate_;
76 
77  // correctEstimatedDeflections:
78  eMatrix2 adaptLeftYawl_, adaptRightYawl_;
79 
80  // correctHip:
81  eMatrixRot rotationA_;
82  eMatrixRot rotationB_;
83  eMatrixRot rotationC_;
84  eMatrixRot rotationD_;
85  eMatrixRot rotationE_;
86  eVector3 flexRateY_, flexRateX_;
87  eVector3 dqZ_, dqX_, dqY_;
88  eVector3 legAngularVelocity_;
89  eMatrixRot rigidRotC_, rigidRotD_, M_;
90 
91  const eVector3 &equivalentAngles(const eMatrixRot &fullRotation);
92  void correctHip(const eVector2 &delta, const eVector2 &deltaDot, eVectorX &q,
93  eVectorX &dq, const Eigen::Array3i &hipIndices);
94 
95  const eArray2 &movingAverage(const eArray2 &x, std::deque<eArray2> &queue,
96  eArray2 &summation);
97 
98  public:
99  Flex();
100 
101  Flex(const FlexSettings &settings);
102 
103  void initialize(const FlexSettings &settings);
104 
105  const eVector2 &computeDeflection(const eArray2 &torques,
106  const eArray2 &delta0,
107  const eArray2 &stiffness,
108  const eArray2 &damping, const double dt);
109 
110  void correctDeflections(const eVector2 &leftFlexingTorque,
111  const eVector2 &rightFlexingTorque, eVectorX &q,
112  eVectorX &dq);
113 
114  void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q,
115  eVectorX &dq);
116 
117  const FlexSettings &getSettings() { return settings_; }
118 
119  void reset();
120 
121  void setLeftFlex0(const eVector2 &delta0) { leftFlex0_ = delta0; }
122  const eVector2 &getLeftFlex0(void) { return leftFlex0_; }
123 
124  void setRightFlex0(const eVector2 &delta0) { rightFlex0_ = delta0; }
125  const eVector2 &getRightFlex0(void) { return rightFlex0_; }
126 
127  const eArray2 &get_summation_LH(void) { return summation_LH_; }
128  const eArray2 &get_summation_RH(void) { return summation_RH_; }
129  const std::deque<eArray2> &get_queue_LH(void) { return queue_LH_; }
130  const std::deque<eArray2> &get_queue_RH(void) { return queue_RH_; }
131 };
132 } // namespace flex
133 
134 #endif
flex::Flex::getSettings
const FlexSettings & getSettings()
Definition: flexi-hips.hpp:117
flex::FlexSettings::right_stiffness
eVector2 right_stiffness
Definition: flexi-hips.hpp:15
flex::FlexSettings::filtered
bool filtered
Definition: flexi-hips.hpp:21
flex::Flex::get_queue_LH
const std::deque< eArray2 > & get_queue_LH(void)
Definition: flexi-hips.hpp:129
fwd.hpp
flex
Definition: flexi-hips.hpp:9
flex::FlexSettings::operator<<
friend std::ostream & operator<<(std::ostream &out, const FlexSettings &obj)
Definition: flexi-hips.hpp:23
flex::FlexSettings::right_hip_indices
Eigen::Array3i right_hip_indices
Definition: flexi-hips.hpp:18
flex::Flex::get_summation_LH
const eArray2 & get_summation_LH(void)
Definition: flexi-hips.hpp:127
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:128
flex::FlexSettings::left_hip_indices
Eigen::Array3i left_hip_indices
Definition: flexi-hips.hpp:17
flex::FlexSettings::left_damping
eVector2 left_damping
Definition: flexi-hips.hpp:14
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:130
flex::Flex
Definition: flexi-hips.hpp:51
flex::Flex::reset
void reset()
Definition: flexi-hips.cpp:162
flex::Flex::getRightFlex0
const eVector2 & getRightFlex0(void)
Definition: flexi-hips.hpp:125
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:37
flex::Flex::correctEstimatedDeflections
void correctEstimatedDeflections(const eVectorX &desiredTorque, eVectorX &q, eVectorX &dq)
Definition: flexi-hips.cpp:119
flex::eVectorX
Eigen::VectorXd eVectorX
Definition: fwd.hpp:12
flex::FlexSettings::dt
double dt
Definition: flexi-hips.hpp:20
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:124
flex::FlexSettings::MA_duration
double MA_duration
Definition: flexi-hips.hpp:20
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:122
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:121