debug servo & draft working uri-ocean-robotics#5

farhangnaderi · Jun 3, 2024 · 3808dd9 · 3808dd9
1 parent a6ba1a3
commit 3808dd9
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 7 deletions.
diff --git a/src/mvp_control/mvp_control.cpp b/src/mvp_control/mvp_control.cpp
@@ -313,22 +313,22 @@ bool MvpControl::f_optimize_thrust(Eigen::VectorXd *t, Eigen::VectorXd u) {
                 A_triplets.emplace_back(base_idx + 2, i, -omega_deltaT);
                 A_triplets.emplace_back(base_idx + 1, i + 1, 1.0);
                 A_triplets.emplace_back(base_idx + 2, i + 1, -1.0);
-                A_triplets.emplace_back(base_idx + 3, i, -std::min(omega_deltaT, gamma_upper - beta));
-                A_triplets.emplace_back(base_idx + 4, i, std::max(-omega_deltaT, gamma_lower + beta));
+                A_triplets.emplace_back(base_idx + 3, i,  -std::min(omega_deltaT,gamma_upper - beta));
+                A_triplets.emplace_back(base_idx + 4, i, std::max(-omega_deltaT, gamma_lower - beta));
                 //A_triplets.emplace_back(base_idx + 3, i, tan(-std::min(gamma_upper, normalized_alpha_upper_beta)));
                 //A_triplets.emplace_back(base_idx + 4, i, tan(std::max(gamma_lower, normalized_alpha_lower_beta)));
                 A_triplets.emplace_back(base_idx + 3, i + 1, 1.0);
                 A_triplets.emplace_back(base_idx + 4, i + 1, -1.0);
 
                 // Log the values
                //ROS_INFO_STREAM("base_idx: " << base_idx);
-                ROS_INFO_STREAM("i: " << i);
+                //ROS_INFO_STREAM("i: " << i);
                 ROS_INFO_STREAM("omega_deltaT: " << omega_deltaT);
                 ROS_INFO_STREAM("gamma_upper: " << gamma_upper);
                 ROS_INFO_STREAM("gamma_lower: " << gamma_lower);
                 ROS_INFO_STREAM("beta: " << beta);
-                ROS_INFO_STREAM("-std::min(omega_deltaT, gamma_upper - beta)): " << -std::min(omega_deltaT, gamma_upper - beta));
-                ROS_INFO_STREAM("std::max(-omega_deltaT, gamma_lower + beta)): " << std::max(-omega_deltaT, gamma_lower + beta));
+                //ROS_INFO_STREAM("-tan(gamma_upper - beta): " << -tan(gamma_upper - beta));
+                //ROS_INFO_STREAM("tan( gamma_lower + beta): " << tan( gamma_upper + beta));
 
                 // Set bounds for the constraints associated with thruster_setting 1
                 if (base_idx >= qp_instance.lower_bounds.size() || base_idx + 4 >= qp_instance.upper_bounds.size()) {

diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
@@ -1131,8 +1131,16 @@ void MvpControlROS::f_control_loop() {
 
                             Eigen::Vector3d cg_x_direction(1, 0, 0);  // X direction in cg_link's x-z plane
 
-                            // Compute the angle in radians
-                            current_angle = acos(cg_x_direction.dot(thruster_x_direction));
+                            // Compute the angle in radians using dot product
+                            double angle = acos(cg_x_direction.dot(thruster_x_direction));
+
+                            // Determine the sign of the angle using cross product
+                            Eigen::Vector3d cross_product = cg_x_direction.cross(thruster_x_direction);
+                            if (cross_product.y() < 0) {
+                                angle = -angle;
+                            }
+
+                            current_angle = angle;
 
                             //Push this so mvp_control OSQP can use it:
                             //m_mvp_control->set_current_angle(joint_name, current_angle);