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/README.md b/README.md index 2aa2b58..e73f69f 100644 --- a/README.md +++ b/README.md @@ -85,9 +85,52 @@ run rviz: ros2 launch eddie_ros rviz.launch.py use_sim:=true ``` -## Plotting cartesian error with RQT Plot +## 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.* -You can visualize the Cartesian error of the end-effectors with `rqt_plot`: +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.right.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.right.pos.x.p 150.0 +``` +## Plotting PID components with [Visualizer](https://github.com/Robots4Sustainability/cart-error-visualizer) + +You can visualize the PID components using pid_component_visualizer.py in our Visualizer repository. +Follow the instructions in the repository on how to install and run the visualizer. + +## Plotting torques with [Visualizer](https://github.com/Robots4Sustainability/cart-error-visualizer) + +The interface will generate a torque_log.csv file in the root of your workspace. +You can plot this file using plot_torque_log.py in our visualizer repository. +Follow the instructions in the repository on how to install and run the visualizer. + +## 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 [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. diff --git a/action/ArmControl.action b/action/ArmControl.action index abec215..91c2896 100644 --- a/action/ArmControl.action +++ b/action/ArmControl.action @@ -1,11 +1,17 @@ # Request: Command to control arm -geometry_msgs/Pose target_pose +geometry_msgs/Pose target_pose # Desired end effector pose + --- # 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 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 -geometry_msgs/Pose current_pose # Current end effector pose -string status_message # Possible status +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 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/include/eddie_ros/interface.hpp b/include/eddie_ros/interface.hpp index 8e84815..487f394 100644 --- a/include/eddie_ros/interface.hpp +++ b/include/eddie_ros/interface.hpp @@ -15,12 +15,16 @@ #include #include #include +#include +#include #include +#include "geometry_msgs/msg/twist_stamped.hpp" #include #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" @@ -57,6 +61,12 @@ 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; + double total = 0.0; +}; class PID { public: @@ -75,7 +85,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; @@ -87,6 +97,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 { @@ -195,14 +250,21 @@ 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(); // 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); @@ -210,8 +272,9 @@ 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); + void publish_pid_components(); public: void run_fsm(); @@ -234,7 +297,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,25 +312,44 @@ 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; + + // Smoothed torque commands + 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; rclcpp::Publisher::SharedPtr left_arm_ee_error_pub; rclcpp::TimerBase::SharedPtr ee_error_timer_; + KDL::JntArray filtered_q_rightarm; + KDL::JntArray filtered_qd_rightarm; + + // The torque values from the previous control cycle + 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; + 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; @@ -326,12 +411,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/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 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 diff --git a/robot_setup.md b/robot_setup.md index 38b8b17..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) @@ -17,6 +18,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: @@ -46,10 +48,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 +74,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. diff --git a/src/interface-parameters.cpp b/src/interface-parameters.cpp index 7ec4565..b203874 100644 --- a/src/interface-parameters.cpp +++ b/src/interface-parameters.cpp @@ -32,17 +32,169 @@ void EddieRosInterface::get_all_parameters() { this->get_parameter("arm_select", param_arm_select); } -// 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 +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.1); + + 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", 120.0); + 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", 25.0); + this->declare_parameter("pid.right.pos.y.d", 2.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", 2.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.5; + + // 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 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.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.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; +} \ No newline at end of file diff --git a/src/interface-test.cpp b/src/interface-test.cpp index 8285bfc..b9563fd 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; @@ -94,8 +94,7 @@ PID::PID(double p_gain, double i_gain, double d_gain, double error_sum_tol, doub } 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) { err_integ = 0.0; err_last = 0.0; kp = p_gain; @@ -127,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) { @@ -190,8 +189,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 +198,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 +206,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 +267,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 +276,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 +284,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 +357,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 +374,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 +446,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 +463,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"); @@ -811,7 +810,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]; @@ -930,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)); @@ -975,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 b780802..fdd2d3d 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,9 +41,11 @@ 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; + return quantity; else if (quantity > upper) - return quantity - upper; + //return quantity - upper; + return quantity; else return 0.0; } @@ -55,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); @@ -91,7 +98,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 +106,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 +114,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 +122,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 +130,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 +138,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 +160,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 +185,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 +202,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(), @@ -215,43 +222,63 @@ 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 (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"; + // 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()); + arm_goal_executing(arm_side) = false; + return; + } + loop_rate.sleep(); + wait_cycles++; + } + // 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) { 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"; + // 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(); 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->final_pose = kdlToPose(get_current_pose_ee(arm_side)); + 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(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->status_message = "Moving to target position - pos_err: " + - std::to_string(position_error) + " rot_err: " + std::to_string(rotation_error); + feedback->current_pose = kdlToPose(current_pose_ee(arm_side)); goal_handle->publish_feedback(feedback); loop_rate.sleep(); @@ -259,17 +286,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)); - 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->final_pose = kdlToPose(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(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); + arm_goal_executing(arm_side) = false; } // Gripper control action server callbacks @@ -281,7 +308,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; @@ -303,7 +330,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); @@ -350,75 +377,30 @@ 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); + gripper_goal_executing(arm_side) = 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]; 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"); } - 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; + gripper_goal_executing(arm_side) = false; } EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &options) @@ -426,8 +408,14 @@ EddieRosInterface::EddieRosInterface(const rclcpp::NodeOptions &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 = {}; @@ -456,7 +444,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,19 +468,44 @@ 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 { RCLCPP_INFO(get_logger(), "Right arm chain constructed successfully"); } + // Smoothed torque commands + if (should_control_right_arm()) { + 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()); + 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}; @@ -527,22 +540,7 @@ 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); - - // 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_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_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); - + RCLCPP_INFO(get_logger(), "Eddie ROS interface node initialized."); // Error publishers @@ -557,11 +555,30 @@ 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); + this->publish_ee_errors(); + } + ); + + // 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); + } + } + 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() { @@ -654,9 +671,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( @@ -668,7 +682,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( @@ -701,6 +714,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) { @@ -976,13 +992,16 @@ 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_DEBUG(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]; + 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); @@ -991,11 +1010,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]; @@ -1007,103 +1029,90 @@ 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; + } } - RCLCPP_DEBUG(get_logger(), "Exiting idle state"); - produce_event(eventData, E_IDLE_EXIT_EXECUTE); -} - -void EddieRosInterface::compile(events *eventData, const EddieState *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) { + compute_cartesian_ctrl(eventData, 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); - } + robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); + robif2b_kinova_gen3_update(&kinova_rightarm); } 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); - } + robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); + robif2b_kinova_gen3_update(&kinova_leftarm); + } + 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); } } -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::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::compute_cartesian_ctrl(events *eventData, EddieState *eddie_state) { + (void)eventData; long cycle_time_msr = eddie_state->time.cycle_time_msr; @@ -1113,16 +1122,45 @@ 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(); + + const double transition_duration = 0.05; 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 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); + + // 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; + + 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( @@ -1152,20 +1190,87 @@ 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); } + + 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 + } + + // 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 { + // Otherwise, use the raw torque for this cycle. + final_torques(i) = tau_ctrl_rightarm(i); + } + } + + // Send torques to robot and store for next cycle 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); + 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"; + } + + /* 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()) { + // Calculate the raw cartesian error 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); + // 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); + + // 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; + + 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( @@ -1195,16 +1300,82 @@ 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); } + + 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 + } + + // 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); - } + } */ } } - void EddieRosInterface::execute(events *eventData, EddieState *eddie_state) { - // RCLCPP_INFO(get_logger(), "In execute 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_DEBUG(get_logger(), "In execute state"); // // Update the EtherCAT state // robif2b_ethercat_update(&ecat); @@ -1216,136 +1387,150 @@ 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(); - - - // 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()); - 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; + 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]; + } + + 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(); + + // 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 + } + + // impedance control for right arm - start pose as target pose + compute_cartesian_ctrl(eventData, eddie_state); - // 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()); + 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]; + } - // 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); + KDL::JntArrayVel q_qd_leftarm(q_leftarm, qd_leftarm); - // 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; + 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 + } - new_target_rightarm = 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); } + // robif2b_kelo_drive_actuator_update(&wheel_act); - // impedance control for right arm - start pose as target pose - compute_cartesian_ctrl(eventData, eddie_state); + 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_pid_components() +{ + const std::array axis_names = {"pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z"}; + auto now = this->get_clock()->now(); - // 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_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()) { - 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]); + } + + 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); + } } } +} + +void EddieRosInterface::publish_ee_errors() { - // robif2b_kelo_drive_actuator_update(&wheel_act); if (should_control_right_arm()) { - robif2b_kg3_robotiq_gripper_update(&kinova_rightgripper); - robif2b_kinova_gen3_update(&kinova_rightarm); + 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()) { - robif2b_kg3_robotiq_gripper_update(&kinova_leftgripper); - robif2b_kinova_gen3_update(&kinova_leftarm); + 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)); } } @@ -1482,4 +1667,4 @@ int main(int argc, char **argv) { rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/src/pid.cpp b/src/pid.cpp new file mode 100644 index 0000000..423c091 --- /dev/null +++ b/src/pid.cpp @@ -0,0 +1,60 @@ +#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; +} + +PIDOutput 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; + + 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) +// plot P term, I term, D term separately for debugging +// p + i + d +// feedback for pos, velocity separately \ No newline at end of file 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); }