Release 3

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

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_<leftarm/rightarm>.ee.<rot/pos>.<x/y/z>.<p/i/d>
+```
+
+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.
 

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

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

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

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,