From a8c882462aa67168653a0c5f5cb62e2f2bfcfaae Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Mon, 14 Jul 2025 08:19:47 +0200
Subject: [PATCH 1/9] added function to calculate max allowable payload for a
 given config

---
 .../dynamic_payload_analysis_core/core.py     | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
index cdfa3dd..0ec6f13 100644
--- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
+++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
@@ -478,6 +478,36 @@ def get_valid_workspace(self, range : int, resolution : int, end_effector_name_l
         return valid_configurations
     
 
+    # TODO : test it
+    def compute_maximum_payloads(self, configs : np.ndarray):
+        """
+        Compute the maximum payload for each provided configuration and return the results with the configs updated with the maximum payload as a new value.
+        :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "arm", "max_payload" }     
+        """
+        # payload limit 
+        payload_limit = 10
+        resolution_payload = 0.01
+
+        for config in configs:
+             # iterate over a possible set of payloads 
+             for mass in np.arange(0, payload_limit , resolution_payload):
+                # Create external forces based on the masses and checked frames TODO change name in arm 7 for example
+                ext_forces = self.create_ext_force(mass, f"gripper_{config['arm']}_finger_joint", config["config"])
+                # calculate the current torques
+                tau = self.compute_inverse_dynamics(config["config"], self.get_zero_velocity(), self.get_zero_acceleration(),extForce=ext_forces)
+                
+                if not self.check_effort_limits(tau,config["arm"]).all():
+                    # if the current payload is not valid, the maximum payload is the previous one
+                    config["max_payload"] = mass - resolution_payload
+                    break
+                else:
+                    # if the current payload is valid, update the maximum payload
+                    config["max_payload"] = mass
+            
+
+        return configs
+
+
 
     def compute_forward_dynamics_aba_method(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, extForce : np.ndarray[pin.Force] = None) -> np.ndarray:
         """

From 890e968e7f567f46c859b7e171c60e7879457578 Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Mon, 14 Jul 2025 09:30:32 +0200
Subject: [PATCH 2/9] improvements in usage in the rviz panel

---
 .../menu_visual.py                            | 54 +++++++++++++++++--
 1 file changed, 51 insertions(+), 3 deletions(-)

diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
index 571703b..e4bcdb5 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
@@ -51,7 +51,7 @@ def __init__(self, node):
         self.selected_configuration = None
 
         # payload mass selection array
-        self.payload_selection = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 1, 1.5, 1.8, 2.0, 2.5, 3.0, 3.5], dtype=float)
+        self.payload_selection = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 1, 1.5, 1.8, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5], dtype=float)
 
         # variable to store selection between torque limits or max torque in the current configuration for color visualization
         self.torque_color = TorqueVisualizationType.TORQUE_LIMITS # default is torque limits
@@ -62,12 +62,16 @@ def __init__(self, node):
         # insert the reset payload button 
         self.reset = self.menu_handler.insert('Reset payloads', parent=self.root_frames, callback=self.callback_reset)
         
+        # insert label for the color menu selection
+        self.label_color_selection = self.menu_handler.insert('Select torque visualization color :',callback=self.callback_label)
+
         # insert the checker for visualization color choice between torque limits or max torque in the current configuration
         self.torque_limits_checker = self.menu_handler.insert('Torque limits',command=str(TorqueVisualizationType.TORQUE_LIMITS.value) , callback=self.callback_color_selection)
         self.max_torque_checker = self.menu_handler.insert('Max torque', command=str(TorqueVisualizationType.MAX_CURRENT_TORQUE.value) , callback=self.callback_color_selection)
 
         self.menu_handler.setVisible(self.torque_limits_checker, False)
         self.menu_handler.setVisible(self.max_torque_checker, False)
+        self.menu_handler.setVisible(self.label_color_selection, False)
         
 
         self.make_menu_marker('menu_frames')
@@ -94,6 +98,15 @@ def insert_frame(self, name):
         self.menu_handler.reApply(self.server)
         self.server.applyChanges()
     
+    def callback_label(self, feedback):
+        """
+        Callback for the label of color selection. Made only to avoid the error of missing callback.
+        
+        Args:
+            feedback: Feedback from the menu selection.
+        """
+        pass
+
     def callback_color_selection(self, feedback):
         """
         Callback for torque selection type to change the visualization color in Rviz.
@@ -151,6 +164,7 @@ def insert_dropdown_configuration(self, configuration : np.ndarray):
         # set visible true for color selection and put the default check state
         self.menu_handler.setVisible(self.torque_limits_checker, True)
         self.menu_handler.setVisible(self.max_torque_checker, True)
+        self.menu_handler.setVisible(self.label_color_selection, True)
         self.menu_handler.setCheckState(self.torque_limits_checker, MenuHandler.CHECKED)
         self.menu_handler.setCheckState(self.max_torque_checker, MenuHandler.UNCHECKED)
 
@@ -223,9 +237,13 @@ def callback_reset(self, feedback):
                     self.menu_handler.setCheckState(sub_item, MenuHandler.UNCHECKED)
             
              # check if the item is the reset payloads or compute workspace item and skip the unchanging of the check state
-            if item.title == "Reset payloads" or item.title == "Compute workspace":
+            if item.title == self.menu_handler.getTitle(self.reset) or item.title == self.menu_handler.getTitle(self.workspace_button):
                 continue
                 
+            if item.title == self.menu_handler.getTitle(self.label_color_selection):
+                self.menu_handler.setVisible(self.label_color_selection, False)
+                continue
+            
             # set the checked of frame to unchecked 
             self.menu_handler.setCheckState(i,MenuHandler.UNCHECKED)
         
@@ -250,6 +268,11 @@ def callback_selection(self, feedback):
         Args:
             feedback: Feedback from the menu selection.
         """
+
+        # reset the sub-menu configuration if the user change payload selection
+        self.reset_sub_menu_configuration()
+        # -------------
+
         # get name of the frame
         handle = feedback.menu_entry_id
         title = self.menu_handler.getTitle(handle)
@@ -269,7 +292,29 @@ def callback_selection(self, feedback):
         # print the current state of the checked frames
         print(f"{self.get_item_state()} \n")
 
-        
+    
+    def reset_sub_menu_configuration(self):
+        """
+        Function to reset the sub-menu configuration and related variables.
+        """
+        # reset calculated configurations in the workspace submenu
+        for i, item in self.menu_handler.entry_contexts_.items():
+            # check if the item is the workspace button and has sub entries and remove them from view
+            if item.sub_entries and item.title == self.menu_handler.getTitle(self.workspace_button):
+                # if the frame has payloads selection, we need to remove it
+                for sub_item in item.sub_entries:
+                    self.menu_handler.setVisible(sub_item, False)
+                    self.menu_handler.setCheckState(sub_item, MenuHandler.UNCHECKED)
+
+        # hide the torque limits and max torque checkboxes when there is no configuration selected
+        self.menu_handler.setVisible(self.torque_limits_checker, False)
+        self.menu_handler.setVisible(self.max_torque_checker, False)
+        self.menu_handler.setVisible(self.label_color_selection, False)
+
+        self.torque_color = TorqueVisualizationType.TORQUE_LIMITS  # reset to default torque limits
+
+        # reset the selected configuration
+        self.selected_configuration = None
     
 
     def update_item(self, name, check: bool):
@@ -328,6 +373,9 @@ def update_payload_selection(self, feedback):
         Args:
             feedback: Feedback from the menu selection.
         """
+        # reset the sub-menu configuration if the user change payload selection
+        self.reset_sub_menu_configuration() 
+
         # get the handle of the selected item (id)
         handle = feedback.menu_entry_id
         # get the title of the selected item (it contains number of kg)

From 8f73ee4537e5cd3eb9dd0985493cd35aae0428a0 Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Mon, 14 Jul 2025 10:28:05 +0200
Subject: [PATCH 3/9] added calculation for max allowable payload in the
 calculated configurations of workspace

---
 .../dynamic_payload_analysis_core/core.py     | 53 +++----------------
 .../menu_visual.py                            |  3 +-
 .../rviz_visualization_menu.py                | 19 ++++---
 3 files changed, 20 insertions(+), 55 deletions(-)

diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
index 0ec6f13..7f70440 100644
--- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
+++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
@@ -25,8 +25,6 @@
 from ikpy import chain
 import os
 
-# TODO : If a workspace calculation is already done, store the results in a file to avoid recalculating it and make only the
-# verification of the payload handling in the workspace.
 
 
 
@@ -189,45 +187,6 @@ def update_configuration(self, q : np.ndarray):
         """
         pin.forwardKinematics(self.model, self.data, q)
         pin.updateFramePlacements(self.model, self.data)
-
-
-    def compute_maximum_payload(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, frame_name : str) -> float:
-        """
-        Compute the forward dynamics acceleration vector.
-        
-        :param q: Joint configuration vector.
-        :param qdot: Joint velocity vector.
-        :param tau: Joint torque vector.
-        :param frame_name: Name of the frame where the force is applied.
-        :return: Acceleration vector.
-        """
-        # TODO : refactor this method
-        
-        # Update the configuration of the robot model
-        self.update_configuration(q)
-
-        # basic idea for forward dynamics: M(q) * a_q + b = tau(q) --> a_q = (tau(q) - b)/ M(q)
-        # with external force, the equation becomes: M(q) * a_q + b = tau(q) + J_traspose(q) * f_ext -- > f_ext = (M(q) * a_q + b - tau(q))/ J_traspose(q)
-
-        qddot0 = np.zeros(self.model.nv) # Initialize acceleration vector
-        
-        # calculate dynamics drift
-        b = pin.rnea(self.model, self.data, q, qdot, qddot0)
-
-        #get jacobian of the frame where the payload is applied
-        J = self.compute_jacobian(q, frame_name)
-
-        tau_r = b - tau
-        
-        try:
-            #a = np.linalg.solve(M, tau - b)
-            # get F = (J_transpose(q))^-1  X ( tau - b ) with b = M(q)*a_q + b || pinv = pseudo-inverse to prevent singularities
-            F_max = np.linalg.pinv(J.T) @ tau_r
-
-        except np.linalg.LinAlgError as e:
-            raise ValueError(f"Failed to solve for acceleration: {e}")
-        
-        return F_max[2] # get the force in z axis of the world frame, which is the maximum force payload
     
 
 
@@ -471,28 +430,30 @@ def get_valid_workspace(self, range : int, resolution : int, end_effector_name_l
             self.configurations_l = self.compute_all_configurations(range,resolution, end_effector_name_left)
             # Compute all configurations within the specified range for the right arm
             self.configurations_r = self.compute_all_configurations(range,resolution, end_effector_name_right)
-            
+        
+        # TODO make here the calculation for the maximum payload for each configuration in order to increase the speed of the verification
+
         # Verify the configurations to check if they are valid for both arms
         valid_configurations = self.verify_configurations(self.configurations_l,self.configurations_r, masses, checked_frames)
         
         return valid_configurations
     
 
-    # TODO : test it
+
     def compute_maximum_payloads(self, configs : np.ndarray):
         """
         Compute the maximum payload for each provided configuration and return the results with the configs updated with the maximum payload as a new value.
         :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "arm", "max_payload" }     
         """
-        # payload limit 
+        # payload limits
         payload_limit = 10
         resolution_payload = 0.01
 
         for config in configs:
              # iterate over a possible set of payloads 
              for mass in np.arange(0, payload_limit , resolution_payload):
-                # Create external forces based on the masses and checked frames TODO change name in arm 7 for example
-                ext_forces = self.create_ext_force(mass, f"gripper_{config['arm']}_finger_joint", config["config"])
+                # Create external forces based on the masses and checked frames
+                ext_forces = self.create_ext_force(mass, f"arm_{config['arm']}_7_joint", config["config"])
                 # calculate the current torques
                 tau = self.compute_inverse_dynamics(config["config"], self.get_zero_velocity(), self.get_zero_acceleration(),extForce=ext_forces)
                 
diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
index e4bcdb5..b35811a 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
@@ -157,7 +157,7 @@ def insert_dropdown_configuration(self, configuration : np.ndarray):
 
         for i, item in enumerate(configuration):
             # insert the parent in the command field to keep track of the parent id
-            last_entry = self.menu_handler.insert(f"Configuration {i} | arm: " + item["arm"], parent=self.workspace_button, command=str(self.workspace_button), callback=self.callback_configuration_selection)
+            last_entry = self.menu_handler.insert(f"Configuration {i} | arm: " + item["arm"] + " | max payload : " + str(item["max_payload"]) , parent=self.workspace_button, command=str(self.workspace_button), callback=self.callback_configuration_selection)
             self.menu_handler.setCheckState(last_entry, MenuHandler.UNCHECKED)
             self.menu_handler.setVisible(last_entry, True)
         
@@ -165,6 +165,7 @@ def insert_dropdown_configuration(self, configuration : np.ndarray):
         self.menu_handler.setVisible(self.torque_limits_checker, True)
         self.menu_handler.setVisible(self.max_torque_checker, True)
         self.menu_handler.setVisible(self.label_color_selection, True)
+        
         self.menu_handler.setCheckState(self.torque_limits_checker, MenuHandler.CHECKED)
         self.menu_handler.setCheckState(self.max_torque_checker, MenuHandler.UNCHECKED)
 
diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
index a6825dd..02f678d 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
@@ -163,7 +163,9 @@ def workspace_calculation(self):
         """
         # if the user choose to compute the workspace area then compute the valid configurations
         if self.menu.get_workspace_state():
-            self.valid_configurations = self.robot.get_valid_workspace(2, 0.20, "gripper_left_finger_joint", "gripper_right_finger_joint", self.masses, self.checked_frames)
+            self.valid_configurations = self.robot.get_valid_workspace(2, 0.20, "arm_left_7_joint", "arm_right_7_joint", self.masses, self.checked_frames)
+
+            self.valid_configurations  = self.robot.compute_maximum_payloads(self.valid_configurations)
             
             # insert the valid configurations in the menu
             self.menu.insert_dropdown_configuration(self.valid_configurations)
@@ -177,7 +179,7 @@ def workspace_calculation(self):
         # if there are valid configurations, publish the workspace area
         if self.valid_configurations is not None:
             # publish the workspace area
-            self.publish_workspace_area(self.valid_configurations)
+            self.publish_workspace_area()
             
             
 
@@ -324,12 +326,10 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray,
         self.publisher_rviz_torque.publish(marker_array)
         
 
-    def publish_workspace_area(self, valid_configs: np.ndarray ):
+    def publish_workspace_area(self):
         """
         Publish the workspace area in RViz using points and labels for the end points.
 
-        Args:
-            valid_configs (np.ndarray): Current valid configurations in the workspace of the robot.
         """
         # Create a MarkerArray to visualize the number of configuration of a specific point in the workspace
         marker_point_names = MarkerArray()
@@ -339,11 +339,14 @@ def publish_workspace_area(self, valid_configs: np.ndarray ):
 
         # calculate the maximum torques for each joint in the current valid configurations for each arm only if the user selected the max current torque visualization
         if self.menu.get_torque_color() == TorqueVisualizationType.MAX_CURRENT_TORQUE:
-            max_torque_for_joint_left, max_torque_for_joint_right = self.robot.get_maximum_torques(valid_configs)
+            max_torque_for_joint_left, max_torque_for_joint_right = self.robot.get_maximum_torques(self.valid_configurations)
+        
+
+        
 
         cont = 0
         # Iterate through the valid configurations and create markers
-        for i, valid_config in enumerate(valid_configs):
+        for i, valid_config in enumerate(self.valid_configurations):
             
             # create the label for the end point (end effector position) of the valid configuration
             marker_point_name = Marker()
@@ -414,7 +417,7 @@ def publish_workspace_area(self, valid_configs: np.ndarray ):
 
 
         # get the unified torque for the valid configurations
-        unified_configurations_torque = self.robot.get_unified_configurations_torque(valid_configs)
+        unified_configurations_torque = self.robot.get_unified_configurations_torque(self.valid_configurations)
 
         # insert points related to the end effector position in the workspace area and with color based on the normalized torque for each configuration
         # this is used to visualize the workspace area with the unified torques for each configuration

From 4f719b5e2a71918c69f919b667affd95021061f5 Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Mon, 14 Jul 2025 14:13:19 +0200
Subject: [PATCH 4/9] fix personal name

---
 dynamic_payload_analysis_core/package.xml | 2 +-
 dynamic_payload_analysis_ros/package.xml  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/dynamic_payload_analysis_core/package.xml b/dynamic_payload_analysis_core/package.xml
index 3b5dfae..faf863a 100644
--- a/dynamic_payload_analysis_core/package.xml
+++ b/dynamic_payload_analysis_core/package.xml
@@ -4,7 +4,7 @@
   <name>dynamic_payload_analysis_core</name>
   <version>0.0.0</version>
   <description>Core package with calculation for torques calculator</description>
-  <maintainer email="enrimoro003@gmail.com">morolinux</maintainer>
+  <maintainer email="enrimoro003@gmail.com">Moro Enrico</maintainer>
   <license>Apache License 2.0</license>
 
   <test_depend>ament_copyright</test_depend>
diff --git a/dynamic_payload_analysis_ros/package.xml b/dynamic_payload_analysis_ros/package.xml
index f39adf4..51ba95e 100644
--- a/dynamic_payload_analysis_ros/package.xml
+++ b/dynamic_payload_analysis_ros/package.xml
@@ -4,7 +4,7 @@
   <name>dynamic_payload_analysis_ros</name>
   <version>0.0.0</version>
   <description>This package provides tools for dynamic payload analysis in robotics with a focus on torque calculations and external force handling.</description>
-  <maintainer email="enrimoro003@gmail.com">morolinux</maintainer>
+  <maintainer email="enrimoro003@gmail.com">Moro Enrico</maintainer>
   <license>Apache License 2.0</license>
 
   <test_depend>ament_copyright</test_depend>

From c9ed0a849ea6af7580ba482f7168ea2527377200 Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Wed, 16 Jul 2025 09:57:06 +0200
Subject: [PATCH 5/9] add method to compute maximum payloads with binary search

---
 .../dynamic_payload_analysis_core/core.py     | 48 +++++++++++--------
 1 file changed, 27 insertions(+), 21 deletions(-)

diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
index 7f70440..bc1502d 100644
--- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
+++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
@@ -439,36 +439,42 @@ def get_valid_workspace(self, range : int, resolution : int, end_effector_name_l
         return valid_configurations
     
 
-
     def compute_maximum_payloads(self, configs : np.ndarray):
         """
         Compute the maximum payload for each provided configuration and return the results with the configs updated with the maximum payload as a new value.
         :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "arm", "max_payload" }     
         """
-        # payload limits
-        payload_limit = 10
-        resolution_payload = 0.01
-
         for config in configs:
-             # iterate over a possible set of payloads 
-             for mass in np.arange(0, payload_limit , resolution_payload):
-                # Create external forces based on the masses and checked frames
-                ext_forces = self.create_ext_force(mass, f"arm_{config['arm']}_7_joint", config["config"])
-                # calculate the current torques
-                tau = self.compute_inverse_dynamics(config["config"], self.get_zero_velocity(), self.get_zero_acceleration(),extForce=ext_forces)
-                
-                if not self.check_effort_limits(tau,config["arm"]).all():
-                    # if the current payload is not valid, the maximum payload is the previous one
-                    config["max_payload"] = mass - resolution_payload
-                    break
-                else:
-                    # if the current payload is valid, update the maximum payload
-                    config["max_payload"] = mass
-            
-
+            config["max_payload"] = self.find_max_payload_binary_search(config, config["arm"], payload_min=0.0, payload_max=10, resolution=0.01)
+        
         return configs
 
 
+    def find_max_payload_binary_search(self, config, arm, payload_min=0.0, payload_max=10.0, resolution=0.01):
+        """
+        Find the maximum payload for a given configuration using binary search.
+        :param config: Configuration dictionary (must contain 'config' key).
+        :param arm: Arm name ('left' or 'right').
+        :param payload_min: Minimum payload to test.
+        :param payload_max: Maximum payload to test.
+        :param resolution: Desired precision.
+        :return: Maximum allowable payload.
+        """
+        low = payload_min
+        high = payload_max
+        max_valid = payload_min
+
+        while high - low > resolution:
+            mid_payload = (low + high) / 2
+            ext_forces = self.create_ext_force(mid_payload, f"arm_{arm}_7_joint", config["config"])
+            tau = self.compute_inverse_dynamics(config["config"], self.get_zero_velocity(), self.get_zero_acceleration(), extForce=ext_forces)
+            if self.check_effort_limits(tau, arm).all():
+                max_valid = mid_payload
+                low = mid_payload
+            else:
+                high = mid_payload
+
+        return max_valid
 
     def compute_forward_dynamics_aba_method(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, extForce : np.ndarray[pin.Force] = None) -> np.ndarray:
         """

From b01622224a2a36876466f123e8de23fcf794c1be Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Wed, 16 Jul 2025 09:57:28 +0200
Subject: [PATCH 6/9] small changes to show maximum payloads

---
 .../dynamic_payload_analysis_ros/menu_visual.py                | 2 +-
 .../dynamic_payload_analysis_ros/rviz_visualization_menu.py    | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
index b35811a..ce6e933 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py
@@ -157,7 +157,7 @@ def insert_dropdown_configuration(self, configuration : np.ndarray):
 
         for i, item in enumerate(configuration):
             # insert the parent in the command field to keep track of the parent id
-            last_entry = self.menu_handler.insert(f"Configuration {i} | arm: " + item["arm"] + " | max payload : " + str(item["max_payload"]) , parent=self.workspace_button, command=str(self.workspace_button), callback=self.callback_configuration_selection)
+            last_entry = self.menu_handler.insert(f"Configuration {i} | arm: " + item["arm"] + " | max payload : " + f"{item['max_payload']:.2f} kg" , parent=self.workspace_button, command=str(self.workspace_button), callback=self.callback_configuration_selection)
             self.menu_handler.setCheckState(last_entry, MenuHandler.UNCHECKED)
             self.menu_handler.setVisible(last_entry, True)
         
diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
index 02f678d..03a5122 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
@@ -165,7 +165,8 @@ def workspace_calculation(self):
         if self.menu.get_workspace_state():
             self.valid_configurations = self.robot.get_valid_workspace(2, 0.20, "arm_left_7_joint", "arm_right_7_joint", self.masses, self.checked_frames)
 
-            self.valid_configurations  = self.robot.compute_maximum_payloads(self.valid_configurations)
+            # compute the maximum payloads for the valid configurations
+            self.valid_configurations = self.robot.compute_maximum_payloads(self.valid_configurations)
             
             # insert the valid configurations in the menu
             self.menu.insert_dropdown_configuration(self.valid_configurations)

From 8689dee310396858f74b27ff092d6ebd0f74b1f0 Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Thu, 17 Jul 2025 16:36:06 +0200
Subject: [PATCH 7/9] added publisher of the maximum payload as point cloud

---
 .../dynamic_payload_analysis_core/core.py     | 38 +++++++--
 .../rviz_visualization_menu.py                | 84 ++++++++++++++++++-
 2 files changed, 111 insertions(+), 11 deletions(-)

diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
index bc1502d..d603d72 100644
--- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
+++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
@@ -445,16 +445,15 @@ def compute_maximum_payloads(self, configs : np.ndarray):
         :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "arm", "max_payload" }     
         """
         for config in configs:
-            config["max_payload"] = self.find_max_payload_binary_search(config, config["arm"], payload_min=0.0, payload_max=10, resolution=0.01)
+            config["max_payload"] = self.find_max_payload_binary_search(config, payload_min=0.0, payload_max=10, resolution=0.01)
         
         return configs
 
 
-    def find_max_payload_binary_search(self, config, arm, payload_min=0.0, payload_max=10.0, resolution=0.01):
+    def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10.0, resolution=0.01):
         """
         Find the maximum payload for a given configuration using binary search.
         :param config: Configuration dictionary (must contain 'config' key).
-        :param arm: Arm name ('left' or 'right').
         :param payload_min: Minimum payload to test.
         :param payload_max: Maximum payload to test.
         :param resolution: Desired precision.
@@ -466,9 +465,9 @@ def find_max_payload_binary_search(self, config, arm, payload_min=0.0, payload_m
 
         while high - low > resolution:
             mid_payload = (low + high) / 2
-            ext_forces = self.create_ext_force(mid_payload, f"arm_{arm}_7_joint", config["config"])
+            ext_forces = self.create_ext_force(mid_payload, f"arm_{config['arm']}_7_joint", config["config"])
             tau = self.compute_inverse_dynamics(config["config"], self.get_zero_velocity(), self.get_zero_acceleration(), extForce=ext_forces)
-            if self.check_effort_limits(tau, arm).all():
+            if self.check_effort_limits(tau, config['arm']).all():
                 max_valid = mid_payload
                 low = mid_payload
             else:
@@ -543,6 +542,19 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda
 
         return max_torques_left, max_torques_right
 
+
+    def get_maximum_payloads(self, valid_configs : np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get the maximum payloads for all configuration in the left and right arm.
+        
+        :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "arm", "max_payload"}].
+        :return: Tuple of arrays of maximum payloads for left and right arms.
+        """
+        
+        max_payload_left = max([config["max_payload"] for config in valid_configs if config["arm"] == "left"])
+        max_payload_right = max([config["max_payload"] for config in valid_configs if config["arm"] == "right"])
+
+        return max_payload_left, max_payload_right
             
         
 
@@ -571,9 +583,21 @@ def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray =
                 else:
                     norm_tau.append(0.0)
 
-
         return norm_tau
-    
+
+
+    def get_normalized_payload(self, payload : np.ndarray, target_payload : float) -> np.ndarray:
+        """
+        Normalize the torques vector to a unified scale.
+        
+        :param payload: Maximum payload for a configuration.
+        :param target_payload: Target payload to normalize the payload to.
+        :return: Normalized payload.
+        """
+        norm_payload = abs(payload) / target_payload
+
+        return norm_payload    
+
 
     def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray:
         """
diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
index 03a5122..2dff63d 100644
--- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
+++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py
@@ -51,6 +51,9 @@ def __init__(self):
         # Pusblisher for point cloud workspace area
         self.publisher_workspace_area = self.create_publisher(MarkerArray, '/workspace_area', 10)
 
+        # Pusblisher for point cloud of maximum payloads in the workspace area
+        self.publisher_maximum_payloads = self.create_publisher(MarkerArray, '/maximum_payloads', 10)
+
         # Publisher for point names in the workspace area
         self.publisher_workspace_area_names = self.create_publisher(MarkerArray, '/workspace_area_names', 10)
 
@@ -181,6 +184,7 @@ def workspace_calculation(self):
         if self.valid_configurations is not None:
             # publish the workspace area
             self.publish_workspace_area()
+            self.publish_maximum_payloads_area()
             
             
 
@@ -342,9 +346,6 @@ def publish_workspace_area(self):
         if self.menu.get_torque_color() == TorqueVisualizationType.MAX_CURRENT_TORQUE:
             max_torque_for_joint_left, max_torque_for_joint_right = self.robot.get_maximum_torques(self.valid_configurations)
         
-
-        
-
         cont = 0
         # Iterate through the valid configurations and create markers
         for i, valid_config in enumerate(self.valid_configurations):
@@ -451,7 +452,82 @@ def publish_workspace_area(self):
         # Publish the marker points and names
         self.publisher_workspace_area.publish(marker_points)
         self.publisher_workspace_area_names.publish(marker_point_names)
-        
+    
+
+    def publish_maximum_payloads_area(self):
+        """
+        Publish the maximum payloads area in RViz using points and labels for the end points.
+        """
+        # Create a MarkerArray to visualize the maximum payloads
+        marker_max_payloads = MarkerArray()
+        marker_label_payloads = MarkerArray()
+
+        # get the maximum payloads for each arm based on the valid configurations
+        max_payload_left, max_payload_right = self.robot.get_maximum_payloads(self.valid_configurations)
+
+        # Iterate through the valid configurations and create markers
+        for i, valid_config in enumerate(self.valid_configurations):
+            
+            # create the label for the end point (end effector position)
+            marker_point_name = Marker()
+            marker_point_name.header.frame_id = "base_link"
+            marker_point_name.header.stamp = self.get_clock().now().to_msg()
+
+            marker_point_name.ns = f"label_payloads_arm_{valid_config['arm']}"
+            marker_point_name.id = i
+            marker_point_name.type = Marker.TEXT_VIEW_FACING
+            marker_point_name.text = f"Config {i} \nMax payload : {valid_config['max_payload']:.2f} kg"
+            marker_point_name.action = Marker.ADD
+            marker_point_name.pose.position.x = valid_config["end_effector_pos"][0]
+            marker_point_name.pose.position.y = valid_config["end_effector_pos"][1]
+            marker_point_name.pose.position.z = valid_config["end_effector_pos"][2]
+            marker_point_name.pose.orientation.w = 1.0
+            marker_point_name.scale.x = 0.02
+            marker_point_name.scale.y = 0.02
+            marker_point_name.scale.z = 0.02
+            marker_point_name.color.a = 1.0  # Alpha
+            marker_point_name.color.r = 1.0  # Red
+            marker_point_name.color.g = 1.0  # Green
+            marker_point_name.color.b = 1.0  # Blue
+            
+            # get the joint positions for the valid configuration
+            joint_positions, offset_z = self.robot.get_joints_placements(valid_config["config"])
+
+            # get the normalized payload for the valid configuration with target as maximum payloads for each arm
+            if valid_config["arm"] == "left":
+                norm_payload = self.robot.get_normalized_payload(valid_config["max_payload"],max_payload_left)
+            else:
+                norm_payload = self.robot.get_normalized_payload(valid_config["max_payload"],max_payload_right)
+             
+            point = Marker()
+            point.header.frame_id = "base_link"
+            point.header.stamp = self.get_clock().now().to_msg()
+            point.ns = f"max_payloads_arm_{valid_config['arm']}"
+            point.id = i
+            point.type = Marker.SPHERE
+            point.action = Marker.ADD
+            point.scale.x = 0.03  # Size of the sphere
+            point.scale.y = 0.03
+            point.scale.z = 0.03
+            
+            point.pose.position.x = valid_config["end_effector_pos"][0]
+            point.pose.position.y = valid_config["end_effector_pos"][1]
+            point.pose.position.z = valid_config["end_effector_pos"][2] - offset_z
+            point.pose.orientation.w = 1.0
+            point.color.a = 1.0  # Alpha
+            point.color.r = norm_payload  # Red
+            point.color.g = 1 - norm_payload  # Green
+            point.color.b = 0.0  # Blue
+            
+            # Add the point to the points array
+            marker_max_payloads.markers.append(point)
+            
+            # Add the marker point name to the marker point names array
+            marker_max_payloads.markers.append(marker_point_name)
+
+        # Publish the maximum payloads markers and labels
+        self.publisher_maximum_payloads.publish(marker_max_payloads)
+
 
     def clear_workspace_area_markers(self):
         """

From f73939b317af591384909079ab2fc786cebbc3dd Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Thu, 17 Jul 2025 17:00:32 +0200
Subject: [PATCH 8/9] Remove files not necessary for PR

---
 doc/README.md                                 |  3 ---
 .../dynamic_payload_analysis_core/__init__.py |  0
 .../resource/dynamic_payload_analysis_core    |  0
 dynamic_payload_analysis_core/setup.cfg       |  4 ---
 dynamic_payload_analysis_core/setup.py        | 25 -----------------
 .../test/test_copyright.py                    | 25 -----------------
 .../test/test_flake8.py                       | 25 -----------------
 .../test/test_pep257.py                       | 23 ----------------
 .../dynamic_payload_analysis_ros/__init__.py  |  0
 .../resource/dynamic_payload_analysis_ros     |  0
 dynamic_payload_analysis_ros/setup.cfg        |  4 ---
 dynamic_payload_analysis_ros/setup.py         | 27 -------------------
 .../test/test_copyright.py                    | 25 -----------------
 .../test/test_flake8.py                       | 25 -----------------
 .../test/test_pep257.py                       | 23 ----------------
 15 files changed, 209 deletions(-)
 delete mode 100644 doc/README.md
 delete mode 100644 dynamic_payload_analysis_core/dynamic_payload_analysis_core/__init__.py
 delete mode 100644 dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core
 delete mode 100644 dynamic_payload_analysis_core/setup.cfg
 delete mode 100644 dynamic_payload_analysis_core/setup.py
 delete mode 100644 dynamic_payload_analysis_core/test/test_copyright.py
 delete mode 100644 dynamic_payload_analysis_core/test/test_flake8.py
 delete mode 100644 dynamic_payload_analysis_core/test/test_pep257.py
 delete mode 100644 dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/__init__.py
 delete mode 100644 dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros
 delete mode 100644 dynamic_payload_analysis_ros/setup.cfg
 delete mode 100644 dynamic_payload_analysis_ros/setup.py
 delete mode 100644 dynamic_payload_analysis_ros/test/test_copyright.py
 delete mode 100644 dynamic_payload_analysis_ros/test/test_flake8.py
 delete mode 100644 dynamic_payload_analysis_ros/test/test_pep257.py

diff --git a/doc/README.md b/doc/README.md
deleted file mode 100644
index 66a490b..0000000
--- a/doc/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# Documentation
-
-This file contains the doucmentation for the user setup and testing anf also to server as a tutorials for the users.
\ No newline at end of file
diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/__init__.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core b/dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core
deleted file mode 100644
index e69de29..0000000
diff --git a/dynamic_payload_analysis_core/setup.cfg b/dynamic_payload_analysis_core/setup.cfg
deleted file mode 100644
index 62c9e13..0000000
--- a/dynamic_payload_analysis_core/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/dynamic_payload_analysis_core
-[install]
-install_scripts=$base/lib/dynamic_payload_analysis_core
diff --git a/dynamic_payload_analysis_core/setup.py b/dynamic_payload_analysis_core/setup.py
deleted file mode 100644
index 70e2a59..0000000
--- a/dynamic_payload_analysis_core/setup.py
+++ /dev/null
@@ -1,25 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'dynamic_payload_analysis_core'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='Enrico Moro',
-    maintainer_email='enrimoro003@gmail.com',
-    description='This package implements core functionalities for dynamic payload analysis in robotics, focusing on torque calculations and external force handling.',
-    license='Apache License 2.0',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-        ],
-    },
-)
diff --git a/dynamic_payload_analysis_core/test/test_copyright.py b/dynamic_payload_analysis_core/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/dynamic_payload_analysis_core/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/dynamic_payload_analysis_core/test/test_flake8.py b/dynamic_payload_analysis_core/test/test_flake8.py
deleted file mode 100644
index 27ee107..0000000
--- a/dynamic_payload_analysis_core/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
diff --git a/dynamic_payload_analysis_core/test/test_pep257.py b/dynamic_payload_analysis_core/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/dynamic_payload_analysis_core/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
diff --git a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/__init__.py b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros b/dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros
deleted file mode 100644
index e69de29..0000000
diff --git a/dynamic_payload_analysis_ros/setup.cfg b/dynamic_payload_analysis_ros/setup.cfg
deleted file mode 100644
index 5a64333..0000000
--- a/dynamic_payload_analysis_ros/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/dynamic_payload_analysis_ros
-[install]
-install_scripts=$base/lib/dynamic_payload_analysis_ros
diff --git a/dynamic_payload_analysis_ros/setup.py b/dynamic_payload_analysis_ros/setup.py
deleted file mode 100644
index 901e1da..0000000
--- a/dynamic_payload_analysis_ros/setup.py
+++ /dev/null
@@ -1,27 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'dynamic_payload_analysis_ros'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='Enrico Moro',
-    maintainer_email='enrimoro003@gmail.com',
-    description='This package provides graphics tools in Rviz for dynamic payload analysis in robotics with a focus on torque calculations and external force handling.',
-    license='Apache License 2.0',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'node_rviz_visualization = dynamic_payload_analysis_ros.rviz_visualization:main',
-            'node_rviz_visualization_menu = dynamic_payload_analysis_ros.rviz_visualization_menu:main',
-        ],
-    },
-)
diff --git a/dynamic_payload_analysis_ros/test/test_copyright.py b/dynamic_payload_analysis_ros/test/test_copyright.py
deleted file mode 100644
index 97a3919..0000000
--- a/dynamic_payload_analysis_ros/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/dynamic_payload_analysis_ros/test/test_flake8.py b/dynamic_payload_analysis_ros/test/test_flake8.py
deleted file mode 100644
index 27ee107..0000000
--- a/dynamic_payload_analysis_ros/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
diff --git a/dynamic_payload_analysis_ros/test/test_pep257.py b/dynamic_payload_analysis_ros/test/test_pep257.py
deleted file mode 100644
index b234a38..0000000
--- a/dynamic_payload_analysis_ros/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'

From 7a659f9294e62e84027942ff4a886aa16b0857db Mon Sep 17 00:00:00 2001
From: Enrico Moro <enrimoro003@gmail.com>
Date: Fri, 18 Jul 2025 08:48:46 +0200
Subject: [PATCH 9/9] Fix type hint variables

---
 .../dynamic_payload_analysis_core/core.py                   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
index d603d72..8e4f475 100644
--- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
+++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py
@@ -95,7 +95,7 @@ def compute_static_collisions(self):
     
         
 
-    def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray[pin.Force] = None) -> np.ndarray:
+    def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray = None) -> np.ndarray:
         """
         Compute the inverse dynamics torque vector.
         
@@ -119,7 +119,7 @@ def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : n
         return tau
     
 
-    def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Union[str | np.ndarray], q : np.ndarray) -> np.ndarray[pin.Force]:
+    def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Union[str | np.ndarray], q : np.ndarray) -> np.ndarray:
         """
         Create external forces vector based on the masses and frame ID.
         The resulting vector will contain the force applied to the specified frame and with the local orientation of the parent joint.
@@ -475,7 +475,7 @@ def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10
 
         return max_valid
 
-    def compute_forward_dynamics_aba_method(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, extForce : np.ndarray[pin.Force] = None) -> np.ndarray:
+    def compute_forward_dynamics_aba_method(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, extForce : np.ndarray = None) -> np.ndarray:
         """
         Compute the forward dynamics acceleration vector with Articulated-Body algorithm(ABA).