Skip to content

Commit 3f44cd4

Browse files
pre-commit-ci[bot]pre-commit-ci[bot]
committed
[pre-commit.ci] auto fixes from pre-commit.com hooks
for more information, see https://pre-commit.ci
1 parent 314a60b commit 3f44cd4

File tree

15 files changed

+843
-641
lines changed

15 files changed

+843
-641
lines changed

‎control/velocity_controller/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ ament_target_dependencies(velocity_controller_node
5252
Eigen3
5353
CasADi
5454
nav_msgs
55-
55+
5656
)
5757

5858
ament_target_dependencies(test_VC_node
@@ -98,7 +98,7 @@ install(
9898

9999
install(DIRECTORY
100100
launch
101-
config
101+
config
102102
DESTINATION share/${PROJECT_NAME}/
103103
)
104104
if(BUILD_TESTING)

‎control/velocity_controller/config/parameters.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,4 +34,4 @@
3434
#Clamp parameter
3535
max_force: 1000.0 #should maybe be 99.5
3636
controller_type: 2 #1 PID 2 LQR
37-
37+

‎control/velocity_controller/include/velocity_controller/LQR_setup.hpp‎

Lines changed: 86 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -1,90 +1,123 @@
11
#pragma once
22

3+
#include <Eigen/Dense>
4+
#include <geometry_msgs/msg/wrench_stamped.hpp>
35
#include <rclcpp/rclcpp.hpp>
46
#include <std_msgs/msg/string.hpp>
5-
#include <geometry_msgs/msg/wrench_stamped.hpp>
67
#include "PID_setup.hpp"
7-
#include <Eigen/Dense>
88
#include "velocity_controller/utilities.hpp"
99

10-
11-
1210
/*class Guidance_values{
1311
//Dataclass to store guidance values for LQR controller
1412
public:
1513
double surge=0.0; double pitch=0.0; double yaw=0.0;
16-
double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0;
14+
double integral_surge=0.0; double integral_pitch=0.0; double
15+
integral_yaw=0.0;
1716
};
1817
*/
19-
class LQRparameters{
20-
//Dataclass to store LQR parameters
21-
public:
22-
double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0;
23-
double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0;
24-
double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0;
25-
double i_weight=0.0; double max_force=0.0;
18+
class LQRparameters {
19+
// Dataclass to store LQR parameters
20+
public:
21+
double q_surge = 0.0;
22+
double q_pitch = 0.0;
23+
double q_yaw = 0.0;
24+
double r_surge = 0.0;
25+
double r_pitch = 0.0;
26+
double r_yaw = 0.0;
27+
double i_surge = 0.0;
28+
double i_pitch = 0.0;
29+
double i_yaw = 0.0;
30+
double i_weight = 0.0;
31+
double max_force = 0.0;
2632
};
2733

2834
/*class angle{
2935
public:
3036
double phit=0.0;
3137
double thetat=0.0;
3238
double psit=0.0;
33-
39+
3440
};*/
3541

36-
struct LQRsolveResult{
37-
Eigen::Matrix<double,3,6> K;
38-
Eigen::Matrix<double,6,6> P;
39-
int INFO=0;
40-
LQRsolveResult(Eigen::Matrix<double,3,6> K,Eigen::Matrix<double,6,6> P, int INFO=0):K(K),P(P),INFO(INFO) {};
42+
struct LQRsolveResult {
43+
Eigen::Matrix<double, 3, 6> K;
44+
Eigen::Matrix<double, 6, 6> P;
45+
int INFO = 0;
46+
LQRsolveResult(Eigen::Matrix<double, 3, 6> K,
47+
Eigen::Matrix<double, 6, 6> P,
48+
int INFO = 0)
49+
: K(K), P(P), INFO(INFO) {};
4150
};
42-
class LQRController{
43-
44-
public:
45-
LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},Eigen::Matrix3d inertia_matrix=Eigen::Matrix3d::Identity());
46-
51+
class LQRController {
52+
public:
53+
LQRController(LQRparameters params = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
54+
Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Identity());
4755

4856
void set_params(LQRparameters params);
49-
Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel);
57+
Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate,
58+
double yaw_rate,
59+
double sway_vel,
60+
double heave_vel);
5061
void set_matrices(Eigen::Matrix3d inertia_matrix);
5162
void update_augmented_matrices(Eigen::Matrix3d coriolis_matrix);
5263

53-
//angle quaternion_to_euler_angle(double w, double x, double y, double z);
64+
// angle quaternion_to_euler_angle(double w, double x, double y, double z);
5465
double ssa(double angle);
5566

56-
std::tuple<double,double> saturate (double value, bool windup, double limit);
57-
double anti_windup(double ki, double error, double integral_sum, bool windup);
58-
Eigen::Vector<double,3> saturate_input(Eigen::Vector<double,3> u);
59-
60-
Eigen::Vector<double,6> update_error(Guidance_data guidance_values, State states);
61-
Eigen::Vector<double,3> calculate_lqr_u(State states, Guidance_data guidance_values);
62-
63-
LQRsolveResult solve_k_p(const Eigen::Matrix<double,6,6> &A,const Eigen::Matrix<double,6,3> &B,const Eigen::Matrix<double,6,6> &Q, const Eigen::Matrix<double,3,3> &R);
64-
65-
//Resets controller
67+
std::tuple<double, double> saturate(double value,
68+
bool windup,
69+
double limit);
70+
double anti_windup(double ki,
71+
double error,
72+
double integral_sum,
73+
bool windup);
74+
Eigen::Vector<double, 3> saturate_input(Eigen::Vector<double, 3> u);
75+
76+
Eigen::Vector<double, 6> update_error(Guidance_data guidance_values,
77+
State states);
78+
Eigen::Vector<double, 3> calculate_lqr_u(State states,
79+
Guidance_data guidance_values);
80+
81+
LQRsolveResult solve_k_p(const Eigen::Matrix<double, 6, 6>& A,
82+
const Eigen::Matrix<double, 6, 3>& B,
83+
const Eigen::Matrix<double, 6, 6>& Q,
84+
const Eigen::Matrix<double, 3, 3>& R);
85+
86+
// Resets controller
6687
void reset_controller();
6788

68-
// VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector<double> &other_matrix)
69-
const double pi=3.14159265358979323846;
70-
double integral_error_surge; double integral_error_pitch; double integral_error_yaw;
71-
bool surge_windup; bool pitch_windup; bool yaw_windup;
72-
double q_surge; double q_pitch; double q_yaw;
73-
double r_surge; double r_pitch; double r_yaw;
74-
double i_surge; double i_pitch; double i_yaw;
75-
double i_weight; double max_force;
89+
// VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector<double>
90+
// &other_matrix)
91+
const double pi = 3.14159265358979323846;
92+
double integral_error_surge;
93+
double integral_error_pitch;
94+
double integral_error_yaw;
95+
bool surge_windup;
96+
bool pitch_windup;
97+
bool yaw_windup;
98+
double q_surge;
99+
double q_pitch;
100+
double q_yaw;
101+
double r_surge;
102+
double r_pitch;
103+
double r_yaw;
104+
double i_surge;
105+
double i_pitch;
106+
double i_yaw;
107+
double i_weight;
108+
double max_force;
76109

77110
Eigen::Matrix3d inertia_matrix_inv;
78-
Eigen::Matrix<double,6,6> state_weight_matrix;
111+
Eigen::Matrix<double, 6, 6> state_weight_matrix;
79112
Eigen::Matrix3d input_weight_matrix;
80-
Eigen::Matrix<double,6,6> augmented_system_matrix;
81-
Eigen::Matrix<double,6,3> augmented_input_matrix;
82-
83-
113+
Eigen::Matrix<double, 6, 6> augmented_system_matrix;
114+
Eigen::Matrix<double, 6, 3> augmented_input_matrix;
84115
};
85116

86-
//Extra operations
87-
Eigen::Matrix3d vector_to_matrix3d(const std::vector<double> &other_matrix);
88-
std::vector<double> matrix3d_to_vector(const Eigen::Matrix3d &mat);
89-
std::vector<std::vector<double>> matrix3d_to_vector2d(const Eigen::Matrix3d &mat);
90-
Eigen::Matrix3d vector2d_to_matrix3d(const std::vector<std::vector<double>> &other_matrix);
117+
// Extra operations
118+
Eigen::Matrix3d vector_to_matrix3d(const std::vector<double>& other_matrix);
119+
std::vector<double> matrix3d_to_vector(const Eigen::Matrix3d& mat);
120+
std::vector<std::vector<double>> matrix3d_to_vector2d(
121+
const Eigen::Matrix3d& mat);
122+
Eigen::Matrix3d vector2d_to_matrix3d(
123+
const std::vector<std::vector<double>>& other_matrix);

‎control/velocity_controller/include/velocity_controller/PID_setup.hpp‎

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,26 @@
11
#pragma once
22

3+
#include <geometry_msgs/msg/wrench_stamped.hpp>
34
#include <rclcpp/rclcpp.hpp>
4-
#include <std_msgs/msg/string.hpp>
55
#include <std_msgs/msg/float64_multi_array.hpp>
6-
#include <geometry_msgs/msg/wrench_stamped.hpp>
6+
#include <std_msgs/msg/string.hpp>
77
#include "utilities.hpp"
88

99
class PID_controller {
10-
public:
11-
PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output);
12-
PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {};
10+
public:
11+
PID_controller(double k_p,
12+
double k_i,
13+
double k_d,
14+
double max_output,
15+
double min_output);
16+
PID_controller(double k_p, double k_i, double k_d)
17+
: PID_controller(k_p, k_i, k_d, 100.0, -100.0) {};
1318
void calculate_thrust(double reference, double current_position, double dt);
1419
void reset_controller();
1520
double output();
1621
void set_output_limits(double min_output, double max_output);
17-
private:
22+
23+
private:
1824
double k_p;
1925
double k_i;
2026
double k_d;
@@ -25,4 +31,3 @@ class PID_controller {
2531
double max_output;
2632
double min_output;
2733
};
28-

‎control/velocity_controller/include/velocity_controller/test_VC.hpp‎

Lines changed: 24 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,45 +1,49 @@
11
#pragma once
22

3-
#include <rclcpp/rclcpp.hpp>
4-
#include <std_msgs/msg/string.hpp>
5-
#include <geometry_msgs/msg/wrench_stamped.hpp>
63
#include <cmath>
4+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
5+
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
6+
#include <geometry_msgs/msg/wrench_stamped.hpp>
7+
#include <rclcpp/rclcpp.hpp>
78
#include <std_msgs/msg/float64_multi_array.hpp>
9+
#include <std_msgs/msg/string.hpp>
810
#include "velocity_controller/PID_setup.hpp"
911
#include "velocity_controller/velocity_controller.hpp"
10-
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
11-
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
12-
#include "vortex_msgs/msg/los_guidance.hpp"
12+
#include "vortex_msgs/msg/los_guidance.hpp"
1313

14-
class test_VC : public rclcpp::Node{
15-
public:
14+
class test_VC : public rclcpp::Node {
15+
public:
1616
test_VC();
17-
//Callback functions
17+
// Callback functions
1818
void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg);
1919
void send_guidance();
2020
void send_state();
21-
22-
//Variables
23-
//guidance_data reference;
21+
22+
// Variables
23+
// guidance_data reference;
2424
Guidance_data current_state;
25-
//Subscribers and publishers
26-
rclcpp::Publisher<vortex_msgs::msg::LOSGuidance>::SharedPtr publisher_guidance;
25+
// Subscribers and publishers
26+
rclcpp::Publisher<vortex_msgs::msg::LOSGuidance>::SharedPtr
27+
publisher_guidance;
2728
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odom;
28-
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr subscription_thrust;
29-
//Timers
29+
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
30+
subscription_thrust;
31+
// Timers
3032
rclcpp::TimerBase::SharedPtr timer_;
3133
rclcpp::Clock::SharedPtr clock_;
32-
//Messages
34+
// Messages
3335
std::vector<double> thrust_vector;
3436
vortex_msgs::msg::LOSGuidance reference_msg;
3537

36-
//Topics
38+
// Topics
3739
std::string topic_odom;
3840
std::string topic_thrust;
3941
std::string topic_guidance;
4042

41-
//MSGS
43+
// MSGS
4244
nav_msgs::msg::Odometry odom_msg;
4345
};
4446

45-
geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw);
47+
geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll,
48+
double pitch,
49+
double yaw);

‎control/velocity_controller/include/velocity_controller/utilities.hpp‎

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -4,29 +4,33 @@
44
#include "std_msgs/msg/float64_multi_array.hpp"
55
#include "vortex_msgs/msg/los_guidance.hpp"
66

7-
8-
class angle{
9-
public:
10-
double phit=0.0;
11-
double thetat=0.0;
12-
double psit=0.0;
7+
class angle {
8+
public:
9+
double phit = 0.0;
10+
double thetat = 0.0;
11+
double psit = 0.0;
1312
};
1413
angle quaternion_to_euler_angle(double w, double x, double y, double z);
1514

16-
class State{
17-
//Dataclass to store state values for LQR controller
18-
public:
19-
double surge=0.0, pitch=0.0, yaw=0.0, pitch_rate=0.0, yaw_rate=0.0, heave_vel=0, sway_vel=0;
20-
double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0;
15+
class State {
16+
// Dataclass to store state values for LQR controller
17+
public:
18+
double surge = 0.0, pitch = 0.0, yaw = 0.0, pitch_rate = 0.0,
19+
yaw_rate = 0.0, heave_vel = 0, sway_vel = 0;
20+
double integral_surge = 0.0;
21+
double integral_pitch = 0.0;
22+
double integral_yaw = 0.0;
2123
};
2224

23-
class Guidance_data:public State{
24-
public:
25-
//double surge; double pitch; double yaw;
26-
Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{};
27-
Guidance_data(double surge, double pitch, double yaw):State{surge, pitch, yaw} {};
28-
Guidance_data():State{0, 0, 0} {};
29-
25+
class Guidance_data : public State {
26+
public:
27+
// double surge; double pitch; double yaw;
28+
Guidance_data(vortex_msgs::msg::LOSGuidance msg)
29+
: State{msg.surge, msg.pitch, msg.yaw} {};
30+
Guidance_data(double surge, double pitch, double yaw)
31+
: State{surge, pitch, yaw} {};
32+
Guidance_data() : State{0, 0, 0} {};
33+
3034
Guidance_data operator-(const Guidance_data& other) const;
3135
Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg);
3236
};

0 commit comments

Comments
 (0)