Conversation
chrstrom
left a comment
chrstrom
left a comment
There was a problem hiding this comment.
See that this is still in draft, but a few comments to include with the rest of the TODOs before publishing 💯
| integral_yaw=0.0; | ||
| }; | ||
| */ | ||
| class LQRparameters { |
There was a problem hiding this comment.
Would make this a struct (usually call these "POD" structs (Plain Old Data))
There was a problem hiding this comment.
(Goes for everywhere else as well, not gonna add review comments in the other places)
| double q_surge; | ||
| double q_pitch; | ||
| double q_yaw; | ||
| double r_surge; | ||
| double r_pitch; | ||
| double r_yaw; | ||
| double i_surge; | ||
| double i_pitch; | ||
| double i_yaw; | ||
| double i_weight; | ||
| double max_force; |
There was a problem hiding this comment.
Just hold an LQRparameters object?
Also: I'd initialize all the members here, just to avoid the potential error of using an uninitialized variable (i.e. type var{}; instead of type var;
There was a problem hiding this comment.
(Goes for everywhere else as well, not gonna add review comments in the other places)
|
|
||
| // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector<double> | ||
| // &other_matrix) | ||
| const double pi = 3.14159265358979323846; |
| class angle { | ||
| public: | ||
| double phit = 0.0; | ||
| double thetat = 0.0; | ||
| double psit = 0.0; | ||
| }; | ||
| angle quaternion_to_euler_angle(double w, double x, double y, double z); |
|
|
||
| class test_VC : public rclcpp::Node { |
There was a problem hiding this comment.
should keep to a ConsistentStyle here and a few other places
| std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); | ||
| std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); | ||
| std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); |
There was a problem hiding this comment.
Think you can use structured bindings here instead of std::tie
| augmented_system_matrix << system_matrix(0, 0), system_matrix(0, 1), | ||
| system_matrix(0, 2), 0, 0, 0, system_matrix(1, 0), system_matrix(1, 1), | ||
| system_matrix(1, 2), 0, 0, 0, system_matrix(2, 0), system_matrix(2, 1), | ||
| system_matrix(2, 2), 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, | ||
| 0, -1, 0, 0, 0; | ||
| augmented_input_matrix << inertia_matrix_inv(0, 0), | ||
| inertia_matrix_inv(0, 1), inertia_matrix_inv(0, 2), 0, 0, 0, | ||
| inertia_matrix_inv(1, 0), inertia_matrix_inv(1, 1), | ||
| inertia_matrix_inv(1, 2), 0, 0, 0, inertia_matrix_inv(2, 0), | ||
| inertia_matrix_inv(2, 1), inertia_matrix_inv(2, 2), 0, 0, 0; |
There was a problem hiding this comment.
You should be able to initialize this with ::Zero() then use .topLeftCorner<>, .bottomLeftCorner<> and .leftCols<> block operations to set the non-zero blocks. Arguably easier to read that way
| extern "C" { | ||
| // Fortran subroutine for solving symplectic Schur decomposition(double | ||
| // precision version) | ||
| void sb02mt_(const char* JOBG, | ||
| const char* JOBL, |
There was a problem hiding this comment.
Genuine curiosity, where did this come from? (If you still want to keep the online solver, Eigen supports LDLT out-of-the-box. Alternatively, you could make a separate solver that runs entirely offline or just once during construction?)
| Eigen::Matrix3d vector_to_matrix3d(const std::vector<double>& other_matrix) { | ||
| Eigen::Matrix3d mat; | ||
| for (int i = 0; i < 3; ++i) | ||
| for (int j = 0; j < 3; ++j) | ||
| mat(i, j) = other_matrix[i * 3 + j]; | ||
| return mat; | ||
| } | ||
| std::vector<double> matrix3d_to_vector(const Eigen::Matrix3d& mat) { | ||
| std::vector<double> other_matrix(9); | ||
| for (int i = 0; i < 3; ++i) | ||
| for (int j = 0; j < 3; ++j) | ||
| other_matrix[i * 3 + j] = mat(i, j); | ||
| return other_matrix; | ||
| } | ||
|
|
||
| std::vector<std::vector<double>> matrix3d_to_vector2d( | ||
| const Eigen::Matrix3d& mat) { | ||
| std::vector<std::vector<double>> other_matrix(3, std::vector<double>(3)); | ||
| for (int i = 0; i < 3; ++i) | ||
| for (int j = 0; j < 3; ++j) | ||
| other_matrix[i][j] = mat(i, j); | ||
| return other_matrix; | ||
| } | ||
|
|
||
| Eigen::Matrix3d vector2d_to_matrix3d( | ||
| const std::vector<std::vector<double>>& other_matrix) { | ||
| Eigen::Matrix3d mat; | ||
| for (int i = 0; i < 3; ++i) | ||
| for (int j = 0; j < 3; ++j) | ||
| mat(i, j) = other_matrix[i][j]; | ||
| return mat; | ||
| } |
There was a problem hiding this comment.
I'd use Eigen::Map to avoid needing any loops here.
Just a few things you'd want to look out for though:
- You are implying a "correct" size for the input vector - this should be checked (constexpr'd / static-asserted if possible)
- Would you want to support both row-major and column-major? If not, might be good to specify.
| cmake_minimum_required(VERSION 3.8) | ||
| project(velocity_controller) |
There was a problem hiding this comment.
For the top-level CMakeLists: There should be some slides somewhere in the C++ learning materials that covers this, but TLDR:
- Build non-ros code as a library that can be linked into the ros/test executables
- Would be nice to have a separate cmakelists for src (non-ros), src (ros) and tests
- You should only build tests if -DBUILD_TESTING is ON/True
…set function in LQR, and added OMM to launch file
|
#697 moved here |
6 participants
Current status:
Implemented LQR regulator og solver.
Need to find/calculate system matrix and change solver
Made framework for PID regulator:
Need to tune parameters and find out why it is so slow and cant reach the reference
Made parameter and launch file
Need to change to getting parameters from global paramter file and not local
The regulator communicates with the rest of the system
Next steps:
Get a model of the system, the A matrix
Tune and fix the PID
Make a better solution for swapping control types
Change LQR solver, alternatively make myself, can retry DRAKE
Implement MPC
Further goals:
Improve system for getting parameters, make them into lists and not single values
Implement a reset off the controller
Change it into a lifecycle Node