1 #ifndef _FLEXIBILITY_COMPENSATION_ 2 #define _FLEXIBILITY_COMPENSATION_ 30 out <<
"FlexSettings:\n";
38 out <<
" dt: " << obj.
dt <<
"\n" << std::endl;
48 test &= lhs.
dt == rhs.
dt;
59 unsigned long MA_samples_;
60 const eMatrix2 xy_to_yx = (
eMatrix2() << 0, 1, 1, 0).finished();
63 std::deque<eArray2> queue_LH_, queue_RH_;
66 eArray2 temp_damping_, temp_actuation_, temp_full_torque_, temp_stiff_;
67 eArray2 temp_equiv_stiff_, temp_compliance_;
68 eArray2 summation_LH_, summation_RH_, average_;
69 unsigned long queueSize_;
75 eVector2 leftFlex0_ = eVector2::Zero();
76 eVector2 rightFlex0_ = eVector2::Zero();
78 eVector2 leftFlexRate_, rightFlexRate_;
81 eMatrix2 adaptLeftYawl_, adaptRightYawl_;
84 eMatrixRot rotationA_;
85 eMatrixRot rotationB_;
86 eMatrixRot rotationC_;
87 eMatrixRot rotationD_;
88 eMatrixRot rotationE_;
92 eMatrixRot rigidRotC_, rigidRotD_, M_;
94 const eVector3 &equivalentAngles(
const eMatrixRot &fullRotation);
95 void correctHip(
const eVector2 &delta,
const eVector2 &deltaDot, eVectorX &q,
96 eVectorX &dq,
const Eigen::Array3i &hipIndices);
100 const eArray2 &movingAverage(
const eArray2 &
x, std::deque<eArray2> &queue,
110 const eVector2 &computeDeflection(
const eArray2 &torques,
111 const eArray2 &delta0,
112 const eArray2 &stiffness,
113 const eArray2 &damping,
const double dt);
115 void correctDeflections(
const eVector2 &leftFlexingTorque,
116 const eVector2 &rightFlexingTorque, eVectorX &q,
119 void correctEstimatedDeflections(
const eVectorX &desiredTorque, eVectorX &q,
131 rightFlex0_ = delta0;
void resetLeftFlex0()
Definition: flexibility_compensation.hpp:124
void setRightFlex0(const eVector2 &delta0)
Definition: flexibility_compensation.hpp:130
Eigen::Matrix2d eMatrix2
Definition: flexibility_compensation.hpp:13
eVector2 left_stiffness
Definition: flexibility_compensation.hpp:20
void setLeftFlex0(const eVector2 &delta0)
Definition: flexibility_compensation.hpp:127
Eigen::Array3i left_hip_indices
Definition: flexibility_compensation.hpp:24
friend std::ostream & operator<<(std::ostream &out, const FlexSettings &obj)
Definition: flexibility_compensation.hpp:29
Definition: flexibility_compensation.hpp:18
Eigen::Array3i right_hip_indices
Definition: flexibility_compensation.hpp:25
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
friend bool operator==(const FlexSettings &lhs, const FlexSettings &rhs)
Definition: flexibility_compensation.hpp:42
eVector2 left_damping
Definition: flexibility_compensation.hpp:21
Eigen::Matrix3d eMatrixRot
Definition: flexibility_compensation.hpp:11
void resetRightFlex0()
Definition: flexibility_compensation.hpp:125
Eigen::VectorXd eVectorX
Definition: flexibility_compensation.hpp:12
Eigen::Vector2d eVector2
Definition: fwd.hpp:108
double MA_duration
Definition: flexibility_compensation.hpp:27
const FlexSettings & getSettings()
Definition: flexibility_compensation.hpp:122
double dt
Definition: flexibility_compensation.hpp:27
Definition: activation-quad-ref.hpp:19
eVector2 right_stiffness
Definition: flexibility_compensation.hpp:22
Definition: flexibility_compensation.hpp:56
Eigen::Array2d eArray2
Definition: flexibility_compensation.hpp:10
eVector2 right_damping
Definition: flexibility_compensation.hpp:23