From ac8cf7ce411e877b8cdf24c4e6d6b87c056c297e Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Thu, 30 Oct 2025 15:43:41 +0100 Subject: [PATCH 01/35] update pid values and cartesian error publisher --- src/interface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index b780802..d7509f0 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -529,9 +529,9 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) std::make_unique(rightarm_chain, root_acc_rightarm.vel); // PID controller gains - pid_rightarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9); - pid_rightarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9); - pid_rightarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9); + pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9); + pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9); + pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9); pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9); pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9); pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9); @@ -557,7 +557,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) ); } this->ee_error_timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), + std::chrono::milliseconds(1), [this]() { this->publish_ee_errors(&eddie_state); } From 15e0a2b58ede92910a10f615c6faa0dee25bca7e Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Thu, 20 Nov 2025 18:54:17 +0100 Subject: [PATCH 02/35] refactor action definitions for arm and gripper control, add status codes and waypoints for arm control --- action/ArmControl.action | 20 ++++++++++++++------ action/GripperControl.action | 18 +++++++++++++----- 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/action/ArmControl.action b/action/ArmControl.action index abec215..c41ecb7 100644 --- a/action/ArmControl.action +++ b/action/ArmControl.action @@ -1,11 +1,19 @@ # Request: Command to control arm -geometry_msgs/Pose target_pose +geometry_msgs/Pose[] target_waypoints # Desired end effector poses, in sequence + --- # Result: Final status of the arm movement -bool success # Success -string message # Result status message -geometry_msgs/Pose final_pose # Final end effector pose achieved +int32 result_code # Result code +int32 SUCCESS = 0 +int32 INVALID_GOAL = 1 +int32 GOAL_TIMEOUT = 2 +int32 WAYPOINT_TIMEOUT = 3 +int32 CANCELLED = 4 + +string result_message # Description of the result +geometry_msgs/Pose final_pose # Final end effector pose achieved + --- # Feedback: Progress updates during arm movement -geometry_msgs/Pose current_pose # Current end effector pose -string status_message # Possible status +int32 current_waypoint_index # Index of the current waypoint being approached +geometry_msgs/Pose current_pose # Current end effector pose diff --git a/action/GripperControl.action b/action/GripperControl.action index ef5fd8b..fad8e78 100644 --- a/action/GripperControl.action +++ b/action/GripperControl.action @@ -1,13 +1,21 @@ # Request: Command to control gripper -float64 target_position # 0.0 = fully closed, 1.0 = fully open +float64 target_position # 100.0 = fully closed, 0.0 = fully open float64 velocity # Maximum velocity (optional, use default if 0.0) float64 force # Maximum force/effort (optional, use default if 0.0) + --- # Result: Final status of the gripper operation -bool success # Success -string message # Result status message +int32 result_code # Result code +int32 SUCCESS = 0 +int32 INVALID_GOAL = 1 +int32 GOAL_TIMEOUT = 2 +int32 CANCELLED = 3 + +string result_message # Description of the result float64 final_position # Final gripper position achieved + --- # Feedback: Progress updates during gripper movement -float64 current_position # Current gripper position -string status_message # Possible status message +float64 measured_position # Current gripper position +float64 measured_velocity # Current gripper velocity +float64 measured_current # Current gripper current draw From 2647538e7240ddbf9d65db58bc2e8df165fadd05 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 20 Nov 2025 20:23:01 +0100 Subject: [PATCH 03/35] Add deadband parameter to PID controller and update initialization --- include/eddie_ros/interface.hpp | 8 +- src/interface-test.cpp | 6 +- src/interface.cpp | 192 +++++++++++++++++++++++++++----- 3 files changed, 173 insertions(+), 33 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 8e84815..3ff195d 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -65,14 +65,17 @@ class PID { double i_gain, double d_gain, double error_sum_tol = 1.0, - double decay_rate = 0.0); + double decay_rate = 0.0, + double deadband = 0.0 + ); void set_gains( double p_gain, double i_gain, double d_gain, double error_sum_tol = 1.0, - double decay_rate = 0.0 + double decay_rate = 0.0, + double deadband = 0.0 ); double control(double error, double dt = 1.0); @@ -85,6 +88,7 @@ class PID { double kd; double err_sum_tol; double decay_rate; + double deadband; }; struct EddieState { diff --git a/src/interface-test.cpp b/src/interface-test.cpp index 8285bfc..2abd7b5 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -83,7 +83,7 @@ PoseType kdlToPose(const KDL::Frame& frame) { return pose; } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { +PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -91,10 +91,11 @@ PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, doub kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; + this->deadband = deadband; } void PID::set_gains( - double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate + double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband ) { err_integ = 0.0; err_last = 0.0; @@ -103,6 +104,7 @@ void PID::set_gains( kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; + this->deadband = deadband; } double PID::control(double error, double dt) { diff --git a/src/interface.cpp b/src/interface.cpp index d7509f0..6cc433a 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -375,7 +375,7 @@ void EddieRosInterface::execute_gripper_control( get_gripper_execution_flag(arm_side).store(false); } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { +PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -383,10 +383,11 @@ PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, doub kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; + this->deadband = deadband; } void PID::set_gains( - double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate + double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband ) { err_integ = 0.0; err_last = 0.0; @@ -395,29 +396,31 @@ void PID::set_gains( kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; + this->deadband = deadband; } double PID::control(double error, double dt) { - double err_diff = (error - err_last) / dt; - - if (fabs(error) > 0.0) { - // Accumulate the integral when error is non-zero - err_integ += error * dt; - - // Clamp the integral term to prevent runaway accumulation - if (err_integ > err_sum_tol) { - err_integ = err_sum_tol; - } else if (err_integ < -err_sum_tol) { - err_integ = -err_sum_tol; - } - } else { - // Decay the integral term when the error is zero + // Deadband check + if (std::abs(error) < this->deadband) { + // Decay the integral term smoothly inside the deadband err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; + // Always update last error to avoid derivative kick + err_last = error; + return 0.0; } - // err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; + // Derivative term + double err_diff = (error - err_last) / dt; + + // Integral term accumulation and clamping + err_integ += error * dt; + if (err_integ > err_sum_tol) err_integ = err_sum_tol; + if (err_integ < -err_sum_tol) err_integ = -err_sum_tol; + + // Update last error err_last = error; + // PID output return kp * error + ki * err_integ + kd * err_diff; } @@ -528,20 +531,151 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) rne_id_solver_rightarm = std::make_unique(rightarm_chain, root_acc_rightarm.vel); + + RCLCPP_INFO(this->get_logger(), "Declaring PID parameters..."); + + const double default_pos_deadband = 0.005; + const double default_rot_deadband = 0.02; + + // - Declare parameters for the RIGHT arm + // Position + this->declare_parameter("pid.right.pos.x.p", 150.0); + this->declare_parameter("pid.right.pos.x.i", 20.0); + this->declare_parameter("pid.right.pos.x.d", 10.0); + this->declare_parameter("pid.right.pos.y.p", 150.0); + this->declare_parameter("pid.right.pos.y.i", 20.0); + this->declare_parameter("pid.right.pos.y.d", 10.0); + this->declare_parameter("pid.right.pos.z.p", 150.0); + this->declare_parameter("pid.right.pos.z.i", 20.0); + this->declare_parameter("pid.right.pos.z.d", 10.0); + this->declare_parameter("pid.right.pos.deadband", default_pos_deadband); + + // Rotation + this->declare_parameter("pid.right.rot.x.p", 5.0); + this->declare_parameter("pid.right.rot.x.i", 0.0); + this->declare_parameter("pid.right.rot.x.d", 2.0); + this->declare_parameter("pid.right.rot.y.p", 5.0); + this->declare_parameter("pid.right.rot.y.i", 0.0); + this->declare_parameter("pid.right.rot.y.d", 2.0); + this->declare_parameter("pid.right.rot.z.p", 5.0); + this->declare_parameter("pid.right.rot.z.i", 0.0); + this->declare_parameter("pid.right.rot.z.d", 2.0); + this->declare_parameter("pid.right.rot.deadband", default_rot_deadband); + + // - Declare parameters for the LEFT arm + // Position + this->declare_parameter("pid.left.pos.x.p", 70.0); + this->declare_parameter("pid.left.pos.x.i", 0.0); + this->declare_parameter("pid.left.pos.x.d", 4.0); + this->declare_parameter("pid.left.pos.y.p", 70.0); + this->declare_parameter("pid.left.pos.y.i", 0.0); + this->declare_parameter("pid.left.pos.y.d", 4.0); + this->declare_parameter("pid.left.pos.z.p", 150.0); + this->declare_parameter("pid.left.pos.z.i", 8.0); + this->declare_parameter("pid.left.pos.z.d", 10.0); + this->declare_parameter("pid.left.pos.deadband", default_pos_deadband); + + // Rotation + this->declare_parameter("pid.left.rot.x.p", 5.0); + this->declare_parameter("pid.left.rot.x.i", 0.0); + this->declare_parameter("pid.left.rot.x.d", 2.0); + this->declare_parameter("pid.left.rot.y.p", 5.0); + this->declare_parameter("pid.left.rot.y.i", 0.0); + this->declare_parameter("pid.left.rot.y.d", 2.0); + this->declare_parameter("pid.left.rot.z.p", 5.0); + this->declare_parameter("pid.left.rot.z.i", 0.0); + this->declare_parameter("pid.left.rot.z.d", 2.0); + this->declare_parameter("pid.left.rot.deadband", default_rot_deadband); + + // - Get the Parameter Values into local variables + + // Get values for the RIGHT arm + double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); + double r_pos_x_i = this->get_parameter("pid.right.pos.x.i").as_double(); + double r_pos_x_d = this->get_parameter("pid.right.pos.x.d").as_double(); + double r_pos_y_p = this->get_parameter("pid.right.pos.y.p").as_double(); + double r_pos_y_i = this->get_parameter("pid.right.pos.y.i").as_double(); + double r_pos_y_d = this->get_parameter("pid.right.pos.y.d").as_double(); + double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); + double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); + double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); + double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); + + double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); + double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); + double r_rot_x_d = this->get_parameter("pid.right.rot.x.d").as_double(); + double r_rot_y_p = this->get_parameter("pid.right.rot.y.p").as_double(); + double r_rot_y_i = this->get_parameter("pid.right.rot.y.i").as_double(); + double r_rot_y_d = this->get_parameter("pid.right.rot.y.d").as_double(); + double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); + double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); + double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); + double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); + + // Get values for the LEFT arm + double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); + double l_pos_x_i = this->get_parameter("pid.left.pos.x.i").as_double(); + double l_pos_x_d = this->get_parameter("pid.left.pos.x.d").as_double(); + double l_pos_y_p = this->get_parameter("pid.left.pos.y.p").as_double(); + double l_pos_y_i = this->get_parameter("pid.left.pos.y.i").as_double(); + double l_pos_y_d = this->get_parameter("pid.left.pos.y.d").as_double(); + double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); + double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); + double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); + double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); + + double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); + double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); + double l_rot_x_d = this->get_parameter("pid.left.rot.x.d").as_double(); + double l_rot_y_p = this->get_parameter("pid.left.rot.y.p").as_double(); + double l_rot_y_i = this->get_parameter("pid.left.rot.y.i").as_double(); + double l_rot_y_d = this->get_parameter("pid.left.rot.y.d").as_double(); + double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); + double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); + double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); + double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); + + // - Set PID Gains Using the Granular Values + const double error_sum_tol = 0.9; + // const double decay_rate = 1.0; + + // Set PID controller gains for the RIGHT arm + pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, r_pos_deadband); + pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, r_pos_deadband); + pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, r_pos_deadband); + + pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, r_rot_deadband); + pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, r_rot_deadband); + pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, r_rot_deadband); + + RCLCPP_INFO(this->get_logger(), "Right Arm PID gains loaded from parameters."); + + // Set PID controller gains for the LEFT arm + pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, l_pos_deadband); + pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, l_pos_deadband); + pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, l_pos_deadband); + + pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, l_rot_deadband); + pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, l_rot_deadband); + pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, l_rot_deadband); + + RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); + + // PID controller gains - pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9); - pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9); - pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9); - pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9); - pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9); - pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9); +/* pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); + pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); + pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); + pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); + pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); + pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); - pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9); - pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9); - pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9); - pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9); - pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9); - pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9); + pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9, position_deadband); + pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9, position_deadband); + pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9, position_deadband); + pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); + pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); + pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); */ RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); From 0cf09571ed262d0a68248f9e42d5b75012419797 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 20 Nov 2025 21:23:15 +0100 Subject: [PATCH 04/35] Add dynamic parameter callback for adjustments --- src/interface.cpp | 73 +++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 61 insertions(+), 12 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index 6cc433a..1e0c479 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -663,19 +663,19 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // PID controller gains -/* pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); - pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); - pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9, position_deadband); - pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); - pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); - pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); +/* pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); + pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); + pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); + pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); + pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); + pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); - pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9, position_deadband); - pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9, position_deadband); - pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9, position_deadband); - pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); - pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); - pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, rotation_deadband); */ + pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9, l_pos_deadband); + pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9, l_pos_deadband); + pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9, l_pos_deadband); + pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); + pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); + pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); */ RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); @@ -696,6 +696,55 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) this->publish_ee_errors(&eddie_state); } ); + + // Register parameter callback for dynamic PID gain updates + callback_handle_ = this->add_on_set_parameters_callback( + [this](const std::vector ¶ms) { + for (const auto ¶m : params) { + // Right arm PID gains + if (param.get_name() == "pid_rightarm_ee_pos_x_p") pid_rightarm_ee_pos_x.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_x_i") pid_rightarm_ee_pos_x.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_x_d") pid_rightarm_ee_pos_x.kd = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_y_p") pid_rightarm_ee_pos_y.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_y_i") pid_rightarm_ee_pos_y.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_y_d") pid_rightarm_ee_pos_y.kd = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_z_p") pid_rightarm_ee_pos_z.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_z_i") pid_rightarm_ee_pos_z.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_pos_z_d") pid_rightarm_ee_pos_z.kd = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_x_p") pid_rightarm_ee_rot_x.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_x_i") pid_rightarm_ee_rot_x.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_x_d") pid_rightarm_ee_rot_x.kd = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_y_p") pid_rightarm_ee_rot_y.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_y_i") pid_rightarm_ee_rot_y.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_y_d") pid_rightarm_ee_rot_y.kd = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_z_p") pid_rightarm_ee_rot_z.kp = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_z_i") pid_rightarm_ee_rot_z.ki = param.as_double(); + if (param.get_name() == "pid_rightarm_ee_rot_z_d") pid_rightarm_ee_rot_z.kd = param.as_double(); + // Left arm PID gains + if (param.get_name() == "pid_leftarm_ee_pos_x_p") pid_leftarm_ee_pos_x.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_x_i") pid_leftarm_ee_pos_x.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_x_d") pid_leftarm_ee_pos_x.kd = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_y_p") pid_leftarm_ee_pos_y.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_y_i") pid_leftarm_ee_pos_y.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_y_d") pid_leftarm_ee_pos_y.kd = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_z_p") pid_leftarm_ee_pos_z.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_z_i") pid_leftarm_ee_pos_z.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_pos_z_d") pid_leftarm_ee_pos_z.kd = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_x_p") pid_leftarm_ee_rot_x.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_x_i") pid_leftarm_ee_rot_x.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_x_d") pid_leftarm_ee_rot_x.kd = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_y_p") pid_leftarm_ee_rot_y.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_y_i") pid_leftarm_ee_rot_y.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_y_d") pid_leftarm_ee_rot_y.kd = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_z_p") pid_leftarm_ee_rot_z.kp = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_z_i") pid_leftarm_ee_rot_z.ki = param.as_double(); + if (param.get_name() == "pid_leftarm_ee_rot_z_d") pid_leftarm_ee_rot_z.kd = param.as_double(); + } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + } + ); } void EddieRosInterface::initialize_action_servers() { From 48d0f53211983ad95dc9f69e7182579f2c3e934f Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 20 Nov 2025 21:34:08 +0100 Subject: [PATCH 05/35] Add section for adjusting PID gains at runtime in README --- README.md | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/README.md b/README.md index 2aa2b58..6a48ec3 100644 --- a/README.md +++ b/README.md @@ -85,6 +85,33 @@ run rviz: ros2 launch eddie_ros rviz.launch.py use_sim:=true ``` +## Adjusting PID gains on run-time + +First, make sure to run the `eddie_ros_interface` node as described above. + +Then, in a new terminal, source your ROS2 workspace. + +Check current PID gain values: + +```bash +ros2 param get /eddie_ros_interface pid_rightarm_ee_pos_x_p +``` + +You can check other values, such as: +```bash +pid__ee___

+``` + +e.g., +- pid_rightarm_ee_pos_x_p +- pid_leftarm_ee_pos_y_i +- pid_rightarm_ee_rot_z_d + +Set a new value for a PID gain: +```bash +ros2 param set /eddie_ros_interface pid_rightarm_ee_pos_x_p 150.0 +``` + ## Plotting cartesian error with RQT Plot You can visualize the Cartesian error of the end-effectors with `rqt_plot`: From fc9d2fb5ebd0ce10d11602176ea15bef74e7d66a Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 26 Nov 2025 21:26:29 +0100 Subject: [PATCH 06/35] fix missing eddie description dependency --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index b81f915..2ccbafe 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ robif2b coord2b + eddie_description orocos_kdl From 9c5ef6e747d3797020b06cdeaa57065672d16549 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 26 Nov 2025 21:50:06 +0100 Subject: [PATCH 07/35] refactor arm control action interface to use updated status codes --- action/ArmControl.action | 6 ++---- src/interface.cpp | 35 ++++++++++++++++++----------------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/action/ArmControl.action b/action/ArmControl.action index c41ecb7..91c2896 100644 --- a/action/ArmControl.action +++ b/action/ArmControl.action @@ -1,5 +1,5 @@ # Request: Command to control arm -geometry_msgs/Pose[] target_waypoints # Desired end effector poses, in sequence +geometry_msgs/Pose target_pose # Desired end effector pose --- # Result: Final status of the arm movement @@ -7,13 +7,11 @@ int32 result_code # Result code int32 SUCCESS = 0 int32 INVALID_GOAL = 1 int32 GOAL_TIMEOUT = 2 -int32 WAYPOINT_TIMEOUT = 3 -int32 CANCELLED = 4 +int32 CANCELLED = 3 string result_message # Description of the result geometry_msgs/Pose final_pose # Final end effector pose achieved --- # Feedback: Progress updates during arm movement -int32 current_waypoint_index # Index of the current waypoint being approached geometry_msgs/Pose current_pose # Current end effector pose diff --git a/src/interface.cpp b/src/interface.cpp index d7509f0..14e18d7 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -222,8 +222,8 @@ void EddieRosInterface::execute_arm_control( for (int i = 0; (i < max_iterations) && rclcpp::ok(); ++i) { if (goal_handle->is_canceling()) { - result->success = false; - result->message = arm_side + " arm control goal was canceled"; + result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; + result->result_message = arm_side + " arm control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); get_arm_execution_flag(arm_side).store(false); @@ -238,8 +238,8 @@ void EddieRosInterface::execute_arm_control( double rotation_error = pose_error.rot.Norm(); if (position_error < position_tolerance && rotation_error < rotation_tolerance) { - result->success = true; - result->message = arm_side + " arm successfully reached target position"; + result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; + result->result_message = arm_side + " arm successfully reached target position"; result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal succeeded - pose error: pos=%.4f rot=%.4f", @@ -250,8 +250,8 @@ void EddieRosInterface::execute_arm_control( // Update progress with current pose and error information feedback->current_pose = kdlToPose(get_current_pose_ee(arm_side)); - feedback->status_message = "Moving to target position - pos_err: " + - std::to_string(position_error) + " rot_err: " + std::to_string(rotation_error); + // feedback->status_message = "Moving to target position - pos_err: " + + // std::to_string(position_error) + " rot_err: " + std::to_string(rotation_error); goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -260,13 +260,12 @@ void EddieRosInterface::execute_arm_control( // If we reach here, the goal timed out if (rclcpp::ok()) { KDL::Twist final_error = KDL::diff(get_target_pose_ee(arm_side), get_current_pose_ee(arm_side)); - result->success = false; - result->message = arm_side + " arm control goal timed out - final error: pos=" + - std::to_string(final_error.vel.Norm()) + " rot=" + std::to_string(final_error.rot.Norm()); + result->result_code = eddie_ros::action::ArmControl::Result::GOAL_TIMEOUT; + result->result_message = arm_side + " arm control goal timed out - final error: pos=" + + std::to_string(final_error.vel.Norm()) + " rot=" + std::to_string(final_error.rot.Norm()); result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); goal_handle->abort(result); - RCLCPP_WARN(this->get_logger(), "%s arm control goal timed out after %d iterations", - arm_side.c_str(), max_iterations); + RCLCPP_WARN(this->get_logger(), "%s arm control goal timed out after %d iterations", arm_side.c_str(), max_iterations); } get_arm_execution_flag(arm_side).store(false); @@ -350,23 +349,25 @@ void EddieRosInterface::execute_gripper_control( for (int i = 0; (i < 500) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { - result->success = false; - result->message = arm_side + " gripper control goal was canceled"; + result->result_code = eddie_ros::action::GripperControl::Result::CANCELLED; + result->result_message = arm_side + " gripper control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s gripper control goal canceled", arm_side.c_str()); get_gripper_execution_flag(arm_side).store(false); return; } // Update progress with current gripper position - feedback->current_position = arm_state.gripper_pos_msr[0]; - feedback->status_message = "Moving to target position"; + feedback->measured_position = arm_state.gripper_pos_msr[0]; + feedback->measured_velocity = arm_state.gripper_vel_msr[0]; + feedback->measured_current = arm_state.gripper_cur_msr[0]; + // feedback->status_message = "Moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); } // Check if goal was achieved if (rclcpp::ok()) { - result->success = true; - result->message = arm_side + " gripper successfully moved."; + result->result_code = eddie_ros::action::GripperControl::Result::SUCCESS; + result->result_message = arm_side + " gripper successfully moved."; result->final_position = arm_state.gripper_pos_msr[0]; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Gripper control goal succeeded"); From 0e6e96d0c8181c4ba9b465b5a35b4bd03102e2d9 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 26 Nov 2025 21:50:44 +0100 Subject: [PATCH 08/35] refactor test and sim interfaces to use updated status codes --- src/interface-test.cpp | 44 +++++++++++++++++++++--------------------- src/sim-interface.cpp | 12 ++++++++---- 2 files changed, 30 insertions(+), 26 deletions(-) diff --git a/src/interface-test.cpp b/src/interface-test.cpp index 8285bfc..fd3ac62 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -190,8 +190,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) for (int i = 0; (i < 100) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { - result->success = false; - result->message = "Right arm control goal was canceled"; + result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; + result->result_message = "Right arm control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Right arm control goal canceled"); return; @@ -199,7 +199,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Update progress with current pose feedback->current_pose = kdlToPosecurrent_pose)>(pose_rightarm_ee); - feedback->status_message = "Right arm moving to target position"; + // feedback->status_message = "Right arm moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -207,8 +207,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Check if goal was achieved if (rclcpp::ok()) { - result->success = true; - result->message = "Right arm successfully moved to target position"; + result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; + result->result_message = "Right arm successfully moved to target position"; result->final_pose = kdlToPosefinal_pose)>(target_pose_rightarm_ee); goal_handle->succeed(result); } @@ -268,8 +268,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) for (int i = 0; (i < 100) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { - result->success = false; - result->message = "Left arm control goal was canceled"; + result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; + result->result_message = "Left arm control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Left arm control goal canceled"); return; @@ -277,7 +277,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Update progress with current pose feedback->current_pose = kdlToPosecurrent_pose)>(pose_leftarm_ee); - feedback->status_message = "Left arm moving to target position"; + // feedback->status_message = "Left arm moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -285,8 +285,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Check if goal was achieved if (rclcpp::ok()) { - result->success = true; - result->message = "Left arm successfully moved to target position"; + result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; + result->result_message = "Left arm successfully moved to target position"; result->final_pose = kdlToPosefinal_pose)>(target_pose_leftarm_ee); goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Left arm control goal succeeded"); @@ -358,16 +358,16 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) for (int i = 0; (i < 50) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { - result->success = false; - result->message = "Right gripper control goal was canceled"; + result->result_code = eddie_ros::action::GripperControl::Result::CANCELLED; + result->result_message = "Right gripper control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Right gripper control goal canceled"); return; } // Update progress with current gripper position - feedback->current_position = this->eddie_state.kinova_rightarm_state.gripper_pos_msr[0]; - feedback->status_message = "Right gripper moving to target position"; + feedback->measured_position = this->eddie_state.kinova_rightarm_state.gripper_pos_msr[0]; + // feedback->status_message = "Right gripper moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -375,8 +375,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Check if goal was achieved if (rclcpp::ok()) { - result->success = true; - result->message = "Right gripper successfully moved to target position"; + result->result_code = eddie_ros::action::GripperControl::Result::SUCCESS; + result->result_message = "Right gripper successfully moved to target position"; result->final_position = this->eddie_state.kinova_rightarm_state.gripper_pos_msr[0]; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Right gripper control goal succeeded"); @@ -447,16 +447,16 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) for (int i = 0; (i < 50) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { - result->success = false; - result->message = "Left gripper control goal was canceled"; + result->result_code = eddie_ros::action::GripperControl::Result::CANCELLED; + result->result_message = "Left gripper control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Left gripper control goal canceled"); return; } // Update progress with current gripper position - feedback->current_position = this->eddie_state.kinova_leftarm_state.gripper_pos_msr[0]; - feedback->status_message = "Left gripper moving to target position"; + feedback->measured_position = this->eddie_state.kinova_leftarm_state.gripper_pos_msr[0]; + // feedback->status_message = "Left gripper moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -464,8 +464,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Check if goal was achieved if (rclcpp::ok()) { - result->success = true; - result->message = "Left gripper successfully moved to target position"; + result->result_code = eddie_ros::action::GripperControl::Result::SUCCESS; + result->result_message = "Left gripper successfully moved to target position"; result->final_position = this->eddie_state.kinova_leftarm_state.gripper_pos_msr[0]; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Left gripper control goal succeeded"); diff --git a/src/sim-interface.cpp b/src/sim-interface.cpp index 5615e88..b4fa851 100644 --- a/src/sim-interface.cpp +++ b/src/sim-interface.cpp @@ -283,7 +283,8 @@ class SimInterfaceNode : public rclcpp::Node for (int i = 0; i < num_steps; ++i) { if (goal_handle->is_canceling()) { is_right_arm ? is_right_arm_busy_ = false : is_left_arm_busy_ = false; - result->success = false; + result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; + result->result_message = "Arm control goal was canceled"; goal_handle->canceled(result); return; } @@ -308,7 +309,8 @@ class SimInterfaceNode : public rclcpp::Node if (rclcpp::ok()) { is_right_arm ? is_right_arm_busy_ = false : is_left_arm_busy_ = false; - result->success = true; + result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; + result->result_message = "Arm successfully moved to target pose"; goal_handle->succeed(result); } } @@ -359,7 +361,8 @@ class SimInterfaceNode : public rclcpp::Node for (int i = 0; i < num_steps; ++i) { if (goal_handle->is_canceling()) { - result->success = false; + result->result_code = eddie_ros::action::GripperControl::Result::CANCELLED; + result->result_message = "Gripper control goal was canceled"; goal_handle->canceled(result); return; } @@ -370,7 +373,8 @@ class SimInterfaceNode : public rclcpp::Node gripper_position = target_position; // Ensure it ends at the exact target if (rclcpp::ok()) { - result->success = true; + result->result_code = eddie_ros::action::GripperControl::Result::SUCCESS; + result->result_message = "Gripper successfully moved to target position"; result->final_position = gripper_position; goal_handle->succeed(result); } From 1387cb75e688421ee725af8602f3d02f9c596933 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke <75437449+bhunecke@users.noreply.github.com> Date: Wed, 26 Nov 2025 22:00:50 +0100 Subject: [PATCH 09/35] Update robot_setup.md for clarity and redundancy Clarified instructions for cloning repositories and added a note about dependent packages for 'eddie_description'. Removed redundant cloning step. --- robot_setup.md | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/robot_setup.md b/robot_setup.md index 38b8b17..5249d54 100644 --- a/robot_setup.md +++ b/robot_setup.md @@ -46,10 +46,11 @@ mkdir -p ~/r4s/src cd ~/r4s ``` -2. Inside the `r4s` directory clone the required repositories: +2. Inside the `r4s` directory clone the required repositories and the dependent packages for `eddie_description`: ```bash vcs import src < src/eddie-ros/r4s.repos +vcs import src < src/eddie_description/dep.repos ``` 3. Install the dependencies using `rosdep`: @@ -71,12 +72,6 @@ You can now source the workspace: source install/setup.bash ``` -Also clone the dependent packages for eddie_description: - -```bash -vcs import src < src/eddie_description/dep.repos -``` - ### Setup steps for SOEM To communicate with the robot base using EtherCAT, you need to install the SOEM library. From 1af9be24b859ea3eccbf39fbc0df11eaba8ed74a Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Tue, 2 Dec 2025 15:33:33 +0100 Subject: [PATCH 10/35] refactor: update idle state to run gravity compensation --- include/eddie_ros/interface.hpp | 2 +- src/interface-test.cpp | 2 +- src/interface.cpp | 171 +++++++++++++++----------------- 3 files changed, 82 insertions(+), 93 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 8e84815..52712d1 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -202,7 +202,7 @@ class EddieRosInterface : public rclcpp::Node { // sm methods void configure(events *eventData, EddieState *eddie_state); - void idle(events *eventData, const EddieState *eddie_state); + void idle(events *eventData, EddieState *eddie_state); void compile(events *eventData, const EddieState *eddie_state); void execute(events *eventData, EddieState *eddie_state); diff --git a/src/interface-test.cpp b/src/interface-test.cpp index fd3ac62..9d1daad 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -811,7 +811,7 @@ void EddieRosInterface::configure(events *eventData, EddieState *eddie_state) { produce_event(eventData, E_CONFIGURE_EXIT); } -void EddieRosInterface::idle(events *eventData, const EddieState *eddie_state) { +void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { if (should_control_right_arm()) { for (int i = 0; i < num_jnts_rightarm; i++) { q_rightarm(i) = eddie_state->kinova_rightarm_state.pos_msr[i]; diff --git a/src/interface.cpp b/src/interface.cpp index 14e18d7..d9b85da 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -216,8 +216,8 @@ void EddieRosInterface::execute_arm_control( rclcpp::Rate loop_rate(100); // TODO: this definitely needs some tweaking - const double position_tolerance = 0.04; // 4cm - const double rotation_tolerance = 0.05; // ~3 degrees + const double position_tolerance = 0.02; // 2cm + // const double rotation_tolerance = 0.05; // ~3 degrees const int max_iterations = 1000; // Timeout after 10 seconds at 100Hz for (int i = 0; (i < max_iterations) && rclcpp::ok(); ++i) { @@ -237,7 +237,7 @@ void EddieRosInterface::execute_arm_control( double position_error = pose_error.vel.Norm(); double rotation_error = pose_error.rot.Norm(); - if (position_error < position_tolerance && rotation_error < rotation_tolerance) { + if (position_error < position_tolerance /*&& rotation_error < rotation_tolerance*/) { result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; result->result_message = arm_side + " arm successfully reached target position"; result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); @@ -250,8 +250,6 @@ void EddieRosInterface::execute_arm_control( // Update progress with current pose and error information feedback->current_pose = kdlToPose(get_current_pose_ee(arm_side)); - // feedback->status_message = "Moving to target position - pos_err: " + - // std::to_string(position_error) + " rot_err: " + std::to_string(rotation_error); goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -265,7 +263,8 @@ void EddieRosInterface::execute_arm_control( std::to_string(final_error.vel.Norm()) + " rot=" + std::to_string(final_error.rot.Norm()); result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); goal_handle->abort(result); - RCLCPP_WARN(this->get_logger(), "%s arm control goal timed out after %d iterations", arm_side.c_str(), max_iterations); + RCLCPP_WARN(this->get_logger(), "%s arm control goal timed out after %d iterations, final error: pos=%.4f rot=%.4f", + arm_side.c_str(), max_iterations, final_error.vel.Norm(), final_error.rot.Norm()); } get_arm_execution_flag(arm_side).store(false); @@ -360,7 +359,6 @@ void EddieRosInterface::execute_gripper_control( feedback->measured_position = arm_state.gripper_pos_msr[0]; feedback->measured_velocity = arm_state.gripper_vel_msr[0]; feedback->measured_current = arm_state.gripper_cur_msr[0]; - // feedback->status_message = "Moving to target position"; goal_handle->publish_feedback(feedback); loop_rate.sleep(); } @@ -530,16 +528,16 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) std::make_unique(rightarm_chain, root_acc_rightarm.vel); // PID controller gains - pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9); - pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9); - pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9); + pid_rightarm_ee_pos_x.set_gains(70.0, 20.0, 10.0, 0.9); + pid_rightarm_ee_pos_y.set_gains(70.0, 20.0, 10.0, 0.9); + pid_rightarm_ee_pos_z.set_gains(90.0, 20.0, 10.0, 0.9); pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9); pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9); pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9); - pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9); - pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9); - pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9); + pid_leftarm_ee_pos_x.set_gains(70.0, 20., 10.0, 0.9); + pid_leftarm_ee_pos_y.set_gains(70.0, 20., 10.0, 0.9); + pid_leftarm_ee_pos_z.set_gains(90.0, 20.0, 10.0, 0.9); pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9); pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9); pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9); @@ -655,9 +653,6 @@ void EddieRosInterface::initialize_action_servers() { }; // Create action servers based on which arms are being controlled - RCLCPP_INFO(get_logger(), "Should control right arm: %s", should_control_right_arm() ? "true" : "false"); //TODO remove - RCLCPP_INFO(get_logger(), "Should control left arm: %s", should_control_left_arm() ? "true" : "false"); //TODO remove - if (should_control_right_arm()) { RCLCPP_INFO(get_logger(), "Creating action servers for the RIGHT arm"); action_server_right_arm_control_ = rclcpp_action::create_server( @@ -977,10 +972,11 @@ void EddieRosInterface::configure(events *eventData, EddieState *eddie_state) { produce_event(eventData, E_CONFIGURE_EXIT); } -void EddieRosInterface::idle(events *eventData, const EddieState *eddie_state) { +void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { + RCLCPP_INFO(get_logger(), "In idle state"); if (should_control_right_arm()) { - robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); - robif2b_kinova_gen3_update(&kinova_rightarm); + // robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); + // robif2b_kinova_gen3_update(&kinova_rightarm); for (int i = 0; i < num_jnts_rightarm; i++) { q_rightarm(i) = eddie_state->kinova_rightarm_state.pos_msr[i]; qd_rightarm(i) = eddie_state->kinova_rightarm_state.vel_msr[i]; @@ -992,11 +988,14 @@ void EddieRosInterface::idle(events *eventData, const EddieState *eddie_state) { KDL::FrameVel _twist_rightarm_ee; fvk_twist_rightarm_ee.JntToCart(q_qd_rightarm, _twist_rightarm_ee); twist_rightarm_ee = _twist_rightarm_ee.deriv(); - target_pose_rightarm_ee = pose_rightarm_ee; + // Initialize target pose if it hasn't been set yet + if (target_pose_rightarm_ee.p == KDL::Vector::Zero()) { + target_pose_rightarm_ee = pose_rightarm_ee; + } } if (should_control_left_arm()) { - robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); - robif2b_kinova_gen3_update(&kinova_leftarm); + // robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); + // robif2b_kinova_gen3_update(&kinova_leftarm); for (int i = 0; i < num_jnts_leftarm; i++) { q_leftarm(i) = eddie_state->kinova_leftarm_state.pos_msr[i]; qd_leftarm(i) = eddie_state->kinova_leftarm_state.vel_msr[i]; @@ -1008,10 +1007,22 @@ void EddieRosInterface::idle(events *eventData, const EddieState *eddie_state) { KDL::FrameVel _twist_leftarm_ee; fvk_twist_leftarm_ee.JntToCart(q_qd_leftarm, _twist_leftarm_ee); twist_leftarm_ee = _twist_leftarm_ee.deriv(); - target_pose_leftarm_ee = pose_leftarm_ee; + // Initialize target pose if it hasn't been set yet + if (target_pose_leftarm_ee.p == KDL::Vector::Zero()) { + target_pose_leftarm_ee = pose_leftarm_ee; + } + } + compute_cartesian_ctrl(eventData, eddie_state); + if (should_control_right_arm()) { + robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); + robif2b_kinova_gen3_update(&kinova_rightarm); + } + if (should_control_left_arm()) { + robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); + robif2b_kinova_gen3_update(&kinova_leftarm); } RCLCPP_DEBUG(get_logger(), "Exiting idle state"); - produce_event(eventData, E_IDLE_EXIT_EXECUTE); + // produce_event(eventData, E_IDLE_EXIT_EXECUTE); } void EddieRosInterface::compile(events *eventData, const EddieState *eddie_state) { @@ -1205,7 +1216,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { - // RCLCPP_INFO(get_logger(), "In execute state"); + RCLCPP_INFO(get_logger(), "In execute state"); // // Update the EtherCAT state // robif2b_ethercat_update(&ecat); @@ -1243,60 +1254,38 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { fvk_twist_leftarm_ee.JntToCart(q_qd_leftarm, _twist_leftarm_ee); twist_leftarm_ee = _twist_leftarm_ee.deriv(); - - // Log arm position - // double roll, pitch, yaw; - // _twist_leftarm_ee.GetFrame().M.GetRPY(roll, pitch, yaw); - - // KDL::Rotation R; - // R = KDL::Rotation::RPY( - // roll, - // pitch, - // yaw - // ); - // double ra; - // double rb; - // double rc; - // R.GetRPY(ra, rb, rc); - // RCLCPP_INFO(get_logger(), "Left Arm EE Pose: Position: [%f, %f, %f], Orientation: [%f, %f, %f]", - // pose_leftarm_ee.p.x(), pose_leftarm_ee.p.y(), pose_leftarm_ee.p.z(), - // ra, rb, rc); - - - // Apply relative target poses from action goals (replaces the demo 20cm movement) // Set new target pose for left arm from action goal if (should_control_left_arm() && new_target_leftarm) { KDL::Frame new_target_pose_leftarm_ee = pose_leftarm_ee * target_pose_leftarm_relative; - RCLCPP_INFO(get_logger(), "Applying action goal target pose for left arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", - new_target_pose_leftarm_ee.p.x(), new_target_pose_leftarm_ee.p.y(), new_target_pose_leftarm_ee.p.z(), - pose_leftarm_ee.p.x(), pose_leftarm_ee.p.y(), pose_leftarm_ee.p.z()); + // RCLCPP_INFO(get_logger(), "Applying action goal target pose for left arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", + // new_target_pose_leftarm_ee.p.x(), new_target_pose_leftarm_ee.p.y(), new_target_pose_leftarm_ee.p.z(), + // pose_leftarm_ee.p.x(), pose_leftarm_ee.p.y(), pose_leftarm_ee.p.z()); target_pose_leftarm_ee = new_target_pose_leftarm_ee; new_target_leftarm = false; // Reset flag } - // Set new target pose for right arm from action goal if (should_control_right_arm() && new_target_rightarm) { // Apply relative transformation in end-effector frame KDL::Frame new_target_pose_rightarm_ee = pose_rightarm_ee * target_pose_rightarm_relative; // relative transformation being applied - RCLCPP_INFO(get_logger(), "Applying relative transform: offset(%.3f, %.3f, %.3f)", - target_pose_rightarm_relative.p.x(), target_pose_rightarm_relative.p.y(), target_pose_rightarm_relative.p.z()); + // RCLCPP_INFO(get_logger(), "Applying relative transform: offset(%.3f, %.3f, %.3f)", + // target_pose_rightarm_relative.p.x(), target_pose_rightarm_relative.p.y(), target_pose_rightarm_relative.p.z()); // end-effector orientation - double roll, pitch, yaw; - pose_rightarm_ee.M.GetRPY(roll, pitch, yaw); - RCLCPP_INFO(get_logger(), "End-effector orientation (RPY): [%.3f, %.3f, %.3f] rad", roll, pitch, yaw); + // double roll, pitch, yaw; + // pose_rightarm_ee.M.GetRPY(roll, pitch, yaw); + // RCLCPP_INFO(get_logger(), "End-effector orientation (RPY): [%.3f, %.3f, %.3f] rad", roll, pitch, yaw); // relative Z movement translations in base frame - KDL::Vector ee_z_axis = pose_rightarm_ee.M.UnitZ(); // End-effectors Z-axis in base frame - RCLCPP_INFO(get_logger(), "EE Z-axis in base frame: [%.3f, %.3f, %.3f]", ee_z_axis.x(), ee_z_axis.y(), ee_z_axis.z()); + // KDL::Vector ee_z_axis = pose_rightarm_ee.M.UnitZ(); // End-effectors Z-axis in base frame + // RCLCPP_INFO(get_logger(), "EE Z-axis in base frame: [%.3f, %.3f, %.3f]", ee_z_axis.x(), ee_z_axis.y(), ee_z_axis.z()); - RCLCPP_INFO(get_logger(), "Applying action goal target pose for right arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", - new_target_pose_rightarm_ee.p.x(), new_target_pose_rightarm_ee.p.y(), new_target_pose_rightarm_ee.p.z(), - pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); + // RCLCPP_INFO(get_logger(), "Applying action goal target pose for right arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", + // new_target_pose_rightarm_ee.p.x(), new_target_pose_rightarm_ee.p.y(), new_target_pose_rightarm_ee.p.z(), + // pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); target_pose_rightarm_ee = new_target_pose_rightarm_ee; new_target_rightarm = false; // Reset flag @@ -1307,37 +1296,37 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { compute_cartesian_ctrl(eventData, eddie_state); // Show target vs current pose occasionally and current gripper commands - static int debug_counter = 0; - if (++debug_counter % 1000 == 0) { // Every 1000 cycles (1 second at 1kHz) - if (should_control_right_arm()) { - KDL::Twist pose_error = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", - pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), - pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); - RCLCPP_INFO(get_logger(), "Right arm target pose: Position: [%f, %f, %f]", - target_pose_rightarm_ee.p.x(), target_pose_rightarm_ee.p.y(), target_pose_rightarm_ee.p.z()); - RCLCPP_INFO(get_logger(), "Right arm current pose: Position: [%f, %f, %f]", - pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); - RCLCPP_INFO(get_logger(), "Right arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", - eddie_state->kinova_rightarm_state.gripper_pos_cmd[0], - eddie_state->kinova_rightarm_state.gripper_vel_cmd[0], - eddie_state->kinova_rightarm_state.gripper_frc_cmd[0]); - RCLCPP_INFO(get_logger(), "Right arm gripper metrics: pos=%.3f, vel=%.3f, force=%.3f", - eddie_state->kinova_rightarm_state.gripper_pos_msr[0], - eddie_state->kinova_rightarm_state.gripper_vel_msr[0], - eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); - } - if (should_control_left_arm()) { - KDL::Twist pose_error = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - RCLCPP_INFO(get_logger(), "Left arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", - pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), - pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); - RCLCPP_INFO(get_logger(), "Left arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", - eddie_state->kinova_leftarm_state.gripper_pos_cmd[0], - eddie_state->kinova_leftarm_state.gripper_vel_cmd[0], - eddie_state->kinova_leftarm_state.gripper_frc_cmd[0]); - } - } + // static int debug_counter = 0; + // if (++debug_counter % 1000 == 0) { // Every 1000 cycles (1 second at 1kHz) + // if (should_control_right_arm()) { + // KDL::Twist pose_error = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); + // RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", + // pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), + // pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); + // RCLCPP_INFO(get_logger(), "Right arm target pose: Position: [%f, %f, %f]", + // target_pose_rightarm_ee.p.x(), target_pose_rightarm_ee.p.y(), target_pose_rightarm_ee.p.z()); + // RCLCPP_INFO(get_logger(), "Right arm current pose: Position: [%f, %f, %f]", + // pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); + // RCLCPP_INFO(get_logger(), "Right arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", + // eddie_state->kinova_rightarm_state.gripper_pos_cmd[0], + // eddie_state->kinova_rightarm_state.gripper_vel_cmd[0], + // eddie_state->kinova_rightarm_state.gripper_frc_cmd[0]); + // RCLCPP_INFO(get_logger(), "Right arm gripper metrics: pos=%.3f, vel=%.3f, force=%.3f", + // eddie_state->kinova_rightarm_state.gripper_pos_msr[0], + // eddie_state->kinova_rightarm_state.gripper_vel_msr[0], + // eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); + // } + // if (should_control_left_arm()) { + // KDL::Twist pose_error = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); + // RCLCPP_INFO(get_logger(), "Left arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", + // pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), + // pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); + // RCLCPP_INFO(get_logger(), "Left arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", + // eddie_state->kinova_leftarm_state.gripper_pos_cmd[0], + // eddie_state->kinova_leftarm_state.gripper_vel_cmd[0], + // eddie_state->kinova_leftarm_state.gripper_frc_cmd[0]); + // } + // } // robif2b_kelo_drive_actuator_update(&wheel_act); if (should_control_right_arm()) { From 185dc2de48e62712931e5759d27a5e9b7113c73d Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 4 Dec 2025 16:49:05 +0100 Subject: [PATCH 11/35] Undo changes in PID control --- README.md | 12 +++++----- src/interface.cpp | 59 ++++++++++++++++++++++++----------------------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 6a48ec3..f5342c4 100644 --- a/README.md +++ b/README.md @@ -94,22 +94,22 @@ Then, in a new terminal, source your ROS2 workspace. Check current PID gain values: ```bash -ros2 param get /eddie_ros_interface pid_rightarm_ee_pos_x_p +ros2 param get /eddie_ros_interface pid.right.pos.x.p ``` You can check other values, such as: ```bash -pid__ee___

+pid_.ee...

``` e.g., -- pid_rightarm_ee_pos_x_p -- pid_leftarm_ee_pos_y_i -- pid_rightarm_ee_rot_z_d +- pid.rightarm.ee.pos.x.p +- pid.leftarm.ee.pos.y.i +- pid.rightarm.ee.rot.z.d Set a new value for a PID gain: ```bash -ros2 param set /eddie_ros_interface pid_rightarm_ee_pos_x_p 150.0 +ros2 param set /eddie_ros_interface pid.right.pos.x.p 150.0 ``` ## Plotting cartesian error with RQT Plot diff --git a/src/interface.cpp b/src/interface.cpp index 1e0c479..2ec4823 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -400,27 +400,27 @@ void PID::set_gains( } double PID::control(double error, double dt) { - // Deadband check - if (std::abs(error) < this->deadband) { - // Decay the integral term smoothly inside the deadband - err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; - // Always update last error to avoid derivative kick - err_last = error; - return 0.0; - } - // Derivative term double err_diff = (error - err_last) / dt; - // Integral term accumulation and clamping - err_integ += error * dt; - if (err_integ > err_sum_tol) err_integ = err_sum_tol; - if (err_integ < -err_sum_tol) err_integ = -err_sum_tol; + if (fabs(error) > 0.0) { + // Accumulate the integral when error is non-zero + err_integ += error * dt; - // Update last error + // Clamp the integral term to prevent runaway accumulation + if (err_integ > err_sum_tol) { + err_integ = err_sum_tol; + } else if (err_integ < -err_sum_tol) { + err_integ = -err_sum_tol; + } + } else { + // Decay the integral term when the error is zero + err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; + } + + // err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; err_last = error; - // PID output return kp * error + ki * err_integ + kd * err_diff; } @@ -637,27 +637,27 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // - Set PID Gains Using the Granular Values const double error_sum_tol = 0.9; - // const double decay_rate = 1.0; + const double decay_rate = 0.0; // Set PID controller gains for the RIGHT arm - pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, r_pos_deadband); - pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, r_pos_deadband); - pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, r_pos_deadband); + pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate, r_pos_deadband); + pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, decay_rate, r_pos_deadband); + pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, decay_rate, r_pos_deadband); - pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, r_rot_deadband); - pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, r_rot_deadband); - pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, r_rot_deadband); + pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, decay_rate, r_rot_deadband); + pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, decay_rate, r_rot_deadband); + pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, decay_rate, r_rot_deadband); RCLCPP_INFO(this->get_logger(), "Right Arm PID gains loaded from parameters."); // Set PID controller gains for the LEFT arm - pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, l_pos_deadband); - pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, l_pos_deadband); - pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, l_pos_deadband); + pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, decay_rate, l_pos_deadband); + pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, decay_rate, l_pos_deadband); + pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, decay_rate, l_pos_deadband); - pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, l_rot_deadband); - pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, l_rot_deadband); - pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, l_rot_deadband); + pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, decay_rate, l_rot_deadband); + pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate, l_rot_deadband); + pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate, l_rot_deadband); RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); @@ -1298,6 +1298,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } if (should_control_right_arm()) { + // here do bilateral constraint instead KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); double fx_right = pid_rightarm_ee_pos_x.control(delta_pose_rightarm_ee.vel.x(), cycle_time); @@ -1665,4 +1666,4 @@ int main(int argc, char **argv) { rclcpp::shutdown(); return 0; -} +} \ No newline at end of file From 7aa0d189651c7aea9fddacd5e3768d9dddd9070c Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 4 Dec 2025 17:03:02 +0100 Subject: [PATCH 12/35] Revert the PID class changes --- include/eddie_ros/interface.hpp | 8 ++------ src/interface.cpp | 31 ++++++++++++++----------------- 2 files changed, 16 insertions(+), 23 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 3ff195d..8e84815 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -65,17 +65,14 @@ class PID { double i_gain, double d_gain, double error_sum_tol = 1.0, - double decay_rate = 0.0, - double deadband = 0.0 - ); + double decay_rate = 0.0); void set_gains( double p_gain, double i_gain, double d_gain, double error_sum_tol = 1.0, - double decay_rate = 0.0, - double deadband = 0.0 + double decay_rate = 0.0 ); double control(double error, double dt = 1.0); @@ -88,7 +85,6 @@ class PID { double kd; double err_sum_tol; double decay_rate; - double deadband; }; struct EddieState { diff --git a/src/interface.cpp b/src/interface.cpp index 2ec4823..53ba551 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -375,7 +375,7 @@ void EddieRosInterface::execute_gripper_control( get_gripper_execution_flag(arm_side).store(false); } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband) { +PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -383,12 +383,10 @@ PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, doub kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; - this->deadband = deadband; } void PID::set_gains( - double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband -) { + double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -396,7 +394,6 @@ void PID::set_gains( kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; - this->deadband = deadband; } double PID::control(double error, double dt) { @@ -640,24 +637,24 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) const double decay_rate = 0.0; // Set PID controller gains for the RIGHT arm - pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate, r_pos_deadband); - pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, decay_rate, r_pos_deadband); - pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, decay_rate, r_pos_deadband); + pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate); + pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, decay_rate); + pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, decay_rate); - pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, decay_rate, r_rot_deadband); - pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, decay_rate, r_rot_deadband); - pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, decay_rate, r_rot_deadband); + pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, decay_rate); + pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, decay_rate); + pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, decay_rate); RCLCPP_INFO(this->get_logger(), "Right Arm PID gains loaded from parameters."); // Set PID controller gains for the LEFT arm - pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, decay_rate, l_pos_deadband); - pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, decay_rate, l_pos_deadband); - pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, decay_rate, l_pos_deadband); + pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, decay_rate); + pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, decay_rate); + pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, decay_rate); - pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, decay_rate, l_rot_deadband); - pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate, l_rot_deadband); - pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate, l_rot_deadband); + pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, decay_rate); + pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate); + pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate); RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); From 11aa2defc4d284d02db3285da316cf133541e279 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 4 Dec 2025 17:18:30 +0100 Subject: [PATCH 13/35] Implement the deadband in compute_cartesian_ctrl --- src/interface-test.cpp | 7 ++--- src/interface.cpp | 70 ++++++++++++++++++++++-------------------- 2 files changed, 39 insertions(+), 38 deletions(-) diff --git a/src/interface-test.cpp b/src/interface-test.cpp index 2abd7b5..a678ed8 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -83,7 +83,7 @@ PoseType kdlToPose(const KDL::Frame& frame) { return pose; } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband) { +PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -91,12 +91,10 @@ PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, doub kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; - this->deadband = deadband; } void PID::set_gains( - double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate, double deadband -) { + double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -104,7 +102,6 @@ void PID::set_gains( kd = d_gain; err_sum_tol = error_sum_tol; this->decay_rate = decay_rate; - this->deadband = deadband; } double PID::control(double error, double dt) { diff --git a/src/interface.cpp b/src/interface.cpp index 53ba551..5587c95 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -596,7 +596,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); - double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); @@ -607,7 +606,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); - double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); // Get values for the LEFT arm double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); @@ -619,7 +617,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); - double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); @@ -630,7 +627,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); - double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); // - Set PID Gains Using the Granular Values const double error_sum_tol = 0.9; @@ -658,22 +654,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); - - // PID controller gains -/* pid_rightarm_ee_pos_x.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); - pid_rightarm_ee_pos_y.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); - pid_rightarm_ee_pos_z.set_gains(150.0, 20.0, 10.0, 0.9, r_pos_deadband); - pid_rightarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); - pid_rightarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); - pid_rightarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, r_rot_deadband); - - pid_leftarm_ee_pos_x.set_gains(70.0, 0., 4.0, 0.9, l_pos_deadband); - pid_leftarm_ee_pos_y.set_gains(70.0, 0., 4.0, 0.9, l_pos_deadband); - pid_leftarm_ee_pos_z.set_gains(150.0, 8.0, 10.0, 0.9, l_pos_deadband); - pid_leftarm_ee_rot_x.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); - pid_leftarm_ee_rot_y.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); - pid_leftarm_ee_rot_z.set_gains(5.0, 0., 2.0, 0.9, l_rot_deadband); */ - RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); // Error publishers @@ -1293,17 +1273,33 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed RCLCPP_ERROR(get_logger(), "Invalid cycle time: %ld", cycle_time_msr); return; } + + // Get deadband values from parameters + double pos_deadband_right = this->get_parameter("pid.right.pos.deadband").as_double(); + double rot_deadband_right = this->get_parameter("pid.right.rot.deadband").as_double(); + double pos_deadband_left = this->get_parameter("pid.left.pos.deadband").as_double(); + double rot_deadband_left = this->get_parameter("pid.left.rot.deadband").as_double(); if (should_control_right_arm()) { - // here do bilateral constraint instead + // Calculate the raw cartesian error KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - double fx_right = pid_rightarm_ee_pos_x.control(delta_pose_rightarm_ee.vel.x(), cycle_time); - double fy_right = pid_rightarm_ee_pos_y.control(delta_pose_rightarm_ee.vel.y(), cycle_time); - double fz_right = pid_rightarm_ee_pos_z.control(delta_pose_rightarm_ee.vel.z(), cycle_time); - double mx_right = pid_rightarm_ee_rot_x.control(delta_pose_rightarm_ee.rot.x(), cycle_time); - double my_right = pid_rightarm_ee_rot_y.control(delta_pose_rightarm_ee.rot.y(), cycle_time); - double mz_right = pid_rightarm_ee_rot_z.control(delta_pose_rightarm_ee.rot.z(), cycle_time); + // Apply bilateral constraint to each error component + double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); + double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); + double error_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.z(), -pos_deadband_right, pos_deadband_right); + + double error_rot_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.x(), -rot_deadband_right, rot_deadband_right); + double error_rot_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.y(), -rot_deadband_right, rot_deadband_right); + double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); + + // Pass the processed error to the PID controllers + double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); + double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); + double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); + double mx_right = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); + double my_right = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); + double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); KDL::Wrench f_ext_ee_rightarm = KDL::Wrench(KDL::Vector(fx_right, fy_right, fz_right), KDL::Vector(mx_right, my_right, mz_right)); KDL::Wrench f_ext_ee_rightarm_wrt_ee = KDL::Wrench( @@ -1341,12 +1337,20 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - double fx_left = pid_leftarm_ee_pos_x.control(delta_pose_leftarm_ee.vel.x(), cycle_time); - double fy_left = pid_leftarm_ee_pos_y.control(delta_pose_leftarm_ee.vel.y(), cycle_time); - double fz_left = pid_leftarm_ee_pos_z.control(delta_pose_leftarm_ee.vel.z(), cycle_time); - double mx_left = pid_leftarm_ee_rot_x.control(delta_pose_leftarm_ee.rot.x(), cycle_time); - double my_left = pid_leftarm_ee_rot_y.control(delta_pose_leftarm_ee.rot.y(), cycle_time); - double mz_left = pid_leftarm_ee_rot_z.control(delta_pose_leftarm_ee.rot.z(), cycle_time); + double error_x = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.x(), -pos_deadband_left, pos_deadband_left); + double error_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.y(), -pos_deadband_left, pos_deadband_left); + double error_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.z(), -pos_deadband_left, pos_deadband_left); + + double error_rot_x = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.x(), -rot_deadband_left, rot_deadband_left); + double error_rot_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.y(), -rot_deadband_left, rot_deadband_left); + double error_rot_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.z(), -rot_deadband_left, rot_deadband_left); + + double fx_left = pid_leftarm_ee_pos_x.control(error_x, cycle_time); + double fy_left = pid_leftarm_ee_pos_y.control(error_y, cycle_time); + double fz_left = pid_leftarm_ee_pos_z.control(error_z, cycle_time); + double mx_left = pid_leftarm_ee_rot_x.control(error_rot_x, cycle_time); + double my_left = pid_leftarm_ee_rot_y.control(error_rot_y, cycle_time); + double mz_left = pid_leftarm_ee_rot_z.control(error_rot_z, cycle_time); KDL::Wrench f_ext_ee_leftarm = KDL::Wrench(KDL::Vector(fx_left, fy_left, fz_left), KDL::Vector(mx_left, my_left, mz_left)); KDL::Wrench f_ext_ee_leftarm_wrt_ee = KDL::Wrench( From cdd247c5f3662720788cfa6b6a61043519bd57e1 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 4 Dec 2025 18:08:16 +0100 Subject: [PATCH 14/35] Implement low-pass filter to smooth the torque commands This was added since if we want to change the gains dynamically, we do not want the torques to spike up. However, at the moment, every single torque command is passed through the filter before being sent to the motors (smoothing does not only happen on gain change). --- include/eddie_ros/interface.hpp | 4 ++++ src/interface.cpp | 38 +++++++++++++++++++++++++++++++-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 8e84815..82601c4 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -260,6 +260,10 @@ class EddieRosInterface : public rclcpp::Node { bool new_target_leftarm = false; bool new_target_rightarm = false; + // Smoothed torque commands + KDL::JntArray smoothed_torques_right_; + KDL::JntArray smoothed_torques_left_; + // Error publishers rclcpp::Publisher::SharedPtr right_arm_ee_error_pub; rclcpp::Publisher::SharedPtr left_arm_ee_error_pub; diff --git a/src/interface.cpp b/src/interface.cpp index 5587c95..da4ef41 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -493,6 +493,16 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) RCLCPP_INFO(get_logger(), "Right arm chain constructed successfully"); } + // Smoothed torque commands + if (should_control_right_arm()) { + smoothed_torques_right_.resize(rightarm_chain.getNrOfJoints()); + smoothed_torques_right_.data.setZero(); + } + if (should_control_left_arm()) { + smoothed_torques_left_.resize(leftarm_chain.getNrOfJoints()); + smoothed_torques_left_.data.setZero(); + } + // joint inertias: const std::vector joint_inertia{0.5580, 0.5580, 0.5580, 0.5580, 0.1389, 0.1389, 0.1389}; @@ -534,6 +544,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) const double default_pos_deadband = 0.005; const double default_rot_deadband = 0.02; + this->declare_parameter("torque_smoothing_alpha", 0.05); + // - Declare parameters for the RIGHT arm // Position this->declare_parameter("pid.right.pos.x.p", 150.0); @@ -1279,6 +1291,9 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double rot_deadband_right = this->get_parameter("pid.right.rot.deadband").as_double(); double pos_deadband_left = this->get_parameter("pid.left.pos.deadband").as_double(); double rot_deadband_left = this->get_parameter("pid.left.rot.deadband").as_double(); + + // Get smoothing factor value from parameters + const double alpha = this->get_parameter("torque_smoothing_alpha").as_double(); if (should_control_right_arm()) { // Calculate the raw cartesian error @@ -1329,10 +1344,20 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (r_right < 0) { RCLCPP_ERROR(get_logger(), "Right arm RNE ID solver failed with error code: %d", r_right); } + // Apply the low-pass filter to smooth the torque commands + for (unsigned int i = 0; i < num_jnts_rightarm; i++) { + smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); + } + // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_rightarm; i++) { + saturate(&smoothed_torques_right_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right_(i); + } + +/* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); - } + } */ } if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); @@ -1380,10 +1405,19 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (r_left < 0) { RCLCPP_ERROR(get_logger(), "Left arm RNE ID solver failed with error code: %d", r_left); } + // Apply the low-pass filter to smooth the torque commands + for (unsigned int i = 0; i < num_jnts_leftarm; i++) { + smoothed_torques_left_(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left_(i); + } + // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_leftarm; i++) { + saturate(&smoothed_torques_left_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + this->eddie_state.kinova_leftarm_state.eff_cmd[i] = smoothed_torques_left_(i); + } +/* for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_leftarm_state.eff_cmd[i] = tau_ctrl_leftarm(i); - } + } */ } } From f32cfa45ce5fe45e9b19228934d9ea6a41817e27 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Thu, 4 Dec 2025 18:27:52 +0100 Subject: [PATCH 15/35] Add publishers to visualize the raw vs. smoothed torques --- include/eddie_ros/interface.hpp | 10 +++++++++ src/interface.cpp | 38 +++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 82601c4..a685a91 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -213,6 +213,12 @@ class EddieRosInterface : public rclcpp::Node { void publish_ee_errors(EddieState *eddie_state); void publish_joint_states(EddieState *eddie_state); + void publish_torque_debug_info( + const KDL::JntArray& raw_torques, + const KDL::JntArray& smoothed_torques, + const std::string& arm_side + ); + public: void run_fsm(); @@ -269,6 +275,10 @@ class EddieRosInterface : public rclcpp::Node { rclcpp::Publisher::SharedPtr left_arm_ee_error_pub; rclcpp::TimerBase::SharedPtr ee_error_timer_; + // Torque command publishers + rclcpp::Publisher::SharedPtr raw_torque_publisher_; + rclcpp::Publisher::SharedPtr smoothed_torque_publisher_; + // Flags to track if arms are currently executing goals std::atomic rightarm_goal_executing = false; std::atomic leftarm_goal_executing = false; diff --git a/src/interface.cpp b/src/interface.cpp index da4ef41..1459051 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -686,6 +686,10 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) } ); + // Torque publishers + raw_torque_publisher_ = this->create_publisher("/raw_torques", 10); + smoothed_torque_publisher_ = this->create_publisher("/smoothed_torques", 10); + // Register parameter callback for dynamic PID gain updates callback_handle_ = this->add_on_set_parameters_callback( [this](const std::vector ¶ms) { @@ -1245,6 +1249,38 @@ void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddi } } +void EddieRosInterface::publish_torque_debug_info( + const KDL::JntArray& raw_torques, + const KDL::JntArray& smoothed_torques, + const std::string& arm_side) +{ + // Check if anyone is actually subscribed to the topics + if (raw_torque_publisher_->get_subscription_count() == 0 && + smoothed_torque_publisher_->get_subscription_count() == 0) { + return; + } + + auto now = this->get_clock()->now(); + + // Publish raw torques + auto raw_torque_msg = sensor_msgs::msg::JointState(); + raw_torque_msg.header.stamp = now; + for (unsigned int i = 0; i < raw_torques.rows(); i++) { + raw_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); + raw_torque_msg.effort.push_back(raw_torques(i)); + } + raw_torque_publisher_->publish(raw_torque_msg); + + // Publish smoothed torques + auto smoothed_torque_msg = sensor_msgs::msg::JointState(); + smoothed_torque_msg.header.stamp = now; + for (unsigned int i = 0; i < smoothed_torques.rows(); i++) { + smoothed_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); + smoothed_torque_msg.effort.push_back(smoothed_torques(i)); + } + smoothed_torque_publisher_->publish(smoothed_torque_msg); +} + void EddieRosInterface::publish_ee_errors(EddieState *eddie_state) { if (should_control_right_arm()) { @@ -1348,6 +1384,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed for (unsigned int i = 0; i < num_jnts_rightarm; i++) { smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); } + publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right_, "right"); // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&smoothed_torques_right_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); @@ -1409,6 +1446,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed for (unsigned int i = 0; i < num_jnts_leftarm; i++) { smoothed_torques_left_(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left_(i); } + publish_torque_debug_info(tau_ctrl_leftarm, smoothed_torques_left_, "left"); // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&smoothed_torques_left_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); From 85a444295f7a391b08a95097766dc47263797cbb Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Sun, 7 Dec 2025 16:29:27 +0100 Subject: [PATCH 16/35] add idle and execute state transitions based on execution flags --- src/interface.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index d9b85da..4027d22 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -1021,8 +1021,11 @@ void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } - RCLCPP_DEBUG(get_logger(), "Exiting idle state"); - // produce_event(eventData, E_IDLE_EXIT_EXECUTE); + if (get_arm_execution_flag("right") || get_arm_execution_flag("left")) { + RCLCPP_INFO(get_logger(), "Execution flag set, transitioning to EXECUTE state"); + RCLCPP_DEBUG(get_logger(), "Exiting idle state"); + produce_event(eventData, E_IDLE_EXIT_EXECUTE); + } } void EddieRosInterface::compile(events *eventData, const EddieState *eddie_state) { @@ -1337,6 +1340,11 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } + if (!get_arm_execution_flag("right") && !get_arm_execution_flag("left")) { + RCLCPP_INFO(get_logger(), "Execution flags cleared, transitioning to IDLE state"); + RCLCPP_DEBUG(get_logger(), "Exiting execute state"); + produce_event(eventData, E_IDLE_ENTERED); + } } void EddieRosInterface::publish_joint_states(EddieState *eddie_state) { From ebc44ff0957475c2b99e59304aa3a6266cc751d8 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Tue, 9 Dec 2025 13:38:48 +0100 Subject: [PATCH 17/35] Fix evaluate bilateral constraint return Also modified the file to not use the gripper, since we do not have access to the gripper at the moment. Removed the rotation tolerance as well for now. --- src/interface.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index 1459051..4e1c338 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -40,11 +40,13 @@ double evaluate_greater_than_constraint(double quantity, double threshold) { double evaluate_bilateral_constraint(double quantity, double lower, double upper) { if (quantity < lower) - return lower - quantity; + return quantity - lower; else if (quantity > upper) return quantity - upper; else return 0.0; + // in this case, maybe wecan remove the P controller so it just usses integrator to go slowly to zero ? + // or integrator term wiht low P } void saturate(double *value, double min, double max) { @@ -237,7 +239,7 @@ void EddieRosInterface::execute_arm_control( double position_error = pose_error.vel.Norm(); double rotation_error = pose_error.rot.Norm(); - if (position_error < position_tolerance && rotation_error < rotation_tolerance) { + if (position_error < position_tolerance /* && rotation_error < rotation_tolerance*/) { result->success = true; result->message = arm_side + " arm successfully reached target position"; result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); @@ -456,7 +458,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) } else { RCLCPP_INFO(get_logger(), "Left arm chain constructed successfully"); } - if (!tree.getChain("base_link", "kinova_right_grasp_link", rightarm_chain)) { + if (!tree.getChain("base_link", "kinova_left_grasp_link", rightarm_chain)) { RCLCPP_ERROR(get_logger(), "Failed to get right arm chain"); exit(11); } else { @@ -480,13 +482,15 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) } RCLCPP_INFO(this->get_logger(), "Successfully loaded KDL tree from parameter."); - if (!tree.getChain("eddie_base_link", "eddie_left_arm_robotiq_85_grasp_link", leftarm_chain)) { + //if (!tree.getChain("eddie_base_link", "eddie_left_arm_robotiq_85_grasp_link", leftarm_chain)) { + if (!tree.getChain("eddie_base_link", "eddie_left_arm_end_effector_link", leftarm_chain)) { RCLCPP_ERROR(get_logger(), "Failed to get left arm chain. Check link names in URDF."); exit(11); } else { RCLCPP_INFO(get_logger(), "Left arm chain constructed successfully"); } - if (!tree.getChain("eddie_base_link", "eddie_right_arm_robotiq_85_grasp_link", rightarm_chain)) { + //if (!tree.getChain("eddie_base_link", "eddie_right_arm_robotiq_85_grasp_link", rightarm_chain)) { + if (!tree.getChain("eddie_base_link", "eddie_right_arm_end_effector_link", rightarm_chain)) { RCLCPP_ERROR(get_logger(), "Failed to get right arm chain. Check link names in URDF."); exit(11); } else { @@ -542,13 +546,13 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), "Declaring PID parameters..."); const double default_pos_deadband = 0.005; - const double default_rot_deadband = 0.02; + const double default_rot_deadband = 0.005; this->declare_parameter("torque_smoothing_alpha", 0.05); // - Declare parameters for the RIGHT arm // Position - this->declare_parameter("pid.right.pos.x.p", 150.0); + this->declare_parameter("pid.right.pos.x.p", 80.0); this->declare_parameter("pid.right.pos.x.i", 20.0); this->declare_parameter("pid.right.pos.x.d", 10.0); this->declare_parameter("pid.right.pos.y.p", 150.0); @@ -1339,7 +1343,6 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); double error_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.z(), -pos_deadband_right, pos_deadband_right); - double error_rot_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.x(), -rot_deadband_right, rot_deadband_right); double error_rot_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.y(), -rot_deadband_right, rot_deadband_right); double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); @@ -1381,6 +1384,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed RCLCPP_ERROR(get_logger(), "Right arm RNE ID solver failed with error code: %d", r_right); } // Apply the low-pass filter to smooth the torque commands + // TODO: only do this when changing gains dynamically! for (unsigned int i = 0; i < num_jnts_rightarm; i++) { smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); } From dc3b0cf328f35c5d0c6356bc3bec9bfb8d2d4b4f Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Tue, 9 Dec 2025 14:11:19 +0100 Subject: [PATCH 18/35] add execute to idle transition and update event handling --- include/eddie_ros/eddie_ros.fsm | 9 ++++++++- include/eddie_ros/eddie_ros.fsm.hpp | 17 ++++++++++++++++- src/interface.cpp | 26 +++++++++++++++++++++----- 3 files changed, 45 insertions(+), 7 deletions(-) diff --git a/include/eddie_ros/eddie_ros.fsm b/include/eddie_ros/eddie_ros.fsm index c241c28..4742b74 100644 --- a/include/eddie_ros/eddie_ros.fsm +++ b/include/eddie_ros/eddie_ros.fsm @@ -6,7 +6,7 @@ START_STATE: @S_START CURRENT_STATE: @S_START END_STATE: @S_EXIT -EVENTS: E_CONFIGURE_ENTERED,E_CONFIGURE_EXIT,E_IDLE_ENTERED,E_IDLE_EXIT_EXECUTE,E_IDLE_EXIT_COMPILE,E_COMPILE_ENTERED,E_COMPILE_EXIT,E_EXECUTE_ENTERED,E_STEP +EVENTS: E_CONFIGURE_ENTERED,E_CONFIGURE_EXIT,E_IDLE_ENTERED,E_IDLE_EXIT_EXECUTE,E_IDLE_EXIT_COMPILE,E_COMPILE_ENTERED,E_COMPILE_EXIT,E_EXECUTE_ENTERED,E_EXECUTE_EXIT_IDLE,E_STEP TRANSITIONS: T_START_CONFIGURE: @@ -30,6 +30,9 @@ TRANSITIONS: T_EXECUTE_EXECUTE: FROM: @S_EXECUTE TO: @S_EXECUTE + T_EXECUTE_IDLE: + FROM: @S_EXECUTE + TO: @S_IDLE REACTIONS: R_E_CONFIGURE_EXIT: @@ -48,6 +51,10 @@ REACTIONS: WHEN: @E_COMPILE_EXIT DO: @T_COMPILE_EXECUTE FIRES: @E_EXECUTE_ENTERED + R_E_EXECUTE_EXIT_IDLE: + WHEN: @E_EXECUTE_EXIT_IDLE + DO: @T_EXECUTE_IDLE + FIRES: @E_IDLE_ENTERED R_E_STEP1: WHEN: @E_STEP diff --git a/include/eddie_ros/eddie_ros.fsm.hpp b/include/eddie_ros/eddie_ros.fsm.hpp index 81a05f4..a615882 100644 --- a/include/eddie_ros/eddie_ros.fsm.hpp +++ b/include/eddie_ros/eddie_ros.fsm.hpp @@ -73,6 +73,7 @@ enum e_events { E_COMPILE_ENTERED, E_COMPILE_EXIT, E_EXECUTE_ENTERED, + E_EXECUTE_EXIT_IDLE, E_STEP, NUM_EVENTS }; @@ -86,6 +87,7 @@ enum e_transitions { T_IDLE_COMPILE, T_COMPILE_EXECUTE, T_EXECUTE_EXECUTE, + T_EXECUTE_IDLE, NUM_TRANSITIONS }; @@ -95,6 +97,7 @@ enum e_reactions { R_E_IDLE_EXIT_EXECUTE, R_E_IDLE_EXIT_COMPILE, R_E_COMPILE_EXIT, + R_E_EXECUTE_EXIT_IDLE, R_E_STEP1, R_E_STEP2, R_E_STEP3, @@ -140,7 +143,11 @@ inline struct transition transitions[NUM_TRANSITIONS] = { { .startStateIndex = S_EXECUTE, .endStateIndex = S_EXECUTE, - } + }, + { + .startStateIndex = S_EXECUTE, + .endStateIndex = S_IDLE, + } }; // sm reaction table @@ -177,6 +184,14 @@ inline struct event_reaction reactions[NUM_REACTIONS] = { E_EXECUTE_ENTERED }, }, + { + .conditionEventIndex = E_EXECUTE_EXIT_IDLE, + .transitionIndex = T_EXECUTE_IDLE, + .numFiredEvents = 1, + .firedEventIndices = new unsigned int[1]{ + E_IDLE_ENTERED + }, + }, { .conditionEventIndex = E_STEP, .transitionIndex = T_START_CONFIGURE, diff --git a/src/interface.cpp b/src/interface.cpp index 4027d22..25020b7 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -215,6 +215,22 @@ void EddieRosInterface::execute_arm_control( auto result = std::make_shared(); rclcpp::Rate loop_rate(100); + + // Wait for the main loop to update the target pose + int wait_cycles = 0; + while (get_new_target_flag(arm_side) && rclcpp::ok() && wait_cycles < 100) { // Wait up to 1 second + if (goal_handle->is_canceling()) { + result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; + result->result_message = arm_side + " arm control goal was canceled"; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); + get_arm_execution_flag(arm_side).store(false); + return; + } + loop_rate.sleep(); + wait_cycles++; + } + // TODO: this definitely needs some tweaking const double position_tolerance = 0.02; // 2cm // const double rotation_tolerance = 0.05; // ~3 degrees @@ -479,13 +495,13 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) } RCLCPP_INFO(this->get_logger(), "Successfully loaded KDL tree from parameter."); - if (!tree.getChain("eddie_base_link", "eddie_left_arm_robotiq_85_grasp_link", leftarm_chain)) { + if (!tree.getChain("eddie_base_link", "eddie_left_arm_end_effector_link", leftarm_chain)) { //TODO: revert to eddie_left_arm_robotiq_85_grasp_link RCLCPP_ERROR(get_logger(), "Failed to get left arm chain. Check link names in URDF."); exit(11); } else { RCLCPP_INFO(get_logger(), "Left arm chain constructed successfully"); } - if (!tree.getChain("eddie_base_link", "eddie_right_arm_robotiq_85_grasp_link", rightarm_chain)) { + if (!tree.getChain("eddie_base_link", "eddie_right_arm_end_effector_link", rightarm_chain)) { //TODO: revert to eddie_right_arm_robotiq_85_grasp_link RCLCPP_ERROR(get_logger(), "Failed to get right arm chain. Check link names in URDF."); exit(11); } else { @@ -1021,7 +1037,7 @@ void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } - if (get_arm_execution_flag("right") || get_arm_execution_flag("left")) { + if (get_arm_execution_flag("right").load() || get_arm_execution_flag("left").load()) { RCLCPP_INFO(get_logger(), "Execution flag set, transitioning to EXECUTE state"); RCLCPP_DEBUG(get_logger(), "Exiting idle state"); produce_event(eventData, E_IDLE_EXIT_EXECUTE); @@ -1340,10 +1356,10 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } - if (!get_arm_execution_flag("right") && !get_arm_execution_flag("left")) { + if (!get_arm_execution_flag("right").load() && !get_arm_execution_flag("left").load()) { RCLCPP_INFO(get_logger(), "Execution flags cleared, transitioning to IDLE state"); RCLCPP_DEBUG(get_logger(), "Exiting execute state"); - produce_event(eventData, E_IDLE_ENTERED); + produce_event(eventData, E_EXECUTE_EXIT_IDLE); } } From f3498f7e40e609bfe1b63d33327be8437a6af923 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Tue, 9 Dec 2025 15:58:53 +0100 Subject: [PATCH 19/35] Adjust the P-gain for the controller based on deadband --- src/interface.cpp | 63 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 62 insertions(+), 1 deletion(-) diff --git a/src/interface.cpp b/src/interface.cpp index 4e1c338..a810337 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -1339,6 +1339,16 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed // Calculate the raw cartesian error KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); + double original_p_pos_x = this->get_parameter("pid.right.pos.x.p").as_double(); + double original_p_pos_y = this->get_parameter("pid.right.pos.y.p").as_double(); + double original_p_pos_z = this->get_parameter("pid.right.pos.z.p").as_double(); + + double original_p_rot_x = this->get_parameter("pid.right.rot.x.p").as_double(); + double original_p_rot_y = this->get_parameter("pid.right.rot.y.p").as_double(); + double original_p_rot_z = this->get_parameter("pid.right.rot.z.p").as_double(); + + const double low_gain_factor = 0.1; // Use 10% of the P-gain inside the deadband + // Apply bilateral constraint to each error component double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); @@ -1346,11 +1356,31 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_rot_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.x(), -rot_deadband_right, rot_deadband_right); double error_rot_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.y(), -rot_deadband_right, rot_deadband_right); double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); - + // Pass the processed error to the PID controllers + // Dynamically adjust the P-gain for the controller based on deadband + if (error_x == 0.0) { + pid_rightarm_ee_pos_x.kp = original_p_pos_x * low_gain_factor; // Set to low gain + } double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); + pid_rightarm_ee_pos_x.kp = original_p_pos_x; // Restore to high gain for next cycle + + if (error_y == 0.0) { + pid_rightarm_ee_pos_y.kp = original_p_pos_y * low_gain_factor; + } double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); + pid_rightarm_ee_pos_y.kp = original_p_pos_y; + + if (error_z == 0.0) { + pid_rightarm_ee_pos_z.kp = original_p_pos_z * low_gain_factor; + } double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); + pid_rightarm_ee_pos_z.kp = original_p_pos_z; + + // Pass the processed error to the PID controllers + // double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); + // double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); + // double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); double mx_right = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); double my_right = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); @@ -1403,6 +1433,16 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); + double original_p_pos_x = this->get_parameter("pid.left.pos.x.p").as_double(); + double original_p_pos_y = this->get_parameter("pid.left.pos.y.p").as_double(); + double original_p_pos_z = this->get_parameter("pid.left.pos.z.p").as_double(); + + double original_p_rot_x = this->get_parameter("pid.left.rot.x.p").as_double(); + double original_p_rot_y = this->get_parameter("pid.left.rot.y.p").as_double(); + double original_p_rot_z = this->get_parameter("pid.left.rot.z.p").as_double(); + + const double low_gain_factor = 0.1; // Use 10% of the P-gain inside the deadband + double error_x = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.x(), -pos_deadband_left, pos_deadband_left); double error_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.y(), -pos_deadband_left, pos_deadband_left); double error_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.z(), -pos_deadband_left, pos_deadband_left); @@ -1411,9 +1451,30 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_rot_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.y(), -rot_deadband_left, rot_deadband_left); double error_rot_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.z(), -rot_deadband_left, rot_deadband_left); + // Pass the processed error to the PID controllers + // Dynamically adjust the P-gain for the controller based on deadband + if (error_x == 0.0) { + pid_leftarm_ee_pos_x.kp = original_p_pos_x * low_gain_factor; // Set to low gain + } double fx_left = pid_leftarm_ee_pos_x.control(error_x, cycle_time); + pid_leftarm_ee_pos_x.kp = original_p_pos_x; // Restore to high gain for next cycle + + if (error_y == 0.0) { + pid_leftarm_ee_pos_y.kp = original_p_pos_y * low_gain_factor; + } double fy_left = pid_leftarm_ee_pos_y.control(error_y, cycle_time); + pid_leftarm_ee_pos_y.kp = original_p_pos_y; + + if (error_z == 0.0) { + pid_leftarm_ee_pos_z.kp = original_p_pos_z * low_gain_factor; + } double fz_left = pid_leftarm_ee_pos_z.control(error_z, cycle_time); + pid_leftarm_ee_pos_z.kp = original_p_pos_z; + + + // double fx_left = pid_leftarm_ee_pos_x.control(error_x, cycle_time); + // double fy_left = pid_leftarm_ee_pos_y.control(error_y, cycle_time); + // double fz_left = pid_leftarm_ee_pos_z.control(error_z, cycle_time); double mx_left = pid_leftarm_ee_rot_x.control(error_rot_x, cycle_time); double my_left = pid_leftarm_ee_rot_y.control(error_rot_y, cycle_time); double mz_left = pid_leftarm_ee_rot_z.control(error_rot_z, cycle_time); From 11d30991f6bb5327fd9b0295ccbe8efc503371da Mon Sep 17 00:00:00 2001 From: Bastian Hunecke <75437449+bhunecke@users.noreply.github.com> Date: Tue, 9 Dec 2025 17:11:33 +0100 Subject: [PATCH 20/35] Include Zenoh router installation instructions Added instructions for installing Zenoh router. --- robot_setup.md | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_setup.md b/robot_setup.md index 5249d54..355cbb5 100644 --- a/robot_setup.md +++ b/robot_setup.md @@ -17,6 +17,7 @@ Also see the documentation for: - Ubuntu 24.04 - ROS 2 Jazzy: Follow the instructions in the [ROS 2 installation guide](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) - SSH keys set up for your GitHub account: Follow the instructions in the [GitHub SSH key guide](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) to generate a new SSH key and add it to your account. +- Zenoh router: Install it using the command from [the documentation](https://docs.ros.org/en/jazzy/Installation/RMW-Implementations/Non-DDS-Implementations/Working-with-Zenoh.html#installation-packages). `rosdep` for installing dependencies. Install it with: From fa89bc6c7cb6dbb186b8936bc0cddbf39389b059 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Tue, 9 Dec 2025 18:34:02 +0100 Subject: [PATCH 21/35] Activate smoothing based on dynamic gain change Create a dedicated pid.cpp file --- CMakeLists.txt | 8 +- include/eddie_ros/interface.hpp | 14 +++ src/interface-parameters.cpp | 173 ++++++++++++++++++++++++++++++++ src/interface-test.cpp | 4 +- src/interface.cpp | 67 ++++++++++--- src/pid.cpp | 47 +++++++++ 6 files changed, 293 insertions(+), 20 deletions(-) create mode 100644 src/pid.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e29ca2..6de9899 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,6 +47,7 @@ include_directories( add_executable(eddie_ros_interface src/interface.cpp src/interface-parameters.cpp + src/pid.cpp ) ament_target_dependencies(eddie_ros_interface @@ -68,7 +69,8 @@ target_link_libraries(eddie_ros_interface # task planner node add_executable(task_planner_node src/task-planner.cpp - src/interface-parameters.cpp + #src/interface-parameters.cpp + src/pid.cpp ) ament_target_dependencies(task_planner_node rclcpp @@ -87,7 +89,8 @@ target_link_libraries(task_planner_node add_executable(sim_interface_node src/sim-interface.cpp - src/interface-parameters.cpp + #src/interface-parameters.cpp + #src/pid.cpp ) ament_target_dependencies(sim_interface_node rclcpp @@ -111,6 +114,7 @@ target_link_libraries(sim_interface_node add_executable(eddie_ros_interface_test src/interface-test.cpp src/interface-parameters.cpp + src/pid.cpp ) ament_target_dependencies(eddie_ros_interface_test diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index a685a91..a6e68a2 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -21,6 +21,7 @@ #include #include #include "sensor_msgs/msg/joint_state.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" #include "eddie_ros/action/arm_control.hpp" #include "eddie_ros/action/gripper_control.hpp" @@ -195,8 +196,15 @@ class EddieRosInterface : public rclcpp::Node { void declare_all_parameters(); + void declare_pid_gains(); + void get_all_parameters(); + void reload_pid_gains(); + + rcl_interfaces::msg::SetParametersResult parameters_callback( + const std::vector ¶meters); + // Action server void initialize_action_servers(); @@ -279,6 +287,12 @@ class EddieRosInterface : public rclcpp::Node { rclcpp::Publisher::SharedPtr raw_torque_publisher_; rclcpp::Publisher::SharedPtr smoothed_torque_publisher_; + // State for conditional smoothing + std::atomic right_arm_smoothing_active_{false}; + std::atomic left_arm_smoothing_active_{false}; + std::chrono::steady_clock::time_point right_arm_smoothing_start_time_; + std::chrono::steady_clock::time_point left_arm_smoothing_start_time_; + // Flags to track if arms are currently executing goals std::atomic rightarm_goal_executing = false; std::atomic leftarm_goal_executing = false; diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index 7ec4565..64dcc87 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -32,6 +32,179 @@ void EddieRosInterface::get_all_parameters() { this->get_parameter("arm_select", param_arm_select); } +void EddieRosInterface::declare_pid_gains() { + + RCLCPP_INFO(this->get_logger(), "Declaring PID gains from parameter server..."); + + // Torque smoothing parameter + this->declare_parameter("torque_smoothing_alpha", 0.05); + + const double default_pos_deadband = 0.005; + const double default_rot_deadband = 0.02; + + // Declare parameters for the RIGHT arm + this->declare_parameter("pid.right.pos.x.p", 80.0); + this->declare_parameter("pid.right.pos.x.i", 20.0); + this->declare_parameter("pid.right.pos.x.d", 10.0); + this->declare_parameter("pid.right.pos.y.p", 150.0); + this->declare_parameter("pid.right.pos.y.i", 20.0); + this->declare_parameter("pid.right.pos.y.d", 10.0); + this->declare_parameter("pid.right.pos.z.p", 150.0); + this->declare_parameter("pid.right.pos.z.i", 20.0); + this->declare_parameter("pid.right.pos.z.d", 10.0); + this->declare_parameter("pid.right.pos.deadband", default_pos_deadband); + + this->declare_parameter("pid.right.rot.x.p", 5.0); + this->declare_parameter("pid.right.rot.x.i", 0.0); + this->declare_parameter("pid.right.rot.x.d", 2.0); + this->declare_parameter("pid.right.rot.y.p", 5.0); + this->declare_parameter("pid.right.rot.y.i", 0.0); + this->declare_parameter("pid.right.rot.y.d", 2.0); + this->declare_parameter("pid.right.rot.z.p", 5.0); + this->declare_parameter("pid.right.rot.z.i", 0.0); + this->declare_parameter("pid.right.rot.z.d", 2.0); + this->declare_parameter("pid.right.rot.deadband", default_rot_deadband); + + // Declare parameters for the LEFT arm + this->declare_parameter("pid.left.pos.x.p", 70.0); + this->declare_parameter("pid.left.pos.x.i", 0.0); + this->declare_parameter("pid.left.pos.x.d", 4.0); + this->declare_parameter("pid.left.pos.y.p", 70.0); + this->declare_parameter("pid.left.pos.y.i", 0.0); + this->declare_parameter("pid.left.pos.y.d", 4.0); + this->declare_parameter("pid.left.pos.z.p", 150.0); + this->declare_parameter("pid.left.pos.z.i", 8.0); + this->declare_parameter("pid.left.pos.z.d", 10.0); + this->declare_parameter("pid.left.pos.deadband", default_pos_deadband); + + this->declare_parameter("pid.left.rot.x.p", 5.0); + this->declare_parameter("pid.left.rot.x.i", 0.0); + this->declare_parameter("pid.left.rot.x.d", 2.0); + this->declare_parameter("pid.left.rot.y.p", 5.0); + this->declare_parameter("pid.left.rot.y.i", 0.0); + this->declare_parameter("pid.left.rot.y.d", 2.0); + this->declare_parameter("pid.left.rot.z.p", 5.0); + this->declare_parameter("pid.left.rot.z.i", 0.0); + this->declare_parameter("pid.left.rot.z.d", 2.0); + this->declare_parameter("pid.left.rot.deadband", default_rot_deadband); +} + +void EddieRosInterface::reload_pid_gains() +{ + RCLCPP_INFO(this->get_logger(), "Reloading all PID gains from parameter server..."); + + const double error_sum_tol = 0.9; + const double decay_rate = 0.0; + + // Get values for the RIGHT arm + double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); + double r_pos_x_i = this->get_parameter("pid.right.pos.x.i").as_double(); + double r_pos_x_d = this->get_parameter("pid.right.pos.x.d").as_double(); + double r_pos_y_p = this->get_parameter("pid.right.pos.y.p").as_double(); + double r_pos_y_i = this->get_parameter("pid.right.pos.y.i").as_double(); + double r_pos_y_d = this->get_parameter("pid.right.pos.y.d").as_double(); + double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); + double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); + double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); + double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); + + double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); + double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); + double r_rot_x_d = this->get_parameter("pid.right.rot.x.d").as_double(); + double r_rot_y_p = this->get_parameter("pid.right.rot.y.p").as_double(); + double r_rot_y_i = this->get_parameter("pid.right.rot.y.i").as_double(); + double r_rot_y_d = this->get_parameter("pid.right.rot.y.d").as_double(); + double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); + double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); + double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); + double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); + + // Get values for the LEFT arm + double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); + double l_pos_x_i = this->get_parameter("pid.left.pos.x.i").as_double(); + double l_pos_x_d = this->get_parameter("pid.left.pos.x.d").as_double(); + double l_pos_y_p = this->get_parameter("pid.left.pos.y.p").as_double(); + double l_pos_y_i = this->get_parameter("pid.left.pos.y.i").as_double(); + double l_pos_y_d = this->get_parameter("pid.left.pos.y.d").as_double(); + double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); + double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); + double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); + double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); + + double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); + double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); + double l_rot_x_d = this->get_parameter("pid.left.rot.x.d").as_double(); + double l_rot_y_p = this->get_parameter("pid.left.rot.y.p").as_double(); + double l_rot_y_i = this->get_parameter("pid.left.rot.y.i").as_double(); + double l_rot_y_d = this->get_parameter("pid.left.rot.y.d").as_double(); + double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); + double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); + double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); + double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); + + // Set PID controller gains for the RIGHT arm + pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate); + pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, decay_rate); + pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, decay_rate); + + pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, decay_rate); + pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, decay_rate); + pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, decay_rate); + + // Set PID controller gains for the LEFT arm + pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, decay_rate); + pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, decay_rate); + pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, decay_rate); + + pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, decay_rate); + pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate); + pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate); + + RCLCPP_INFO(this->get_logger(), "PID gains have been reloaded."); +} + + +rcl_interfaces::msg::SetParametersResult EddieRosInterface::parameters_callback( + const std::vector ¶meters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + bool gains_changed = false; + + for (const auto ¶m : parameters) { + std::string name = param.get_name(); + + // Check if any PID parameter was changed + if (name.rfind("pid.", 0) == 0) { + gains_changed = true; + // Activate smoothing based on which arm's gain was changed + if (name.rfind("pid.right", 0) == 0) { + RCLCPP_INFO(this->get_logger(), "Right arm PID parameter changed. Activating torque smoothing."); + this->right_arm_smoothing_start_time_ = std::chrono::steady_clock::now(); + this->right_arm_smoothing_active_.store(true); + } + else if (name.rfind("pid.left", 0) == 0) { + RCLCPP_INFO(this->get_logger(), "Left arm PID parameter changed. Activating torque smoothing."); + this->left_arm_smoothing_start_time_ = std::chrono::steady_clock::now(); + this->left_arm_smoothing_active_.store(true); + } + } + else if (name == "torque_smoothing_alpha") { + RCLCPP_INFO(this->get_logger(), "torque_smoothing_alpha parameter changed."); + // No action here for now + } + } + + // If any PID gain was in the list of changed parameters, reload all of them. + if (gains_changed) { + this->reload_pid_gains(); + } + + return result; +} + // rcl_interfaces::msg::SetParametersResult // EddieRosInterface::parametersCallback(const std::vector ¶meters) { // rcl_interfaces::msg::SetParametersResult result; diff --git a/src/interface-test.cpp b/src/interface-test.cpp index a678ed8..f598199 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -83,7 +83,7 @@ PoseType kdlToPose(const KDL::Frame& frame) { return pose; } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { +/* PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -126,7 +126,7 @@ double PID::control(double error, double dt) { err_last = error; return kp * error + ki * err_integ + kd * err_diff; -} +} */ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) : rclcpp::Node("eddie_ros_interface", options) { diff --git a/src/interface.cpp b/src/interface.cpp index a810337..c5dff34 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -377,7 +377,7 @@ void EddieRosInterface::execute_gripper_control( get_gripper_execution_flag(arm_side).store(false); } -PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { +/* PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -421,15 +421,21 @@ double PID::control(double error, double dt) { err_last = error; return kp * error + ki * err_integ + kd * err_diff; -} +} */ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) : rclcpp::Node("eddie_ros_interface", options) { signal(SIGINT, sigint_handler); + // Declare parameters this->declare_all_parameters(); + this->declare_pid_gains(); + this->reload_pid_gains(); + + this->callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&EddieRosInterface::parameters_callback, this, std::placeholders::_1)); eddie_state = {}; ecat = {}; @@ -541,16 +547,15 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) f_ext_rightarm.resize(num_segs_rightarm); rne_id_solver_rightarm = std::make_unique(rightarm_chain, root_acc_rightarm.vel); + + //RCLCPP_INFO(this->get_logger(), "Declaring PID parameters..."); - - RCLCPP_INFO(this->get_logger(), "Declaring PID parameters..."); - - const double default_pos_deadband = 0.005; +/* const double default_pos_deadband = 0.005; const double default_rot_deadband = 0.005; - this->declare_parameter("torque_smoothing_alpha", 0.05); + this->declare_parameter("torque_smoothing_alpha", 0.05); */ - // - Declare parameters for the RIGHT arm + /* // - Declare parameters for the RIGHT arm // Position this->declare_parameter("pid.right.pos.x.p", 80.0); this->declare_parameter("pid.right.pos.x.i", 20.0); @@ -598,12 +603,12 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) this->declare_parameter("pid.left.rot.z.p", 5.0); this->declare_parameter("pid.left.rot.z.i", 0.0); this->declare_parameter("pid.left.rot.z.d", 2.0); - this->declare_parameter("pid.left.rot.deadband", default_rot_deadband); + this->declare_parameter("pid.left.rot.deadband", default_rot_deadband); */ // - Get the Parameter Values into local variables // Get values for the RIGHT arm - double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); + /* double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); double r_pos_x_i = this->get_parameter("pid.right.pos.x.i").as_double(); double r_pos_x_d = this->get_parameter("pid.right.pos.x.d").as_double(); double r_pos_y_p = this->get_parameter("pid.right.pos.y.p").as_double(); @@ -668,7 +673,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate); pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate); - RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); + RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); */ RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); @@ -694,7 +699,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) raw_torque_publisher_ = this->create_publisher("/raw_torques", 10); smoothed_torque_publisher_ = this->create_publisher("/smoothed_torques", 10); - // Register parameter callback for dynamic PID gain updates +/* // Register parameter callback for dynamic PID gain updates callback_handle_ = this->add_on_set_parameters_callback( [this](const std::vector ¶ms) { for (const auto ¶m : params) { @@ -741,7 +746,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) result.successful = true; return result; } - ); + ); */ } void EddieRosInterface::initialize_action_servers() { @@ -1334,6 +1339,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed // Get smoothing factor value from parameters const double alpha = this->get_parameter("torque_smoothing_alpha").as_double(); + const std::chrono::milliseconds smoothing_duration(200); if (should_control_right_arm()) { // Calculate the raw cartesian error @@ -1353,6 +1359,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); double error_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.z(), -pos_deadband_right, pos_deadband_right); + double error_rot_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.x(), -rot_deadband_right, rot_deadband_right); double error_rot_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.y(), -rot_deadband_right, rot_deadband_right); double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); @@ -1415,7 +1422,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } // Apply the low-pass filter to smooth the torque commands // TODO: only do this when changing gains dynamically! - for (unsigned int i = 0; i < num_jnts_rightarm; i++) { +/* for (unsigned int i = 0; i < num_jnts_rightarm; i++) { smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); } publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right_, "right"); @@ -1423,12 +1430,40 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&smoothed_torques_right_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right_(i); + } */ +/* bool apply_smoothing = right_arm_smoothing_active_.load(); + + if (apply_smoothing) { + auto elapsed = std::chrono::steady_clock::now() - right_arm_smoothing_start_time_; + if (elapsed < smoothing_duration) { + // We are in the smoothing period, apply the filter + for (unsigned int i = 0; i < num_jnts_rightarm; i++) { + smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); + } + } else { + // Smoothing duration has passed, turn it off. + right_arm_smoothing_active_.store(false); + apply_smoothing = false; + // On the first step after disabling, snap the smoothed value to the raw value + smoothed_torques_right_ = tau_ctrl_rightarm; + RCLCPP_INFO(this->get_logger(), "Right arm torque smoothing deactivated."); + } } + + // Use the raw command if smoothing is not active, otherwise use the smoothed one. + const KDL::JntArray& final_torques = apply_smoothing ? smoothed_torques_right_ : tau_ctrl_rightarm; -/* for (int i = 0; i < num_jnts_rightarm; i++) { + // send the final torques to the robot + for (int i = 0; i < num_jnts_rightarm; i++) { + double torque_to_send = final_torques(i); + saturate(&torque_to_send, -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + eddie_state->kinova_rightarm_state.eff_cmd[i] = torque_to_send; + } + */ + for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); - } */ + } } if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); diff --git a/src/pid.cpp b/src/pid.cpp new file mode 100644 index 0000000..68a9368 --- /dev/null +++ b/src/pid.cpp @@ -0,0 +1,47 @@ +#include "eddie_ros/interface.hpp" + +PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { + err_integ = 0.0; + err_last = 0.0; + kp = p_gain; + ki = i_gain; + kd = d_gain; + err_sum_tol = error_sum_tol; + this->decay_rate = decay_rate; +} + +void PID::set_gains( + double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { + err_integ = 0.0; + err_last = 0.0; + kp = p_gain; + ki = i_gain; + kd = d_gain; + err_sum_tol = error_sum_tol; + this->decay_rate = decay_rate; +} + +double PID::control(double error, double dt) { + + double err_diff = (error - err_last) / dt; + + if (fabs(error) > 0.0) { + // Accumulate the integral when error is non-zero + err_integ += error * dt; + + // Clamp the integral term to prevent runaway accumulation + if (err_integ > err_sum_tol) { + err_integ = err_sum_tol; + } else if (err_integ < -err_sum_tol) { + err_integ = -err_sum_tol; + } + } else { + // Decay the integral term when the error is zero + err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; + } + + // err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; + err_last = error; + + return kp * error + ki * err_integ + kd * err_diff; +} From 3838bb0484bfaeff6e46be579fbbfd7930fc7736 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke <75437449+bhunecke@users.noreply.github.com> Date: Wed, 10 Dec 2025 13:31:57 +0100 Subject: [PATCH 22/35] Change version to 'dev' for eddie-ros and robif2b --- r4s.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/r4s.repos b/r4s.repos index e56b940..81f8925 100644 --- a/r4s.repos +++ b/r4s.repos @@ -10,7 +10,7 @@ repositories: eddie-ros: type: git url: git@github.com:Robots4Sustainability/eddie-ros.git - version: main + version: dev eddie_pmu_control: type: git url: git@github.com:secorolab/eddie_pmu_control.git @@ -22,7 +22,7 @@ repositories: robif2b: type: git url: git@github.com:secorolab/robif2b.git - version: feature/high-level + version: dev ros2_kortex_vision: type: git url: https://github.com/Kinovarobotics/ros2_kortex_vision.git From 4c3f6ed0a99cacdcbfa22319b28f5f38d35e7fc0 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke <75437449+bhunecke@users.noreply.github.com> Date: Wed, 10 Dec 2025 13:38:09 +0100 Subject: [PATCH 23/35] Revise Cartesian error visualization instructions Updated section on visualizing Cartesian error to reference the Cartesian Error Visualizer and provided installation instructions. --- README.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 2aa2b58..1f00ae9 100644 --- a/README.md +++ b/README.md @@ -85,9 +85,14 @@ run rviz: ros2 launch eddie_ros rviz.launch.py use_sim:=true ``` -## Plotting cartesian error with RQT Plot +## Plotting cartesian error with [Cartesian Error Visualizer](https://github.com/Robots4Sustainability/cart-error-visualizer) -You can visualize the Cartesian error of the end-effectors with `rqt_plot`: +You can visualize the Cartesian error of the end-effectors with [Cartesian Error Visualizer](https://github.com/Robots4Sustainability/cart-error-visualizer). +Follow the instructions in the repository on how to install and run the visualizer. + +### Plotting with `rqt_plot` (not recommended) + +You can also visualize the Cartesian error of the end-effectors with `rqt_plot`: First, make sure to run the `eddie_ros_interface` node as described above. From aee8c3dbbec14da09268abea51bc8c8810b0a027 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 10 Dec 2025 14:34:32 +0100 Subject: [PATCH 24/35] fix goal cancel callback by clearing relative target and reset new target flag, clean up commented out code --- src/interface.cpp | 58 ++++------------------------------------------- 1 file changed, 4 insertions(+), 54 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index 25020b7..7a80778 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -222,6 +222,8 @@ void EddieRosInterface::execute_arm_control( if (goal_handle->is_canceling()) { result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; result->result_message = arm_side + " arm control goal was canceled"; + get_target_pose_relative(arm_side) = KDL::Frame::Identity(); // Clear relative target + get_new_target_flag(arm_side) = true; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); get_arm_execution_flag(arm_side).store(false); @@ -240,6 +242,8 @@ void EddieRosInterface::execute_arm_control( if (goal_handle->is_canceling()) { result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; result->result_message = arm_side + " arm control goal was canceled"; + get_target_pose_relative(arm_side) = KDL::Frame::Identity(); // Clear relative target + get_new_target_flag(arm_side) = true; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); get_arm_execution_flag(arm_side).store(false); @@ -1276,10 +1280,6 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { // Set new target pose for left arm from action goal if (should_control_left_arm() && new_target_leftarm) { KDL::Frame new_target_pose_leftarm_ee = pose_leftarm_ee * target_pose_leftarm_relative; - - // RCLCPP_INFO(get_logger(), "Applying action goal target pose for left arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", - // new_target_pose_leftarm_ee.p.x(), new_target_pose_leftarm_ee.p.y(), new_target_pose_leftarm_ee.p.z(), - // pose_leftarm_ee.p.x(), pose_leftarm_ee.p.y(), pose_leftarm_ee.p.z()); target_pose_leftarm_ee = new_target_pose_leftarm_ee; new_target_leftarm = false; // Reset flag @@ -1288,23 +1288,6 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { if (should_control_right_arm() && new_target_rightarm) { // Apply relative transformation in end-effector frame KDL::Frame new_target_pose_rightarm_ee = pose_rightarm_ee * target_pose_rightarm_relative; - - // relative transformation being applied - // RCLCPP_INFO(get_logger(), "Applying relative transform: offset(%.3f, %.3f, %.3f)", - // target_pose_rightarm_relative.p.x(), target_pose_rightarm_relative.p.y(), target_pose_rightarm_relative.p.z()); - - // end-effector orientation - // double roll, pitch, yaw; - // pose_rightarm_ee.M.GetRPY(roll, pitch, yaw); - // RCLCPP_INFO(get_logger(), "End-effector orientation (RPY): [%.3f, %.3f, %.3f] rad", roll, pitch, yaw); - - // relative Z movement translations in base frame - // KDL::Vector ee_z_axis = pose_rightarm_ee.M.UnitZ(); // End-effectors Z-axis in base frame - // RCLCPP_INFO(get_logger(), "EE Z-axis in base frame: [%.3f, %.3f, %.3f]", ee_z_axis.x(), ee_z_axis.y(), ee_z_axis.z()); - - // RCLCPP_INFO(get_logger(), "Applying action goal target pose for right arm: Position: [%f, %f, %f] (Current: [%f, %f, %f])", - // new_target_pose_rightarm_ee.p.x(), new_target_pose_rightarm_ee.p.y(), new_target_pose_rightarm_ee.p.z(), - // pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); target_pose_rightarm_ee = new_target_pose_rightarm_ee; new_target_rightarm = false; // Reset flag @@ -1314,39 +1297,6 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { // impedance control for right arm - start pose as target pose compute_cartesian_ctrl(eventData, eddie_state); - // Show target vs current pose occasionally and current gripper commands - // static int debug_counter = 0; - // if (++debug_counter % 1000 == 0) { // Every 1000 cycles (1 second at 1kHz) - // if (should_control_right_arm()) { - // KDL::Twist pose_error = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - // RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", - // pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), - // pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); - // RCLCPP_INFO(get_logger(), "Right arm target pose: Position: [%f, %f, %f]", - // target_pose_rightarm_ee.p.x(), target_pose_rightarm_ee.p.y(), target_pose_rightarm_ee.p.z()); - // RCLCPP_INFO(get_logger(), "Right arm current pose: Position: [%f, %f, %f]", - // pose_rightarm_ee.p.x(), pose_rightarm_ee.p.y(), pose_rightarm_ee.p.z()); - // RCLCPP_INFO(get_logger(), "Right arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", - // eddie_state->kinova_rightarm_state.gripper_pos_cmd[0], - // eddie_state->kinova_rightarm_state.gripper_vel_cmd[0], - // eddie_state->kinova_rightarm_state.gripper_frc_cmd[0]); - // RCLCPP_INFO(get_logger(), "Right arm gripper metrics: pos=%.3f, vel=%.3f, force=%.3f", - // eddie_state->kinova_rightarm_state.gripper_pos_msr[0], - // eddie_state->kinova_rightarm_state.gripper_vel_msr[0], - // eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); - // } - // if (should_control_left_arm()) { - // KDL::Twist pose_error = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - // RCLCPP_INFO(get_logger(), "Left arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", - // pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), - // pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); - // RCLCPP_INFO(get_logger(), "Left arm gripper commands: pos=%.3f, vel=%.3f, force=%.3f", - // eddie_state->kinova_leftarm_state.gripper_pos_cmd[0], - // eddie_state->kinova_leftarm_state.gripper_vel_cmd[0], - // eddie_state->kinova_leftarm_state.gripper_frc_cmd[0]); - // } - // } - // robif2b_kelo_drive_actuator_update(&wheel_act); if (should_control_right_arm()) { robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); From 8bc4de5db19bad32dc6a6768975b359b77bcd2cc Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 10 Dec 2025 15:32:16 +0100 Subject: [PATCH 25/35] refactor: rename helper methods for clarity and remove unused parameters --- include/eddie_ros/interface.hpp | 29 ++- src/interface.cpp | 333 ++++++++++++++++---------------- 2 files changed, 183 insertions(+), 179 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 52712d1..c4cac52 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -210,7 +210,7 @@ class EddieRosInterface : public rclcpp::Node { void compute_gravity_comp(events *eventData, EddieState *eddie_state); void compute_cartesian_ctrl(events *eventData, EddieState *eddie_state); - void publish_ee_errors(EddieState *eddie_state); + void publish_ee_errors(); void publish_joint_states(EddieState *eddie_state); public: @@ -234,7 +234,10 @@ class EddieRosInterface : public rclcpp::Node { KDL::JntArray tau_ctrl_leftarm; KDL::Wrenches f_ext_leftarm; KDL::Frame pose_leftarm_ee; + KDL::Frame target_pose_leftarm_ee; + KDL::Frame target_pose_leftarm_relative; KDL::Twist twist_leftarm_ee; + bool new_target_leftarm = false; std::unique_ptr rne_id_solver_leftarm; int num_jnts_rightarm; @@ -246,19 +249,11 @@ class EddieRosInterface : public rclcpp::Node { KDL::JntArray tau_ctrl_rightarm; KDL::Wrenches f_ext_rightarm; KDL::Frame pose_rightarm_ee; - KDL::Twist twist_rightarm_ee; - std::unique_ptr rne_id_solver_rightarm; - - KDL::Frame target_pose_leftarm_ee; KDL::Frame target_pose_rightarm_ee; - KDL::Vector target_pose_wrt_ee; - KDL::Frame target_pose_offset; - - // Relative target poses from action goals - KDL::Frame target_pose_leftarm_relative; KDL::Frame target_pose_rightarm_relative; - bool new_target_leftarm = false; + KDL::Twist twist_rightarm_ee; bool new_target_rightarm = false; + std::unique_ptr rne_id_solver_rightarm; // Error publishers rclcpp::Publisher::SharedPtr right_arm_ee_error_pub; @@ -326,12 +321,12 @@ class EddieRosInterface : public rclcpp::Node { const std::string& arm_side); // Helper methods to get arm-specific data - std::atomic& get_arm_execution_flag(const std::string& arm_side); - std::atomic& get_gripper_execution_flag(const std::string& arm_side); - KDL::Frame& get_target_pose_ee(const std::string& arm_side); - KDL::Frame& get_current_pose_ee(const std::string& arm_side); - KDL::Frame& get_target_pose_relative(const std::string& arm_side); - bool& get_new_target_flag(const std::string& arm_side); + std::atomic& arm_goal_executing(const std::string& arm_side); + std::atomic& gripper_goal_executing(const std::string& arm_side); + KDL::Frame& target_pose_ee(const std::string& arm_side); + KDL::Frame& current_pose_ee(const std::string& arm_side); + KDL::Frame& target_pose_relative(const std::string& arm_side); + bool& has_new_target(const std::string& arm_side); EddieState::KinovaArmState& get_arm_state(const std::string& arm_side); // Action servers diff --git a/src/interface.cpp b/src/interface.cpp index 7a80778..1a7cf20 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -91,7 +91,7 @@ bool EddieRosInterface::should_control_right_arm() const { } // Helper functions to get arm-specific data -std::atomic& EddieRosInterface::get_arm_execution_flag(const std::string& arm_side) { +std::atomic& EddieRosInterface::arm_goal_executing(const std::string& arm_side) { if (arm_side == "left") { return leftarm_goal_executing; } else { @@ -99,7 +99,7 @@ std::atomic& EddieRosInterface::get_arm_execution_flag(const std::string& } } -std::atomic& EddieRosInterface::get_gripper_execution_flag(const std::string& arm_side) { +std::atomic& EddieRosInterface::gripper_goal_executing(const std::string& arm_side) { if (arm_side == "left") { return leftgripper_goal_executing; } else { @@ -107,7 +107,7 @@ std::atomic& EddieRosInterface::get_gripper_execution_flag(const std::stri } } -KDL::Frame& EddieRosInterface::get_target_pose_ee(const std::string& arm_side) { +KDL::Frame& EddieRosInterface::target_pose_ee(const std::string& arm_side) { if (arm_side == "left") { return target_pose_leftarm_ee; } else { @@ -115,7 +115,7 @@ KDL::Frame& EddieRosInterface::get_target_pose_ee(const std::string& arm_side) { } } -KDL::Frame& EddieRosInterface::get_current_pose_ee(const std::string& arm_side) { +KDL::Frame& EddieRosInterface::current_pose_ee(const std::string& arm_side) { if (arm_side == "left") { return pose_leftarm_ee; } else { @@ -123,7 +123,7 @@ KDL::Frame& EddieRosInterface::get_current_pose_ee(const std::string& arm_side) } } -KDL::Frame& EddieRosInterface::get_target_pose_relative(const std::string& arm_side) { +KDL::Frame& EddieRosInterface::target_pose_relative(const std::string& arm_side) { if (arm_side == "left") { return target_pose_leftarm_relative; } else { @@ -131,7 +131,7 @@ KDL::Frame& EddieRosInterface::get_target_pose_relative(const std::string& arm_s } } -bool& EddieRosInterface::get_new_target_flag(const std::string& arm_side) { +bool& EddieRosInterface::has_new_target(const std::string& arm_side) { if (arm_side == "left") { return new_target_leftarm; } else { @@ -153,10 +153,10 @@ rclcpp_action::GoalResponse EddieRosInterface::handle_arm_goal( std::shared_ptr goal, const std::string& arm_side) { - (void)uuid; (void)goal; // avoid unused variable warnings + (void)uuid; (void)goal; // Check if a goal is already executing for this arm - if (get_arm_execution_flag(arm_side).load()) { + if (arm_goal_executing(arm_side)) { RCLCPP_WARN(this->get_logger(), "Rejecting %s arm goal request - another goal is already executing.", arm_side.c_str()); return rclcpp_action::GoalResponse::REJECT; @@ -178,7 +178,7 @@ void EddieRosInterface::handle_arm_accepted( const auto goal = goal_handle->get_goal(); // Set the execution flag to prevent new goals from being accepted - get_arm_execution_flag(arm_side).store(true); + arm_goal_executing(arm_side) = true; // Convert target pose to KDL::Frame (this will be treated as relative to current EE pose) KDL::Frame relative_target_pose = poseToKDL(goal->target_pose); @@ -195,8 +195,8 @@ void EddieRosInterface::handle_arm_accepted( } // Store the relative target pose and set flags to apply it in the next execute cycle - get_target_pose_relative(arm_side) = relative_target_pose; - get_new_target_flag(arm_side) = true; + target_pose_relative(arm_side) = relative_target_pose; + has_new_target(arm_side) = true; RCLCPP_INFO(this->get_logger(), "Set relative target pose for %s arm: offset(%.3f, %.3f, %.3f)", arm_side.c_str(), @@ -218,15 +218,16 @@ void EddieRosInterface::execute_arm_control( // Wait for the main loop to update the target pose int wait_cycles = 0; - while (get_new_target_flag(arm_side) && rclcpp::ok() && wait_cycles < 100) { // Wait up to 1 second + while (has_new_target(arm_side) && rclcpp::ok() && wait_cycles < 100) { // Wait up to 1 second if (goal_handle->is_canceling()) { result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; result->result_message = arm_side + " arm control goal was canceled"; - get_target_pose_relative(arm_side) = KDL::Frame::Identity(); // Clear relative target - get_new_target_flag(arm_side) = true; + // Clear relative target + target_pose_relative(arm_side) = KDL::Frame::Identity(); + has_new_target(arm_side) = true; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); - get_arm_execution_flag(arm_side).store(false); + arm_goal_executing(arm_side) = false; return; } loop_rate.sleep(); @@ -242,16 +243,17 @@ void EddieRosInterface::execute_arm_control( if (goal_handle->is_canceling()) { result->result_code = eddie_ros::action::ArmControl::Result::CANCELLED; result->result_message = arm_side + " arm control goal was canceled"; - get_target_pose_relative(arm_side) = KDL::Frame::Identity(); // Clear relative target - get_new_target_flag(arm_side) = true; + // Clear relative target + target_pose_relative(arm_side) = KDL::Frame::Identity(); + has_new_target(arm_side) = true; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal canceled", arm_side.c_str()); - get_arm_execution_flag(arm_side).store(false); + arm_goal_executing(arm_side) = false; return; } // Calculate the real-time error between current and target pose - KDL::Twist pose_error = KDL::diff(get_target_pose_ee(arm_side), get_current_pose_ee(arm_side)); + KDL::Twist pose_error = KDL::diff(target_pose_ee(arm_side), current_pose_ee(arm_side)); // Check if the error is within tolerance double position_error = pose_error.vel.Norm(); @@ -260,16 +262,16 @@ void EddieRosInterface::execute_arm_control( if (position_error < position_tolerance /*&& rotation_error < rotation_tolerance*/) { result->result_code = eddie_ros::action::ArmControl::Result::SUCCESS; result->result_message = arm_side + " arm successfully reached target position"; - result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); + result->final_pose = kdlToPose(current_pose_ee(arm_side)); goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "%s arm control goal succeeded - pose error: pos=%.4f rot=%.4f", arm_side.c_str(), position_error, rotation_error); - get_arm_execution_flag(arm_side).store(false); + arm_goal_executing(arm_side) = false; return; } // Update progress with current pose and error information - feedback->current_pose = kdlToPose(get_current_pose_ee(arm_side)); + feedback->current_pose = kdlToPose(current_pose_ee(arm_side)); goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -277,17 +279,17 @@ void EddieRosInterface::execute_arm_control( // If we reach here, the goal timed out if (rclcpp::ok()) { - KDL::Twist final_error = KDL::diff(get_target_pose_ee(arm_side), get_current_pose_ee(arm_side)); + KDL::Twist final_error = KDL::diff(target_pose_ee(arm_side), current_pose_ee(arm_side)); result->result_code = eddie_ros::action::ArmControl::Result::GOAL_TIMEOUT; result->result_message = arm_side + " arm control goal timed out - final error: pos=" + std::to_string(final_error.vel.Norm()) + " rot=" + std::to_string(final_error.rot.Norm()); - result->final_pose = kdlToPose(get_current_pose_ee(arm_side)); + result->final_pose = kdlToPose(current_pose_ee(arm_side)); goal_handle->abort(result); RCLCPP_WARN(this->get_logger(), "%s arm control goal timed out after %d iterations, final error: pos=%.4f rot=%.4f", arm_side.c_str(), max_iterations, final_error.vel.Norm(), final_error.rot.Norm()); } - get_arm_execution_flag(arm_side).store(false); + arm_goal_executing(arm_side) = false; } // Gripper control action server callbacks @@ -299,7 +301,7 @@ rclcpp_action::GoalResponse EddieRosInterface::handle_gripper_goal( (void)uuid; (void)goal; // Check if a goal is already executing for this gripper - if (get_gripper_execution_flag(arm_side).load()) { + if (gripper_goal_executing(arm_side)) { RCLCPP_WARN(this->get_logger(), "Rejecting %s gripper goal request - another goal is already executing.", arm_side.c_str()); return rclcpp_action::GoalResponse::REJECT; @@ -321,7 +323,7 @@ void EddieRosInterface::handle_gripper_accepted( const auto goal = goal_handle->get_goal(); // Set the execution flag to prevent new goals from being accepted - get_gripper_execution_flag(arm_side).store(true); + gripper_goal_executing(arm_side) = true; // Get the appropriate arm state auto& arm_state = get_arm_state(arm_side); @@ -372,7 +374,7 @@ void EddieRosInterface::execute_gripper_control( result->result_message = arm_side + " gripper control goal was canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "%s gripper control goal canceled", arm_side.c_str()); - get_gripper_execution_flag(arm_side).store(false); + gripper_goal_executing(arm_side) = false; return; } // Update progress with current gripper position @@ -391,7 +393,7 @@ void EddieRosInterface::execute_gripper_control( RCLCPP_INFO(this->get_logger(), "Gripper control goal succeeded"); } - get_gripper_execution_flag(arm_side).store(false); + gripper_goal_executing(arm_side) = false; } PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { @@ -578,7 +580,7 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) this->ee_error_timer_ = this->create_wall_timer( std::chrono::milliseconds(1), [this]() { - this->publish_ee_errors(&eddie_state); + this->publish_ee_errors(); } ); } @@ -684,7 +686,6 @@ void EddieRosInterface::initialize_action_servers() { handle_goal_right_gripper, handle_cancel_right_gripper, handle_accepted_right_gripper ); } - if (should_control_left_arm()) { RCLCPP_INFO(get_logger(), "Creating action servers for the LEFT arm"); action_server_left_arm_control_ = rclcpp_action::create_server( @@ -1041,7 +1042,7 @@ void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } - if (get_arm_execution_flag("right").load() || get_arm_execution_flag("left").load()) { + if (arm_goal_executing("right") || arm_goal_executing("left")) { RCLCPP_INFO(get_logger(), "Execution flag set, transitioning to EXECUTE state"); RCLCPP_DEBUG(get_logger(), "Exiting idle state"); produce_event(eventData, E_IDLE_EXIT_EXECUTE); @@ -1049,96 +1050,68 @@ void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { } void EddieRosInterface::compile(events *eventData, const EddieState *eddie_state) { + (void)eddie_state; RCLCPP_DEBUG(get_logger(), "Exiting compile state"); produce_event(eventData, E_COMPILE_EXIT); } -void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddie_state) { - if (should_control_right_arm()) { - for (auto &wrench : f_ext_rightarm) { - wrench = KDL::Wrench::Zero(); - } - int r = 0; - KDL::JntArrayVel jnt_array_vel_rightarm(q_rightarm, qd_rightarm); - KDL::Twist jd_qd_rightarm; - KDL::Twist xdd_minus_jd_qd_rightarm; - KDL::Twist xdd; - KDL::ChainJntToJacDotSolver jnt_to_jac_dot_solver_rightarm(rightarm_chain); - KDL::ChainIkSolverVel_pinv ik_solver_vel_rightarm(rightarm_chain); - jnt_to_jac_dot_solver_rightarm.JntToJacDot(jnt_array_vel_rightarm, jd_qd_rightarm); - xdd_minus_jd_qd_rightarm = xdd - jd_qd_rightarm; - ik_solver_vel_rightarm.CartToJnt(q_rightarm, xdd_minus_jd_qd_rightarm, qdd_rightarm); - r = rne_id_solver_rightarm->CartToJnt( - q_rightarm, qd_rightarm, qdd_rightarm, f_ext_rightarm, tau_ctrl_rightarm - ); - if (r < 0) { - RCLCPP_ERROR(get_logger(), "Right arm RNE ID solver failed with error code: %d", r); - return; - } - for (int i = 0; i < num_jnts_rightarm; i++) { - saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); - } - } - if (should_control_left_arm()) { - for (auto &wrench : f_ext_leftarm) { - wrench = KDL::Wrench::Zero(); - } - int r = 0; - KDL::JntArrayVel jnt_array_vel_leftarm(q_leftarm, qd_leftarm); - KDL::Twist jd_qd_leftarm; - KDL::Twist xdd_minus_jd_qd_leftarm; - KDL::Twist xdd_left; - KDL::ChainJntToJacDotSolver jnt_to_jac_dot_solver_leftarm(leftarm_chain); - KDL::ChainIkSolverVel_pinv ik_solver_vel_leftarm(leftarm_chain); - jnt_to_jac_dot_solver_leftarm.JntToJacDot(jnt_array_vel_leftarm, jd_qd_leftarm); - xdd_minus_jd_qd_leftarm = xdd_left - jd_qd_leftarm; - ik_solver_vel_leftarm.CartToJnt(q_leftarm, xdd_minus_jd_qd_leftarm, qdd_leftarm); - r = rne_id_solver_leftarm->CartToJnt( - q_leftarm, qd_leftarm, qdd_leftarm, f_ext_leftarm, tau_ctrl_leftarm - ); - if (r < 0) { - RCLCPP_ERROR(get_logger(), "Left arm RNE ID solver failed with error code: %d", r); - return; - } - for (int i = 0; i < num_jnts_leftarm; i++) { - saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - eddie_state->kinova_leftarm_state.eff_cmd[i] = tau_ctrl_leftarm(i); - } - } -} - -void EddieRosInterface::publish_ee_errors(EddieState *eddie_state) { - - if (should_control_right_arm()) { - KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - auto twist_msg = std::make_unique(); - // Linear error - twist_msg->linear.x = delta_pose_rightarm_ee.vel.x(); - twist_msg->linear.y = delta_pose_rightarm_ee.vel.y(); - twist_msg->linear.z = delta_pose_rightarm_ee.vel.z(); - // Angular error - twist_msg->angular.x = delta_pose_rightarm_ee.rot.x(); - twist_msg->angular.y = delta_pose_rightarm_ee.rot.y(); - twist_msg->angular.z = delta_pose_rightarm_ee.rot.z(); - right_arm_ee_error_pub->publish(std::move(twist_msg)); - } - if (should_control_left_arm()) { - KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - auto twist_msg = std::make_unique(); - // Linear error - twist_msg->linear.x = delta_pose_leftarm_ee.vel.x(); - twist_msg->linear.y = delta_pose_leftarm_ee.vel.y(); - twist_msg->linear.z = delta_pose_leftarm_ee.vel.z(); - // Angular error - twist_msg->angular.x = delta_pose_leftarm_ee.rot.x(); - twist_msg->angular.y = delta_pose_leftarm_ee.rot.y(); - twist_msg->angular.z = delta_pose_leftarm_ee.rot.z(); - left_arm_ee_error_pub->publish(std::move(twist_msg)); - } -} +// void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddie_state) { +// if (should_control_right_arm()) { +// for (auto &wrench : f_ext_rightarm) { +// wrench = KDL::Wrench::Zero(); +// } +// int r = 0; +// KDL::JntArrayVel jnt_array_vel_rightarm(q_rightarm, qd_rightarm); +// KDL::Twist jd_qd_rightarm; +// KDL::Twist xdd_minus_jd_qd_rightarm; +// KDL::Twist xdd; +// KDL::ChainJntToJacDotSolver jnt_to_jac_dot_solver_rightarm(rightarm_chain); +// KDL::ChainIkSolverVel_pinv ik_solver_vel_rightarm(rightarm_chain); +// jnt_to_jac_dot_solver_rightarm.JntToJacDot(jnt_array_vel_rightarm, jd_qd_rightarm); +// xdd_minus_jd_qd_rightarm = xdd - jd_qd_rightarm; +// ik_solver_vel_rightarm.CartToJnt(q_rightarm, xdd_minus_jd_qd_rightarm, qdd_rightarm); +// r = rne_id_solver_rightarm->CartToJnt( +// q_rightarm, qd_rightarm, qdd_rightarm, f_ext_rightarm, tau_ctrl_rightarm +// ); +// if (r < 0) { +// RCLCPP_ERROR(get_logger(), "Right arm RNE ID solver failed with error code: %d", r); +// return; +// } +// for (int i = 0; i < num_jnts_rightarm; i++) { +// saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); +// eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); +// } +// } +// if (should_control_left_arm()) { +// for (auto &wrench : f_ext_leftarm) { +// wrench = KDL::Wrench::Zero(); +// } +// int r = 0; +// KDL::JntArrayVel jnt_array_vel_leftarm(q_leftarm, qd_leftarm); +// KDL::Twist jd_qd_leftarm; +// KDL::Twist xdd_minus_jd_qd_leftarm; +// KDL::Twist xdd_left; +// KDL::ChainJntToJacDotSolver jnt_to_jac_dot_solver_leftarm(leftarm_chain); +// KDL::ChainIkSolverVel_pinv ik_solver_vel_leftarm(leftarm_chain); +// jnt_to_jac_dot_solver_leftarm.JntToJacDot(jnt_array_vel_leftarm, jd_qd_leftarm); +// xdd_minus_jd_qd_leftarm = xdd_left - jd_qd_leftarm; +// ik_solver_vel_leftarm.CartToJnt(q_leftarm, xdd_minus_jd_qd_leftarm, qdd_leftarm); +// r = rne_id_solver_leftarm->CartToJnt( +// q_leftarm, qd_leftarm, qdd_leftarm, f_ext_leftarm, tau_ctrl_leftarm +// ); +// if (r < 0) { +// RCLCPP_ERROR(get_logger(), "Left arm RNE ID solver failed with error code: %d", r); +// return; +// } +// for (int i = 0; i < num_jnts_leftarm; i++) { +// saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); +// eddie_state->kinova_leftarm_state.eff_cmd[i] = tau_ctrl_leftarm(i); +// } +// } +// } void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *eddie_state) { + (void)eventData; long cycle_time_msr = eddie_state->time.cycle_time_msr; @@ -1237,7 +1210,6 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } } - void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { RCLCPP_INFO(get_logger(), "In execute state"); @@ -1251,68 +1223,105 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { // robif2b_kelo_drive_imu_update(&imu); // robif2b_eddie_power_board_update(&power_board); - for (int i = 0; i < num_jnts_rightarm; i++) { - q_rightarm(i) = eddie_state->kinova_rightarm_state.pos_msr[i]; - qd_rightarm(i) = eddie_state->kinova_rightarm_state.vel_msr[i]; - } - for (int i = 0; i < num_jnts_leftarm; i++) { - q_leftarm(i) = eddie_state->kinova_leftarm_state.pos_msr[i]; - qd_leftarm(i) = eddie_state->kinova_leftarm_state.vel_msr[i]; - } - - KDL::JntArrayVel q_qd_rightarm(q_rightarm, qd_rightarm); - KDL::JntArrayVel q_qd_leftarm(q_leftarm, qd_leftarm); - - KDL::ChainFkSolverPos_recursive fpk_pose_rightarm_ee(rightarm_chain); - fpk_pose_rightarm_ee.JntToCart(q_rightarm, pose_rightarm_ee); - KDL::ChainFkSolverVel_recursive fvk_twist_rightarm_ee(rightarm_chain); - KDL::FrameVel _twist_rightarm_ee; - fvk_twist_rightarm_ee.JntToCart(q_qd_rightarm, _twist_rightarm_ee); - twist_rightarm_ee = _twist_rightarm_ee.deriv(); - - KDL::ChainFkSolverPos_recursive fpk_pose_leftarm_ee(leftarm_chain); - fpk_pose_leftarm_ee.JntToCart(q_leftarm, pose_leftarm_ee); - KDL::ChainFkSolverVel_recursive fvk_twist_leftarm_ee(leftarm_chain); - KDL::FrameVel _twist_leftarm_ee; - fvk_twist_leftarm_ee.JntToCart(q_qd_leftarm, _twist_leftarm_ee); - twist_leftarm_ee = _twist_leftarm_ee.deriv(); - - // Set new target pose for left arm from action goal - if (should_control_left_arm() && new_target_leftarm) { - KDL::Frame new_target_pose_leftarm_ee = pose_leftarm_ee * target_pose_leftarm_relative; - target_pose_leftarm_ee = new_target_pose_leftarm_ee; - - new_target_leftarm = false; // Reset flag - } - // Set new target pose for right arm from action goal - if (should_control_right_arm() && new_target_rightarm) { - // Apply relative transformation in end-effector frame - KDL::Frame new_target_pose_rightarm_ee = pose_rightarm_ee * target_pose_rightarm_relative; - target_pose_rightarm_ee = new_target_pose_rightarm_ee; + if (should_control_right_arm()) { + for (int i = 0; i < num_jnts_rightarm; i++) { + q_rightarm(i) = eddie_state->kinova_rightarm_state.pos_msr[i]; + qd_rightarm(i) = eddie_state->kinova_rightarm_state.vel_msr[i]; + } - new_target_rightarm = false; // Reset flag - } + KDL::JntArrayVel q_qd_rightarm(q_rightarm, qd_rightarm); + KDL::ChainFkSolverPos_recursive fpk_pose_rightarm_ee(rightarm_chain); + fpk_pose_rightarm_ee.JntToCart(q_rightarm, pose_rightarm_ee); + KDL::ChainFkSolverVel_recursive fvk_twist_rightarm_ee(rightarm_chain); + KDL::FrameVel _twist_rightarm_ee; + fvk_twist_rightarm_ee.JntToCart(q_qd_rightarm, _twist_rightarm_ee); + twist_rightarm_ee = _twist_rightarm_ee.deriv(); - // impedance control for right arm - start pose as target pose - compute_cartesian_ctrl(eventData, eddie_state); + // Set new target pose for right arm from action goal + if (new_target_rightarm) { + // Apply relative transformation in end-effector frame + KDL::Frame new_target_pose_rightarm_ee = pose_rightarm_ee * target_pose_rightarm_relative; + target_pose_rightarm_ee = new_target_pose_rightarm_ee; + + new_target_rightarm = false; // Reset flag + } - // robif2b_kelo_drive_actuator_update(&wheel_act); - if (should_control_right_arm()) { + // impedance control for right arm - start pose as target pose + compute_cartesian_ctrl(eventData, eddie_state); + robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); robif2b_kinova_gen3_update(&kinova_rightarm); } if (should_control_left_arm()) { + for (int i = 0; i < num_jnts_leftarm; i++) { + q_leftarm(i) = eddie_state->kinova_leftarm_state.pos_msr[i]; + qd_leftarm(i) = eddie_state->kinova_leftarm_state.vel_msr[i]; + } + + KDL::JntArrayVel q_qd_leftarm(q_leftarm, qd_leftarm); + + KDL::ChainFkSolverPos_recursive fpk_pose_leftarm_ee(leftarm_chain); + fpk_pose_leftarm_ee.JntToCart(q_leftarm, pose_leftarm_ee); + KDL::ChainFkSolverVel_recursive fvk_twist_leftarm_ee(leftarm_chain); + KDL::FrameVel _twist_leftarm_ee; + fvk_twist_leftarm_ee.JntToCart(q_qd_leftarm, _twist_leftarm_ee); + twist_leftarm_ee = _twist_leftarm_ee.deriv(); + + // Set new target pose for left arm from action goal + if (should_control_left_arm() && new_target_leftarm) { + KDL::Frame new_target_pose_leftarm_ee = pose_leftarm_ee * target_pose_leftarm_relative; + target_pose_leftarm_ee = new_target_pose_leftarm_ee; + + new_target_leftarm = false; // Reset flag + } + + // impedance control for right arm - start pose as target pose + compute_cartesian_ctrl(eventData, eddie_state); + robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); robif2b_kinova_gen3_update(&kinova_leftarm); } - if (!get_arm_execution_flag("right").load() && !get_arm_execution_flag("left").load()) { + + // robif2b_kelo_drive_actuator_update(&wheel_act); + + if (!arm_goal_executing("right") && !arm_goal_executing("left")) { RCLCPP_INFO(get_logger(), "Execution flags cleared, transitioning to IDLE state"); RCLCPP_DEBUG(get_logger(), "Exiting execute state"); produce_event(eventData, E_EXECUTE_EXIT_IDLE); } } +void EddieRosInterface::publish_ee_errors() { + + if (should_control_right_arm()) { + KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); + auto twist_msg = std::make_unique(); + // Linear error + twist_msg->linear.x = delta_pose_rightarm_ee.vel.x(); + twist_msg->linear.y = delta_pose_rightarm_ee.vel.y(); + twist_msg->linear.z = delta_pose_rightarm_ee.vel.z(); + // Angular error + twist_msg->angular.x = delta_pose_rightarm_ee.rot.x(); + twist_msg->angular.y = delta_pose_rightarm_ee.rot.y(); + twist_msg->angular.z = delta_pose_rightarm_ee.rot.z(); + right_arm_ee_error_pub->publish(std::move(twist_msg)); + } + if (should_control_left_arm()) { + KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); + auto twist_msg = std::make_unique(); + // Linear error + twist_msg->linear.x = delta_pose_leftarm_ee.vel.x(); + twist_msg->linear.y = delta_pose_leftarm_ee.vel.y(); + twist_msg->linear.z = delta_pose_leftarm_ee.vel.z(); + // Angular error + twist_msg->angular.x = delta_pose_leftarm_ee.rot.x(); + twist_msg->angular.y = delta_pose_leftarm_ee.rot.y(); + twist_msg->angular.z = delta_pose_leftarm_ee.rot.z(); + left_arm_ee_error_pub->publish(std::move(twist_msg)); + } +} + void EddieRosInterface::publish_joint_states(EddieState *eddie_state) { // Create a JointState message. auto joint_state_msg = sensor_msgs::msg::JointState(); From 3a1f4f9e18a1b136cf342a1d00462bfa5bfc7ae6 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 10 Dec 2025 15:36:58 +0100 Subject: [PATCH 26/35] update logging --- src/interface.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index 1a7cf20..ea8f29c 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -24,7 +24,10 @@ static long timespec_to_usec(const struct timespec *t) { return t->tv_sec * USEC_IN_SEC + t->tv_nsec / NSEC_IN_USEC; } -void sigint_handler(int signum) { keep_running = 0; } +void sigint_handler(int signum) { + (void)signum; + keep_running = 0; +} double evaluate_equality_constraint(double quantity, double reference) { return quantity - reference; @@ -994,7 +997,7 @@ void EddieRosInterface::configure(events *eventData, EddieState *eddie_state) { } void EddieRosInterface::idle(events *eventData, EddieState *eddie_state) { - RCLCPP_INFO(get_logger(), "In idle state"); + RCLCPP_DEBUG(get_logger(), "In idle state"); if (should_control_right_arm()) { // robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); // robif2b_kinova_gen3_update(&kinova_rightarm); @@ -1211,7 +1214,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { - RCLCPP_INFO(get_logger(), "In execute state"); + RCLCPP_DEBUG(get_logger(), "In execute state"); // // Update the EtherCAT state // robif2b_ethercat_update(&ecat); From 49332dc49688af4fd8e093dade4f980fbf420256 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 10 Dec 2025 15:39:58 +0100 Subject: [PATCH 27/35] update signal handler --- src/interface.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/interface.cpp b/src/interface.cpp index ea8f29c..1a274e8 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -24,10 +24,7 @@ static long timespec_to_usec(const struct timespec *t) { return t->tv_sec * USEC_IN_SEC + t->tv_nsec / NSEC_IN_USEC; } -void sigint_handler(int signum) { - (void)signum; - keep_running = 0; -} +void sigint_handler(int signum) { keep_running = 0; } double evaluate_equality_constraint(double quantity, double reference) { return quantity - reference; From 95ecfba3ffa9ebc7bbed546b3fb187276a01443c Mon Sep 17 00:00:00 2001 From: rinaalo Date: Wed, 10 Dec 2025 17:02:02 +0100 Subject: [PATCH 28/35] Reverted adjusting p-gain changes, since controller already handles that Changed evaluate bilateral constraint function Made torque smoothing only active during dynamic parameter change --- include/eddie_ros/interface.hpp | 3 +- src/interface-parameters.cpp | 8 ++-- src/interface.cpp | 77 ++++++++++++--------------------- 3 files changed, 34 insertions(+), 54 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index a6e68a2..c5e014d 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -224,7 +224,8 @@ class EddieRosInterface : public rclcpp::Node { void publish_torque_debug_info( const KDL::JntArray& raw_torques, const KDL::JntArray& smoothed_torques, - const std::string& arm_side + const std::string& arm_side, + bool is_smoothing_active ); public: diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index 64dcc87..ccc5d9b 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -106,7 +106,7 @@ void EddieRosInterface::reload_pid_gains() double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); - double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); + //double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); @@ -117,7 +117,7 @@ void EddieRosInterface::reload_pid_gains() double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); - double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); + //double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); // Get values for the LEFT arm double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); @@ -129,7 +129,7 @@ void EddieRosInterface::reload_pid_gains() double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); - double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); + //double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); @@ -140,7 +140,7 @@ void EddieRosInterface::reload_pid_gains() double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); - double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); + //double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); // Set PID controller gains for the RIGHT arm pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate); diff --git a/src/interface.cpp b/src/interface.cpp index c5dff34..ade9ad0 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "eddie_ros/action/arm_control.hpp" @@ -40,13 +41,13 @@ double evaluate_greater_than_constraint(double quantity, double threshold) { double evaluate_bilateral_constraint(double quantity, double lower, double upper) { if (quantity < lower) - return quantity - lower; + //return quantity - lower; + return quantity; else if (quantity > upper) - return quantity - upper; + //return quantity - upper; + return quantity; else return 0.0; - // in this case, maybe wecan remove the P controller so it just usses integrator to go slowly to zero ? - // or integrator term wiht low P } void saturate(double *value, double min, double max) { @@ -1261,7 +1262,8 @@ void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddi void EddieRosInterface::publish_torque_debug_info( const KDL::JntArray& raw_torques, const KDL::JntArray& smoothed_torques, - const std::string& arm_side) + const std::string& arm_side, + bool is_smoothing_active) { // Check if anyone is actually subscribed to the topics if (raw_torque_publisher_->get_subscription_count() == 0 && @@ -1285,7 +1287,13 @@ void EddieRosInterface::publish_torque_debug_info( smoothed_torque_msg.header.stamp = now; for (unsigned int i = 0; i < smoothed_torques.rows(); i++) { smoothed_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); - smoothed_torque_msg.effort.push_back(smoothed_torques(i)); + if (is_smoothing_active) { + // Filter is on: Send the actual smoothed value + smoothed_torque_msg.effort.push_back(smoothed_torques(i)); + } else { + // Filter is off: Send NaN + smoothed_torque_msg.effort.push_back(std::numeric_limits::quiet_NaN()); + } } smoothed_torque_publisher_->publish(smoothed_torque_msg); } @@ -1339,22 +1347,12 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed // Get smoothing factor value from parameters const double alpha = this->get_parameter("torque_smoothing_alpha").as_double(); - const std::chrono::milliseconds smoothing_duration(200); + const std::chrono::milliseconds smoothing_duration(4000); if (should_control_right_arm()) { // Calculate the raw cartesian error KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - double original_p_pos_x = this->get_parameter("pid.right.pos.x.p").as_double(); - double original_p_pos_y = this->get_parameter("pid.right.pos.y.p").as_double(); - double original_p_pos_z = this->get_parameter("pid.right.pos.z.p").as_double(); - - double original_p_rot_x = this->get_parameter("pid.right.rot.x.p").as_double(); - double original_p_rot_y = this->get_parameter("pid.right.rot.y.p").as_double(); - double original_p_rot_z = this->get_parameter("pid.right.rot.z.p").as_double(); - - const double low_gain_factor = 0.1; // Use 10% of the P-gain inside the deadband - // Apply bilateral constraint to each error component double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); @@ -1365,29 +1363,9 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); // Pass the processed error to the PID controllers - // Dynamically adjust the P-gain for the controller based on deadband - if (error_x == 0.0) { - pid_rightarm_ee_pos_x.kp = original_p_pos_x * low_gain_factor; // Set to low gain - } double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); - pid_rightarm_ee_pos_x.kp = original_p_pos_x; // Restore to high gain for next cycle - - if (error_y == 0.0) { - pid_rightarm_ee_pos_y.kp = original_p_pos_y * low_gain_factor; - } double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); - pid_rightarm_ee_pos_y.kp = original_p_pos_y; - - if (error_z == 0.0) { - pid_rightarm_ee_pos_z.kp = original_p_pos_z * low_gain_factor; - } double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); - pid_rightarm_ee_pos_z.kp = original_p_pos_z; - - // Pass the processed error to the PID controllers - // double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); - // double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); - // double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); double mx_right = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); double my_right = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); @@ -1431,17 +1409,17 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed saturate(&smoothed_torques_right_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right_(i); } */ -/* bool apply_smoothing = right_arm_smoothing_active_.load(); + bool apply_smoothing = right_arm_smoothing_active_.load(); if (apply_smoothing) { auto elapsed = std::chrono::steady_clock::now() - right_arm_smoothing_start_time_; if (elapsed < smoothing_duration) { - // We are in the smoothing period, apply the filter + // In the smoothing period, apply the low-pass filter for (unsigned int i = 0; i < num_jnts_rightarm; i++) { smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); } } else { - // Smoothing duration has passed, turn it off. + // Smoothing duration has passed, turn filter off. right_arm_smoothing_active_.store(false); apply_smoothing = false; // On the first step after disabling, snap the smoothed value to the raw value @@ -1449,6 +1427,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed RCLCPP_INFO(this->get_logger(), "Right arm torque smoothing deactivated."); } } + publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right_, "right", apply_smoothing); // Use the raw command if smoothing is not active, otherwise use the smoothed one. const KDL::JntArray& final_torques = apply_smoothing ? smoothed_torques_right_ : tau_ctrl_rightarm; @@ -1459,11 +1438,11 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed saturate(&torque_to_send, -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = torque_to_send; } - */ - for (int i = 0; i < num_jnts_rightarm; i++) { + + /* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); - } + } */ } if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); @@ -1543,7 +1522,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed RCLCPP_ERROR(get_logger(), "Left arm RNE ID solver failed with error code: %d", r_left); } // Apply the low-pass filter to smooth the torque commands - for (unsigned int i = 0; i < num_jnts_leftarm; i++) { +/* for (unsigned int i = 0; i < num_jnts_leftarm; i++) { smoothed_torques_left_(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left_(i); } publish_torque_debug_info(tau_ctrl_leftarm, smoothed_torques_left_, "left"); @@ -1551,11 +1530,11 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&smoothed_torques_left_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); this->eddie_state.kinova_leftarm_state.eff_cmd[i] = smoothed_torques_left_(i); - } -/* for (int i = 0; i < num_jnts_leftarm; i++) { + } */ + for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_leftarm_state.eff_cmd[i] = tau_ctrl_leftarm(i); - } */ + } } } @@ -1667,7 +1646,7 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { if (++debug_counter % 1000 == 0) { // Every 1000 cycles (1 second at 1kHz) if (should_control_right_arm()) { KDL::Twist pose_error = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", + /* RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); RCLCPP_INFO(get_logger(), "Right arm target pose: Position: [%f, %f, %f]", @@ -1681,7 +1660,7 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { RCLCPP_INFO(get_logger(), "Right arm gripper metrics: pos=%.3f, vel=%.3f, force=%.3f", eddie_state->kinova_rightarm_state.gripper_pos_msr[0], eddie_state->kinova_rightarm_state.gripper_vel_msr[0], - eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); + eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); */ } if (should_control_left_arm()) { KDL::Twist pose_error = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); From cf4c8b48f2352829305cfc5c74528d940b7af8f2 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Fri, 12 Dec 2025 14:37:28 +0100 Subject: [PATCH 29/35] Refactor torque smoothing parameters and publishers for consistency --- include/eddie_ros/interface.hpp | 17 +++++---- src/interface-parameters.cpp | 10 ++--- src/interface.cpp | 65 ++++++++++++++++++--------------- 3 files changed, 49 insertions(+), 43 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index c5e014d..6729cf4 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -276,8 +276,9 @@ class EddieRosInterface : public rclcpp::Node { bool new_target_rightarm = false; // Smoothed torque commands - KDL::JntArray smoothed_torques_right_; - KDL::JntArray smoothed_torques_left_; + KDL::JntArray smoothed_torques_right; + KDL::JntArray smoothed_torques_left; + std::ofstream torque_log_file; // Error publishers rclcpp::Publisher::SharedPtr right_arm_ee_error_pub; @@ -285,14 +286,14 @@ class EddieRosInterface : public rclcpp::Node { rclcpp::TimerBase::SharedPtr ee_error_timer_; // Torque command publishers - rclcpp::Publisher::SharedPtr raw_torque_publisher_; - rclcpp::Publisher::SharedPtr smoothed_torque_publisher_; + rclcpp::Publisher::SharedPtr raw_torque_publisher; + rclcpp::Publisher::SharedPtr smoothed_torque_publisher; // State for conditional smoothing - std::atomic right_arm_smoothing_active_{false}; - std::atomic left_arm_smoothing_active_{false}; - std::chrono::steady_clock::time_point right_arm_smoothing_start_time_; - std::chrono::steady_clock::time_point left_arm_smoothing_start_time_; + std::atomic right_arm_smoothing_active{false}; + std::atomic left_arm_smoothing_active{false}; + std::chrono::steady_clock::time_point right_arm_smoothing_start_time; + std::chrono::steady_clock::time_point left_arm_smoothing_start_time; // Flags to track if arms are currently executing goals std::atomic rightarm_goal_executing = false; diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index ccc5d9b..0798a05 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -37,7 +37,7 @@ void EddieRosInterface::declare_pid_gains() { RCLCPP_INFO(this->get_logger(), "Declaring PID gains from parameter server..."); // Torque smoothing parameter - this->declare_parameter("torque_smoothing_alpha", 0.05); + this->declare_parameter("torque_smoothing_alpha", 0.1); const double default_pos_deadband = 0.005; const double default_rot_deadband = 0.02; @@ -182,13 +182,13 @@ rcl_interfaces::msg::SetParametersResult EddieRosInterface::parameters_callback( // Activate smoothing based on which arm's gain was changed if (name.rfind("pid.right", 0) == 0) { RCLCPP_INFO(this->get_logger(), "Right arm PID parameter changed. Activating torque smoothing."); - this->right_arm_smoothing_start_time_ = std::chrono::steady_clock::now(); - this->right_arm_smoothing_active_.store(true); + this->right_arm_smoothing_start_time = std::chrono::steady_clock::now(); + this->right_arm_smoothing_active.store(true); } else if (name.rfind("pid.left", 0) == 0) { RCLCPP_INFO(this->get_logger(), "Left arm PID parameter changed. Activating torque smoothing."); - this->left_arm_smoothing_start_time_ = std::chrono::steady_clock::now(); - this->left_arm_smoothing_active_.store(true); + this->left_arm_smoothing_start_time = std::chrono::steady_clock::now(); + this->left_arm_smoothing_active.store(true); } } else if (name == "torque_smoothing_alpha") { diff --git a/src/interface.cpp b/src/interface.cpp index ade9ad0..439955e 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -506,12 +506,12 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Smoothed torque commands if (should_control_right_arm()) { - smoothed_torques_right_.resize(rightarm_chain.getNrOfJoints()); - smoothed_torques_right_.data.setZero(); + smoothed_torques_right.resize(rightarm_chain.getNrOfJoints()); + smoothed_torques_right.data.setZero(); } if (should_control_left_arm()) { - smoothed_torques_left_.resize(leftarm_chain.getNrOfJoints()); - smoothed_torques_left_.data.setZero(); + smoothed_torques_left.resize(leftarm_chain.getNrOfJoints()); + smoothed_torques_left.data.setZero(); } // joint inertias: @@ -697,8 +697,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) ); // Torque publishers - raw_torque_publisher_ = this->create_publisher("/raw_torques", 10); - smoothed_torque_publisher_ = this->create_publisher("/smoothed_torques", 10); + raw_torque_publisher = this->create_publisher("/raw_torques", 10); + smoothed_torque_publisher = this->create_publisher("/smoothed_torques", 10); /* // Register parameter callback for dynamic PID gain updates callback_handle_ = this->add_on_set_parameters_callback( @@ -1266,8 +1266,8 @@ void EddieRosInterface::publish_torque_debug_info( bool is_smoothing_active) { // Check if anyone is actually subscribed to the topics - if (raw_torque_publisher_->get_subscription_count() == 0 && - smoothed_torque_publisher_->get_subscription_count() == 0) { + if (raw_torque_publisher->get_subscription_count() == 0 && + smoothed_torque_publisher->get_subscription_count() == 0) { return; } @@ -1280,22 +1280,23 @@ void EddieRosInterface::publish_torque_debug_info( raw_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); raw_torque_msg.effort.push_back(raw_torques(i)); } - raw_torque_publisher_->publish(raw_torque_msg); + raw_torque_publisher->publish(raw_torque_msg); // Publish smoothed torques auto smoothed_torque_msg = sensor_msgs::msg::JointState(); smoothed_torque_msg.header.stamp = now; for (unsigned int i = 0; i < smoothed_torques.rows(); i++) { smoothed_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); - if (is_smoothing_active) { + /* if (is_smoothing_active) { // Filter is on: Send the actual smoothed value smoothed_torque_msg.effort.push_back(smoothed_torques(i)); } else { // Filter is off: Send NaN smoothed_torque_msg.effort.push_back(std::numeric_limits::quiet_NaN()); - } + } */ + smoothed_torque_msg.effort.push_back(smoothed_torques(i)); } - smoothed_torque_publisher_->publish(smoothed_torque_msg); + smoothed_torque_publisher->publish(smoothed_torque_msg); } void EddieRosInterface::publish_ee_errors(EddieState *eddie_state) { @@ -1401,44 +1402,48 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed // Apply the low-pass filter to smooth the torque commands // TODO: only do this when changing gains dynamically! /* for (unsigned int i = 0; i < num_jnts_rightarm; i++) { - smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); + smoothed_torques_right(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right(i); } - publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right_, "right"); + publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right, "right"); // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_rightarm; i++) { - saturate(&smoothed_torques_right_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right_(i); + saturate(&smoothed_torques_right(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right(i); } */ - bool apply_smoothing = right_arm_smoothing_active_.load(); + bool apply_smoothing = right_arm_smoothing_active.load(); if (apply_smoothing) { - auto elapsed = std::chrono::steady_clock::now() - right_arm_smoothing_start_time_; + auto elapsed = std::chrono::steady_clock::now() - right_arm_smoothing_start_time; if (elapsed < smoothing_duration) { // In the smoothing period, apply the low-pass filter for (unsigned int i = 0; i < num_jnts_rightarm; i++) { - smoothed_torques_right_(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right_(i); + smoothed_torques_right(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right(i); } } else { // Smoothing duration has passed, turn filter off. - right_arm_smoothing_active_.store(false); + right_arm_smoothing_active.store(false); apply_smoothing = false; // On the first step after disabling, snap the smoothed value to the raw value - smoothed_torques_right_ = tau_ctrl_rightarm; + smoothed_torques_right = tau_ctrl_rightarm; RCLCPP_INFO(this->get_logger(), "Right arm torque smoothing deactivated."); } } - publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right_, "right", apply_smoothing); - // Use the raw command if smoothing is not active, otherwise use the smoothed one. - const KDL::JntArray& final_torques = apply_smoothing ? smoothed_torques_right_ : tau_ctrl_rightarm; + if (!apply_smoothing) { + // If smoothing is off, ensure the smoothed state tracks the raw state. + smoothed_torques_right = tau_ctrl_rightarm; + } + + publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right, "right", apply_smoothing); // send the final torques to the robot for (int i = 0; i < num_jnts_rightarm; i++) { - double torque_to_send = final_torques(i); + double torque_to_send = smoothed_torques_right(i); saturate(&torque_to_send, -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - eddie_state->kinova_rightarm_state.eff_cmd[i] = torque_to_send; + this->eddie_state.kinova_rightarm_state.eff_cmd[i] = torque_to_send; } + /* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); @@ -1523,13 +1528,13 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } // Apply the low-pass filter to smooth the torque commands /* for (unsigned int i = 0; i < num_jnts_leftarm; i++) { - smoothed_torques_left_(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left_(i); + smoothed_torques_left(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left(i); } - publish_torque_debug_info(tau_ctrl_leftarm, smoothed_torques_left_, "left"); + publish_torque_debug_info(tau_ctrl_leftarm, smoothed_torques_left, "left"); // Send the smoothed torques to the robot for (int i = 0; i < num_jnts_leftarm; i++) { - saturate(&smoothed_torques_left_(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - this->eddie_state.kinova_leftarm_state.eff_cmd[i] = smoothed_torques_left_(i); + saturate(&smoothed_torques_left(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + this->eddie_state.kinova_leftarm_state.eff_cmd[i] = smoothed_torques_left(i); } */ for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); From 10e321ccf1b192a14d83ae0373c55644c9bc8f8b Mon Sep 17 00:00:00 2001 From: rinaalo Date: Fri, 12 Dec 2025 18:59:06 +0100 Subject: [PATCH 30/35] Add TorqueInterpolator for smooth torque transitions and update smoothing logic --- include/eddie_ros/interface.hpp | 66 +++++++- src/interface-parameters.cpp | 6 +- src/interface.cpp | 287 +++++++------------------------- src/pid.cpp | 5 + 4 files changed, 125 insertions(+), 239 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 6729cf4..8f02ca7 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -88,6 +89,51 @@ class PID { double decay_rate; }; +struct TorqueInterpolator { + // Manage a smooth transition for a torque value + bool is_active = false; + double start_value = 0.0; + double end_value = 0.0; + std::chrono::steady_clock::time_point start_time; + std::chrono::duration duration; + + double a0, a1, a2, a3; + + // Start a new trajectory + void start(double from, double to, double transition_duration_s) { + is_active = true; + start_value = from; + end_value = to; + duration = std::chrono::duration(transition_duration_s); + start_time = std::chrono::steady_clock::now(); + + // Calculate cubic polynomial coefficients for smooth start/end velocity + a0 = start_value; + a1 = 0; + a2 = 3 * (end_value - start_value) / (transition_duration_s * transition_duration_s); + a3 = -2 * (end_value - start_value) / (transition_duration_s * transition_duration_s * transition_duration_s); + } + + // Get the current value + double get_value() { + if (!is_active) { + return end_value; + } + + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - start_time; + double t = elapsed.count(); + + if (t >= duration.count()) { + is_active = false; + return end_value; + } + + // Evaluate the cubic polynomial T(t) + return a0 + a1 * t + a2 * t * t + a3 * t * t * t; + } +}; + struct EddieState { int num_drives; struct { @@ -224,9 +270,7 @@ class EddieRosInterface : public rclcpp::Node { void publish_torque_debug_info( const KDL::JntArray& raw_torques, const KDL::JntArray& smoothed_torques, - const std::string& arm_side, - bool is_smoothing_active - ); + const std::string& arm_side); public: void run_fsm(); @@ -289,12 +333,18 @@ class EddieRosInterface : public rclcpp::Node { rclcpp::Publisher::SharedPtr raw_torque_publisher; rclcpp::Publisher::SharedPtr smoothed_torque_publisher; - // State for conditional smoothing - std::atomic right_arm_smoothing_active{false}; - std::atomic left_arm_smoothing_active{false}; - std::chrono::steady_clock::time_point right_arm_smoothing_start_time; - std::chrono::steady_clock::time_point left_arm_smoothing_start_time; + // The torque values from the previous control cycle + KDL::JntArray last_sent_torques_right; + KDL::JntArray last_sent_torques_left; + + // Interpolators (one for each joint) + std::vector right_arm_torque_interpolators; + std::vector left_arm_torque_interpolators; + // State for conditional smoothing + std::atomic right_arm_smoothing_start =false; + std::atomic left_arm_smoothing_start = false; + // Flags to track if arms are currently executing goals std::atomic rightarm_goal_executing = false; std::atomic leftarm_goal_executing = false; diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index 0798a05..e58cfa3 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -182,13 +182,11 @@ rcl_interfaces::msg::SetParametersResult EddieRosInterface::parameters_callback( // Activate smoothing based on which arm's gain was changed if (name.rfind("pid.right", 0) == 0) { RCLCPP_INFO(this->get_logger(), "Right arm PID parameter changed. Activating torque smoothing."); - this->right_arm_smoothing_start_time = std::chrono::steady_clock::now(); - this->right_arm_smoothing_active.store(true); + this->right_arm_smoothing_start.store(true); } else if (name.rfind("pid.left", 0) == 0) { RCLCPP_INFO(this->get_logger(), "Left arm PID parameter changed. Activating torque smoothing."); - this->left_arm_smoothing_start_time = std::chrono::steady_clock::now(); - this->left_arm_smoothing_active.store(true); + this->left_arm_smoothing_start.store(true); } } else if (name == "torque_smoothing_alpha") { diff --git a/src/interface.cpp b/src/interface.cpp index 439955e..b298ada 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -506,13 +506,26 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Smoothed torque commands if (should_control_right_arm()) { - smoothed_torques_right.resize(rightarm_chain.getNrOfJoints()); - smoothed_torques_right.data.setZero(); + last_sent_torques_right.resize(rightarm_chain.getNrOfJoints()); + last_sent_torques_right.data.setZero(); + right_arm_torque_interpolators.resize(rightarm_chain.getNrOfJoints()); } if (should_control_left_arm()) { - smoothed_torques_left.resize(leftarm_chain.getNrOfJoints()); - smoothed_torques_left.data.setZero(); - } + last_sent_torques_left.resize(leftarm_chain.getNrOfJoints()); + last_sent_torques_left.data.setZero(); + left_arm_torque_interpolators.resize(leftarm_chain.getNrOfJoints()); + } + + // Log into a CSV file + torque_log_file.open("torque_log.csv"); + torque_log_file << "timestamp," + << "raw_torque_j0,smoothed_torque_j0," + << "raw_torque_j1,smoothed_torque_j1," + << "raw_torque_j2,smoothed_torque_j2," + << "raw_torque_j3,smoothed_torque_j3," + << "raw_torque_j4,smoothed_torque_j4," + << "raw_torque_j5,smoothed_torque_j5," + << "raw_torque_j6,smoothed_torque_j6\n"; // joint inertias: const std::vector joint_inertia{0.5580, 0.5580, 0.5580, 0.5580, 0.1389, 0.1389, 0.1389}; @@ -549,133 +562,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) rne_id_solver_rightarm = std::make_unique(rightarm_chain, root_acc_rightarm.vel); - //RCLCPP_INFO(this->get_logger(), "Declaring PID parameters..."); - -/* const double default_pos_deadband = 0.005; - const double default_rot_deadband = 0.005; - - this->declare_parameter("torque_smoothing_alpha", 0.05); */ - - /* // - Declare parameters for the RIGHT arm - // Position - this->declare_parameter("pid.right.pos.x.p", 80.0); - this->declare_parameter("pid.right.pos.x.i", 20.0); - this->declare_parameter("pid.right.pos.x.d", 10.0); - this->declare_parameter("pid.right.pos.y.p", 150.0); - this->declare_parameter("pid.right.pos.y.i", 20.0); - this->declare_parameter("pid.right.pos.y.d", 10.0); - this->declare_parameter("pid.right.pos.z.p", 150.0); - this->declare_parameter("pid.right.pos.z.i", 20.0); - this->declare_parameter("pid.right.pos.z.d", 10.0); - this->declare_parameter("pid.right.pos.deadband", default_pos_deadband); - - // Rotation - this->declare_parameter("pid.right.rot.x.p", 5.0); - this->declare_parameter("pid.right.rot.x.i", 0.0); - this->declare_parameter("pid.right.rot.x.d", 2.0); - this->declare_parameter("pid.right.rot.y.p", 5.0); - this->declare_parameter("pid.right.rot.y.i", 0.0); - this->declare_parameter("pid.right.rot.y.d", 2.0); - this->declare_parameter("pid.right.rot.z.p", 5.0); - this->declare_parameter("pid.right.rot.z.i", 0.0); - this->declare_parameter("pid.right.rot.z.d", 2.0); - this->declare_parameter("pid.right.rot.deadband", default_rot_deadband); - - // - Declare parameters for the LEFT arm - // Position - this->declare_parameter("pid.left.pos.x.p", 70.0); - this->declare_parameter("pid.left.pos.x.i", 0.0); - this->declare_parameter("pid.left.pos.x.d", 4.0); - this->declare_parameter("pid.left.pos.y.p", 70.0); - this->declare_parameter("pid.left.pos.y.i", 0.0); - this->declare_parameter("pid.left.pos.y.d", 4.0); - this->declare_parameter("pid.left.pos.z.p", 150.0); - this->declare_parameter("pid.left.pos.z.i", 8.0); - this->declare_parameter("pid.left.pos.z.d", 10.0); - this->declare_parameter("pid.left.pos.deadband", default_pos_deadband); - - // Rotation - this->declare_parameter("pid.left.rot.x.p", 5.0); - this->declare_parameter("pid.left.rot.x.i", 0.0); - this->declare_parameter("pid.left.rot.x.d", 2.0); - this->declare_parameter("pid.left.rot.y.p", 5.0); - this->declare_parameter("pid.left.rot.y.i", 0.0); - this->declare_parameter("pid.left.rot.y.d", 2.0); - this->declare_parameter("pid.left.rot.z.p", 5.0); - this->declare_parameter("pid.left.rot.z.i", 0.0); - this->declare_parameter("pid.left.rot.z.d", 2.0); - this->declare_parameter("pid.left.rot.deadband", default_rot_deadband); */ - - // - Get the Parameter Values into local variables - - // Get values for the RIGHT arm - /* double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); - double r_pos_x_i = this->get_parameter("pid.right.pos.x.i").as_double(); - double r_pos_x_d = this->get_parameter("pid.right.pos.x.d").as_double(); - double r_pos_y_p = this->get_parameter("pid.right.pos.y.p").as_double(); - double r_pos_y_i = this->get_parameter("pid.right.pos.y.i").as_double(); - double r_pos_y_d = this->get_parameter("pid.right.pos.y.d").as_double(); - double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); - double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); - double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); - - double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); - double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); - double r_rot_x_d = this->get_parameter("pid.right.rot.x.d").as_double(); - double r_rot_y_p = this->get_parameter("pid.right.rot.y.p").as_double(); - double r_rot_y_i = this->get_parameter("pid.right.rot.y.i").as_double(); - double r_rot_y_d = this->get_parameter("pid.right.rot.y.d").as_double(); - double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); - double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); - double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); - - // Get values for the LEFT arm - double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); - double l_pos_x_i = this->get_parameter("pid.left.pos.x.i").as_double(); - double l_pos_x_d = this->get_parameter("pid.left.pos.x.d").as_double(); - double l_pos_y_p = this->get_parameter("pid.left.pos.y.p").as_double(); - double l_pos_y_i = this->get_parameter("pid.left.pos.y.i").as_double(); - double l_pos_y_d = this->get_parameter("pid.left.pos.y.d").as_double(); - double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); - double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); - double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); - - double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); - double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); - double l_rot_x_d = this->get_parameter("pid.left.rot.x.d").as_double(); - double l_rot_y_p = this->get_parameter("pid.left.rot.y.p").as_double(); - double l_rot_y_i = this->get_parameter("pid.left.rot.y.i").as_double(); - double l_rot_y_d = this->get_parameter("pid.left.rot.y.d").as_double(); - double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); - double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); - double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); - - // - Set PID Gains Using the Granular Values - const double error_sum_tol = 0.9; - const double decay_rate = 0.0; - - // Set PID controller gains for the RIGHT arm - pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate); - pid_rightarm_ee_pos_y.set_gains(r_pos_y_p, r_pos_y_i, r_pos_y_d, error_sum_tol, decay_rate); - pid_rightarm_ee_pos_z.set_gains(r_pos_z_p, r_pos_z_i, r_pos_z_d, error_sum_tol, decay_rate); - - pid_rightarm_ee_rot_x.set_gains(r_rot_x_p, r_rot_x_i, r_rot_x_d, error_sum_tol, decay_rate); - pid_rightarm_ee_rot_y.set_gains(r_rot_y_p, r_rot_y_i, r_rot_y_d, error_sum_tol, decay_rate); - pid_rightarm_ee_rot_z.set_gains(r_rot_z_p, r_rot_z_i, r_rot_z_d, error_sum_tol, decay_rate); - - RCLCPP_INFO(this->get_logger(), "Right Arm PID gains loaded from parameters."); - - // Set PID controller gains for the LEFT arm - pid_leftarm_ee_pos_x.set_gains(l_pos_x_p, l_pos_x_i, l_pos_x_d, error_sum_tol, decay_rate); - pid_leftarm_ee_pos_y.set_gains(l_pos_y_p, l_pos_y_i, l_pos_y_d, error_sum_tol, decay_rate); - pid_leftarm_ee_pos_z.set_gains(l_pos_z_p, l_pos_z_i, l_pos_z_d, error_sum_tol, decay_rate); - - pid_leftarm_ee_rot_x.set_gains(l_rot_x_p, l_rot_x_i, l_rot_x_d, error_sum_tol, decay_rate); - pid_leftarm_ee_rot_y.set_gains(l_rot_y_p, l_rot_y_i, l_rot_y_d, error_sum_tol, decay_rate); - pid_leftarm_ee_rot_z.set_gains(l_rot_z_p, l_rot_z_i, l_rot_z_d, error_sum_tol, decay_rate); - - RCLCPP_INFO(this->get_logger(), "Left Arm PID gains loaded from parameters."); */ - RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); // Error publishers @@ -699,55 +585,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) // Torque publishers raw_torque_publisher = this->create_publisher("/raw_torques", 10); smoothed_torque_publisher = this->create_publisher("/smoothed_torques", 10); - -/* // Register parameter callback for dynamic PID gain updates - callback_handle_ = this->add_on_set_parameters_callback( - [this](const std::vector ¶ms) { - for (const auto ¶m : params) { - // Right arm PID gains - if (param.get_name() == "pid_rightarm_ee_pos_x_p") pid_rightarm_ee_pos_x.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_x_i") pid_rightarm_ee_pos_x.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_x_d") pid_rightarm_ee_pos_x.kd = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_y_p") pid_rightarm_ee_pos_y.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_y_i") pid_rightarm_ee_pos_y.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_y_d") pid_rightarm_ee_pos_y.kd = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_z_p") pid_rightarm_ee_pos_z.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_z_i") pid_rightarm_ee_pos_z.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_pos_z_d") pid_rightarm_ee_pos_z.kd = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_x_p") pid_rightarm_ee_rot_x.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_x_i") pid_rightarm_ee_rot_x.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_x_d") pid_rightarm_ee_rot_x.kd = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_y_p") pid_rightarm_ee_rot_y.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_y_i") pid_rightarm_ee_rot_y.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_y_d") pid_rightarm_ee_rot_y.kd = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_z_p") pid_rightarm_ee_rot_z.kp = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_z_i") pid_rightarm_ee_rot_z.ki = param.as_double(); - if (param.get_name() == "pid_rightarm_ee_rot_z_d") pid_rightarm_ee_rot_z.kd = param.as_double(); - // Left arm PID gains - if (param.get_name() == "pid_leftarm_ee_pos_x_p") pid_leftarm_ee_pos_x.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_x_i") pid_leftarm_ee_pos_x.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_x_d") pid_leftarm_ee_pos_x.kd = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_y_p") pid_leftarm_ee_pos_y.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_y_i") pid_leftarm_ee_pos_y.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_y_d") pid_leftarm_ee_pos_y.kd = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_z_p") pid_leftarm_ee_pos_z.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_z_i") pid_leftarm_ee_pos_z.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_pos_z_d") pid_leftarm_ee_pos_z.kd = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_x_p") pid_leftarm_ee_rot_x.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_x_i") pid_leftarm_ee_rot_x.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_x_d") pid_leftarm_ee_rot_x.kd = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_y_p") pid_leftarm_ee_rot_y.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_y_i") pid_leftarm_ee_rot_y.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_y_d") pid_leftarm_ee_rot_y.kd = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_z_p") pid_leftarm_ee_rot_z.kp = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_z_i") pid_leftarm_ee_rot_z.ki = param.as_double(); - if (param.get_name() == "pid_leftarm_ee_rot_z_d") pid_leftarm_ee_rot_z.kd = param.as_double(); - } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - return result; - } - ); */ } void EddieRosInterface::initialize_action_servers() { @@ -887,6 +724,9 @@ EddieRosInterface::~EddieRosInterface() { robif2b_kinova_gen3_stop(&kinova_leftarm); robif2b_kinova_gen3_shutdown(&kinova_leftarm); } + if (torque_log_file.is_open()) { + torque_log_file.close(); + } } void EddieRosInterface::configure(events *eventData, EddieState *eddie_state) { @@ -1262,8 +1102,7 @@ void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddi void EddieRosInterface::publish_torque_debug_info( const KDL::JntArray& raw_torques, const KDL::JntArray& smoothed_torques, - const std::string& arm_side, - bool is_smoothing_active) + const std::string& arm_side) { // Check if anyone is actually subscribed to the topics if (raw_torque_publisher->get_subscription_count() == 0 && @@ -1287,13 +1126,6 @@ void EddieRosInterface::publish_torque_debug_info( smoothed_torque_msg.header.stamp = now; for (unsigned int i = 0; i < smoothed_torques.rows(); i++) { smoothed_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); - /* if (is_smoothing_active) { - // Filter is on: Send the actual smoothed value - smoothed_torque_msg.effort.push_back(smoothed_torques(i)); - } else { - // Filter is off: Send NaN - smoothed_torque_msg.effort.push_back(std::numeric_limits::quiet_NaN()); - } */ smoothed_torque_msg.effort.push_back(smoothed_torques(i)); } smoothed_torque_publisher->publish(smoothed_torque_msg); @@ -1346,9 +1178,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double pos_deadband_left = this->get_parameter("pid.left.pos.deadband").as_double(); double rot_deadband_left = this->get_parameter("pid.left.rot.deadband").as_double(); - // Get smoothing factor value from parameters - const double alpha = this->get_parameter("torque_smoothing_alpha").as_double(); - const std::chrono::milliseconds smoothing_duration(4000); + const double transition_duration = 0.05; if (should_control_right_arm()) { // Calculate the raw cartesian error @@ -1399,50 +1229,53 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (r_right < 0) { RCLCPP_ERROR(get_logger(), "Right arm RNE ID solver failed with error code: %d", r_right); } - // Apply the low-pass filter to smooth the torque commands - // TODO: only do this when changing gains dynamically! -/* for (unsigned int i = 0; i < num_jnts_rightarm; i++) { - smoothed_torques_right(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right(i); + + if (right_arm_smoothing_start.load()) { + RCLCPP_INFO(this->get_logger(), "Right arm gains changed. Starting torque interpolation."); + + // Start an interpolator for each joint. + for (unsigned int i = 0; i < num_jnts_rightarm; i++) { + right_arm_torque_interpolators[i].start( + last_sent_torques_right(i), + tau_ctrl_rightarm(i), + transition_duration + ); + } + right_arm_smoothing_start.store(false); // Reset the flag } - publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right, "right"); - // Send the smoothed torques to the robot - for (int i = 0; i < num_jnts_rightarm; i++) { - saturate(&smoothed_torques_right(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - this->eddie_state.kinova_rightarm_state.eff_cmd[i] = smoothed_torques_right(i); - } */ - bool apply_smoothing = right_arm_smoothing_active.load(); - - if (apply_smoothing) { - auto elapsed = std::chrono::steady_clock::now() - right_arm_smoothing_start_time; - if (elapsed < smoothing_duration) { - // In the smoothing period, apply the low-pass filter - for (unsigned int i = 0; i < num_jnts_rightarm; i++) { - smoothed_torques_right(i) = alpha * tau_ctrl_rightarm(i) + (1.0 - alpha) * smoothed_torques_right(i); - } + + // Determine the final torque to send + KDL::JntArray final_torques(num_jnts_rightarm); + for (unsigned int i = 0; i < num_jnts_rightarm; i++) { + if (right_arm_torque_interpolators[i].is_active) { + // If we are interpolating, get the value from the trajectory. + final_torques(i) = right_arm_torque_interpolators[i].get_value(); } else { - // Smoothing duration has passed, turn filter off. - right_arm_smoothing_active.store(false); - apply_smoothing = false; - // On the first step after disabling, snap the smoothed value to the raw value - smoothed_torques_right = tau_ctrl_rightarm; - RCLCPP_INFO(this->get_logger(), "Right arm torque smoothing deactivated."); + // Otherwise, use the raw torque for this cycle. + final_torques(i) = tau_ctrl_rightarm(i); } } - - if (!apply_smoothing) { - // If smoothing is off, ensure the smoothed state tracks the raw state. - smoothed_torques_right = tau_ctrl_rightarm; - } - - publish_torque_debug_info(tau_ctrl_rightarm, smoothed_torques_right, "right", apply_smoothing); - // send the final torques to the robot + // Send torques to robot and store for next cycle for (int i = 0; i < num_jnts_rightarm; i++) { - double torque_to_send = smoothed_torques_right(i); + double torque_to_send = final_torques(i); saturate(&torque_to_send, -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); this->eddie_state.kinova_rightarm_state.eff_cmd[i] = torque_to_send; } + + // Store the final sent torque for the next cycle. + last_sent_torques_right = final_torques; + + if (torque_log_file.is_open()) { + torque_log_file << this->get_clock()->now().nanoseconds(); + for (unsigned int i = 0; i < num_jnts_rightarm; ++i) { + torque_log_file << "," << tau_ctrl_rightarm(i) + << "," << final_torques(i); + } + torque_log_file << "\n"; + } + publish_torque_debug_info(tau_ctrl_rightarm, final_torques, "right"); /* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); diff --git a/src/pid.cpp b/src/pid.cpp index 68a9368..2ddde70 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -45,3 +45,8 @@ double PID::control(double error, double dt) { return kp * error + ki * err_integ + kd * err_diff; } + +// clamp integral term to prevent windup - max and min value it can have (based on these plots below) +// plot P term, I term, D term separately for debugging +// p + i + d +// feedback for pos, velocity, acceleration(?), effort(?) separately \ No newline at end of file From 14a73daca38f4c92d42ae72e7bf41e7bfce91f58 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Sun, 14 Dec 2025 22:15:41 +0100 Subject: [PATCH 31/35] Refactor PID control to return structured output and add publishers for PID components --- include/eddie_ros/interface.hpp | 16 +++- src/interface-test.cpp | 24 +++--- src/interface.cpp | 132 +++++++++++++++++++++----------- src/pid.cpp | 12 ++- 4 files changed, 125 insertions(+), 59 deletions(-) diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 8f02ca7..7326e11 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -16,8 +16,10 @@ #include #include #include +#include #include +#include "geometry_msgs/msg/twist_stamped.hpp" #include #include #include @@ -60,6 +62,11 @@ double evaluate_greater_than_constraint(double quantity, double threshold); double evaluate_bilateral_constraint(double quantity, double lower, double upper); void saturate(double *value, double min, double max); +struct PIDOutput { + double p = 0.0, i = 0.0, d = 0.0; + double total = 0.0; +}; + class PID { public: PID() = default; @@ -77,7 +84,7 @@ class PID { double decay_rate = 0.0 ); - double control(double error, double dt = 1.0); + PIDOutput control(double error, double dt = 1.0); public: double err_integ; @@ -272,6 +279,10 @@ class EddieRosInterface : public rclcpp::Node { const KDL::JntArray& smoothed_torques, const std::string& arm_side); + void publish_pid_components( + const std::string& arm_side, + const std::array& outputs); + public: void run_fsm(); @@ -337,6 +348,9 @@ class EddieRosInterface : public rclcpp::Node { KDL::JntArray last_sent_torques_right; KDL::JntArray last_sent_torques_left; + // PID debug publishers + std::map::SharedPtr> pid_component_publishers; + // Interpolators (one for each joint) std::vector right_arm_torque_interpolators; std::vector left_arm_torque_interpolators; diff --git a/src/interface-test.cpp b/src/interface-test.cpp index f598199..da7be52 100644 --- a/src/interface-test.cpp +++ b/src/interface-test.cpp @@ -929,12 +929,12 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (should_control_right_arm()) { KDL::Twist delta_pose_rightarm_ee = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - double fx = pid_rightarm_ee_pos_x.control(delta_pose_rightarm_ee.vel.x(), cycle_time); - double fy = pid_rightarm_ee_pos_y.control(delta_pose_rightarm_ee.vel.y(), cycle_time); - double fz = pid_rightarm_ee_pos_z.control(delta_pose_rightarm_ee.vel.z(), cycle_time); - double mx = pid_rightarm_ee_rot_x.control(delta_pose_rightarm_ee.rot.x(), cycle_time); - double my = pid_rightarm_ee_rot_y.control(delta_pose_rightarm_ee.rot.y(), cycle_time); - double mz = pid_rightarm_ee_rot_z.control(delta_pose_rightarm_ee.rot.z(), cycle_time); + double fx = pid_rightarm_ee_pos_x.control(delta_pose_rightarm_ee.vel.x(), cycle_time).total; + double fy = pid_rightarm_ee_pos_y.control(delta_pose_rightarm_ee.vel.y(), cycle_time).total; + double fz = pid_rightarm_ee_pos_z.control(delta_pose_rightarm_ee.vel.z(), cycle_time).total; + double mx = pid_rightarm_ee_rot_x.control(delta_pose_rightarm_ee.rot.x(), cycle_time).total; + double my = pid_rightarm_ee_rot_y.control(delta_pose_rightarm_ee.rot.y(), cycle_time).total; + double mz = pid_rightarm_ee_rot_z.control(delta_pose_rightarm_ee.rot.z(), cycle_time).total; KDL::Wrench f_ext_ee_rightarm = KDL::Wrench(KDL::Vector(fx, fy, fz), KDL::Vector(mx, my, mz)); @@ -974,12 +974,12 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (should_control_left_arm()) { KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - double fx = pid_leftarm_ee_pos_x.control(delta_pose_leftarm_ee.vel.x(), cycle_time); - double fy = pid_leftarm_ee_pos_y.control(delta_pose_leftarm_ee.vel.y(), cycle_time); - double fz = pid_leftarm_ee_pos_z.control(delta_pose_leftarm_ee.vel.z(), cycle_time); - double mx = pid_leftarm_ee_rot_x.control(delta_pose_leftarm_ee.rot.x(), cycle_time); - double my = pid_leftarm_ee_rot_y.control(delta_pose_leftarm_ee.rot.y(), cycle_time); - double mz = pid_leftarm_ee_rot_z.control(delta_pose_leftarm_ee.rot.z(), cycle_time); + double fx = pid_leftarm_ee_pos_x.control(delta_pose_leftarm_ee.vel.x(), cycle_time).total; + double fy = pid_leftarm_ee_pos_y.control(delta_pose_leftarm_ee.vel.y(), cycle_time).total; + double fz = pid_leftarm_ee_pos_z.control(delta_pose_leftarm_ee.vel.z(), cycle_time).total; + double mx = pid_leftarm_ee_rot_x.control(delta_pose_leftarm_ee.rot.x(), cycle_time).total; + double my = pid_leftarm_ee_rot_y.control(delta_pose_leftarm_ee.rot.y(), cycle_time).total; + double mz = pid_leftarm_ee_rot_z.control(delta_pose_leftarm_ee.rot.z(), cycle_time).total; KDL::Wrench f_ext_ee_leftarm = KDL::Wrench(KDL::Vector(fx, fy, fz), KDL::Vector(mx, my, mz)); diff --git a/src/interface.cpp b/src/interface.cpp index b298ada..8d390b7 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -582,6 +582,20 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) } ); + // PID control publishers + if (should_control_right_arm()) { + for (const auto& axis : {"pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z"}) { + std::string topic_name = "right_arm/pid_components/" + std::string(axis); + pid_component_publishers[topic_name] = this->create_publisher(topic_name, 10); + } + } + if (should_control_left_arm()) { + for (const auto& axis : {"pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z"}) { + std::string topic_name = "left_arm/pid_components/" + std::string(axis); + pid_component_publishers[topic_name] = this->create_publisher(topic_name, 10); + } + } + // Torque publishers raw_torque_publisher = this->create_publisher("/raw_torques", 10); smoothed_torque_publisher = this->create_publisher("/smoothed_torques", 10); @@ -1131,6 +1145,37 @@ void EddieRosInterface::publish_torque_debug_info( smoothed_torque_publisher->publish(smoothed_torque_msg); } +void EddieRosInterface::publish_pid_components( + const std::string& arm_side, + const std::array& outputs) +{ + // map an index to an axis name + const std::array axis_names = {"pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z"}; + + auto now = this->get_clock()->now(); + + for (int i = 0; i < 6; ++i) { + const std::string& axis_name = axis_names[i]; + const PIDOutput& output = outputs[i]; + std::string topic_name = arm_side + "_arm/pid_components/" + axis_name; + + // Check if the publisher exists and has subscribers + if (pid_component_publishers.count(topic_name) && + pid_component_publishers[topic_name]->get_subscription_count() > 0) + { + auto msg = geometry_msgs::msg::TwistStamped(); + msg.header.stamp = now; + + msg.twist.linear.x = output.p; // P term + msg.twist.linear.y = output.i; // I term + msg.twist.linear.z = output.d; // D term + msg.twist.angular.x = output.total; // Total + + pid_component_publishers[topic_name]->publish(msg); + } + } +} + void EddieRosInterface::publish_ee_errors(EddieState *eddie_state) { if (should_control_right_arm()) { @@ -1188,18 +1233,35 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double error_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.x(), -pos_deadband_right, pos_deadband_right); double error_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.y(), -pos_deadband_right, pos_deadband_right); double error_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.vel.z(), -pos_deadband_right, pos_deadband_right); - double error_rot_x = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.x(), -rot_deadband_right, rot_deadband_right); double error_rot_y = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.y(), -rot_deadband_right, rot_deadband_right); double error_rot_z = evaluate_bilateral_constraint(delta_pose_rightarm_ee.rot.z(), -rot_deadband_right, rot_deadband_right); - // Pass the processed error to the PID controllers - double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); + // Compute PID outputs for each axis + std::array right_outputs; + right_outputs[0] = pid_rightarm_ee_pos_x.control(error_x, cycle_time); + right_outputs[1] = pid_rightarm_ee_pos_y.control(error_y, cycle_time); + right_outputs[2] = pid_rightarm_ee_pos_z.control(error_z, cycle_time); + right_outputs[3] = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); + right_outputs[4] = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); + right_outputs[5] = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); + + // Extract the final force command from PID outputs + double fx_right = right_outputs[0].total; + double fy_right = right_outputs[1].total; + double fz_right = right_outputs[2].total; + double mx_right = right_outputs[3].total; + double my_right = right_outputs[4].total; + double mz_right = right_outputs[5].total; + + publish_pid_components("right", right_outputs); + +/* double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); double mx_right = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); double my_right = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); - double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); + double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); */ KDL::Wrench f_ext_ee_rightarm = KDL::Wrench(KDL::Vector(fx_right, fy_right, fz_right), KDL::Vector(mx_right, my_right, mz_right)); KDL::Wrench f_ext_ee_rightarm_wrt_ee = KDL::Wrench( @@ -1275,7 +1337,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed torque_log_file << "\n"; } - publish_torque_debug_info(tau_ctrl_rightarm, final_torques, "right"); + //publish_torque_debug_info(tau_ctrl_rightarm, final_torques, "right"); /* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); @@ -1283,53 +1345,35 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } */ } if (should_control_left_arm()) { + // Calculate the raw cartesian error KDL::Twist delta_pose_leftarm_ee = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); - double original_p_pos_x = this->get_parameter("pid.left.pos.x.p").as_double(); - double original_p_pos_y = this->get_parameter("pid.left.pos.y.p").as_double(); - double original_p_pos_z = this->get_parameter("pid.left.pos.z.p").as_double(); - - double original_p_rot_x = this->get_parameter("pid.left.rot.x.p").as_double(); - double original_p_rot_y = this->get_parameter("pid.left.rot.y.p").as_double(); - double original_p_rot_z = this->get_parameter("pid.left.rot.z.p").as_double(); - - const double low_gain_factor = 0.1; // Use 10% of the P-gain inside the deadband - + // Apply bilateral constraint to each error component double error_x = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.x(), -pos_deadband_left, pos_deadband_left); double error_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.y(), -pos_deadband_left, pos_deadband_left); double error_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.vel.z(), -pos_deadband_left, pos_deadband_left); - double error_rot_x = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.x(), -rot_deadband_left, rot_deadband_left); double error_rot_y = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.y(), -rot_deadband_left, rot_deadband_left); double error_rot_z = evaluate_bilateral_constraint(delta_pose_leftarm_ee.rot.z(), -rot_deadband_left, rot_deadband_left); - - // Pass the processed error to the PID controllers - // Dynamically adjust the P-gain for the controller based on deadband - if (error_x == 0.0) { - pid_leftarm_ee_pos_x.kp = original_p_pos_x * low_gain_factor; // Set to low gain - } - double fx_left = pid_leftarm_ee_pos_x.control(error_x, cycle_time); - pid_leftarm_ee_pos_x.kp = original_p_pos_x; // Restore to high gain for next cycle - - if (error_y == 0.0) { - pid_leftarm_ee_pos_y.kp = original_p_pos_y * low_gain_factor; - } - double fy_left = pid_leftarm_ee_pos_y.control(error_y, cycle_time); - pid_leftarm_ee_pos_y.kp = original_p_pos_y; - - if (error_z == 0.0) { - pid_leftarm_ee_pos_z.kp = original_p_pos_z * low_gain_factor; - } - double fz_left = pid_leftarm_ee_pos_z.control(error_z, cycle_time); - pid_leftarm_ee_pos_z.kp = original_p_pos_z; - - - // double fx_left = pid_leftarm_ee_pos_x.control(error_x, cycle_time); - // double fy_left = pid_leftarm_ee_pos_y.control(error_y, cycle_time); - // double fz_left = pid_leftarm_ee_pos_z.control(error_z, cycle_time); - double mx_left = pid_leftarm_ee_rot_x.control(error_rot_x, cycle_time); - double my_left = pid_leftarm_ee_rot_y.control(error_rot_y, cycle_time); - double mz_left = pid_leftarm_ee_rot_z.control(error_rot_z, cycle_time); + + // Compute PID outputs for each axis + std::array left_outputs; + left_outputs[0] = pid_leftarm_ee_pos_x.control(error_x, cycle_time); + left_outputs[1] = pid_leftarm_ee_pos_y.control(error_y, cycle_time); + left_outputs[2] = pid_leftarm_ee_pos_z.control(error_z, cycle_time); + left_outputs[3] = pid_leftarm_ee_rot_x.control(error_rot_x, cycle_time); + left_outputs[4] = pid_leftarm_ee_rot_y.control(error_rot_y, cycle_time); + left_outputs[5] = pid_leftarm_ee_rot_z.control(error_rot_z, cycle_time); + + // Extract the final force command from PID outputs + double fx_left = left_outputs[0].total; + double fy_left = left_outputs[1].total; + double fz_left = left_outputs[2].total; + double mx_left = left_outputs[3].total; + double my_left = left_outputs[4].total; + double mz_left = left_outputs[5].total; + + publish_pid_components("left", left_outputs); KDL::Wrench f_ext_ee_leftarm = KDL::Wrench(KDL::Vector(fx_left, fy_left, fz_left), KDL::Vector(mx_left, my_left, mz_left)); KDL::Wrench f_ext_ee_leftarm_wrt_ee = KDL::Wrench( diff --git a/src/pid.cpp b/src/pid.cpp index 2ddde70..4aa6993 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -21,7 +21,7 @@ void PID::set_gains( this->decay_rate = decay_rate; } -double PID::control(double error, double dt) { +PIDOutput PID::control(double error, double dt) { double err_diff = (error - err_last) / dt; @@ -42,8 +42,16 @@ double PID::control(double error, double dt) { // err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; err_last = error; + + //return kp * error + ki * err_integ + kd * err_diff; - return kp * error + ki * err_integ + kd * err_diff; + PIDOutput output; + output.p = kp * error; + output.i = ki * err_integ; + output.d = kd * err_diff; + output.total = output.p + output.i + output.d; + + return output; } // clamp integral term to prevent windup - max and min value it can have (based on these plots below) From 95a5c6491adc246cfe0ef7f87270d685fd207c04 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Tue, 16 Dec 2025 15:02:51 +0100 Subject: [PATCH 32/35] Add a low pass filter function (not being used right now) Could be used for filtering sensor data, but might cause some delays, so decided not to use it right now. Also removed torque publishers since we replaced them with logs. --- README.md | 2 +- include/eddie_ros/interface.hpp | 16 ++----- src/interface-parameters.cpp | 12 +++--- src/interface.cpp | 75 +++++++++++++-------------------- 4 files changed, 40 insertions(+), 65 deletions(-) diff --git a/README.md b/README.md index f5342c4..a04ba22 100644 --- a/README.md +++ b/README.md @@ -85,7 +85,7 @@ run rviz: ros2 launch eddie_ros rviz.launch.py use_sim:=true ``` -## Adjusting PID gains on run-time +## Adjusting PID gains on run-time (only for testing!) First, make sure to run the `eddie_ros_interface` node as described above. diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 7326e11..be04414 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -61,6 +61,7 @@ double evaluate_less_than_constraint(double quantity, double threshold); double evaluate_greater_than_constraint(double quantity, double threshold); double evaluate_bilateral_constraint(double quantity, double lower, double upper); void saturate(double *value, double min, double max); +double low_pass_filter(double raw_value, double previous_filtered_value, double alpha); struct PIDOutput { double p = 0.0, i = 0.0, d = 0.0; @@ -273,15 +274,7 @@ class EddieRosInterface : public rclcpp::Node { void compute_cartesian_ctrl(events *eventData, EddieState *eddie_state); void publish_ee_errors(EddieState *eddie_state); void publish_joint_states(EddieState *eddie_state); - - void publish_torque_debug_info( - const KDL::JntArray& raw_torques, - const KDL::JntArray& smoothed_torques, - const std::string& arm_side); - - void publish_pid_components( - const std::string& arm_side, - const std::array& outputs); + void publish_pid_components(const std::string& arm_side, const std::array& outputs); public: void run_fsm(); @@ -340,9 +333,8 @@ class EddieRosInterface : public rclcpp::Node { rclcpp::Publisher::SharedPtr left_arm_ee_error_pub; rclcpp::TimerBase::SharedPtr ee_error_timer_; - // Torque command publishers - rclcpp::Publisher::SharedPtr raw_torque_publisher; - rclcpp::Publisher::SharedPtr smoothed_torque_publisher; + KDL::JntArray filtered_q_rightarm; + KDL::JntArray filtered_qd_rightarm; // The torque values from the previous control cycle KDL::JntArray last_sent_torques_right; diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index e58cfa3..98784ac 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -39,16 +39,16 @@ void EddieRosInterface::declare_pid_gains() { // Torque smoothing parameter this->declare_parameter("torque_smoothing_alpha", 0.1); - const double default_pos_deadband = 0.005; + const double default_pos_deadband = 0.002; const double default_rot_deadband = 0.02; // Declare parameters for the RIGHT arm - this->declare_parameter("pid.right.pos.x.p", 80.0); + this->declare_parameter("pid.right.pos.x.p", 120.0); this->declare_parameter("pid.right.pos.x.i", 20.0); - this->declare_parameter("pid.right.pos.x.d", 10.0); - this->declare_parameter("pid.right.pos.y.p", 150.0); + this->declare_parameter("pid.right.pos.x.d", 2.0); + this->declare_parameter("pid.right.pos.y.p", 120.0); this->declare_parameter("pid.right.pos.y.i", 20.0); - this->declare_parameter("pid.right.pos.y.d", 10.0); + this->declare_parameter("pid.right.pos.y.d", 2.0); this->declare_parameter("pid.right.pos.z.p", 150.0); this->declare_parameter("pid.right.pos.z.i", 20.0); this->declare_parameter("pid.right.pos.z.d", 10.0); @@ -94,7 +94,7 @@ void EddieRosInterface::reload_pid_gains() RCLCPP_INFO(this->get_logger(), "Reloading all PID gains from parameter server..."); const double error_sum_tol = 0.9; - const double decay_rate = 0.0; + const double decay_rate = 0.5; // Get values for the RIGHT arm double r_pos_x_p = this->get_parameter("pid.right.pos.x.p").as_double(); diff --git a/src/interface.cpp b/src/interface.cpp index 8d390b7..99df4a8 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -58,6 +58,10 @@ void saturate(double *value, double min, double max) { } } +double low_pass_filter(double raw_value, double previous_filtered_value, double alpha) { + return alpha * raw_value + (1.0 - alpha) * previous_filtered_value; +} + // Convert geometry_msgs::Pose to KDL::Frame KDL::Frame poseToKDL(const geometry_msgs::msg::Pose& pose) { KDL::Vector position(pose.position.x, pose.position.y, pose.position.z); @@ -509,6 +513,8 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) last_sent_torques_right.resize(rightarm_chain.getNrOfJoints()); last_sent_torques_right.data.setZero(); right_arm_torque_interpolators.resize(rightarm_chain.getNrOfJoints()); + filtered_q_rightarm.resize(rightarm_chain.getNrOfJoints()); + filtered_qd_rightarm.resize(rightarm_chain.getNrOfJoints()); } if (should_control_left_arm()) { last_sent_torques_left.resize(leftarm_chain.getNrOfJoints()); @@ -595,10 +601,6 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) pid_component_publishers[topic_name] = this->create_publisher(topic_name, 10); } } - - // Torque publishers - raw_torque_publisher = this->create_publisher("/raw_torques", 10); - smoothed_torque_publisher = this->create_publisher("/smoothed_torques", 10); } void EddieRosInterface::initialize_action_servers() { @@ -1023,6 +1025,8 @@ void EddieRosInterface::idle(events *eventData, const EddieState *eddie_state) { for (int i = 0; i < num_jnts_rightarm; i++) { q_rightarm(i) = eddie_state->kinova_rightarm_state.pos_msr[i]; qd_rightarm(i) = eddie_state->kinova_rightarm_state.vel_msr[i]; + filtered_q_rightarm(i) = q_rightarm(i); + filtered_qd_rightarm(i) = qd_rightarm(i); } KDL::JntArrayVel q_qd_rightarm(q_rightarm, qd_rightarm); KDL::ChainFkSolverPos_recursive fpk_pose_rightarm_ee(rightarm_chain); @@ -1113,38 +1117,6 @@ void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddi } } -void EddieRosInterface::publish_torque_debug_info( - const KDL::JntArray& raw_torques, - const KDL::JntArray& smoothed_torques, - const std::string& arm_side) -{ - // Check if anyone is actually subscribed to the topics - if (raw_torque_publisher->get_subscription_count() == 0 && - smoothed_torque_publisher->get_subscription_count() == 0) { - return; - } - - auto now = this->get_clock()->now(); - - // Publish raw torques - auto raw_torque_msg = sensor_msgs::msg::JointState(); - raw_torque_msg.header.stamp = now; - for (unsigned int i = 0; i < raw_torques.rows(); i++) { - raw_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); - raw_torque_msg.effort.push_back(raw_torques(i)); - } - raw_torque_publisher->publish(raw_torque_msg); - - // Publish smoothed torques - auto smoothed_torque_msg = sensor_msgs::msg::JointState(); - smoothed_torque_msg.header.stamp = now; - for (unsigned int i = 0; i < smoothed_torques.rows(); i++) { - smoothed_torque_msg.name.push_back(arm_side + "_joint_" + std::to_string(i)); - smoothed_torque_msg.effort.push_back(smoothed_torques(i)); - } - smoothed_torque_publisher->publish(smoothed_torque_msg); -} - void EddieRosInterface::publish_pid_components( const std::string& arm_side, const std::array& outputs) @@ -1256,13 +1228,6 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed publish_pid_components("right", right_outputs); -/* double fx_right = pid_rightarm_ee_pos_x.control(error_x, cycle_time); - double fy_right = pid_rightarm_ee_pos_y.control(error_y, cycle_time); - double fz_right = pid_rightarm_ee_pos_z.control(error_z, cycle_time); - double mx_right = pid_rightarm_ee_rot_x.control(error_rot_x, cycle_time); - double my_right = pid_rightarm_ee_rot_y.control(error_rot_y, cycle_time); - double mz_right = pid_rightarm_ee_rot_z.control(error_rot_z, cycle_time); */ - KDL::Wrench f_ext_ee_rightarm = KDL::Wrench(KDL::Vector(fx_right, fy_right, fz_right), KDL::Vector(mx_right, my_right, mz_right)); KDL::Wrench f_ext_ee_rightarm_wrt_ee = KDL::Wrench( pose_rightarm_ee.M.Inverse() * f_ext_ee_rightarm.force, @@ -1337,8 +1302,6 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed torque_log_file << "\n"; } - //publish_torque_debug_info(tau_ctrl_rightarm, final_torques, "right"); - /* for (int i = 0; i < num_jnts_rightarm; i++) { saturate(&tau_ctrl_rightarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_rightarm_state.eff_cmd[i] = tau_ctrl_rightarm(i); @@ -1420,8 +1383,28 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } } - void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { + // Define a filter alpha for the sensor inputs. + // alpha = 1.0 means no filtering, alpha = 0.0 means full filtering + // if alpha is too small, the response will be very slow. + /* + const double sensor_filter_alpha = 0.9; // 90% new data, 10% old data + + if (should_control_right_arm()) { + for (int i = 0; i < num_jnts_rightarm; i++) { + // Read raw data + double raw_pos = eddie_state->kinova_rightarm_state.pos_msr[i]; + double raw_vel = eddie_state->kinova_rightarm_state.vel_msr[i]; + + // Apply low-pass filter + filtered_q_rightarm(i) = low_pass_filter(raw_pos, filtered_q_rightarm(i), sensor_filter_alpha); + filtered_qd_rightarm(i) = low_pass_filter(raw_vel, filtered_qd_rightarm(i), sensor_filter_alpha); + + // Use the filtered data for control + q_rightarm(i) = filtered_q_rightarm(i); + qd_rightarm(i) = filtered_qd_rightarm(i); + } + } */ // RCLCPP_INFO(get_logger(), "In execute state"); // // Update the EtherCAT state From 40fee23a9709ab03f9a573fc3b4d49e0b8c9d586 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Tue, 16 Dec 2025 17:21:10 +0100 Subject: [PATCH 33/35] Change some pid values, clean up ccode --- src/interface-parameters.cpp | 27 +++---------------- src/interface.cpp | 52 +++--------------------------------- src/pid.cpp | 2 +- 3 files changed, 8 insertions(+), 73 deletions(-) diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index 98784ac..b203874 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -47,11 +47,11 @@ void EddieRosInterface::declare_pid_gains() { this->declare_parameter("pid.right.pos.x.i", 20.0); this->declare_parameter("pid.right.pos.x.d", 2.0); this->declare_parameter("pid.right.pos.y.p", 120.0); - this->declare_parameter("pid.right.pos.y.i", 20.0); + this->declare_parameter("pid.right.pos.y.i", 25.0); this->declare_parameter("pid.right.pos.y.d", 2.0); - this->declare_parameter("pid.right.pos.z.p", 150.0); + this->declare_parameter("pid.right.pos.z.p", 160.0); this->declare_parameter("pid.right.pos.z.i", 20.0); - this->declare_parameter("pid.right.pos.z.d", 10.0); + this->declare_parameter("pid.right.pos.z.d", 2.0); this->declare_parameter("pid.right.pos.deadband", default_pos_deadband); this->declare_parameter("pid.right.rot.x.p", 5.0); @@ -106,7 +106,6 @@ void EddieRosInterface::reload_pid_gains() double r_pos_z_p = this->get_parameter("pid.right.pos.z.p").as_double(); double r_pos_z_i = this->get_parameter("pid.right.pos.z.i").as_double(); double r_pos_z_d = this->get_parameter("pid.right.pos.z.d").as_double(); - //double r_pos_deadband = this->get_parameter("pid.right.pos.deadband").as_double(); double r_rot_x_p = this->get_parameter("pid.right.rot.x.p").as_double(); double r_rot_x_i = this->get_parameter("pid.right.rot.x.i").as_double(); @@ -117,7 +116,6 @@ void EddieRosInterface::reload_pid_gains() double r_rot_z_p = this->get_parameter("pid.right.rot.z.p").as_double(); double r_rot_z_i = this->get_parameter("pid.right.rot.z.i").as_double(); double r_rot_z_d = this->get_parameter("pid.right.rot.z.d").as_double(); - //double r_rot_deadband = this->get_parameter("pid.right.rot.deadband").as_double(); // Get values for the LEFT arm double l_pos_x_p = this->get_parameter("pid.left.pos.x.p").as_double(); @@ -129,7 +127,6 @@ void EddieRosInterface::reload_pid_gains() double l_pos_z_p = this->get_parameter("pid.left.pos.z.p").as_double(); double l_pos_z_i = this->get_parameter("pid.left.pos.z.i").as_double(); double l_pos_z_d = this->get_parameter("pid.left.pos.z.d").as_double(); - //double l_pos_deadband = this->get_parameter("pid.left.pos.deadband").as_double(); double l_rot_x_p = this->get_parameter("pid.left.rot.x.p").as_double(); double l_rot_x_i = this->get_parameter("pid.left.rot.x.i").as_double(); @@ -140,7 +137,6 @@ void EddieRosInterface::reload_pid_gains() double l_rot_z_p = this->get_parameter("pid.left.rot.z.p").as_double(); double l_rot_z_i = this->get_parameter("pid.left.rot.z.i").as_double(); double l_rot_z_d = this->get_parameter("pid.left.rot.z.d").as_double(); - //double l_rot_deadband = this->get_parameter("pid.left.rot.deadband").as_double(); // Set PID controller gains for the RIGHT arm pid_rightarm_ee_pos_x.set_gains(r_pos_x_p, r_pos_x_i, r_pos_x_d, error_sum_tol, decay_rate); @@ -201,19 +197,4 @@ rcl_interfaces::msg::SetParametersResult EddieRosInterface::parameters_callback( } return result; -} - -// rcl_interfaces::msg::SetParametersResult -// EddieRosInterface::parametersCallback(const std::vector ¶meters) { -// rcl_interfaces::msg::SetParametersResult result; -// result.successful = true; -// result.reason = "Success"; - -// for (const auto ¶m : parameters) { -// if (param.get_name() == "ethernet_if") { -// this->param_ethernet_if = param.get_value(); -// } -// } - -// return result; -// } \ No newline at end of file +} \ No newline at end of file diff --git a/src/interface.cpp b/src/interface.cpp index 99df4a8..b0a1845 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -382,52 +382,6 @@ void EddieRosInterface::execute_gripper_control( get_gripper_execution_flag(arm_side).store(false); } -/* PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { - err_integ = 0.0; - err_last = 0.0; - kp = p_gain; - ki = i_gain; - kd = d_gain; - err_sum_tol = error_sum_tol; - this->decay_rate = decay_rate; -} - -void PID::set_gains( - double p_gain, double i_gain, double d_gain, double error_sum_tol, double decay_rate) { - err_integ = 0.0; - err_last = 0.0; - kp = p_gain; - ki = i_gain; - kd = d_gain; - err_sum_tol = error_sum_tol; - this->decay_rate = decay_rate; -} - -double PID::control(double error, double dt) { - - double err_diff = (error - err_last) / dt; - - if (fabs(error) > 0.0) { - // Accumulate the integral when error is non-zero - err_integ += error * dt; - - // Clamp the integral term to prevent runaway accumulation - if (err_integ > err_sum_tol) { - err_integ = err_sum_tol; - } else if (err_integ < -err_sum_tol) { - err_integ = -err_sum_tol; - } - } else { - // Decay the integral term when the error is zero - err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; - } - - // err_integ = decay_rate * err_integ + (1.0 - decay_rate) * error; - err_last = error; - - return kp * error + ki * err_integ + kd * err_diff; -} */ - EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) : rclcpp::Node("eddie_ros_interface", options) { @@ -1384,10 +1338,10 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed } void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { + /* // Define a filter alpha for the sensor inputs. // alpha = 1.0 means no filtering, alpha = 0.0 means full filtering // if alpha is too small, the response will be very slow. - /* const double sensor_filter_alpha = 0.9; // 90% new data, 10% old data if (should_control_right_arm()) { @@ -1511,7 +1465,7 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { if (++debug_counter % 1000 == 0) { // Every 1000 cycles (1 second at 1kHz) if (should_control_right_arm()) { KDL::Twist pose_error = KDL::diff(target_pose_rightarm_ee, pose_rightarm_ee); - /* RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", + RCLCPP_INFO(get_logger(), "Right arm pose error: pos(%.3f, %.3f, %.3f) rot(%.3f, %.3f, %.3f)", pose_error.vel.x(), pose_error.vel.y(), pose_error.vel.z(), pose_error.rot.x(), pose_error.rot.y(), pose_error.rot.z()); RCLCPP_INFO(get_logger(), "Right arm target pose: Position: [%f, %f, %f]", @@ -1525,7 +1479,7 @@ void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { RCLCPP_INFO(get_logger(), "Right arm gripper metrics: pos=%.3f, vel=%.3f, force=%.3f", eddie_state->kinova_rightarm_state.gripper_pos_msr[0], eddie_state->kinova_rightarm_state.gripper_vel_msr[0], - eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); */ + eddie_state->kinova_rightarm_state.gripper_cur_msr[0]); } if (should_control_left_arm()) { KDL::Twist pose_error = KDL::diff(target_pose_leftarm_ee, pose_leftarm_ee); diff --git a/src/pid.cpp b/src/pid.cpp index 4aa6993..423c091 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -57,4 +57,4 @@ PIDOutput PID::control(double error, double dt) { // clamp integral term to prevent windup - max and min value it can have (based on these plots below) // plot P term, I term, D term separately for debugging // p + i + d -// feedback for pos, velocity, acceleration(?), effort(?) separately \ No newline at end of file +// feedback for pos, velocity separately \ No newline at end of file From 742e199744ab6942fe7a206a61120114dc28df98 Mon Sep 17 00:00:00 2001 From: rinaalo Date: Wed, 17 Dec 2025 12:30:03 +0100 Subject: [PATCH 34/35] Publishing pid components with a timer --- README.md | 1 + include/eddie_ros/interface.hpp | 6 +- src/interface.cpp | 128 +++++++++++++++++++++++--------- 3 files changed, 99 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index a04ba22..adcacea 100644 --- a/README.md +++ b/README.md @@ -86,6 +86,7 @@ ros2 launch eddie_ros rviz.launch.py use_sim:=true ``` ## Adjusting PID gains on run-time (only for testing!) +*Note: We are only dynamically adjusting the PID gains at the moment for testing. PID gains will not be changed during run-time in the actual deployment of the robot.* First, make sure to run the `eddie_ros_interface` node as described above. diff --git a/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index be04414..290927b 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -274,7 +274,7 @@ class EddieRosInterface : public rclcpp::Node { void compute_cartesian_ctrl(events *eventData, EddieState *eddie_state); void publish_ee_errors(EddieState *eddie_state); void publish_joint_states(EddieState *eddie_state); - void publish_pid_components(const std::string& arm_side, const std::array& outputs); + void publish_pid_components(); public: void run_fsm(); @@ -340,8 +340,12 @@ class EddieRosInterface : public rclcpp::Node { KDL::JntArray last_sent_torques_right; KDL::JntArray last_sent_torques_left; + std::array latest_right_pid_outputs; + std::array latest_left_pid_outputs; + // PID debug publishers std::map::SharedPtr> pid_component_publishers; + rclcpp::TimerBase::SharedPtr pid_component_timer; // Interpolators (one for each joint) std::vector right_arm_torque_interpolators; diff --git a/src/interface.cpp b/src/interface.cpp index b0a1845..f39ddd3 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -555,6 +555,11 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) pid_component_publishers[topic_name] = this->create_publisher(topic_name, 10); } } + this->pid_component_timer = this->create_wall_timer( + std::chrono::milliseconds(1), + std::bind(&EddieRosInterface::publish_pid_components, this) + ); + RCLCPP_INFO(this->get_logger(), "PID component publisher timer started at 50 Hz."); } void EddieRosInterface::initialize_action_servers() { @@ -1071,33 +1076,50 @@ void EddieRosInterface::compute_gravity_comp(events *eventData, EddieState *eddi } } -void EddieRosInterface::publish_pid_components( - const std::string& arm_side, - const std::array& outputs) +void EddieRosInterface::publish_pid_components() { - // map an index to an axis name const std::array axis_names = {"pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z"}; - auto now = this->get_clock()->now(); - for (int i = 0; i < 6; ++i) { - const std::string& axis_name = axis_names[i]; - const PIDOutput& output = outputs[i]; - std::string topic_name = arm_side + "_arm/pid_components/" + axis_name; - - // Check if the publisher exists and has subscribers - if (pid_component_publishers.count(topic_name) && - pid_component_publishers[topic_name]->get_subscription_count() > 0) - { - auto msg = geometry_msgs::msg::TwistStamped(); - msg.header.stamp = now; - - msg.twist.linear.x = output.p; // P term - msg.twist.linear.y = output.i; // I term - msg.twist.linear.z = output.d; // D term - msg.twist.angular.x = output.total; // Total - - pid_component_publishers[topic_name]->publish(msg); + if (should_control_right_arm()) { + // Loop through all 6 axes for the right arm + for (int i = 0; i < 6; ++i) { + const std::string& axis_name = axis_names[i]; + const PIDOutput& output = latest_right_pid_outputs[i]; + std::string topic_name = "right_arm/pid_components/" + axis_name; + + if (pid_component_publishers.count(topic_name) && + pid_component_publishers[topic_name]->get_subscription_count() > 0) + { + auto msg = geometry_msgs::msg::TwistStamped(); + msg.header.stamp = now; + msg.twist.linear.x = output.p; + msg.twist.linear.y = output.i; + msg.twist.linear.z = output.d; + msg.twist.angular.x = output.total; + pid_component_publishers[topic_name]->publish(msg); + } + } + } + + if (should_control_left_arm()) { + // Loop through all 6 axes for the left arm + for (int i = 0; i < 6; ++i) { + const std::string& axis_name = axis_names[i]; + const PIDOutput& output = latest_left_pid_outputs[i]; + std::string topic_name = "left_arm/pid_components/" + axis_name; + + if (pid_component_publishers.count(topic_name) && + pid_component_publishers[topic_name]->get_subscription_count() > 0) + { + auto msg = geometry_msgs::msg::TwistStamped(); + msg.header.stamp = now; + msg.twist.linear.x = output.p; + msg.twist.linear.y = output.i; + msg.twist.linear.z = output.d; + msg.twist.angular.x = output.total; + pid_component_publishers[topic_name]->publish(msg); + } } } } @@ -1180,7 +1202,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double my_right = right_outputs[4].total; double mz_right = right_outputs[5].total; - publish_pid_components("right", right_outputs); + latest_right_pid_outputs = right_outputs; KDL::Wrench f_ext_ee_rightarm = KDL::Wrench(KDL::Vector(fx_right, fy_right, fz_right), KDL::Vector(mx_right, my_right, mz_right)); KDL::Wrench f_ext_ee_rightarm_wrt_ee = KDL::Wrench( @@ -1290,7 +1312,7 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed double my_left = left_outputs[4].total; double mz_left = left_outputs[5].total; - publish_pid_components("left", left_outputs); + latest_left_pid_outputs = left_outputs; KDL::Wrench f_ext_ee_leftarm = KDL::Wrench(KDL::Vector(fx_left, fy_left, fz_left), KDL::Vector(mx_left, my_left, mz_left)); KDL::Wrench f_ext_ee_leftarm_wrt_ee = KDL::Wrench( @@ -1320,20 +1342,56 @@ void EddieRosInterface::compute_cartesian_ctrl(events *eventData, EddieState *ed if (r_left < 0) { RCLCPP_ERROR(get_logger(), "Left arm RNE ID solver failed with error code: %d", r_left); } - // Apply the low-pass filter to smooth the torque commands -/* for (unsigned int i = 0; i < num_jnts_leftarm; i++) { - smoothed_torques_left(i) = alpha * tau_ctrl_leftarm(i) + (1.0 - alpha) * smoothed_torques_left(i); + + if (left_arm_smoothing_start.load()) { + RCLCPP_INFO(this->get_logger(), "Left arm gains changed. Starting torque interpolation."); + + // Start an interpolator for each joint. + for (unsigned int i = 0; i < num_jnts_leftarm; i++) { + left_arm_torque_interpolators[i].start( + last_sent_torques_left(i), + tau_ctrl_leftarm(i), + transition_duration + ); + } + left_arm_smoothing_start.store(false); // Reset the flag } - publish_torque_debug_info(tau_ctrl_leftarm, smoothed_torques_left, "left"); - // Send the smoothed torques to the robot - for (int i = 0; i < num_jnts_leftarm; i++) { - saturate(&smoothed_torques_left(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); - this->eddie_state.kinova_leftarm_state.eff_cmd[i] = smoothed_torques_left(i); - } */ + + // Determine the final torque to send + KDL::JntArray final_torques(num_jnts_leftarm); + for (unsigned int i = 0; i < num_jnts_leftarm; i++) { + if (left_arm_torque_interpolators[i].is_active) { + // If we are interpolating, get the value from the trajectory. + final_torques(i) = left_arm_torque_interpolators[i].get_value(); + } else { + // Otherwise, use the raw torque for this cycle. + final_torques(i) = tau_ctrl_leftarm(i); + } + } + + // Send torques to robot and store for next cycle for (int i = 0; i < num_jnts_leftarm; i++) { + double torque_to_send = final_torques(i); + saturate(&torque_to_send, -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); + this->eddie_state.kinova_leftarm_state.eff_cmd[i] = torque_to_send; + } + + // Store the final sent torque for the next cycle. + last_sent_torques_left = final_torques; + + if (torque_log_file.is_open()) { + torque_log_file << this->get_clock()->now().nanoseconds(); + for (unsigned int i = 0; i < num_jnts_leftarm; ++i) { + torque_log_file << "," << tau_ctrl_leftarm(i) + << "," << final_torques(i); + } + torque_log_file << "\n"; + } + +/* for (int i = 0; i < num_jnts_leftarm; i++) { saturate(&tau_ctrl_leftarm(i), -KINOVA_TAU_CMD_LIMIT, KINOVA_TAU_CMD_LIMIT); eddie_state->kinova_leftarm_state.eff_cmd[i] = tau_ctrl_leftarm(i); - } + } */ } } From fa244fd85cba7c2f6178ea2f287805361a039e55 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke <75437449+bhunecke@users.noreply.github.com> Date: Wed, 17 Dec 2025 13:07:49 +0100 Subject: [PATCH 35/35] Add eddie_description link to robot_setup.md --- robot_setup.md | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_setup.md b/robot_setup.md index 355cbb5..a065ad3 100644 --- a/robot_setup.md +++ b/robot_setup.md @@ -6,6 +6,7 @@ Also see the documentation for: - [coord2b](https://github.com/rosym-project/coord2b) - [eddie-ros](https://github.com/secorolab/eddie-ros) - [eddie_pmu_control](https://github.com/secorolab/eddie_pmu_control) +- [eddie_description](https://github.com/secorolab/eddie_description) - [orocos_kinematics_dynamics](https://github.com/secorolab/orocos_kinematics_dynamics) - [robif2b](https://github.com/secorolab/robif2b) - [ros2_kortex_vision](https://github.com/Kinovarobotics/ros2_kortex_vision)