Henrik/velocity control #641
Conversation
for more information, see https://pre-commit.ci
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
Andeshog
left a comment
Andeshog
left a comment
There was a problem hiding this comment.
Is there a reason why you have both a PID and an LQR, or is it still in the testing phase? How much better, if at all, is the LQR compared to a PI surge controller + PID pitch and yaw?
|
|
||
| topics: | ||
| odom_topic: /orca/odom #Odometry | ||
| twist_topic: /dvl/twist #Twist | ||
| pose_topic: /dvl/pose #Pose | ||
| guidance_topic: /guidance/los #Guidance | ||
| thrust_topic: /orca/wrench_input #Thrust | ||
| softwareoperation_topic: /softwareOperationMode #Software Operation | ||
| killswitch_topic: /softwareKillSwitch #Kill Switch |
| #pragma once | ||
|
|
There was a problem hiding this comment.
To keep consistent with the rest of the codebase, maybe stick to
#ifndef XXXX_XXXXX_HPP
#define XXXX_XXXXX_HPP
//
code here
//
#endif // XXXX_XXXXX_HPP| std::tuple<double, double> saturate(double value, | ||
| bool windup, | ||
| double limit); | ||
| double anti_windup(double ki, | ||
| double error, | ||
| double integral_sum, | ||
| bool windup); | ||
| Eigen::Vector<double, 3> saturate_input(Eigen::Vector<double, 3> u); |
There was a problem hiding this comment.
Maybe these vortex-utils functions could be used instead?
| class LQRController { | ||
| public: | ||
| LQRController(LQRparameters params = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, | ||
| Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Identity()); |
There was a problem hiding this comment.
The parameters could be passed as const reference here to signal they wont be modified in the constructor.
LQRController(const LQRparameters& params, const Eigen::Matrix3d& inertia_matrix)There was a problem hiding this comment.
The same goes for other places as well 👍
Core Guidelines on const
F.16 "In" parameters
|
|
||
| // Resets controller | ||
| void reset_controller(); |
There was a problem hiding this comment.
This comment just repeats what the code says. Here are some nice guidelines regarding comments NL
There was a problem hiding this comment.
Also, for the documentation you could use Doxygen for example
| class angle { | ||
| public: | ||
| double phit = 0.0; | ||
| double thetat = 0.0; | ||
| double psit = 0.0; | ||
| }; |
There was a problem hiding this comment.
What does the t at the end mean? For state generally, consider using Eta and Nu from vortex::utils::types. Several advantages of doing that:
- Clearer to see your intent
- (Explicitly) keeping consistent with Fossen -> easier to read and understand the code
- They already include functionality and combines well with Eigen
- Less error prone since they are already tested.
If you feel they miss some functionality, feel free to implement and make a PR over at vortex-utils 💯
| LQRparameters lqr_parameters; | ||
| std::vector<double> inertia_matrix; |
There was a problem hiding this comment.
These two dont need to be members of the ros-class, since they are only used to create the controller
| rclcpp::QoS orca_QoS(2); | ||
| orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); | ||
|
|
There was a problem hiding this comment.
Premade QoS profiles are also in vortex-utils 😄
#include <vortex/utils/qos_profiles.hpp>| void publish_thrust(); | ||
| void calc_thrust(); |
There was a problem hiding this comment.
Feels unnecessary to have timers for two things that really are sequential. This also force you to store data in additional class members, which can be avoided here. All the ros node have to do is pass desired and current drone state to the controller and receive the control input, which is then packed into a WrenchStamped and published.
| // void twist_callback(const | ||
| // geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); void | ||
| // pose_callback(const | ||
| // geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr); | ||
| void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); |
There was a problem hiding this comment.
Pose and twist is what is currently used. If the new ESKF implementation decides to publish Odom instead, then the whole codebase needs refactoring anyway.
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