Add balance control to foot controller #1636
Conversation
| void FusedFromQuat(const Eigen::Quaterniond& q, double& fusedPitch, double& fusedRoll); | ||
| Eigen::Quaterniond QuatFromFused(double fusedPitch, double fusedRoll); |
There was a problem hiding this comment.
Documentation directives. These also look like they should be inside the class?
There was a problem hiding this comment.
tbh these should be replaced with standard roll/pitch/yaw euler utilities (rpy_intrinsic_to_mat)
| FusedFromQuat(Hwt_quat, fused_pitch, fused_roll); | ||
|
|
||
| if (log_level <= DEBUG) { | ||
| // graph the correction being applied |
There was a problem hiding this comment.
| // graph the correction being applied | |
| // Graph the correction being applied |
| /// derivative calculations. | ||
| NUClear::clock::time_point last_update_time{}; | ||
|
|
||
| public: |
There was a problem hiding this comment.
All these functions seem like they're just used within the class, so they should be in the private section?
| // Conversion: Quaternion --> Fused angles (2D) | ||
| void FusedFromQuat(const Eigen::Quaterniond& q, double& fusedPitch, double& fusedRoll) { | ||
| // Calculate the fused pitch and roll | ||
| double stheta = 2.0 * (q.y() * q.w() - q.x() * q.z()); | ||
| double sphi = 2.0 * (q.y() * q.z() + q.x() * q.w()); | ||
| stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1] | ||
| sphi = (sphi >= 1.0 ? 1.0 : (sphi <= -1.0 ? -1.0 : sphi)); // Coerce sphi to [-1,1] | ||
| fusedPitch = asin(stheta); | ||
| fusedRoll = asin(sphi); | ||
| } | ||
|
|
||
| Eigen::Quaterniond QuatFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true | ||
| { |
| @@ -17,3 +19,17 @@ servo_gains: | |||
| R_KNEE: 30 | |||
| R_ANKLE_PITCH: 30 | |||
| R_ANKLE_ROLL: 30 | |||
|
|
|||
| balance: | |||
| enabled: false | |||
There was a problem hiding this comment.
is there much of a reason to have this? It seems like all the modules (kick and walk) that use the balance control have their own configs that get passed into this module.
| @@ -90,6 +90,9 @@ namespace utility::skill { | |||
|
|
|||
| /// @brief Option to only switch between planted foot if the next foot is planted | |||
| bool only_switch_when_planted = false; | |||
|
|
|||
| /// @brief Option to use balance control when walking | |||
| bool use_balance_control = false; | |||
There was a problem hiding this comment.
This is never used for walk generator stuff, so I think it should be moved to a variable on the walk/kick class
There was a problem hiding this comment.
we should add a check for the amount of error to prevent issues if the robot falls over, so if the robot is X degrees from desired setpoint, pause applying control
…/NUbots into lcraft/kick-balance-tests
P.S. It's probably not critical that we have this in and use it for robocup. I was thinking it might help with the tilting during get ups and starting to walk again when the batteries get low.