From a8c882462aa67168653a0c5f5cb62e2f2bfcfaae Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 14 Jul 2025 08:19:47 +0200 Subject: [PATCH 01/44] 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 Date: Mon, 14 Jul 2025 09:30:32 +0200 Subject: [PATCH 02/44] 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 Date: Mon, 14 Jul 2025 10:28:05 +0200 Subject: [PATCH 03/44] 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 Date: Mon, 14 Jul 2025 14:13:19 +0200 Subject: [PATCH 04/44] 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 @@ dynamic_payload_analysis_core 0.0.0 Core package with calculation for torques calculator - morolinux + Moro Enrico Apache License 2.0 ament_copyright 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 @@ dynamic_payload_analysis_ros 0.0.0 This package provides tools for dynamic payload analysis in robotics with a focus on torque calculations and external force handling. - morolinux + Moro Enrico Apache License 2.0 ament_copyright From c9ed0a849ea6af7580ba482f7168ea2527377200 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 16 Jul 2025 09:57:06 +0200 Subject: [PATCH 05/44] 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 Date: Wed, 16 Jul 2025 09:57:28 +0200 Subject: [PATCH 06/44] 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 Date: Thu, 17 Jul 2025 16:36:06 +0200 Subject: [PATCH 07/44] 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 Date: Thu, 17 Jul 2025 17:00:32 +0200 Subject: [PATCH 08/44] 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 Date: Fri, 18 Jul 2025 08:48:46 +0200 Subject: [PATCH 09/44] 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). From f0a2b23e4eb28e7b064e3b9a66d8bbf29fe0866e Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 21 Jul 2025 10:13:53 +0200 Subject: [PATCH 10/44] added menu for tree selection --- .../menu_visual.py | 113 ++++++++++++++++++ 1 file changed, 113 insertions(+) mode change 100644 => 100755 dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py 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 old mode 100644 new mode 100755 index ce6e933..ce00c66 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -38,6 +38,9 @@ def __init__(self, node): # array to store the checked orunchecked frames and payload selection self.frames_selection = np.array([],dtype=object) + # array to store the selected frame in the subtree menu + self.subtree_selection = np.array([], dtype=object) + # create handler for menu self.menu_handler = MenuHandler() @@ -97,7 +100,47 @@ def insert_frame(self, name): # apply changes self.menu_handler.reApply(self.server) self.server.applyChanges() + + + def insert_subtree(self, tree_identifier : str, joint_names : np.ndarray, joint_ids : np.ndarray): + """ + Insert a new item(subtree) in the checkbox menu of frames. + + Args: + name (str) : name of the subtree + """ + last_item = self.menu_handler.insert(f"Tree {tree_identifier}",command= str(tree_identifier), callback=self.callback_tree_selection) + self.menu_handler.setCheckState(last_item, MenuHandler.UNCHECKED) + self.menu_handler.setVisible(last_item, True) + + joints_list = np.array([], dtype=object) + + + for joint_name,id in zip(joint_names, joint_ids): + # insert the joint as a sub-menu of the subtree + last_entry = self.menu_handler.insert(f"{joint_name}", parent=last_item, command=str(last_item), callback=self.callback_joint_tree_selection) + self.menu_handler.setCheckState(last_entry, MenuHandler.UNCHECKED) + self.menu_handler.setVisible(last_entry, True) + joints_list = np.append(joints_list,{"joint_name" : joint_name, "joint_id" : id}) + + self.subtree_selection = np.append(self.subtree_selection, {"tree" : tree_identifier, "joints" : joints_list, "selected_joint_id": None}) + + # apply changes + self.menu_handler.reApply(self.server) + self.server.applyChanges() + + def callback_tree_selection(self, feedback): + """ + Callback for the tree selection, Made only to avoid the error of missing callback. + + Args: + feedback: Feedback from the menu selection. + """ + pass + + + def callback_label(self, feedback): """ Callback for the label of color selection. Made only to avoid the error of missing callback. @@ -107,6 +150,56 @@ def callback_label(self, feedback): """ pass + def callback_joint_tree_selection(self, feedback): + """ + Callback for the joint selection in the subtree to change the selection of the joints. + + Args: + feedback: Feedback from the menu selection. + """ + # get the handle of the selected item (id) + handle = feedback.menu_entry_id + # get the title of the selected item (it contains joint name) + title = self.menu_handler.getTitle(handle) + + # get the entry object from the menu handler with all the informations about the item (command field is used to store the parent id) + config_context = self.menu_handler.entry_contexts_[handle] + + # get the parent id of the selected joint stored in the command field (parent == tree identifier) + parent_id = int(config_context.command) + # get the entry object of the parent + parent_context = self.menu_handler.entry_contexts_[parent_id] + + # reset all the selections in the tree sub-menu if the joint was already selected + if self.menu_handler.getCheckState(handle) == MenuHandler.CHECKED: + parent_context.check_state = MenuHandler.UNCHECKED + self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED) + + self.update_joint_tree_selection(int(parent_context.command), title, False) + else: + # set the checkbox as checked + parent_context.check_state = MenuHandler.CHECKED + + # reset all the selections in the tree sub-menu + # check if a item is already checked, if so remove it and set to unchecked to prevent multiple joint selection in the same menu + for item in parent_context.sub_entries: + # check if the item is checked + if self.menu_handler.getCheckState(item) == MenuHandler.CHECKED: + # if the configuration is already checked, we need to uncheck it + self.menu_handler.setCheckState(item, MenuHandler.UNCHECKED) + + # set the selected configuration checkbox as checked + self.menu_handler.setCheckState(handle, MenuHandler.CHECKED) + + # set the joint as checked + self.update_joint_tree_selection(int(parent_context.command), title, True) + + # apply changes + self.menu_handler.reApply(self.server) + self.server.applyChanges() + + + def callback_color_selection(self, feedback): """ Callback for torque selection type to change the visualization color in Rviz. @@ -157,6 +250,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 + # TODO 6) : change arm to subtree identifier 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) @@ -318,6 +412,25 @@ def reset_sub_menu_configuration(self): self.selected_configuration = None + def update_joint_tree_selection(self,tree_identifier: int, joint_name : str, value: bool): + """ + Update the state of a joint in the subtree menu. + + Args: + tree_identifier (int): Identifier of the subtree. + joint_name (str): Name of the joint to update. + joint_id (np.ndarray): ID of the joint to update. + value (bool): New state of the joint (checked or unchecked). + """ + + for item in self.subtree_selection: + if item['tree'] == tree_identifier: + for joint in item['joints']: + if joint['joint_name'] == joint_name: + item['selected_joint_id'] = joint['joint_id'] + break + + def update_item(self, name, check: bool): """ Update the state of an item(frame) in the array of selected frames with payload. From 23bc7561754aa2ee731485b8941451cdc86f1d9f Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 21 Jul 2025 11:49:50 +0200 Subject: [PATCH 11/44] small changes to fit the array format --- .../menu_visual.py | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 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 ce00c66..2a3f61f 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -72,9 +72,13 @@ def __init__(self, node): 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) + # set visible false for the torque limits and max torque checkboxes, they will be displayed only when configurations are inserted in the menu 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) + + # label for tree selection + self.label_tree_selection = self.menu_handler.insert('Select end effector point for each tree :', callback=self.callback_tree_selection) self.make_menu_marker('menu_frames') @@ -115,7 +119,6 @@ def insert_subtree(self, tree_identifier : str, joint_names : np.ndarray, joint_ joints_list = np.array([], dtype=object) - for joint_name,id in zip(joint_names, joint_ids): # insert the joint as a sub-menu of the subtree last_entry = self.menu_handler.insert(f"{joint_name}", parent=last_item, command=str(last_item), callback=self.callback_joint_tree_selection) @@ -140,7 +143,6 @@ def callback_tree_selection(self, feedback): pass - def callback_label(self, feedback): """ Callback for the label of color selection. Made only to avoid the error of missing callback. @@ -175,7 +177,7 @@ def callback_joint_tree_selection(self, feedback): parent_context.check_state = MenuHandler.UNCHECKED self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED) - self.update_joint_tree_selection(int(parent_context.command), title, False) + self.update_joint_tree_selection(int(parent_context.command), title) else: # set the checkbox as checked parent_context.check_state = MenuHandler.CHECKED @@ -192,7 +194,7 @@ def callback_joint_tree_selection(self, feedback): self.menu_handler.setCheckState(handle, MenuHandler.CHECKED) # set the joint as checked - self.update_joint_tree_selection(int(parent_context.command), title, True) + self.update_joint_tree_selection(int(parent_context.command), title) # apply changes self.menu_handler.reApply(self.server) @@ -412,7 +414,7 @@ def reset_sub_menu_configuration(self): self.selected_configuration = None - def update_joint_tree_selection(self,tree_identifier: int, joint_name : str, value: bool): + def update_joint_tree_selection(self, tree_identifier: int, joint_name : str): """ Update the state of a joint in the subtree menu. @@ -426,9 +428,12 @@ def update_joint_tree_selection(self,tree_identifier: int, joint_name : str, val for item in self.subtree_selection: if item['tree'] == tree_identifier: for joint in item['joints']: - if joint['joint_name'] == joint_name: + if joint['joint_name'] == joint_name and item['selected_joint_id'] != joint['joint_id']: item['selected_joint_id'] = joint['joint_id'] break + elif joint['joint_name'] == joint_name and item['selected_joint_id'] == joint['joint_id']: + item['selected_joint_id'] = None + break def update_item(self, name, check: bool): @@ -522,6 +527,14 @@ def update_payload_selection(self, feedback): self.menu_handler.reApply(self.server) self.server.applyChanges() + def get_joint_tree_selection(self) -> np.ndarray: + """ + Return the selected joint in the subtree menu. + + Returns: + np.ndarray: Array of selected joints in the subtree menu. + """ + return self.subtree_selection def get_torque_color(self) -> TorqueVisualizationType: """ From 2299bb67f18ca41fbae636e3a1063195dfd85bb2 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 21 Jul 2025 17:16:47 +0200 Subject: [PATCH 12/44] Added first version with tree selection --- .../dynamic_payload_analysis_core/core.py | 111 +++++++++++++++--- .../rviz_visualization_menu.py | 63 +++++----- 2 files changed, 133 insertions(+), 41 deletions(-) mode change 100644 => 100755 dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py mode change 100644 => 100755 dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/rviz_visualization_menu.py diff --git a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py old mode 100644 new mode 100755 index 8e4f475..1333b14 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -65,9 +65,17 @@ def __init__(self, robot_description : Union[str, Path]): # get the default collisions in the robot model to avoid take them into account in the computations self.default_collisions = self.compute_static_collisions() - # create array to store all possible configurations for left and right arms - self.configurations_l = None - self.configurations_r = None + # compute main trees of the robot model + self.root_joints = ["arm_left_1_joint", "arm_right_1_joint", "leg_left_1_joint", "leg_right_1_joint", "head_1_joint"] + self.compute_sub_trees() + + + # array to store all configurations for the robot model + self.configurations = np.array([], dtype=object) + + # create items for each tree in the robot model + for tree in self.sub_trees: + self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "joint_selected_id" : None}) def compute_static_collisions(self): @@ -93,6 +101,54 @@ def compute_static_collisions(self): return collision_pairs + def compute_sub_trees(self): + """ + Compute the sub-trees of the robot model. + This method is used to compute the sub-trees of the robot model + """ + + # array to store the sub-trees + trees = np.array([], dtype=object) + + for sub_tree in self.model.subtrees: + # get the root joint ID of the sub-tree + root_joint_id = sub_tree[0] + # get the joint names in the sub-tree + joint_names = [self.model.names[joint_id] for joint_id in sub_tree] + # get the joint IDs in the sub-tree + joint_ids = [self.model.getJointId(joint_name) for joint_name in joint_names] + # append the sub-tree to the array + trees = np.append(trees, {"joint_names": joint_names, "joint_ids": joint_ids, "root_joint_id": root_joint_id}) + + self.sub_trees = self.compute_main_trees(trees) + + + + def compute_main_trees(self, trees: np.ndarray) -> np.ndarray: + """ + Compute the main trees of the robot model based on the sub-trees. + + :param trees: Array of sub-trees of the robot model. + :return: Array of main trees of the robot model. + """ + + # array to store the main trees + main_trees = np.array([], dtype=object) + + # remove first element of sub_trees because it is the root of the whole robot model + #sub_trees = sub_trees[1:] + + root_joint_ids = [self.model.getJointId(joint_name) for joint_name in self.root_joints] + + # Iterate over all joints in the model + id = 0 + for tree in trees: + if tree["root_joint_id"] in root_joint_ids: + main_trees = np.append(main_trees, {"tree_id": id, "joint_names": tree['joint_names'], "joint_ids": tree['joint_ids'],"root_joint_id": tree["root_joint_id"], "selected_joint_id": None}) + id += 1 + + + return main_trees def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray = None) -> np.ndarray: @@ -291,7 +347,7 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n - def compute_all_configurations(self, range : int, resolution : int, end_effector_name : str) -> np.ndarray: + def compute_all_configurations(self, range : int, resolution : int) -> np.ndarray: """ Compute all configurations for the robot model within a specified range. @@ -356,7 +412,7 @@ def verify_configurations(self, configurations_left : np.ndarray, configurations # Compute the inverse dynamics for the current configuration without external forces tau = self.compute_inverse_dynamics(q["config"], self.get_zero_velocity(), self.get_zero_acceleration()) - # Check if the torques are within the effort limits + # Check if the torques are within the effort limits # TODO 2) : change it to calculate the efort limits of the subtree if self.check_effort_limits(tau,"left").all(): valid = True # Compute all the collisions @@ -373,6 +429,7 @@ def verify_configurations(self, configurations_left : np.ndarray, configurations break if valid: + # TODO 4) : change arm to subtree identifier valid_configurations.append({"config" : q["config"], "end_effector_pos" : q["end_effector_pos"], "tau" : tau, "arm" : "left" }) # check valid configurations for right arm @@ -412,7 +469,7 @@ def verify_configurations(self, configurations_left : np.ndarray, configurations return np.array(valid_configurations, dtype=object) - def get_valid_workspace(self, range : int, resolution : int, end_effector_name_left : str, end_effector_name_right, masses : np.ndarray, checked_frames: np.ndarray) -> np.ndarray: + def get_valid_workspace(self, range : int, resolution : int, masses : np.ndarray, checked_frames: np.ndarray) -> np.ndarray: """ Get the valid workspace of the robot model by computing all configurations within a specified range. @@ -424,14 +481,15 @@ def get_valid_workspace(self, range : int, resolution : int, end_effector_name_l :param checked_frames (np.ndarray): Array of frame names where the external forces are applied. :return: Array of valid configurations that achieve the desired end effector position in format: [{"config", "end_effector_pos, "tau", "arm"}]. """ - # compute all configurations for the left and right arms if they are not already computed - if self.configurations_l is None or self.configurations_r is None: - # Compute all configurations within the specified range for the left arm - 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 + # compute all configurations for the selected trees + for tree in self.sub_trees: + pass + + + if self.configurations is None: + # TODO 1) : Instead of end_effector_name_left and end_effector_name_right, use the checked names provided by the main trees object + # Compute all configurations + self.configurations = self.compute_all_configurations(range,resolution) # 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) @@ -465,6 +523,7 @@ def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10 while high - low > resolution: mid_payload = (low + high) / 2 + # TODO 5): remove hardcoded arm frame, use selected joint of the subtree submenu 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, config['arm']).all(): @@ -519,6 +578,15 @@ def compute_jacobian(self, q : np.ndarray, frame_name : str) -> np.ndarray: return J_frame + def get_subtrees(self) -> np.ndarray: + """ + Get the sub-trees of the robot model. + + :return: Array of sub-trees of the robot model. + """ + return self.sub_trees + + def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: """ Get the maximum torques for each joint in all valid configurations. @@ -735,6 +803,7 @@ def check_joint_limits(self, q : np.ndarray) -> np.ndarray: return within_limits + # TODO 3) : change it, instead of arm name use sub tree identifier def check_effort_limits(self, tau : np.ndarray, arm : str = None) -> np.ndarray: """ Check if the torques vector is within the effort limits of the robot model. @@ -780,6 +849,20 @@ def check_effort_limits(self, tau : np.ndarray, arm : str = None) -> np.ndarray: return within_limits + def set_joint_tree_selection(self, tree_id : int, joint_id: int): + """ + Set the joint tree selection for the robot model. + + :param tree_id: ID of the tree to select. + :param joint_id: ID of the joint to select. + """ + for tree in self.sub_trees: + if tree["tree_id"] == tree_id: + # Set the selected joint in the tree + tree["selected_joint_id"] = joint_id + return + + def set_position(self, pos_joints : list[float], name_positions : list[str] ) -> 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 old mode 100644 new mode 100755 index 2dff63d..ea9730a --- 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 @@ -24,7 +24,7 @@ from visualization_msgs.msg import Marker, MarkerArray from dynamic_payload_analysis_ros.menu_visual import MenuPayload from dynamic_payload_analysis_ros.menu_visual import TorqueVisualizationType - +from builtin_interfaces.msg import Time @@ -82,13 +82,13 @@ def __init__(self): self.selected_configuration = None # timer to compute the valid workspace area - self.timer_workspace_calculation = self.create_timer(1, self.workspace_calculation) + self.timer_workspace_calculation = self.create_timer(1.0, self.workspace_calculation) # timer to publish the selected configuration in joint states - self.timer_publish_configuration = self.create_timer(2, self.publish_selected_configuration) + self.timer_publish_configuration = self.create_timer(2.0, self.publish_selected_configuration) # timer to calculate the external forces based on the checked frames in the menu - self.timer_update_checked_frames = self.create_timer(0.5, self.update_checked_frames) + self.timer_update_checked_items = self.create_timer(0.5, self.update_checked_items) # timer to publish the external forces as arrows in RViz - self.timer_publish_force = self.create_timer(1, self.publish_payload_force) + self.timer_publish_force = self.create_timer(1.0, self.publish_payload_force) self.get_logger().info('Robot description subscriber node started') @@ -108,6 +108,10 @@ def robot_description_callback(self, msg): # Add the frame to the menu for payload selection for frame in self.robot.get_active_frames(): self.menu.insert_frame(frame) + + # Add subtree to the menu + for i, subtree in enumerate(self.robot.get_subtrees()): + self.menu.insert_subtree(subtree['tree_id'], subtree["joint_names"], subtree["joint_ids"]) # self.robot.print_configuration() @@ -166,7 +170,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, "arm_left_7_joint", "arm_right_7_joint", self.masses, self.checked_frames) + # TODO : Change parameters to be more general: instead of names, use selected joints of the + # checkbox menu for the subtree + self.valid_configurations = self.robot.get_valid_workspace(2, 0.20, self.masses, self.checked_frames) # compute the maximum payloads for the valid configurations self.valid_configurations = self.robot.compute_maximum_payloads(self.valid_configurations) @@ -220,9 +226,9 @@ def publish_selected_configuration(self): self.publisher_joint_states.publish(joint_state) - def update_checked_frames(self): + def update_checked_items(self): """ - Function to update the external forces based on the checked frames in the menu. + Function to update the external forces based on the checked frames in the menu and joint selection in the subtree menu. """ # create the array with only the checked frames (with external force applied) self.checked_frames = np.array([check_frame["name"] for check_frame in self.menu.get_item_state() if check_frame['checked']]) @@ -234,6 +240,12 @@ def update_checked_frames(self): # if there are no checked frames, set the masses to None self.masses = None + selected_joint_ids = [[joint["tree"], joint["selected_joint_id"]] for joint in self.menu.get_joint_tree_selection()] + for tree_id, joint_id in selected_joint_ids: + # set the joint tree selection in the robot based on the selected joints in the menu + self.robot.set_joint_tree_selection(tree_id, joint_id) + + def joint_states_callback(self, msg): """ @@ -299,7 +311,7 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, if "gripper" not in joint['name']: marker = Marker() marker.header.frame_id = "base_link" - marker.header.stamp = self.get_clock().now().to_msg() + marker.header.stamp = Time() marker.ns = "torque_visualization" marker.id = i marker.type = Marker.TEXT_VIEW_FACING @@ -344,7 +356,7 @@ def publish_workspace_area(self): # 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(self.valid_configurations) + max_torque_for_joint = self.robot.get_maximum_torques(self.valid_configurations) cont = 0 # Iterate through the valid configurations and create markers @@ -353,9 +365,9 @@ def publish_workspace_area(self): # create the label for the end point (end effector position) of the valid configuration 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.header.stamp = Time() - marker_point_name.ns = f"workspace_area_{valid_config['arm']}_arm" + marker_point_name.ns = f"workspace_area__tree_{valid_config['tree_id']}" marker_point_name.id = i + 1 marker_point_name.type = Marker.TEXT_VIEW_FACING marker_point_name.text = f"Config {i}" @@ -380,15 +392,13 @@ def publish_workspace_area(self): norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"]) else: # if the user selected the max torque, use the maximum torques for the joint - if valid_config["arm"] == "left": - norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"],max_torque_for_joint_left) - else: - norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"],max_torque_for_joint_right) + norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"],max_torque_for_joint, valid_config["tree_id"]) # insert points related to the end effector position in the workspace area and with color based on the normalized torques for each joint for joint_pos,tau in zip(joint_positions,norm_joints_torques): # print only the points for the corrisponding arm, in this way we can visualize the workspace area for each arm separately and avoid confusion - if valid_config["arm"] in joint_pos["name"] : + # TODO + if self.robot.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): point = Marker() point.header.frame_id = "base_link" point.header.stamp = self.get_clock().now().to_msg() @@ -426,8 +436,8 @@ def publish_workspace_area(self): for i, unified_config in enumerate(unified_configurations_torque): marker_point = Marker() marker_point.header.frame_id = "base_link" - marker_point.header.stamp = self.get_clock().now().to_msg() - marker_point.ns = f"unified_torque_workspace_{unified_config['arm']}_arm" + marker_point.header.stamp = Time() + marker_point.ns = f"unified_torque_workspace_tree_{unified_config['tree_id']}" marker_point.id = i marker_point.type = Marker.SPHERE marker_point.action = Marker.ADD @@ -463,7 +473,7 @@ def publish_maximum_payloads_area(self): 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) + max_payloads = 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): @@ -473,7 +483,7 @@ def publish_maximum_payloads_area(self): 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.ns = f"label_payloads_tree_{valid_config['tree_id']}" 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" @@ -493,16 +503,15 @@ def publish_maximum_payloads_area(self): # 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) + # get the normalized payload for the valid configuration with target as maximum payloads for each tree + max_payload_for_tree = next(payload["max_payload"] for payload in max_payloads if payload["tree_id"] == valid_config["tree_id"]) + norm_payload = self.robot.get_normalized_payload(valid_config["max_payload"], max_payload_for_tree) + 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.ns = f"max_payloads_tree_{valid_config['tree_id']}" point.id = i point.type = Marker.SPHERE point.action = Marker.ADD From fce8be0fbfe4840aaf7217286c8141fbe44bf97f Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 23 Jul 2025 08:37:59 +0200 Subject: [PATCH 13/44] modify core for trees handling --- .../dynamic_payload_analysis_core/core.py | 234 ++++++++++-------- 1 file changed, 131 insertions(+), 103 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 1333b14..280b27d 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -75,7 +75,7 @@ def __init__(self, robot_description : Union[str, Path]): # create items for each tree in the robot model for tree in self.sub_trees: - self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "joint_selected_id" : None}) + self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) def compute_static_collisions(self): @@ -280,7 +280,7 @@ def compute_inverse_kinematics_ikpy(self, q : np.ndarray, end_effector_position: - def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.ndarray, end_effector_name : str) -> np.ndarray: + def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.ndarray, joint_id : str) -> np.ndarray: """ Compute the inverse kinematics for the robot model with joint limits consideration. :param q: current joint configuration vector. @@ -289,8 +289,6 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n :return: Joint configuration vector that achieves the desired end effector position. """ - joint_id = self.model.getJointId(end_effector_name) # Get the joint ID of the end effector - # Set parameters for the inverse kinematics solver eps = 1e-3 # reduce for more precision IT_MAX = 500 # Maximum number of iterations @@ -347,13 +345,13 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n - def compute_all_configurations(self, range : int, resolution : int) -> np.ndarray: + def compute_all_configurations(self, range : int, resolution : int, end_joint_id) -> np.ndarray: """ Compute all configurations for the robot model within a specified range. :param range (int): Range as side of a square where in the center there is the actual position of end effector. :param resolution (int): Resolution of the grid to compute configurations. - :param end_effector_name (str): Name of the end effector joint. + :param end_joint_id (str): Id of the end effector joint selected in the tree. :return : Array of joint configurations that achieve the desired end effector position. """ @@ -375,7 +373,7 @@ def compute_all_configurations(self, range : int, resolution : int) -> np.ndarra for y in np.arange(-range/2, range/2 , resolution): for z in np.arange(0, range , resolution): target_position = pin.SE3(np.eye(3), np.array([x, y, z])) - new_q = self.compute_inverse_kinematics(q, target_position, end_effector_name) + new_q = self.compute_inverse_kinematics(q, target_position, end_joint_id) if new_q is not None: q = new_q @@ -386,20 +384,22 @@ def compute_all_configurations(self, range : int, resolution : int) -> np.ndarra - def verify_configurations(self, configurations_left : np.ndarray, configurations_right : np.ndarray, masses : np.ndarray, checked_frames : np.ndarray) -> np.ndarray: + def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, checked_frames : np.ndarray, tree_id: int, selected_joint_id: int) -> np.ndarray: """ Verify the configurations to check if they are valid. - :param configurations_left: Array of joint configurations to verify for the left arm. - :param configurations_right: Array of joint configurations to verify for the right arm. - :param ext_forces: Array of external forces to apply to the robot model. + :param configurations: Array of joint configurations to verify for the left arm. + :param masses (np.ndarray): Array of masses to apply to the robot model. + :param checked_frames (np.ndarray): Array of frame names where the external forces are applied. + :param tree_id (int): Identifier of the tree to verify the configurations for. + :param selected_joint_id (int): Identifier of the selected joint in the tree to verify the configurations for. :return: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau"}]. """ valid_configurations = [] # check valid configurations for left arm - for q in configurations_left: + for q in configurations: # Update the configuration of the robot model self.update_configuration(q["config"]) @@ -413,7 +413,7 @@ def verify_configurations(self, configurations_left : np.ndarray, configurations tau = self.compute_inverse_dynamics(q["config"], self.get_zero_velocity(), self.get_zero_acceleration()) # Check if the torques are within the effort limits # TODO 2) : change it to calculate the efort limits of the subtree - if self.check_effort_limits(tau,"left").all(): + if self.check_effort_limits(tau= tau, tree_id= tree_id).all(): valid = True # Compute all the collisions pin.computeCollisions(self.model, self.data, self.geom_model, self.geom_data, q["config"], False) @@ -430,41 +430,8 @@ def verify_configurations(self, configurations_left : np.ndarray, configurations if valid: # TODO 4) : change arm to subtree identifier - valid_configurations.append({"config" : q["config"], "end_effector_pos" : q["end_effector_pos"], "tau" : tau, "arm" : "left" }) - - # check valid configurations for right arm - for q in configurations_right: - # Update the configuration of the robot model - self.update_configuration(q["config"]) - - if masses is not None and checked_frames is not None: - # Create external forces based on the masses and checked frames - ext_forces = self.create_ext_force(masses, checked_frames, q["config"]) - # Compute the inverse dynamics for the current configuration - tau = self.compute_inverse_dynamics(q["config"], self.get_zero_velocity(), self.get_zero_acceleration(),extForce=ext_forces) - else: - # Compute the inverse dynamics for the current configuration without external forces - tau = self.compute_inverse_dynamics(q["config"], self.get_zero_velocity(), self.get_zero_acceleration()) + valid_configurations.append({"config" : q["config"], "end_effector_pos" : q["end_effector_pos"], "tau" : tau, "tree_id" : tree_id,"selected_joint_id": selected_joint_id}) - # Check if the torques are within the effort limits - if self.check_effort_limits(tau,"right").all(): - valid = True - # Compute all the collisions - pin.computeCollisions(self.model, self.data, self.geom_model, self.geom_data, q["config"], False) - - # Print the status of collision for all collision pairs - for k in range(len(self.geom_model.collisionPairs)): - cr = self.geom_data.collisionResults[k] - cp = self.geom_model.collisionPairs[k] - - if cr.isCollision() and (cp.first, cp.second, k) not in self.default_collisions: - print(f"Collision detected between {cp.first} and {cp.second} in the right arm configuration.") - valid = False - break - - if valid: - valid_configurations.append({"config" : q["config"], "end_effector_pos" : q["end_effector_pos"], "tau" : tau, "arm" : "right" }) - return np.array(valid_configurations, dtype=object) @@ -475,24 +442,22 @@ def get_valid_workspace(self, range : int, resolution : int, masses : np.ndarray :param range (int): Range as side of a square where in the center there is the actual position of end effector. :param resolution (int): Resolution of the grid to compute configurations. - :param end_effector_name_left (str): Name of the end effector joint to test for the left arm. - :param end_effector_name_right (str): Name of the end effector joint to test for the right arm. :param masses (np.ndarray): Array of masses to apply to the robot model. :param checked_frames (np.ndarray): Array of frame names where the external forces are applied. :return: Array of valid configurations that achieve the desired end effector position in format: [{"config", "end_effector_pos, "tau", "arm"}]. """ - # compute all configurations for the selected trees - for tree in self.sub_trees: - pass - + # compute all configurations for the selected joints of the trees + for tree,configuration in zip(self.sub_trees,self.configurations): + # if the configurations are not computed or the selected joint ID is not the same as in the tree, compute the configurations + if configuration["configurations"] == None or configuration["selected_joint_id"] != tree["selected_joint_id"]: + if tree["selected_joint_id"] is not None: + # Compute all configurations for the current tree + configuration["configurations"] = self.compute_all_configurations(range, resolution,tree["selected_joint_id"]) + # Set the selected joint ID to the current tree's selected joint ID + configuration["selected_joint_id"] = tree["selected_joint_id"] - if self.configurations is None: - # TODO 1) : Instead of end_effector_name_left and end_effector_name_right, use the checked names provided by the main trees object - # Compute all configurations - self.configurations = self.compute_all_configurations(range,resolution) - - # 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) + # Verify the configurations to check if they are valid + valid_configurations = self.verify_configurations(configuration["configurations"], masses, checked_frames, tree["tree_id"], tree["selected_joint_id"]) return valid_configurations @@ -524,9 +489,9 @@ def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10 while high - low > resolution: mid_payload = (low + high) / 2 # TODO 5): remove hardcoded arm frame, use selected joint of the subtree submenu - ext_forces = self.create_ext_force(mid_payload, f"arm_{config['arm']}_7_joint", config["config"]) + ext_forces = self.create_ext_force(mid_payload, self.get_joint_name(config["selected_joint_id"]), 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, config['arm']).all(): + if self.check_effort_limits(tau, config['tree_id']).all(): max_valid = mid_payload low = mid_payload else: @@ -578,6 +543,19 @@ def compute_jacobian(self, q : np.ndarray, frame_name : str) -> np.ndarray: return J_frame + def verify_member_tree(self, tree_id: int, joint_id: int) -> bool: + """ + Verify if the given joint ID is a member of the specified tree. + + :param tree_id: Identifier of the tree to verify. + :param joint_id: Identifier of the joint to verify. + :return: True if the joint is a member of the tree, False otherwise. + """ + + # Check if the joint ID is in the list of joint IDs for the specified tree + return joint_id in self.sub_trees[tree_id]["joint_ids"] + + def get_subtrees(self) -> np.ndarray: """ Get the sub-trees of the robot model. @@ -592,41 +570,57 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda Get the maximum torques for each joint in all valid configurations. :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau"}]. - :return: Arrays of maximum torques for each joint in the current valid configurations for left and right arm. + :return: Arrays of maximum torques for each joint in the current valid configurations for selected trees. """ - + # TODO : Change variable names # Get the number of joints num_joints = len(valid_configs[0]["tau"]) - max_torques_left = np.array([], dtype=float) - max_torques_right = np.array([], dtype=float) + max_torques = np.array([], dtype=float) + joint_torques = np.array([], dtype=object) + abs_torques = np.array([], dtype=object) + selected_trees = [tree for tree in self.sub_trees if tree["selected_joint_id"] is not None] # Find maximum absolute torque for each joint - for i in range(num_joints): - joint_torques_left = [abs(config["tau"][i]) for config in valid_configs if config["arm"] == "left"] - joint_torques_right = [abs(config["tau"][i]) for config in valid_configs if config["arm"] == "right"] + for tree in selected_trees: + for i in range(num_joints): + # Get the joint torques for the current tree + abs_torques = np.append(abs_torques ,{"abs": [abs(config["tau"][i]) for config in valid_configs if config["tree_id"] == tree["tree_id"]]}) + + joint_torques = np.append(joint_torques, {"tree_id": tree["tree_id"], "abs_torques": abs_torques}) + + + for torques in joint_torques: + max_tau = np.array([], dtype=object) + # Get the maximum absolute torque for the current joint + for tau in torques["abs_torques"]: + max_tau = np.append(max_tau, max(tau["abs"])) - max_torques_left = np.append( max_torques_left, max(joint_torques_left)) - max_torques_right = np.append( max_torques_right, max(joint_torques_right)) + max_torques = np.append(max_torques, {"tree_id": torques["tree_id"], "max_values": max_tau}) + - return max_torques_left, max_torques_right + return max_torques - def get_maximum_payloads(self, valid_configs : np.ndarray) -> tuple[np.ndarray, np.ndarray]: + def get_maximum_payloads(self, valid_configs : 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"]) + max_payloads = np.array([], dtype=float) + for tree in self.sub_trees: + payloads = [config["max_payload"] for config in valid_configs if config["tree_id"] == tree["tree_id"]] + if payloads: + max_payload = max(payloads) + max_payloads = np.append(max_payloads, {"tree_id": tree["tree_id"], "max_payload": max_payload}) - return max_payload_left, max_payload_right + return max_payloads - + + - def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = None) -> np.ndarray: + def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = None, tree_id: int = None) -> np.ndarray: """ Normalize the torques vector to a unified scale. @@ -644,10 +638,12 @@ def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = for i, torque in enumerate(tau): norm_tau.append(abs(torque) / self.model.effortLimit[i]) else: + # get the maximum values for the current tree + max_torque = next(item for item in target_torque if item["tree_id"] == tree_id) # Normalize the torques vector to the target torque for i, torque in enumerate(tau): - if target_torque[i] != 0: - norm_tau.append(abs(torque) / target_torque[i]) + if max_torque["max_values"][i] != 0: + norm_tau.append(abs(torque) / max_torque["max_values"][i]) else: norm_tau.append(0.0) @@ -674,8 +670,12 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd :param q: Joint configuration vector. :param valid_configs: Array of """ + torques_sum = np.array([], dtype=float) norm_torques = np.array([], dtype=float) + + # array to store the maximum sum of torques for each tree + max_value_torques = np.array([], dtype=float) sum = 0.0 for valid_config in valid_configs: @@ -688,22 +688,33 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd #if abs(torque) < 50: sum += abs(torque) - torques_sum = np.append(torques_sum, {"sum" : sum, "end_effector_pos" : valid_config["end_effector_pos"], "arm" : valid_config["arm"]}) + torques_sum = np.append(torques_sum, {"sum" : sum, "end_effector_pos" : valid_config["end_effector_pos"], "tree_id" : valid_config["tree_id"]}) sum = 0.0 # reset the sum for the next configuration - # get the maximum torque from the sum of torques for both arms - max_torque_l = max([item["sum"] for item in torques_sum if item["arm"] == "left"]) - max_torque_r = max([item["sum"] for item in torques_sum if item["arm"] == "right"]) + + # get the maximum torque from the sum of torques for all selected trees + for tree in self.sub_trees: + # Get all sum values for the current tree + tree_sums = [item["sum"] for item in torques_sum if item["tree_id"] == tree["tree_id"]] + + if tree_sums: # Check if there are any sums for this tree + max_value = max(tree_sums) + else: + max_value = 0.0 # Default value if no sums found + + max_value_torques = np.append(max_value_torques, {"tree_id": tree["tree_id"], "max_value": max_value}) + # Normalize the torques vector to a unified scale for tau in torques_sum: - if tau["arm"] == "left": - norm_tau = tau["sum"] / max_torque_l - else: - norm_tau = tau["sum"] / max_torque_r + + # Find the corresponding max_value for the current tree_id + max_value = next(item["max_value"] for item in max_value_torques if item["tree_id"] == tau["tree_id"]) + norm_tau = tau["sum"] / max_value + # append the normalized torque to the array - norm_torques = np.append(norm_torques, {"norm_tau" : norm_tau, "end_effector_pos" : tau["end_effector_pos"], "arm" : tau["arm"]}) + norm_torques = np.append(norm_torques, {"norm_tau" : norm_tau, "end_effector_pos" : tau["end_effector_pos"], "tree_id" : tau["tree_id"]}) return norm_torques @@ -804,12 +815,12 @@ def check_joint_limits(self, q : np.ndarray) -> np.ndarray: # TODO 3) : change it, instead of arm name use sub tree identifier - def check_effort_limits(self, tau : np.ndarray, arm : str = None) -> np.ndarray: + def check_effort_limits(self, tau : np.ndarray, tree_id : int = None) -> np.ndarray: """ Check if the torques vector is within the effort limits of the robot model. :param tau: Torques vector to check. - :param arm: Arm name to filter the torques vector, right or left. + :param tree_id: Id of tree to filter control in the torques vector. :return: Array of booleans indicating if each joint is within the effort limits. """ if tau is None: @@ -819,7 +830,7 @@ def check_effort_limits(self, tau : np.ndarray, arm : str = None) -> np.ndarray: within_limits = np.array([], dtype=bool) # if arm is not specified, check all joints - if arm is None: + if tree_id is None: # Check if the torques are within the limits for i in range(self.model.njoints -1): if abs(tau[i]) > self.model.effortLimit[i]: @@ -832,15 +843,21 @@ def check_effort_limits(self, tau : np.ndarray, arm : str = None) -> np.ndarray: print("All joints are within effort limits. \n") else: - # Check if the torques are within the limits - for i in range(self.model.njoints -1): - # TODO arm is "left" or right" but the model has gripper with left and right in the same name - if arm in self.model.names[i+1]: - if abs(tau[i]) > self.model.effortLimit[i]: - print(f"\033[91mJoint {i+1} exceeds effort limit: {tau[i]} > {self.model.effortLimit[i]} \033[0m\n") - within_limits = np.append(within_limits, False) - else: - within_limits = np.append(within_limits, True) + # Check if the torque of joints inside the tree is within the limits + for id in self.sub_trees[tree_id]["joint_ids"]: + if abs(tau[id]) > self.model.effortLimit[id]: + print(f"\033[91mJoint {i+1} exceeds effort limit: {tau[i]} > {self.model.effortLimit[i]} \033[0m\n") + within_limits = np.append(within_limits, False) + else: + within_limits = np.append(within_limits, True) + + # for i in range(self.model.njoints -1): + # if arm in self.model.names[i+1]: + # if abs(tau[i]) > self.model.effortLimit[i]: + # print(f"\033[91mJoint {i+1} exceeds effort limit: {tau[i]} > {self.model.effortLimit[i]} \033[0m\n") + # within_limits = np.append(within_limits, False) + # else: + # within_limits = np.append(within_limits, True) if np.all(within_limits): print("All joints are within effort limits. \n") @@ -939,7 +956,8 @@ def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: self.update_configuration(q) placements = np.array([({"name" : self.model.names[i], - "type" : self.model.joints[i].shortname() , + "type" : self.model.joints[i].shortname() , + "id" : i, "x": self.data.oMi[i].translation[0], "y": self.data.oMi[i].translation[1], "z": self.data.oMi[i].translation[2]}) for i in range(1, self.model.njoints)], dtype=object) @@ -947,6 +965,16 @@ def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: return placements, offset_z + def get_joint_name(self, id_joint: int) -> np.ndarray: + """ + Get the name of the joint by its ID. + + :param id_joint: ID of the joint to get the name for. + :return: Name of the joint. + """ + + return self.model.names[id_joint] + def get_joint_placement(self, joint_id : int) -> dict: """ Get the placement of a specific joint in the robot model. From 6484f7972dc54fc1c7efdb5a1ca02b7b02833f61 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 23 Jul 2025 14:44:09 +0200 Subject: [PATCH 14/44] small changes for new version with tree selection --- .../dynamic_payload_analysis_core/core.py | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 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 280b27d..475c36e 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -135,19 +135,15 @@ def compute_main_trees(self, trees: np.ndarray) -> np.ndarray: # array to store the main trees main_trees = np.array([], dtype=object) - # remove first element of sub_trees because it is the root of the whole robot model - #sub_trees = sub_trees[1:] - root_joint_ids = [self.model.getJointId(joint_name) for joint_name in self.root_joints] # Iterate over all joints in the model id = 0 for tree in trees: if tree["root_joint_id"] in root_joint_ids: - main_trees = np.append(main_trees, {"tree_id": id, "joint_names": tree['joint_names'], "joint_ids": tree['joint_ids'],"root_joint_id": tree["root_joint_id"], "selected_joint_id": None}) + main_trees = np.append(main_trees, {"tree_id": id, "joint_names": tree['joint_names'], "joint_ids": tree['joint_ids'], "root_joint_id": tree["root_joint_id"], "selected_joint_id": None}) id += 1 - return main_trees @@ -393,7 +389,7 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, :param checked_frames (np.ndarray): Array of frame names where the external forces are applied. :param tree_id (int): Identifier of the tree to verify the configurations for. :param selected_joint_id (int): Identifier of the selected joint in the tree to verify the configurations for. - :return: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau"}]. + :return: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "tree_id","selected_joint_id" }]. """ valid_configurations = [] @@ -412,7 +408,7 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, # Compute the inverse dynamics for the current configuration without external forces tau = self.compute_inverse_dynamics(q["config"], self.get_zero_velocity(), self.get_zero_acceleration()) - # Check if the torques are within the effort limits # TODO 2) : change it to calculate the efort limits of the subtree + # Check if the torques are within the effort limits if self.check_effort_limits(tau= tau, tree_id= tree_id).all(): valid = True # Compute all the collisions @@ -429,7 +425,6 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, break if valid: - # TODO 4) : change arm to subtree identifier valid_configurations.append({"config" : q["config"], "end_effector_pos" : q["end_effector_pos"], "tau" : tau, "tree_id" : tree_id,"selected_joint_id": selected_joint_id}) @@ -446,20 +441,35 @@ def get_valid_workspace(self, range : int, resolution : int, masses : np.ndarray :param checked_frames (np.ndarray): Array of frame names where the external forces are applied. :return: Array of valid configurations that achieve the desired end effector position in format: [{"config", "end_effector_pos, "tau", "arm"}]. """ + # create the array to store all current valid configurations + valid_current_configurations = np.array([], dtype=object) + # compute all configurations for the selected joints of the trees for tree,configuration in zip(self.sub_trees,self.configurations): # if the configurations are not computed or the selected joint ID is not the same as in the tree, compute the configurations - if configuration["configurations"] == None or configuration["selected_joint_id"] != tree["selected_joint_id"]: + if configuration["configurations"] is None or configuration["selected_joint_id"] != tree["selected_joint_id"]: if tree["selected_joint_id"] is not None: # Compute all configurations for the current tree configuration["configurations"] = self.compute_all_configurations(range, resolution,tree["selected_joint_id"]) # Set the selected joint ID to the current tree's selected joint ID configuration["selected_joint_id"] = tree["selected_joint_id"] + else: + pass + # if the selected joint ID is None in the tree, I could remove the computed configurations,even though it is not necessary and it could be useful if the + # user selects the joint later - # Verify the configurations to check if they are valid - valid_configurations = self.verify_configurations(configuration["configurations"], masses, checked_frames, tree["tree_id"], tree["selected_joint_id"]) - - return valid_configurations + + if configuration["configurations"] is not None and tree["selected_joint_id"] is not None: + # Verify the configurations to check if they are valid + valid_configurations = self.verify_configurations(configuration["configurations"], masses, checked_frames, tree["tree_id"], tree["selected_joint_id"]) + + # Append the valid configurations to the current valid configurations array + valid_current_configurations = np.append(valid_current_configurations, valid_configurations) + + + + + return valid_current_configurations def compute_maximum_payloads(self, configs : np.ndarray): @@ -488,7 +498,6 @@ def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10 while high - low > resolution: mid_payload = (low + high) / 2 - # TODO 5): remove hardcoded arm frame, use selected joint of the subtree submenu ext_forces = self.create_ext_force(mid_payload, self.get_joint_name(config["selected_joint_id"]), 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, config['tree_id']).all(): @@ -574,6 +583,7 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda """ # TODO : Change variable names # Get the number of joints + num_joints = len(valid_configs[0]["tau"]) max_torques = np.array([], dtype=float) joint_torques = np.array([], dtype=object) @@ -814,7 +824,6 @@ def check_joint_limits(self, q : np.ndarray) -> np.ndarray: return within_limits - # TODO 3) : change it, instead of arm name use sub tree identifier def check_effort_limits(self, tau : np.ndarray, tree_id : int = None) -> np.ndarray: """ Check if the torques vector is within the effort limits of the robot model. @@ -845,8 +854,8 @@ def check_effort_limits(self, tau : np.ndarray, tree_id : int = None) -> np.ndar else: # Check if the torque of joints inside the tree is within the limits for id in self.sub_trees[tree_id]["joint_ids"]: - if abs(tau[id]) > self.model.effortLimit[id]: - print(f"\033[91mJoint {i+1} exceeds effort limit: {tau[i]} > {self.model.effortLimit[i]} \033[0m\n") + if abs(tau[id-1]) > self.model.effortLimit[id-1]: + print(f"\033[91mJoint {id} exceeds effort limit: {tau[id-1]} > {self.model.effortLimit[id-1]} \033[0m\n") within_limits = np.append(within_limits, False) else: within_limits = np.append(within_limits, True) From 29aa4f881c76cdb316334cb24c3643737f0eefdb Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 23 Jul 2025 14:44:48 +0200 Subject: [PATCH 15/44] changes for better user experience with new version --- .../menu_visual.py | 75 ++++++++++++------- 1 file changed, 46 insertions(+), 29 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 2a3f61f..d41ae82 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -77,6 +77,9 @@ def __init__(self, node): self.menu_handler.setVisible(self.max_torque_checker, False) self.menu_handler.setVisible(self.label_color_selection, False) + # set the visibility of the workspace button to false, it will be displayed only when the user selects at least one tree + self.menu_handler.setVisible(self.workspace_button, False) + # label for tree selection self.label_tree_selection = self.menu_handler.insert('Select end effector point for each tree :', callback=self.callback_tree_selection) @@ -159,6 +162,10 @@ def callback_joint_tree_selection(self, feedback): Args: feedback: Feedback from the menu selection. """ + + # reset the sub-menu configuration if the user change joint 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 joint name) @@ -195,6 +202,14 @@ def callback_joint_tree_selection(self, feedback): # set the joint as checked self.update_joint_tree_selection(int(parent_context.command), title) + + # check if there is at least one joint selected in the subtree menu + if self.get_status_joint_tree(): + # set the workspace button visible if there is at least one joint selected in the subtree menu + self.menu_handler.setVisible(self.workspace_button, True) + else: + # hide the workspace button if there is no joint selected in the subtree menu + self.menu_handler.setVisible(self.workspace_button, False) # apply changes self.menu_handler.reApply(self.server) @@ -250,10 +265,13 @@ def insert_dropdown_configuration(self, configuration : np.ndarray): configuration (np.ndarray): Array of configuration to be displayed in the dropdown. """ + if configuration.size == 0: + return + for i, item in enumerate(configuration): # insert the parent in the command field to keep track of the parent id # TODO 6) : change arm to subtree identifier - 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) + last_entry = self.menu_handler.insert(f"Configuration {i} | tree: {item['tree_id']} | 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) @@ -319,39 +337,29 @@ def callback_reset(self, feedback): if item['checked']: item = {"name": item['name'], "checked": False, "payload": 0.0} self.frames_selection[i] = item - - # reset the frames selection menu (i = number of entry, item = object with name, sub entries, etc.) - for i, item in self.menu_handler.entry_contexts_.items(): - if i == 1: - # skip the root item (payload reset) - continue - - # check if the item(frame) has sub entries (payloads selection) - if item.sub_entries: - # 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) - - # check if the item is the reset payloads or compute workspace item and skip the unchanging of the check state - 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 + + + self.reset_sub_menu_configuration() + + for item in self.menu_handler.entry_contexts_[self.root_frames].sub_entries: + for sub_item in self.menu_handler.entry_contexts_[item].sub_entries: + self.menu_handler.setVisible(sub_item, False) + self.menu_handler.setCheckState(sub_item, MenuHandler.UNCHECKED) - # set the checked of frame to unchecked - self.menu_handler.setCheckState(i,MenuHandler.UNCHECKED) + if item != self.reset: + # set the checked of frame to unchecked + self.menu_handler.setCheckState(item,MenuHandler.UNCHECKED) + # reset the selected configuration self.selected_configuration = None - - # hide the torque limits and max torque checkboxes when there is no configuration selected + + #hide the torque limits and max torque checkboxes when there is no configuration selected + self.menu_handler.setVisible(self.label_color_selection, False) self.menu_handler.setVisible(self.torque_limits_checker, False) self.menu_handler.setVisible(self.max_torque_checker, False) - self.torque_color = TorqueVisualizationType.TORQUE_LIMITS # reset to default torque limits + self.torque_color = TorqueVisualizationType.TORQUE_LIMITS # reset to default torque limits# # reapply the menu handler and server changes self.menu_handler.reApply(self.server) @@ -414,6 +422,17 @@ def reset_sub_menu_configuration(self): self.selected_configuration = None + def get_status_joint_tree(self) -> bool: + """ + Check if there is at least one joint selected in the subtree menu. + + Returns: + bool: True if at least one joint is selected, False otherwise. + """ + for item in self.subtree_selection: + if item['selected_joint_id'] is not None: + return True + def update_joint_tree_selection(self, tree_identifier: int, joint_name : str): """ Update the state of a joint in the subtree menu. @@ -421,8 +440,6 @@ def update_joint_tree_selection(self, tree_identifier: int, joint_name : str): Args: tree_identifier (int): Identifier of the subtree. joint_name (str): Name of the joint to update. - joint_id (np.ndarray): ID of the joint to update. - value (bool): New state of the joint (checked or unchecked). """ for item in self.subtree_selection: From c91870610673eb7dad8c1aca2823c93f9dc539a6 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 23 Jul 2025 14:45:18 +0200 Subject: [PATCH 16/44] fix error in publishing markers --- .../rviz_visualization_menu.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) 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 ea9730a..2a14646 100755 --- 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 @@ -170,8 +170,6 @@ def workspace_calculation(self): """ # if the user choose to compute the workspace area then compute the valid configurations if self.menu.get_workspace_state(): - # TODO : Change parameters to be more general: instead of names, use selected joints of the - # checkbox menu for the subtree self.valid_configurations = self.robot.get_valid_workspace(2, 0.20, self.masses, self.checked_frames) # compute the maximum payloads for the valid configurations @@ -307,7 +305,7 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, """ marker_array = MarkerArray() for i, (t, joint) in enumerate(zip(torque, joints_position)): - # remove the gripper joints from the visualization TODO: make it more general (with MIMIC joints) + # TODO : Visualize labels without overlaps if "gripper" not in joint['name']: marker = Marker() marker.header.frame_id = "base_link" @@ -401,7 +399,7 @@ def publish_workspace_area(self): if self.robot.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): point = Marker() point.header.frame_id = "base_link" - point.header.stamp = self.get_clock().now().to_msg() + point.header.stamp = Time() point.ns = joint_pos["name"] point.id = cont point.type = Marker.SPHERE @@ -481,7 +479,7 @@ def publish_maximum_payloads_area(self): # 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.header.stamp = Time() marker_point_name.ns = f"label_payloads_tree_{valid_config['tree_id']}" marker_point_name.id = i @@ -490,7 +488,7 @@ def publish_maximum_payloads_area(self): 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.position.z = valid_config["end_effector_pos"][2] + 0.05 marker_point_name.pose.orientation.w = 1.0 marker_point_name.scale.x = 0.02 marker_point_name.scale.y = 0.02 @@ -510,7 +508,7 @@ def publish_maximum_payloads_area(self): point = Marker() point.header.frame_id = "base_link" - point.header.stamp = self.get_clock().now().to_msg() + point.header.stamp = Time() point.ns = f"max_payloads_tree_{valid_config['tree_id']}" point.id = i point.type = Marker.SPHERE From efb57c3c119cb1411289b032a33940c647c32a94 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 23 Jul 2025 15:04:29 +0200 Subject: [PATCH 17/44] refactor function to calculate maximum torques --- .../dynamic_payload_analysis_core/core.py | 29 ++++++++++++------- 1 file changed, 18 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 475c36e..f082616 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -581,25 +581,32 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau"}]. :return: Arrays of maximum torques for each joint in the current valid configurations for selected trees. """ - # TODO : Change variable names - # Get the number of joints + # Get the number of joints num_joints = len(valid_configs[0]["tau"]) - max_torques = np.array([], dtype=float) - joint_torques = np.array([], dtype=object) - abs_torques = np.array([], dtype=object) - + + # array to store the absolute torques for each joint in the current valid configurations for each selected tree + abs_joint_torques = np.array([], dtype=object) + + # get the selected trees from the sub_trees selected_trees = [tree for tree in self.sub_trees if tree["selected_joint_id"] is not None] - # Find maximum absolute torque for each joint + + # create an array to store the absolute torques for each joint in the current valid configurations for each selected tree for tree in selected_trees: + # array to store the absolute torques for each joint in the current tree + abs_torques = np.array([], dtype=object) + for i in range(num_joints): # Get the joint torques for the current tree - abs_torques = np.append(abs_torques ,{"abs": [abs(config["tau"][i]) for config in valid_configs if config["tree_id"] == tree["tree_id"]]}) + abs_torques = np.append(abs_torques ,{"joint" : i ,"abs": [abs(config["tau"][i]) for config in valid_configs if config["tree_id"] == tree["tree_id"]]}) - joint_torques = np.append(joint_torques, {"tree_id": tree["tree_id"], "abs_torques": abs_torques}) + abs_joint_torques = np.append(abs_joint_torques, {"tree_id": tree["tree_id"], "abs_torques": abs_torques}) - - for torques in joint_torques: + # array to store the maximum absolute torques for each joint in the current valid configurations + max_torques = np.array([], dtype=float) + + # get the maximum absolute torques for each joint in the current valid configurations for each selected tree + for torques in abs_joint_torques: max_tau = np.array([], dtype=object) # Get the maximum absolute torque for the current joint for tau in torques["abs_torques"]: From 4681b7700bc889f90e7e81e5b686ac91cf8375cd Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Sun, 27 Jul 2025 11:09:05 +0200 Subject: [PATCH 18/44] added method to avoid overlaps between torque labels --- .../rviz_visualization_menu.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) 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 2a14646..78ae754 100755 --- 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 @@ -303,9 +303,10 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, status_torques (np.ndarray): The status of the torques, True if the torque is within the limits, False otherwise joints_position (np.ndarray): The positions of the joints where the torques are applied """ + joint_z_prev = {"pos": 0.0, "labels_count": 0} # Variable to store the previous z position of the joint for label offset marker_array = MarkerArray() + for i, (t, joint) in enumerate(zip(torque, joints_position)): - # TODO : Visualize labels without overlaps if "gripper" not in joint['name']: marker = Marker() marker.header.frame_id = "base_link" @@ -320,9 +321,18 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, marker.text = f"{joint['name']}: {t:.2f} Nm" marker.action = Marker.ADD + marker.pose.position.x = joint['x'] marker.pose.position.y = joint['y'] - marker.pose.position.z = joint['z'] + + # method to avoid overlaps between labels + if joint['z'] == joint_z_prev["pos"]: + marker.pose.position.z = joint['z'] + 0.025 * joint_z_prev["labels_count"] # Offset to avoid overlaps between labels + joint_z_prev = {"pos" : joint['z'], "labels_count" : joint_z_prev["labels_count"] + 1} + else: + marker.pose.position.z = joint['z'] + joint_z_prev = {"pos" : marker.pose.position.z, "labels_count" : 1} + marker.pose.orientation.w = 1.0 marker.scale.x = 0.03 marker.scale.y = 0.03 @@ -336,6 +346,8 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, marker.color.r = 1.0 # Red marker.color.g = 0.0 # Green marker.color.b = 0.0 # Blue + + marker_array.markers.append(marker) self.publisher_rviz_torque.publish(marker_array) From ce4366b618ed379ab1a1d26cd182c2c630ca311d Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Sun, 27 Jul 2025 11:27:42 +0200 Subject: [PATCH 19/44] small fix --- .../dynamic_payload_analysis_core/core.py | 4 ++-- .../dynamic_payload_analysis_ros/menu_visual.py | 1 - 2 files changed, 2 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 f082616..b4cadc4 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -478,12 +478,12 @@ 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, 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=15, resolution=0.01) return configs - def find_max_payload_binary_search(self, config, payload_min=0.0, payload_max=10.0, resolution=0.01): + def find_max_payload_binary_search(self, config : np.ndarray, payload_min : float = 0.0, payload_max : float = 10.0, resolution : float = 0.01): """ Find the maximum payload for a given configuration using binary search. :param config: Configuration dictionary (must contain 'config' key). 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 d41ae82..48f8d10 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -270,7 +270,6 @@ 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 - # TODO 6) : change arm to subtree identifier last_entry = self.menu_handler.insert(f"Configuration {i} | tree: {item['tree_id']} | 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) From e8c7c4355b59343ca1bfdd3f2550841c4826c95e Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 31 Jul 2025 18:06:20 +0200 Subject: [PATCH 20/44] added new method to calculate sutrees --- .../dynamic_payload_analysis_core/core.py | 74 ++++++++----------- .../menu_visual.py | 2 +- .../rviz_visualization_menu.py | 5 +- 3 files changed, 34 insertions(+), 47 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 b4cadc4..eff4049 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -65,16 +65,18 @@ def __init__(self, robot_description : Union[str, Path]): # get the default collisions in the robot model to avoid take them into account in the computations self.default_collisions = self.compute_static_collisions() + + + # compute main trees of the robot model - self.root_joints = ["arm_left_1_joint", "arm_right_1_joint", "leg_left_1_joint", "leg_right_1_joint", "head_1_joint"] - self.compute_sub_trees() + self.compute_subtrees() # array to store all configurations for the robot model self.configurations = np.array([], dtype=object) # create items for each tree in the robot model - for tree in self.sub_trees: + for tree in self.subtrees: self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) @@ -101,7 +103,7 @@ def compute_static_collisions(self): return collision_pairs - def compute_sub_trees(self): + def compute_subtrees(self): """ Compute the sub-trees of the robot model. This method is used to compute the sub-trees of the robot model @@ -109,42 +111,28 @@ def compute_sub_trees(self): # array to store the sub-trees trees = np.array([], dtype=object) - - for sub_tree in self.model.subtrees: - # get the root joint ID of the sub-tree - root_joint_id = sub_tree[0] - # get the joint names in the sub-tree - joint_names = [self.model.names[joint_id] for joint_id in sub_tree] - # get the joint IDs in the sub-tree - joint_ids = [self.model.getJointId(joint_name) for joint_name in joint_names] - # append the sub-tree to the array - trees = np.append(trees, {"joint_names": joint_names, "joint_ids": joint_ids, "root_joint_id": root_joint_id}) - - self.sub_trees = self.compute_main_trees(trees) + ## build subtrees: + #print("## Building subtrees ...") + # First, find all the tip joints : joints that are the leaves of the kinematic chain: + tip_joints = [] + for id in range(0, self.model.njoints): + if len(self.model.subtrees[id]) == 1: + tip_joints += [id] - - def compute_main_trees(self, trees: np.ndarray) -> np.ndarray: - """ - Compute the main trees of the robot model based on the sub-trees. + #print("Total number of kinematic trees: ", len(tip_joints) + #print("Tip joints : ", tip_joints) - :param trees: Array of sub-trees of the robot model. - :return: Array of main trees of the robot model. - """ + self.subtrees = np.array([], dtype=object) - # array to store the main trees - main_trees = np.array([], dtype=object) + for i, jointID in enumerate(tip_joints): + joint_tree_ids = self.model.supports[jointID].tolist() + + # get the joint names in the sub-tree + joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] + + self.subtrees = np.append(self.subtrees, {"tree_id": i, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name":self.model.names[jointID] ,"tip_joint_id": jointID, "selected_joint_id": None}) - root_joint_ids = [self.model.getJointId(joint_name) for joint_name in self.root_joints] - - # Iterate over all joints in the model - id = 0 - for tree in trees: - if tree["root_joint_id"] in root_joint_ids: - main_trees = np.append(main_trees, {"tree_id": id, "joint_names": tree['joint_names'], "joint_ids": tree['joint_ids'], "root_joint_id": tree["root_joint_id"], "selected_joint_id": None}) - id += 1 - - return main_trees def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray = None) -> np.ndarray: @@ -445,7 +433,7 @@ def get_valid_workspace(self, range : int, resolution : int, masses : np.ndarray valid_current_configurations = np.array([], dtype=object) # compute all configurations for the selected joints of the trees - for tree,configuration in zip(self.sub_trees,self.configurations): + for tree,configuration in zip(self.subtrees,self.configurations): # if the configurations are not computed or the selected joint ID is not the same as in the tree, compute the configurations if configuration["configurations"] is None or configuration["selected_joint_id"] != tree["selected_joint_id"]: if tree["selected_joint_id"] is not None: @@ -562,7 +550,7 @@ def verify_member_tree(self, tree_id: int, joint_id: int) -> bool: """ # Check if the joint ID is in the list of joint IDs for the specified tree - return joint_id in self.sub_trees[tree_id]["joint_ids"] + return joint_id in self.subtrees[tree_id]["joint_ids"] def get_subtrees(self) -> np.ndarray: @@ -571,7 +559,7 @@ def get_subtrees(self) -> np.ndarray: :return: Array of sub-trees of the robot model. """ - return self.sub_trees + return self.subtrees def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: @@ -589,7 +577,7 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda abs_joint_torques = np.array([], dtype=object) # get the selected trees from the sub_trees - selected_trees = [tree for tree in self.sub_trees if tree["selected_joint_id"] is not None] + selected_trees = [tree for tree in self.subtrees if tree["selected_joint_id"] is not None] # create an array to store the absolute torques for each joint in the current valid configurations for each selected tree for tree in selected_trees: @@ -626,7 +614,7 @@ def get_maximum_payloads(self, valid_configs : np.ndarray) -> np.ndarray: :return: Tuple of arrays of maximum payloads for left and right arms. """ max_payloads = np.array([], dtype=float) - for tree in self.sub_trees: + for tree in self.subtrees: payloads = [config["max_payload"] for config in valid_configs if config["tree_id"] == tree["tree_id"]] if payloads: max_payload = max(payloads) @@ -710,7 +698,7 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd # get the maximum torque from the sum of torques for all selected trees - for tree in self.sub_trees: + for tree in self.subtrees: # Get all sum values for the current tree tree_sums = [item["sum"] for item in torques_sum if item["tree_id"] == tree["tree_id"]] @@ -860,7 +848,7 @@ def check_effort_limits(self, tau : np.ndarray, tree_id : int = None) -> np.ndar else: # Check if the torque of joints inside the tree is within the limits - for id in self.sub_trees[tree_id]["joint_ids"]: + for id in self.subtrees[tree_id]["joint_ids"]: if abs(tau[id-1]) > self.model.effortLimit[id-1]: print(f"\033[91mJoint {id} exceeds effort limit: {tau[id-1]} > {self.model.effortLimit[id-1]} \033[0m\n") within_limits = np.append(within_limits, False) @@ -889,7 +877,7 @@ def set_joint_tree_selection(self, tree_id : int, joint_id: int): :param tree_id: ID of the tree to select. :param joint_id: ID of the joint to select. """ - for tree in self.sub_trees: + for tree in self.subtrees: if tree["tree_id"] == tree_id: # Set the selected joint in the tree tree["selected_joint_id"] = joint_id 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 48f8d10..b0fca5e 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -116,7 +116,7 @@ def insert_subtree(self, tree_identifier : str, joint_names : np.ndarray, joint_ Args: name (str) : name of the subtree """ - last_item = self.menu_handler.insert(f"Tree {tree_identifier}",command= str(tree_identifier), callback=self.callback_tree_selection) + last_item = self.menu_handler.insert(f"Tree: [{tree_identifier}]",command= str(tree_identifier), callback=self.callback_tree_selection) self.menu_handler.setCheckState(last_item, MenuHandler.UNCHECKED) self.menu_handler.setVisible(last_item, 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 78ae754..2a754a3 100755 --- 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 @@ -111,7 +111,7 @@ def robot_description_callback(self, msg): # Add subtree to the menu for i, subtree in enumerate(self.robot.get_subtrees()): - self.menu.insert_subtree(subtree['tree_id'], subtree["joint_names"], subtree["joint_ids"]) + self.menu.insert_subtree(subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"]) # self.robot.print_configuration() @@ -406,8 +406,7 @@ def publish_workspace_area(self): # insert points related to the end effector position in the workspace area and with color based on the normalized torques for each joint for joint_pos,tau in zip(joint_positions,norm_joints_torques): - # print only the points for the corrisponding arm, in this way we can visualize the workspace area for each arm separately and avoid confusion - # TODO + # print only the points for the corrisponding tree_id of the valid configuration if self.robot.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): point = Marker() point.header.frame_id = "base_link" From 1e08a11a63be3fb616eb165fe5063bdac9bef1dd Mon Sep 17 00:00:00 2001 From: Enrico Date: Thu, 31 Jul 2025 20:33:28 +0200 Subject: [PATCH 21/44] added first version with mimic joints --- .../dynamic_payload_analysis_core/core.py | 11 +++++------ .../dynamic_payload_analysis_ros/menu_visual.py | 4 ++-- .../rviz_visualization_menu.py | 2 +- 3 files changed, 8 insertions(+), 9 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 eff4049..86a9002 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -39,8 +39,12 @@ def __init__(self, robot_description : Union[str, Path]): # Load the robot model from path or XML string if isinstance(robot_description, str): - self.model = pin.buildModelFromXML(robot_description) + self.model = pin.buildModelFromXML(robot_description, mimic= True) + self.mimic_joints = self.model.mimicking_joints.tolist() + self.mimic_joint_names = [self.model.names[joint_id] for joint_id in self.mimic_joints] + + self.model = pin.buildModelFromXML(robot_description) # TODO change parser in general for more unique solution # create temporary URDF file from the robot description string @@ -65,13 +69,9 @@ def __init__(self, robot_description : Union[str, Path]): # get the default collisions in the robot model to avoid take them into account in the computations self.default_collisions = self.compute_static_collisions() - - - # compute main trees of the robot model self.compute_subtrees() - # array to store all configurations for the robot model self.configurations = np.array([], dtype=object) @@ -1010,7 +1010,6 @@ def get_mass_matrix(self, q : np.ndarray) -> np.ndarray: return mass_matrix - def get_joints(self) -> np.ndarray: """ Get the array joint names of the robot model. 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 b0fca5e..4e8f76b 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -109,14 +109,14 @@ def insert_frame(self, name): self.server.applyChanges() - def insert_subtree(self, tree_identifier : str, joint_names : np.ndarray, joint_ids : np.ndarray): + def insert_subtree(self, tree_identifier : int,tip_tree_name : str, joint_names : np.ndarray, joint_ids : np.ndarray): """ Insert a new item(subtree) in the checkbox menu of frames. Args: name (str) : name of the subtree """ - last_item = self.menu_handler.insert(f"Tree: [{tree_identifier}]",command= str(tree_identifier), callback=self.callback_tree_selection) + last_item = self.menu_handler.insert(f"Tree: [{tip_tree_name}]",command= str(tree_identifier), callback=self.callback_tree_selection) self.menu_handler.setCheckState(last_item, MenuHandler.UNCHECKED) self.menu_handler.setVisible(last_item, 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 2a754a3..6b23597 100755 --- 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 @@ -111,7 +111,7 @@ def robot_description_callback(self, msg): # Add subtree to the menu for i, subtree in enumerate(self.robot.get_subtrees()): - self.menu.insert_subtree(subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"]) + self.menu.insert_subtree(i,subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"]) # self.robot.print_configuration() From a546db97be537ebde3ecc57a36b029864bd3d7ae Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 1 Aug 2025 09:01:09 +0200 Subject: [PATCH 22/44] add support for mimic joints and add filter in the subtrees --- .../dynamic_payload_analysis_core/core.py | 58 +++++++++++++++---- 1 file changed, 46 insertions(+), 12 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 86a9002..3200a01 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -41,11 +41,9 @@ def __init__(self, robot_description : Union[str, Path]): if isinstance(robot_description, str): self.model = pin.buildModelFromXML(robot_description, mimic= True) - self.mimic_joints = self.model.mimicking_joints.tolist() - self.mimic_joint_names = [self.model.names[joint_id] for joint_id in self.mimic_joints] + self.compute_mimic_joints() self.model = pin.buildModelFromXML(robot_description) - # TODO change parser in general for more unique solution # create temporary URDF file from the robot description string with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', delete=False) as temp_file: @@ -80,6 +78,15 @@ def __init__(self, robot_description : Union[str, Path]): self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) + def compute_mimic_joints(self): + """ + Compute the mimic joints for the robot model if the model is enabled for mimic joints. + """ + self.mimic_joint_ids = self.model.mimicking_joints.tolist() + self.mimicked_joint_ids = self.model.mimicked_joints.tolist() + self.mimic_joint_names = [self.model.names[joint_id] for joint_id in self.mimic_joint_ids] + + def compute_static_collisions(self): """ Compute the static collisions for the robot model. @@ -109,9 +116,6 @@ def compute_subtrees(self): This method is used to compute the sub-trees of the robot model """ - # array to store the sub-trees - trees = np.array([], dtype=object) - ## build subtrees: #print("## Building subtrees ...") # First, find all the tip joints : joints that are the leaves of the kinematic chain: @@ -122,18 +126,48 @@ def compute_subtrees(self): #print("Total number of kinematic trees: ", len(tip_joints) #print("Tip joints : ", tip_joints) - self.subtrees = np.array([], dtype=object) for i, jointID in enumerate(tip_joints): - joint_tree_ids = self.model.supports[jointID].tolist() - - # get the joint names in the sub-tree - joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] + joint_tree_ids = self.get_filtered_subtree(jointID) - self.subtrees = np.append(self.subtrees, {"tree_id": i, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name":self.model.names[jointID] ,"tip_joint_id": jointID, "selected_joint_id": None}) + # insert the sub-tree only if the tip joint is not already in the sub-trees + tip_joint_already_exists = False + for existing_tree in self.subtrees: + if existing_tree["tip_joint_id"] == joint_tree_ids[-1]: + tip_joint_already_exists = True + break + if not tip_joint_already_exists: + # get the joint names in the sub-tree + joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] + + self.subtrees = np.append(self.subtrees, {"tree_id": i, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) + + + def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: + """ + Filter the sub-trees of the robot based on the mimic joints and mimicked joints. + If the current tip joint is not a mimic joint, the subtree is returned as is. + + :param current_tip_id: Id of the current tip joint to filter the subtree. + :return: Filtered tree with tip joint based on the mimicked joint of the mimic joint. + """ + # find mimicked joint of the current tip joint + if current_tip_id in self.mimic_joint_ids: + # get the index of the mimic joint in the mimic_joint_ids + mimic_joint_index = self.mimic_joint_ids.index(current_tip_id) + # get the mimicked joint id + mimicked_joint_id = self.mimicked_joint_ids[mimic_joint_index] + + # filter the subtree to include only the mimicked joint and its children + filtered_subtree = self.model.supports[mimicked_joint_id].tolist() + else: + # if the current tip joint is not a mimic joint, return the subtree as is + filtered_subtree = self.model.supports[current_tip_id].tolist() + return filtered_subtree + def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray = None) -> np.ndarray: """ From 7b4ee7166a7609a3190a177b9dafa135b09f1b0b Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 1 Aug 2025 09:35:58 +0200 Subject: [PATCH 23/44] added parameter for advanced mode --- .../dynamic_payload_analysis_core/core.py | 24 ++++++++++++------- .../rviz_visualization_menu.py | 9 +++++-- 2 files changed, 23 insertions(+), 10 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 3200a01..f65c01a 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -1066,20 +1066,28 @@ def get_frames(self) -> np.ndarray: - def get_active_frames(self) -> np.ndarray: + def get_active_frames(self, all_frames : bool = False) -> np.ndarray: """ Get the array of active joint names in the robot model. - + :param all_frames: If True, return all frames, otherwise return only tip frames. :return: array of active joint names. """ # Get frames where joints are parents frame_names = [] - for i in range(1, self.model.njoints): - for frame in self.model.frames: - if frame.parentJoint == i and frame.type == pin.FrameType.BODY: - frame_names.append(frame.name) - break - + + if all_frames: + for i in range(1, self.model.njoints): + for frame in self.model.frames: + if frame.parentJoint == i and frame.type == pin.FrameType.BODY: + if frame.parentJoint not in self.mimic_joint_ids: + frame_names.append(frame.name) + break + else: + for tree in self.subtrees: + tree["tip_joint_id"] + parent_frame = next((frame for frame in self.model.frames if frame.parentJoint == tree["tip_joint_id"] and frame.type == pin.FrameType.BODY)) + frame_names.append(parent_frame.name) + return np.array(frame_names, dtype=str) 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 6b23597..3d845e3 100755 --- 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 @@ -32,6 +32,11 @@ class RobotDescriptionSubscriber(Node): def __init__(self): super().__init__('node_robot_description_subscriber') + + # add parameters for the node to set the expert mode or the basic mode + self.declare_parameter('advanced_mode', False) + + self.subscription = self.create_subscription( String, '/robot_description', @@ -105,8 +110,8 @@ def robot_description_callback(self, msg): self.robot.print_active_joint() self.robot.print_frames() - # Add the frame to the menu for payload selection - for frame in self.robot.get_active_frames(): + # Add the frame to the menu for payload selection with the advanced mode parameter + for frame in self.robot.get_active_frames(self.get_parameter('advanced_mode').get_parameter_value().bool_value): self.menu.insert_frame(frame) # Add subtree to the menu From 67769d7a41e06f0c2130960d9f84204e7dacf729 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 1 Aug 2025 11:40:12 +0200 Subject: [PATCH 24/44] added new method to get mimic joints from the URDF --- .../dynamic_payload_analysis_core/core.py | 41 ++++++++++++++----- 1 file changed, 31 insertions(+), 10 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 f65c01a..216616e 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -24,7 +24,7 @@ import tempfile from ikpy import chain import os - +import xml.etree.ElementTree as ET @@ -39,10 +39,6 @@ def __init__(self, robot_description : Union[str, Path]): # Load the robot model from path or XML string if isinstance(robot_description, str): - self.model = pin.buildModelFromXML(robot_description, mimic= True) - - self.compute_mimic_joints() - self.model = pin.buildModelFromXML(robot_description) # create temporary URDF file from the robot description string @@ -50,6 +46,8 @@ def __init__(self, robot_description : Union[str, Path]): temp_file.write(robot_description) temp_urdf_path = temp_file.name + self.compute_mimic_joints(temp_urdf_path) + self.geom_model = pin.buildGeomFromUrdf(self.model,temp_urdf_path,pin.GeometryType.COLLISION) # Add collisition pairs @@ -77,14 +75,36 @@ def __init__(self, robot_description : Union[str, Path]): for tree in self.subtrees: self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) + - def compute_mimic_joints(self): + def compute_mimic_joints(self, urdf_path): """ - Compute the mimic joints for the robot model if the model is enabled for mimic joints. + Parses a URDF file to find all mimic joints with mimicked joints and ids. + + Args: + urdf_path (str): The file path to the URDF model. """ - self.mimic_joint_ids = self.model.mimicking_joints.tolist() - self.mimicked_joint_ids = self.model.mimicked_joints.tolist() - self.mimic_joint_names = [self.model.names[joint_id] for joint_id in self.mimic_joint_ids] + try: + tree = ET.parse(urdf_path) + root = tree.getroot() + except ET.ParseError as e: + print(f"Error parsing URDF file: {e}") + return [] + + self.mimic_joint_names = [] + self.mimicked_joint_names = [] + + # Find all tags in the URDF + for joint in root.findall('joint'): + mimic_tag = joint.find('mimic') + + # Check if the joint has a sub-tag + if mimic_tag is not None: + self.mimic_joint_names.append(joint.get('name')) + self.mimicked_joint_names.append(mimic_tag.get('joint')) + + self.mimic_joint_ids = [self.model.getJointId(name) for name in self.mimic_joint_names] + self.mimicked_joint_ids = [self.model.getJointId(name) for name in self.mimicked_joint_names] def compute_static_collisions(self): @@ -632,6 +652,7 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda max_tau = np.array([], dtype=object) # Get the maximum absolute torque for the current joint for tau in torques["abs_torques"]: + #TODO: error if the array is empty, when a tree has no valid configurations max_tau = np.append(max_tau, max(tau["abs"])) max_torques = np.append(max_torques, {"tree_id": torques["tree_id"], "max_values": max_tau}) From b6d6452db8c84030bf985b005c34a813ba2d1de8 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 1 Aug 2025 11:48:16 +0200 Subject: [PATCH 25/44] small changes --- .../dynamic_payload_analysis_core/core.py | 13 ++++++++----- .../rviz_visualization_menu.py | 4 ++-- 2 files changed, 10 insertions(+), 7 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 216616e..bb21422 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -147,7 +147,7 @@ def compute_subtrees(self): #print("Total number of kinematic trees: ", len(tip_joints) #print("Tip joints : ", tip_joints) self.subtrees = np.array([], dtype=object) - + cont = 0 for i, jointID in enumerate(tip_joints): joint_tree_ids = self.get_filtered_subtree(jointID) @@ -162,9 +162,10 @@ def compute_subtrees(self): # get the joint names in the sub-tree joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] - self.subtrees = np.append(self.subtrees, {"tree_id": i, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) - + self.subtrees = np.append(self.subtrees, {"tree_id": cont, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) + cont += 1 + def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: """ Filter the sub-trees of the robot based on the mimic joints and mimicked joints. @@ -652,8 +653,10 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda max_tau = np.array([], dtype=object) # Get the maximum absolute torque for the current joint for tau in torques["abs_torques"]: - #TODO: error if the array is empty, when a tree has no valid configurations - max_tau = np.append(max_tau, max(tau["abs"])) + if len(tau["abs"]) > 0: + max_tau = np.append(max_tau, max(tau["abs"])) + else: + max_tau = np.append(max_tau, 0.0) max_torques = np.append(max_torques, {"tree_id": torques["tree_id"], "max_values": max_tau}) 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 3d845e3..987d3e4 100755 --- 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 @@ -213,7 +213,7 @@ def publish_selected_configuration(self): joint_state.name = [joint["joint_name"] for joint in configs] joint_state.position = [joint["q"] for joint in configs] - #joint_state.position = + self.publisher_joint_states.publish(joint_state) else: @@ -504,7 +504,7 @@ def publish_maximum_payloads_area(self): 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] + 0.05 + 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 From e677dde7bb0d369cb2e5e3789ae52e69b01ec565 Mon Sep 17 00:00:00 2001 From: Enrico Date: Sat, 2 Aug 2025 14:00:37 +0200 Subject: [PATCH 26/44] Change library to get mimic joints --- .../dynamic_payload_analysis_core/core.py | 36 +++++++++---------- 1 file changed, 17 insertions(+), 19 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 bb21422..ee753dc 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -25,7 +25,7 @@ from ikpy import chain import os import xml.etree.ElementTree as ET - +from urdf_parser_py.urdf import URDF class TorqueCalculator: @@ -41,13 +41,14 @@ def __init__(self, robot_description : Union[str, Path]): if isinstance(robot_description, str): self.model = pin.buildModelFromXML(robot_description) + # compute mimic joints + self.compute_mimic_joints(robot_description) + # create temporary URDF file from the robot description string with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', delete=False) as temp_file: temp_file.write(robot_description) temp_urdf_path = temp_file.name - self.compute_mimic_joints(temp_urdf_path) - self.geom_model = pin.buildGeomFromUrdf(self.model,temp_urdf_path,pin.GeometryType.COLLISION) # Add collisition pairs @@ -76,33 +77,30 @@ def __init__(self, robot_description : Union[str, Path]): self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) - - def compute_mimic_joints(self, urdf_path): + def compute_mimic_joints(self, urdf_xml): """ - Parses a URDF file to find all mimic joints with mimicked joints and ids. + Function to find all mimic joints with mimicked joints and ids. Args: - urdf_path (str): The file path to the URDF model. + urdf_xml (str): The string from robot_description topic. """ try: - tree = ET.parse(urdf_path) - root = tree.getroot() + robot = URDF.from_xml_string(urdf_xml) except ET.ParseError as e: - print(f"Error parsing URDF file: {e}") - return [] + print(f"Error parsing URDF xml: {e}") self.mimic_joint_names = [] self.mimicked_joint_names = [] - # Find all tags in the URDF - for joint in root.findall('joint'): - mimic_tag = joint.find('mimic') - - # Check if the joint has a sub-tag - if mimic_tag is not None: - self.mimic_joint_names.append(joint.get('name')) - self.mimicked_joint_names.append(mimic_tag.get('joint')) + # Iterate through all joints in the robot model to find mimic joints + for joint in robot.joints: + if joint.mimic is not None: + # Add the mimic joint name to the list + self.mimic_joint_names.append(joint.name) + # Add the mimicked joint name to the list + self.mimicked_joint_names.append(joint.mimic.joint) + # create lists of joint ids for mimic and mimicked joints self.mimic_joint_ids = [self.model.getJointId(name) for name in self.mimic_joint_names] self.mimicked_joint_ids = [self.model.getJointId(name) for name in self.mimicked_joint_names] From 91cd1a79fa554c8614b12d5f4c7ffc5d13b40def Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 4 Aug 2025 10:34:57 +0200 Subject: [PATCH 27/44] fixed publisher for payload visualization --- .../dynamic_payload_analysis_core/core.py | 9 ++++++--- .../rviz_visualization_menu.py | 12 +++++++++--- 2 files changed, 15 insertions(+), 6 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 ee753dc..0eea31d 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -327,7 +327,7 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n """ # Set parameters for the inverse kinematics solver - eps = 1e-3 # reduce for more precision + eps = 1e-2 # reduce for more precision IT_MAX = 500 # Maximum number of iterations DT = 1e-1 damp = 1e-12 @@ -472,7 +472,7 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, return np.array(valid_configurations, dtype=object) - def get_valid_workspace(self, range : int, resolution : int, masses : np.ndarray, checked_frames: np.ndarray) -> np.ndarray: + def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarray, checked_frames: np.ndarray) -> np.ndarray: """ Get the valid workspace of the robot model by computing all configurations within a specified range. @@ -1035,14 +1035,17 @@ def get_joint_name(self, id_joint: int) -> np.ndarray: return self.model.names[id_joint] - def get_joint_placement(self, joint_id : int) -> dict: + def get_joint_placement(self, joint_id : int, q : np.ndarray) -> dict: """ Get the placement of a specific joint in the robot model. :param joint_id: ID of the joint to get the placement for. + :param q: Joint configuration vector. :return: Dictionary with coordinates x , y , z of the joint. """ + self.update_configuration(q) + if joint_id < 0 or joint_id >= self.model.njoints: raise ValueError(f"Joint ID {joint_id} is out of bounds for the robot model with {self.model.njoints} joints.") 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 987d3e4..95066b8 100755 --- 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 @@ -130,11 +130,17 @@ def publish_payload_force(self): for frame in self.menu.get_item_state(): id_force = self.robot.get_parent_joint_id(frame["name"]) - joint_position = self.robot.get_joint_placement(id_force) + + # use the selected configuration from the menu to get the right joint placement + if self.selected_configuration is not None: + joint_position = self.robot.get_joint_placement(id_force,self.valid_configurations[self.selected_configuration]["config"]) + else: + joint_position = self.robot.get_joint_placement(id_force,self.robot.get_zero_configuration()) + arrow_force = Marker() arrow_force.header.frame_id = "base_link" - arrow_force.header.stamp = self.get_clock().now().to_msg() + arrow_force.header.stamp = Time() arrow_force.ns = "external_force" arrow_force.id = id_force arrow_force.type = Marker.ARROW @@ -175,7 +181,7 @@ 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, self.masses, self.checked_frames) + self.valid_configurations = self.robot.get_valid_workspace(range = 2,resolution = 0.20, masses = self.masses, checked_frames = self.checked_frames) # compute the maximum payloads for the valid configurations self.valid_configurations = self.robot.compute_maximum_payloads(self.valid_configurations) From 06b2ae2fe04233e49663848892e437fbc5bebaf6 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 4 Aug 2025 12:06:00 +0200 Subject: [PATCH 28/44] improvements to avoid not necessary calculations --- .../rviz_visualization_menu.py | 96 ++++++++++++++++--- 1 file changed, 81 insertions(+), 15 deletions(-) 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 95066b8..80bac2c 100755 --- 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 @@ -83,6 +83,15 @@ def __init__(self): # variable to store the currente selected configuration from the workspace menu self.valid_configurations = None + # id of the valid configurations to detect changes + self.id_current_valid_config = None + self.prev_id_current_valid_config = None + + # variable to store markers for the workspace area and maximum payloads area + self.marker_points = None + self.marker_point_names = None + self.marker_max_payloads = None + # variable to store if there is a selected configuration from the workspace menu to visualize self.selected_configuration = None @@ -198,8 +207,8 @@ 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.publish_maximum_payloads_area() + self.publish_workspace_area_maximum_payload_area() + @@ -362,16 +371,19 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, marker_array.markers.append(marker) self.publisher_rviz_torque.publish(marker_array) - + - def publish_workspace_area(self): - """ - Publish the workspace area in RViz using points and labels for the end points. + def generate_workspace_markers(self): """ + Generate markers for the workspace area in RViz. + This function creates markers for the end effector positions of the valid configurations and visualizes them + with colors based on the normalized torques for each joint. + It also creates labels for the end points of the valid configurations. + """ + # Create a MarkerArray to visualize the number of configuration of a specific point in the workspace marker_point_names = MarkerArray() - # Create a Marker for the workspace area using points marker_points = MarkerArray() @@ -447,7 +459,6 @@ def publish_workspace_area(self): # Add the marker point name to the marker point names array marker_point_names.markers.append(marker_point_name) - # get the unified torque for the valid configurations unified_configurations_torque = self.robot.get_unified_configurations_torque(self.valid_configurations) @@ -479,18 +490,54 @@ def publish_workspace_area(self): marker_points.markers.append(marker_point) + return marker_points, marker_point_names + + + + # TODO : try to optimize the function to avoid iterating through all the valid configurations every time + def publish_workspace_area_maximum_payload_area(self): + """ + Publish the workspace area and maximum payloads area in RViz using points and labels for the end points. + """ + + self.id_current_valid_config = self.get_id_current_valid_configurations() + + # check if the valid configurations is different from the previous one + if self.id_current_valid_config != self.prev_id_current_valid_config: + # generate the markers for the workspace area + self.marker_points, self.marker_point_names = self.generate_workspace_markers() + self.marker_max_payloads = self.generate_maximum_payloads_markers() + else: + # if the valid configurations are the same as the previous one, use the previous markers + if self.marker_points is not None and self.marker_point_names is not None: + # update the header stamp of the markers to the current time + for marker in self.marker_points.markers: + marker.header.stamp = Time() + + for marker in self.marker_point_names.markers: + marker.header.stamp = Time() + + if self.marker_max_payloads is not None: + # update the header stamp of the maximum payloads markers to the current time + for marker in self.marker_max_payloads.markers: + marker.header.stamp = Time() + + self.prev_id_current_valid_config = self.id_current_valid_config + # Publish the marker points and names - self.publisher_workspace_area.publish(marker_points) - self.publisher_workspace_area_names.publish(marker_point_names) + self.publisher_workspace_area.publish(self.marker_points) + self.publisher_workspace_area_names.publish(self.marker_point_names) + + # Publish the maximum payloads markers and labels + self.publisher_maximum_payloads.publish(self.marker_max_payloads) - def publish_maximum_payloads_area(self): + def generate_maximum_payloads_markers(self): """ - Publish the maximum payloads area in RViz using points and labels for the end points. + Generate markers for the maximum payloads in the workspace area in RViz. """ # 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_payloads = self.robot.get_maximum_payloads(self.valid_configurations) @@ -554,8 +601,27 @@ def publish_maximum_payloads_area(self): # 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) + return marker_max_payloads + + + def get_id_current_valid_configurations(self): + """Generate a hash of the current valid configurations to detect changes.""" + if self.valid_configurations is None: + return None + + # Create a hash based on configuration data and torque color mode + config_str = str([ + (config.get('tree_id', ''), + tuple(config.get('config', [])), + tuple(config.get('tau', [])), + tuple(config.get('end_effector_pos', [])), + config.get('max_payload', 0)) + for config in self.valid_configurations + ]) + torque_mode = str(self.menu.get_torque_color()) + + return hash(config_str + torque_mode) + def clear_workspace_area_markers(self): From a8ce098eb05bc6624e6a19cdf4a26f7402e09241 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 7 Aug 2025 15:54:46 +0200 Subject: [PATCH 29/44] added configuration parameter for IK resolution --- .../rviz_visualization_menu.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 80bac2c..23083d5 100755 --- 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 @@ -33,8 +33,13 @@ class RobotDescriptionSubscriber(Node): def __init__(self): super().__init__('node_robot_description_subscriber') - # add parameters for the node to set the expert mode or the basic mode + # add parameter for the node to set the expert mode or the basic mode self.declare_parameter('advanced_mode', False) + + # add parameter for the node to set the resolution of IK calculations + self.declare_parameter('resolution_ik', 0.20) + + self.resolution_ik = self.get_parameter('resolution_ik').get_parameter_value().double_value self.subscription = self.create_subscription( @@ -190,7 +195,7 @@ 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(range = 2,resolution = 0.20, masses = self.masses, checked_frames = self.checked_frames) + self.valid_configurations = self.robot.get_valid_workspace(range = 2,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) # compute the maximum payloads for the valid configurations self.valid_configurations = self.robot.compute_maximum_payloads(self.valid_configurations) From 6c3d1d3c4ad13d45ec054130571aca80518323b8 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 8 Aug 2025 10:46:51 +0200 Subject: [PATCH 30/44] added new method for payload selection --- .../dynamic_payload_analysis_core/core.py | 59 +++++++++++----- .../menu_visual.py | 70 +++++++++++++++---- .../rviz_visualization_menu.py | 38 +++++++--- 3 files changed, 129 insertions(+), 38 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 0eea31d..e51d867 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -157,10 +157,12 @@ def compute_subtrees(self): break if not tip_joint_already_exists: + # get the link names in the sub-tree + link_names = self.get_links_from_tree(joint_tree_ids) # get the joint names in the sub-tree joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] - - self.subtrees = np.append(self.subtrees, {"tree_id": cont, "joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) + + self.subtrees = np.append(self.subtrees, {"tree_id": cont, "link_names": link_names ,"joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) cont += 1 @@ -184,6 +186,9 @@ def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: else: # if the current tip joint is not a mimic joint, return the subtree as is filtered_subtree = self.model.supports[current_tip_id].tolist() + + # remove universe joint + filtered_subtree = filtered_subtree[1:] return filtered_subtree @@ -615,6 +620,27 @@ def get_subtrees(self) -> np.ndarray: return self.subtrees + def get_links_from_tree(self, joint_ids: np.ndarray | int) -> np.ndarray: + """ + Get the links from the robot model based on the joint IDs. + + :param joint_ids: Array of joint IDs to get the frames for. + :return: Array of frames corresponding to the joint IDs. + """ + frames = [] + # If joint_ids is a single integer, convert it to a list + if isinstance(joint_ids, int): + joint_ids = [joint_ids] + + for joint_id in joint_ids: + for link in self.model.frames: + if link.parentJoint == joint_id and link.type == pin.FrameType.BODY: + frames.append(link.name) + break + + return np.array(frames, dtype=object) + + def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: """ Get the maximum torques for each joint in all valid configurations. @@ -1091,27 +1117,28 @@ def get_frames(self) -> np.ndarray: - def get_active_frames(self, all_frames : bool = False) -> np.ndarray: + def get_links(self,all_frames : bool = False) -> np.ndarray: """ - Get the array of active joint names in the robot model. - :param all_frames: If True, return all frames, otherwise return only tip frames. - :return: array of active joint names. + Get the array of link names for payload menu. + :param all_frames: If True, return all frames, otherwise return only current tip frames. + :return: array of link names. """ # Get frames where joints are parents frame_names = [] - + + # If all_frames is True, get all link names from the subtrees if all_frames: - for i in range(1, self.model.njoints): - for frame in self.model.frames: - if frame.parentJoint == i and frame.type == pin.FrameType.BODY: - if frame.parentJoint not in self.mimic_joint_ids: - frame_names.append(frame.name) - break + for tree in self.subtrees: + if tree["selected_joint_id"] is not None: + # insert links in the array + for link in tree["link_names"]: + frame_names.append(link) else: + # if all_frames is False, get only the links connected to the selected joint IDs from the subtrees for tree in self.subtrees: - tree["tip_joint_id"] - parent_frame = next((frame for frame in self.model.frames if frame.parentJoint == tree["tip_joint_id"] and frame.type == pin.FrameType.BODY)) - frame_names.append(parent_frame.name) + if tree["selected_joint_id"] is not None: + link_name = self.get_links_from_tree(tree["selected_joint_id"]) + frame_names.append(link_name[0]) return np.array(frame_names, dtype=str) 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 4e8f76b..b21a947 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -35,7 +35,7 @@ def __init__(self, node): # create server for interactive markers self.server = InteractiveMarkerServer(node, 'menu_frames') - # array to store the checked orunchecked frames and payload selection + # array to store the checked or unchecked frames for payload selection self.frames_selection = np.array([],dtype=object) # array to store the selected frame in the subtree menu @@ -53,6 +53,9 @@ def __init__(self, node): # variable to store the selected configuration to display in Rviz self.selected_configuration = None + # variable used as identifier for the payload selection menu items + self.identifier = "id_payload" + # 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, 4.0, 4.5, 5.0, 5.5], dtype=float) @@ -92,29 +95,70 @@ def __init__(self, node): def insert_frame(self, name): """ - Insert a new item(frame) in the checkbox menu of frames. + Insert a new item(frame) in the checkbox menu of frames for payload selection. Args: name (str) : name of the frame """ - last_item = self.menu_handler.insert(name, parent=self.root_frames, callback=self.callback_selection) + last_item = self.menu_handler.insert(name, parent=self.root_frames,command=self.identifier, callback=self.callback_selection) self.menu_handler.setCheckState(last_item, MenuHandler.UNCHECKED) self.menu_handler.setVisible(last_item, True) + # flag to check if the item is already in the checked frames array + flag = False + # add the item to the checked frames array in order to keep track of the checked items - self.frames_selection = np.append(self.frames_selection, {"name": name, "checked" : False, "payload" : 0.0} ) + for i, item in enumerate(self.frames_selection): + if item['name'] == name: + # if the item is already in the array, we need to update it as default + self.frames_selection[i] = {"name": item['name'], "checked": False, "payload": 0.0} + flag = True + break + + if flag is False: + # if the item is not in the array, we need to add it + self.frames_selection = np.append(self.frames_selection, {"name": name, "checked" : False, "payload" : 0.0} ) # apply changes self.menu_handler.reApply(self.server) self.server.applyChanges() - def insert_subtree(self, tree_identifier : int,tip_tree_name : str, joint_names : np.ndarray, joint_ids : np.ndarray): + def remove_frame(self, name): + """ + Remove a item(frame) in the checkbox menu of frames for payload selection. + + Args: + name (str) : name of the frame + """ + # iterate through the menu entries to find the item with the specified name and identifier + # identifier is used to identify the item in the sub menu of payload selection + for i, item in self.menu_handler.entry_contexts_.items(): + if item.title == name and item.command == self.identifier: + self.menu_handler.setCheckState(i, MenuHandler.UNCHECKED) + self.menu_handler.setVisible(i, False) + + # update the frames_selection array to set the item as default value + for i, item in enumerate(self.frames_selection): + if item['name'] == name: + self.frames_selection[i] = {"name": item['name'], "checked": False, "payload": 0.0} + break + + # apply changes + self.menu_handler.reApply(self.server) + self.server.applyChanges() + + + def insert_subtree(self, tree_identifier : int,tip_tree_name : str, joint_names : np.ndarray, joint_ids : np.ndarray, link_names : np.ndarray): """ Insert a new item(subtree) in the checkbox menu of frames. Args: - name (str) : name of the subtree + tree_identifier (int): Identifier of the subtree. + tip_tree_name (str): Name of the tip joint of the subtree. + joint_names (np.ndarray): Names of the joints in the subtree. + joint_ids (np.ndarray): IDs of the joints in the subtree. + link_names (np.ndarray): Names of the links in the subtree. """ last_item = self.menu_handler.insert(f"Tree: [{tip_tree_name}]",command= str(tree_identifier), callback=self.callback_tree_selection) self.menu_handler.setCheckState(last_item, MenuHandler.UNCHECKED) @@ -122,12 +166,12 @@ def insert_subtree(self, tree_identifier : int,tip_tree_name : str, joint_names joints_list = np.array([], dtype=object) - for joint_name,id in zip(joint_names, joint_ids): + for joint_name,id,link_name in zip(joint_names, joint_ids, link_names): # insert the joint as a sub-menu of the subtree - last_entry = self.menu_handler.insert(f"{joint_name}", parent=last_item, command=str(last_item), callback=self.callback_joint_tree_selection) + last_entry = self.menu_handler.insert(f"{link_name}", parent=last_item, command=str(last_item), callback=self.callback_joint_tree_selection) self.menu_handler.setCheckState(last_entry, MenuHandler.UNCHECKED) self.menu_handler.setVisible(last_entry, True) - joints_list = np.append(joints_list,{"joint_name" : joint_name, "joint_id" : id}) + joints_list = np.append(joints_list,{"joint_name" : joint_name, "joint_id" : id,"link_name" : link_name}) self.subtree_selection = np.append(self.subtree_selection, {"tree" : tree_identifier, "joints" : joints_list, "selected_joint_id": None}) @@ -432,22 +476,22 @@ def get_status_joint_tree(self) -> bool: if item['selected_joint_id'] is not None: return True - def update_joint_tree_selection(self, tree_identifier: int, joint_name : str): + def update_joint_tree_selection(self, tree_identifier: int, frame_name : str): """ Update the state of a joint in the subtree menu. Args: tree_identifier (int): Identifier of the subtree. - joint_name (str): Name of the joint to update. + frame_name (str): Name of the selected link in the menu. """ for item in self.subtree_selection: if item['tree'] == tree_identifier: for joint in item['joints']: - if joint['joint_name'] == joint_name and item['selected_joint_id'] != joint['joint_id']: + if joint['link_name'] == frame_name and item['selected_joint_id'] != joint['joint_id']: item['selected_joint_id'] = joint['joint_id'] break - elif joint['joint_name'] == joint_name and item['selected_joint_id'] == joint['joint_id']: + elif joint['link_name'] == frame_name and item['selected_joint_id'] == joint['joint_id']: item['selected_joint_id'] = None break 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 23083d5..6aae665 100755 --- 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 @@ -85,7 +85,10 @@ def __init__(self): # variable to store masses applied on the frames self.masses = None - # variable to store the currente selected configuration from the workspace menu + # variable to store current selected links for payload selection + self.links = None + + # variable to store the current selected configuration from the workspace menu self.valid_configurations = None # id of the valid configurations to detect changes @@ -108,6 +111,8 @@ def __init__(self): self.timer_update_checked_items = self.create_timer(0.5, self.update_checked_items) # timer to publish the external forces as arrows in RViz self.timer_publish_force = self.create_timer(1.0, self.publish_payload_force) + # timer to update items in the menu for payload selection + self.timer_update_payload_selection = self.create_timer(0.5, self.update_payload_selection) self.get_logger().info('Robot description subscriber node started') @@ -124,16 +129,33 @@ def robot_description_callback(self, msg): self.robot.print_active_joint() self.robot.print_frames() - # Add the frame to the menu for payload selection with the advanced mode parameter - for frame in self.robot.get_active_frames(self.get_parameter('advanced_mode').get_parameter_value().bool_value): - self.menu.insert_frame(frame) - # Add subtree to the menu for i, subtree in enumerate(self.robot.get_subtrees()): - self.menu.insert_subtree(i,subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"]) + self.menu.insert_subtree(i,subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"], subtree["link_names"]) + - # self.robot.print_configuration() + def update_payload_selection(self): + """ + Callback function to update the payload selection in the menu. + """ + if self.robot is not None: + # Add the frame to the menu for payload selection with the advanced mode parameter + links = self.robot.get_links(self.get_parameter('advanced_mode').get_parameter_value().bool_value) + + for link in links: + if link in self.links: + continue # Skip if the link is already in the menu + else: + self.menu.insert_frame(str(link)) + + # remove other links that are not in the current link selection + if self.links is not None: + for link in self.links: + if link not in links: + self.menu.remove_frame(str(link)) + + self.links = links # Update the links to the current ones def publish_payload_force(self): """ @@ -498,8 +520,6 @@ def generate_workspace_markers(self): return marker_points, marker_point_names - - # TODO : try to optimize the function to avoid iterating through all the valid configurations every time def publish_workspace_area_maximum_payload_area(self): """ Publish the workspace area and maximum payloads area in RViz using points and labels for the end points. From 23fba76b6409225632dcb3185a41f91dcd19878d Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 8 Aug 2025 11:14:51 +0200 Subject: [PATCH 31/44] changed way of publishing marker labels --- .../rviz_visualization_menu.py | 110 ++++++++---------- 1 file changed, 49 insertions(+), 61 deletions(-) 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 6aae665..38b110d 100755 --- 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 @@ -64,9 +64,6 @@ def __init__(self): # 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) - # subscription to joint states self.joint_states_subscription = self.create_subscription(JointState, '/joint_states', self.joint_states_callback, 10) @@ -74,7 +71,7 @@ def __init__(self): self.publisher_joint_states = self.create_publisher(JointState, '/joint_states', 10) # variable to store the object of the TorqueCalculator - self.robot = None + self.robot_handler = None # variable to store external force applied on the robot self.external_force = None @@ -97,7 +94,6 @@ def __init__(self): # variable to store markers for the workspace area and maximum payloads area self.marker_points = None - self.marker_point_names = None self.marker_max_payloads = None # variable to store if there is a selected configuration from the workspace menu to visualize @@ -125,12 +121,12 @@ def robot_description_callback(self, msg): self.get_logger().info('Received robot description') - self.robot = TorqueCalculator(robot_description = msg.data) - self.robot.print_active_joint() - self.robot.print_frames() + self.robot_handler = TorqueCalculator(robot_description = msg.data) + self.robot_handler.print_active_joint() + self.robot_handler.print_frames() # Add subtree to the menu - for i, subtree in enumerate(self.robot.get_subtrees()): + for i, subtree in enumerate(self.robot_handler.get_subtrees()): self.menu.insert_subtree(i,subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"], subtree["link_names"]) @@ -138,10 +134,10 @@ def update_payload_selection(self): """ Callback function to update the payload selection in the menu. """ - if self.robot is not None: + if self.robot_handler is not None: # Add the frame to the menu for payload selection with the advanced mode parameter - links = self.robot.get_links(self.get_parameter('advanced_mode').get_parameter_value().bool_value) + links = self.robot_handler.get_links(self.get_parameter('advanced_mode').get_parameter_value().bool_value) for link in links: if link in self.links: @@ -165,13 +161,13 @@ def publish_payload_force(self): for frame in self.menu.get_item_state(): - id_force = self.robot.get_parent_joint_id(frame["name"]) + id_force = self.robot_handler.get_parent_joint_id(frame["name"]) # use the selected configuration from the menu to get the right joint placement if self.selected_configuration is not None: - joint_position = self.robot.get_joint_placement(id_force,self.valid_configurations[self.selected_configuration]["config"]) + joint_position = self.robot_handler.get_joint_placement(id_force,self.valid_configurations[self.selected_configuration]["config"]) else: - joint_position = self.robot.get_joint_placement(id_force,self.robot.get_zero_configuration()) + joint_position = self.robot_handler.get_joint_placement(id_force,self.robot_handler.get_zero_configuration()) arrow_force = Marker() @@ -217,10 +213,10 @@ 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(range = 2,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) + self.valid_configurations = self.robot_handler.get_valid_workspace(range = 2,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) # compute the maximum payloads for the valid configurations - self.valid_configurations = self.robot.compute_maximum_payloads(self.valid_configurations) + self.valid_configurations = self.robot_handler.compute_maximum_payloads(self.valid_configurations) # insert the valid configurations in the menu self.menu.insert_dropdown_configuration(self.valid_configurations) @@ -249,7 +245,7 @@ def publish_selected_configuration(self): # if there is a selected configuration, publish the joint states based on the valid configurations calculated previously if self.selected_configuration is not None: - configs = self.robot.get_position_for_joint_states(self.valid_configurations[self.selected_configuration]["config"]) + configs = self.robot_handler.get_position_for_joint_states(self.valid_configurations[self.selected_configuration]["config"]) joint_state = JointState() joint_state.header.stamp = self.get_clock().now().to_msg() @@ -259,13 +255,13 @@ def publish_selected_configuration(self): self.publisher_joint_states.publish(joint_state) else: - if self.robot is not None: + if self.robot_handler is not None: # if there is no selected configuration, publish the joint states with zero positions joint_state = JointState() joint_state.header.stamp = self.get_clock().now().to_msg() - joint_state.name = self.robot.get_joints().tolist() - zero_config = self.robot.get_position_for_joint_states(self.robot.get_zero_configuration()) + joint_state.name = self.robot_handler.get_joints().tolist() + zero_config = self.robot_handler.get_position_for_joint_states(self.robot_handler.get_zero_configuration()) joint_state.position = [joint["q"] for joint in zero_config] self.publisher_joint_states.publish(joint_state) @@ -288,7 +284,7 @@ def update_checked_items(self): selected_joint_ids = [[joint["tree"], joint["selected_joint_id"]] for joint in self.menu.get_joint_tree_selection()] for tree_id, joint_id in selected_joint_ids: # set the joint tree selection in the robot based on the selected joints in the menu - self.robot.set_joint_tree_selection(tree_id, joint_id) + self.robot_handler.set_joint_tree_selection(tree_id, joint_id) @@ -296,35 +292,35 @@ def joint_states_callback(self, msg): """ Callback function for the joint states topic. """ - if self.robot is not None: + if self.robot_handler is not None: # if you are not using the calculated configuration from workspace, you can use the joint states to compute the torques because you don't have already the computed torques if self.selected_configuration is None: self.positions = list(msg.position) self.name_positions = list(msg.name) - v = msg.velocity if msg.velocity else self.robot.get_zero_velocity() + v = msg.velocity if msg.velocity else self.robot_handler.get_zero_velocity() # set the positions based on the joint states - q = self.robot.set_position(self.positions, self.name_positions) - a = self.robot.get_zero_acceleration() + q = self.robot_handler.set_position(self.positions, self.name_positions) + a = self.robot_handler.get_zero_acceleration() # if there are no checked frames, set the external force to None if len(self.checked_frames) != 0: # create the external force with the masses and the checked frames - self.external_force = self.robot.create_ext_force(masses=self.masses, frame_name=self.checked_frames, q=q) + self.external_force = self.robot_handler.create_ext_force(masses=self.masses, frame_name=self.checked_frames, q=q) else: self.external_force = None # compute the inverse dynamics - tau = self.robot.compute_inverse_dynamics(q, v, a, extForce=self.external_force) + tau = self.robot_handler.compute_inverse_dynamics(q, v, a, extForce=self.external_force) # check the effort limits - status_efforts = self.robot.check_effort_limits(tau) + status_efforts = self.robot_handler.check_effort_limits(tau) # print the torques - self.robot.print_torques(tau) + self.robot_handler.print_torques(tau) # get the positions of the joints where the torques are applied based on - joints_position, offset_z = self.robot.get_joints_placements(q) + joints_position, offset_z = self.robot_handler.get_joints_placements(q) # Publish the torques in RViz self.publish_label_torques(tau, status_torques=status_efforts ,joints_position=joints_position) @@ -333,9 +329,9 @@ def joint_states_callback(self, msg): # if you are using the calculated configuration from workspace, you can use the valid configurations to visualize the torques labels # get the positions of the joints where the torques are applied based on - joints_position, offset_z = self.robot.get_joints_placements(self.valid_configurations[self.selected_configuration]["config"]) + joints_position, offset_z = self.robot_handler.get_joints_placements(self.valid_configurations[self.selected_configuration]["config"]) # get the torques status (if the torques are within the limits) - status_efforts = self.robot.check_effort_limits(self.valid_configurations[self.selected_configuration]["tau"]) + status_efforts = self.robot_handler.check_effort_limits(self.valid_configurations[self.selected_configuration]["tau"]) self.publish_label_torques(self.valid_configurations[self.selected_configuration]["tau"], status_torques=status_efforts ,joints_position=joints_position) @@ -409,14 +405,12 @@ def generate_workspace_markers(self): It also creates labels for the end points of the valid configurations. """ - # Create a MarkerArray to visualize the number of configuration of a specific point in the workspace - marker_point_names = MarkerArray() - # Create a Marker for the workspace area using points - marker_points = MarkerArray() + # Create a Marker for the workspace area using points and labels + marker_ws = MarkerArray() # 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 = self.robot.get_maximum_torques(self.valid_configurations) + max_torque_for_joint = self.robot_handler.get_maximum_torques(self.valid_configurations) cont = 0 # Iterate through the valid configurations and create markers @@ -427,7 +421,7 @@ def generate_workspace_markers(self): marker_point_name.header.frame_id = "base_link" marker_point_name.header.stamp = Time() - marker_point_name.ns = f"workspace_area__tree_{valid_config['tree_id']}" + marker_point_name.ns = f"labels_workspace_area__tree_{valid_config['tree_id']}" marker_point_name.id = i + 1 marker_point_name.type = Marker.TEXT_VIEW_FACING marker_point_name.text = f"Config {i}" @@ -445,19 +439,19 @@ def generate_workspace_markers(self): 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"]) + joint_positions, offset_z = self.robot_handler.get_joints_placements(valid_config["config"]) # get the normalized torques for the valid configuration with current target limits for color visualization if self.menu.get_torque_color() == TorqueVisualizationType.TORQUE_LIMITS: - norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"]) + norm_joints_torques = self.robot_handler.get_normalized_torques(valid_config["tau"]) else: # if the user selected the max torque, use the maximum torques for the joint - norm_joints_torques = self.robot.get_normalized_torques(valid_config["tau"],max_torque_for_joint, valid_config["tree_id"]) + norm_joints_torques = self.robot_handler.get_normalized_torques(valid_config["tau"],max_torque_for_joint, valid_config["tree_id"]) # insert points related to the end effector position in the workspace area and with color based on the normalized torques for each joint for joint_pos,tau in zip(joint_positions,norm_joints_torques): # print only the points for the corrisponding tree_id of the valid configuration - if self.robot.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): + if self.robot_handler.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): point = Marker() point.header.frame_id = "base_link" point.header.stamp = Time() @@ -481,13 +475,13 @@ def generate_workspace_markers(self): cont += 1 # Add the point to the points array - marker_points.markers.append(point) + marker_ws.markers.append(point) - # Add the marker point name to the marker point names array - marker_point_names.markers.append(marker_point_name) + # Add the marker point name to the array + marker_ws.markers.append(marker_point_name) # get the unified torque for the valid configurations - unified_configurations_torque = self.robot.get_unified_configurations_torque(self.valid_configurations) + unified_configurations_torque = self.robot_handler.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 @@ -514,10 +508,10 @@ def generate_workspace_markers(self): marker_point.color.b = 0.0 # Blue # Add the marker point to the points array - marker_points.markers.append(marker_point) + marker_ws.markers.append(marker_point) - return marker_points, marker_point_names + return marker_ws def publish_workspace_area_maximum_payload_area(self): @@ -527,21 +521,18 @@ def publish_workspace_area_maximum_payload_area(self): self.id_current_valid_config = self.get_id_current_valid_configurations() - # check if the valid configurations is different from the previous one + # check if the valid configurations are different from the previous one if self.id_current_valid_config != self.prev_id_current_valid_config: # generate the markers for the workspace area - self.marker_points, self.marker_point_names = self.generate_workspace_markers() + self.marker_points = self.generate_workspace_markers() self.marker_max_payloads = self.generate_maximum_payloads_markers() else: # if the valid configurations are the same as the previous one, use the previous markers - if self.marker_points is not None and self.marker_point_names is not None: + if self.marker_points is not None: # update the header stamp of the markers to the current time for marker in self.marker_points.markers: marker.header.stamp = Time() - for marker in self.marker_point_names.markers: - marker.header.stamp = Time() - if self.marker_max_payloads is not None: # update the header stamp of the maximum payloads markers to the current time for marker in self.marker_max_payloads.markers: @@ -549,9 +540,8 @@ def publish_workspace_area_maximum_payload_area(self): self.prev_id_current_valid_config = self.id_current_valid_config - # Publish the marker points and names + # Publish the marker points self.publisher_workspace_area.publish(self.marker_points) - self.publisher_workspace_area_names.publish(self.marker_point_names) # Publish the maximum payloads markers and labels self.publisher_maximum_payloads.publish(self.marker_max_payloads) @@ -565,7 +555,7 @@ def generate_maximum_payloads_markers(self): marker_max_payloads = MarkerArray() # get the maximum payloads for each arm based on the valid configurations - max_payloads = self.robot.get_maximum_payloads(self.valid_configurations) + max_payloads = self.robot_handler.get_maximum_payloads(self.valid_configurations) # Iterate through the valid configurations and create markers for i, valid_config in enumerate(self.valid_configurations): @@ -593,11 +583,11 @@ def generate_maximum_payloads_markers(self): 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"]) + joint_positions, offset_z = self.robot_handler.get_joints_placements(valid_config["config"]) # get the normalized payload for the valid configuration with target as maximum payloads for each tree max_payload_for_tree = next(payload["max_payload"] for payload in max_payloads if payload["tree_id"] == valid_config["tree_id"]) - norm_payload = self.robot.get_normalized_payload(valid_config["max_payload"], max_payload_for_tree) + norm_payload = self.robot_handler.get_normalized_payload(valid_config["max_payload"], max_payload_for_tree) point = Marker() @@ -663,8 +653,6 @@ def clear_workspace_area_markers(self): # Publish the empty MarkerArray to clear the markers self.publisher_workspace_area.publish(marker_array_msg) - self.publisher_workspace_area_names.publish(marker_array_msg) - From 5dc2b44aa200502cae5996013a1bb6ee7238d01b Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 11 Aug 2025 13:30:14 +0200 Subject: [PATCH 32/44] changed name of tip link in tree selection --- .../dynamic_payload_analysis_core/core.py | 2 +- .../dynamic_payload_analysis_ros/menu_visual.py | 2 +- .../dynamic_payload_analysis_ros/rviz_visualization_menu.py | 2 +- 3 files 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 e51d867..6b2b855 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -162,7 +162,7 @@ def compute_subtrees(self): # get the joint names in the sub-tree joint_names = [self.model.names[joint_id] for joint_id in joint_tree_ids] - self.subtrees = np.append(self.subtrees, {"tree_id": cont, "link_names": link_names ,"joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_joint_name": self.model.names[joint_tree_ids[-1]], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) + self.subtrees = np.append(self.subtrees, {"tree_id": cont, "link_names": link_names ,"joint_names": joint_names, "joint_ids": joint_tree_ids,"tip_link_name": link_names[-1], "tip_joint_id": joint_tree_ids[-1], "selected_joint_id": None}) cont += 1 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 b21a947..9a48743 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -155,7 +155,7 @@ def insert_subtree(self, tree_identifier : int,tip_tree_name : str, joint_names Args: tree_identifier (int): Identifier of the subtree. - tip_tree_name (str): Name of the tip joint of the subtree. + tip_tree_name (str): Name of the tip link of the subtree. joint_names (np.ndarray): Names of the joints in the subtree. joint_ids (np.ndarray): IDs of the joints in the subtree. link_names (np.ndarray): Names of the links in the subtree. 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 38b110d..409d84d 100755 --- 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 @@ -127,7 +127,7 @@ def robot_description_callback(self, msg): # Add subtree to the menu for i, subtree in enumerate(self.robot_handler.get_subtrees()): - self.menu.insert_subtree(i,subtree['tip_joint_name'], subtree["joint_names"], subtree["joint_ids"], subtree["link_names"]) + self.menu.insert_subtree(i,subtree['tip_link_name'], subtree["joint_names"], subtree["joint_ids"], subtree["link_names"]) def update_payload_selection(self): From 05376e18ed62c4203993283268d482d7394ed6a2 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 11 Aug 2025 13:45:16 +0200 Subject: [PATCH 33/44] remove not necessary imported libraries --- .../dynamic_payload_analysis_core/core.py | 7 ++----- .../dynamic_payload_analysis_ros/menu_visual.py | 2 -- .../rviz_visualization_menu.py | 3 +-- 3 files changed, 3 insertions(+), 9 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 6b2b855..6c2ae4e 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -20,11 +20,8 @@ from typing import Union from pathlib import Path from numpy.linalg import norm, solve -from optik import Robot, SolverConfig import tempfile -from ikpy import chain import os -import xml.etree.ElementTree as ET from urdf_parser_py.urdf import URDF @@ -86,8 +83,8 @@ def compute_mimic_joints(self, urdf_xml): """ try: robot = URDF.from_xml_string(urdf_xml) - except ET.ParseError as e: - print(f"Error parsing URDF xml: {e}") + except: + print(f"Error parsing URDF xml") self.mimic_joint_names = [] self.mimicked_joint_names = [] 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 9a48743..bf97e1b 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -14,12 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys import numpy as np from interactive_markers import InteractiveMarkerServer from interactive_markers import MenuHandler -import rclpy from visualization_msgs.msg import InteractiveMarker from visualization_msgs.msg import InteractiveMarkerControl from visualization_msgs.msg import Marker 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 409d84d..4fadcdc 100755 --- 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 @@ -16,8 +16,7 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import String, ColorRGBA -from geometry_msgs.msg import WrenchStamped, Point +from std_msgs.msg import String from sensor_msgs.msg import JointState from dynamic_payload_analysis_core.core import TorqueCalculator import numpy as np From 58236d81c077cf791b648dfbd5774ddcfe10f122 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Mon, 11 Aug 2025 15:26:39 +0200 Subject: [PATCH 34/44] adjusted visualization color for unified torque points --- .../dynamic_payload_analysis_core/core.py | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 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 6c2ae4e..a320272 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -758,8 +758,8 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd torques_sum = np.array([], dtype=float) norm_torques = np.array([], dtype=float) - # array to store the maximum sum of torques for each tree - max_value_torques = np.array([], dtype=float) + # array to store max and min sum of torques for each tree + max_min_value_torques = np.array([], dtype=float) sum = 0.0 for valid_config in valid_configs: @@ -768,7 +768,7 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd tau = valid_config["tau"] # calculate the sum of torques for each joint configuration - for joint, torque in zip(q, tau): + for torque in tau: #if abs(torque) < 50: sum += abs(torque) @@ -783,18 +783,22 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd if tree_sums: # Check if there are any sums for this tree max_value = max(tree_sums) + min_value = min(tree_sums) else: - max_value = 0.0 # Default value if no sums found + max_value = 1.0 # Default value if no sums found + min_value = 0.0 # Default value if no sums found - max_value_torques = np.append(max_value_torques, {"tree_id": tree["tree_id"], "max_value": max_value}) + max_min_value_torques = np.append(max_min_value_torques, {"tree_id": tree["tree_id"], "max_value": max_value, "min_value": min_value}) # Normalize the torques vector to a unified scale for tau in torques_sum: - # Find the corresponding max_value for the current tree_id - max_value = next(item["max_value"] for item in max_value_torques if item["tree_id"] == tau["tree_id"]) - norm_tau = tau["sum"] / max_value + # Find the corresponding max_value and min_value for the current tree_id + max_value = next(item["max_value"] for item in max_min_value_torques if item["tree_id"] == tau["tree_id"]) + min_value = next(item["min_value"] for item in max_min_value_torques if item["tree_id"] == tau["tree_id"]) + + norm_tau = (tau["sum"] - min_value ) / ( max_value - min_value) # append the normalized torque to the array From b80d223ba32375ddb326d3e9353195319364d121 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 12 Aug 2025 16:11:15 +0200 Subject: [PATCH 35/44] changed range of IK solver --- .../dynamic_payload_analysis_core/core.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a320272..50f12d6 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -410,7 +410,7 @@ def compute_all_configurations(self, range : int, resolution : int, end_joint_id # Iterate over the range to compute all configurations for x in np.arange(-range/2, range/2 , resolution): for y in np.arange(-range/2, range/2 , resolution): - for z in np.arange(0, range , resolution): + for z in np.arange(-range/2, range/2 , resolution): target_position = pin.SE3(np.eye(3), np.array([x, y, z])) new_q = self.compute_inverse_kinematics(q, target_position, end_joint_id) From c6cf8cc5084d69c1233e81d4b3349ca44bf22492 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 13 Aug 2025 09:27:05 +0200 Subject: [PATCH 36/44] fix range IK --- .../dynamic_payload_analysis_core/core.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 50f12d6..52a019d 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -410,7 +410,7 @@ def compute_all_configurations(self, range : int, resolution : int, end_joint_id # Iterate over the range to compute all configurations for x in np.arange(-range/2, range/2 , resolution): for y in np.arange(-range/2, range/2 , resolution): - for z in np.arange(-range/2, range/2 , resolution): + for z in np.arange(-range/2, range , resolution): target_position = pin.SE3(np.eye(3), np.array([x, y, z])) new_q = self.compute_inverse_kinematics(q, target_position, end_joint_id) From f037ad88051bc9ed2c807a502cb2540bc1bba6ca Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 13 Aug 2025 11:16:02 +0200 Subject: [PATCH 37/44] small change in the range of IK --- .../dynamic_payload_analysis_core/core.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 52a019d..3071838 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -408,8 +408,8 @@ def compute_all_configurations(self, range : int, resolution : int, end_joint_id configurations = [] # Iterate over the range to compute all configurations - for x in np.arange(-range/2, range/2 , resolution): - for y in np.arange(-range/2, range/2 , resolution): + for x in np.arange(-range, range , resolution): + for y in np.arange(-range, range , resolution): for z in np.arange(-range/2, range , resolution): target_position = pin.SE3(np.eye(3), np.array([x, y, z])) new_q = self.compute_inverse_kinematics(q, target_position, end_joint_id) From 9650ee0d35e18fc1111e7d247904cb66c51dd544 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 14 Aug 2025 15:49:01 +0200 Subject: [PATCH 38/44] added removed files --- 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 | 34 +++++++++++++++++++ .../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 | 29 ++++++++++++++++ .../test/test_copyright.py | 25 ++++++++++++++ .../test/test_flake8.py | 25 ++++++++++++++ .../test/test_pep257.py | 23 +++++++++++++ 15 files changed, 220 insertions(+) create mode 100644 doc/README.md create mode 100644 dynamic_payload_analysis_core/dynamic_payload_analysis_core/__init__.py create mode 100644 dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core create mode 100644 dynamic_payload_analysis_core/setup.cfg create mode 100644 dynamic_payload_analysis_core/setup.py create mode 100644 dynamic_payload_analysis_core/test/test_copyright.py create mode 100644 dynamic_payload_analysis_core/test/test_flake8.py create mode 100644 dynamic_payload_analysis_core/test/test_pep257.py create mode 100644 dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/__init__.py create mode 100644 dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros create mode 100644 dynamic_payload_analysis_ros/setup.cfg create mode 100644 dynamic_payload_analysis_ros/setup.py create mode 100644 dynamic_payload_analysis_ros/test/test_copyright.py create mode 100644 dynamic_payload_analysis_ros/test/test_flake8.py create mode 100644 dynamic_payload_analysis_ros/test/test_pep257.py diff --git a/doc/README.md b/doc/README.md new file mode 100644 index 0000000..66a490b --- /dev/null +++ b/doc/README.md @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..e69de29 diff --git a/dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core b/dynamic_payload_analysis_core/resource/dynamic_payload_analysis_core new file mode 100644 index 0000000..e69de29 diff --git a/dynamic_payload_analysis_core/setup.cfg b/dynamic_payload_analysis_core/setup.cfg new file mode 100644 index 0000000..62c9e13 --- /dev/null +++ b/dynamic_payload_analysis_core/setup.cfg @@ -0,0 +1,4 @@ +[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 new file mode 100644 index 0000000..de600e7 --- /dev/null +++ b/dynamic_payload_analysis_core/setup.py @@ -0,0 +1,34 @@ +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']), + ], + 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'], + install_requires=[ + 'setuptools', + 'numpy', + 'urdf-parser-py>=0.0.4', + 'math', + 'typing', + 'pathlib', + 'tempfile', + 'os', + ], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/dynamic_payload_analysis_core/test/test_copyright.py b/dynamic_payload_analysis_core/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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 new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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 new file mode 100644 index 0000000..e69de29 diff --git a/dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros b/dynamic_payload_analysis_ros/resource/dynamic_payload_analysis_ros new file mode 100644 index 0000000..e69de29 diff --git a/dynamic_payload_analysis_ros/setup.cfg b/dynamic_payload_analysis_ros/setup.cfg new file mode 100644 index 0000000..5a64333 --- /dev/null +++ b/dynamic_payload_analysis_ros/setup.cfg @@ -0,0 +1,4 @@ +[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 new file mode 100644 index 0000000..b8e2ae2 --- /dev/null +++ b/dynamic_payload_analysis_ros/setup.py @@ -0,0 +1,29 @@ +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']), + ], + 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'], + install_requires=[ + 'setuptools', + 'numpy' + ], + entry_points={ + 'console_scripts': [ + '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 new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/dynamic_payload_analysis_ros/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/dynamic_payload_analysis_ros/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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 new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/dynamic_payload_analysis_ros/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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 78851594f6d2d12f0632dddfc3a3cf1349794cda Mon Sep 17 00:00:00 2001 From: Enrico Moro <84412351+enrico391@users.noreply.github.com> Date: Thu, 14 Aug 2025 15:53:57 +0200 Subject: [PATCH 39/44] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../dynamic_payload_analysis_core/core.py | 9 ++------- 1 file changed, 2 insertions(+), 7 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 52a019d..c9e22b6 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -91,7 +91,7 @@ def compute_mimic_joints(self, urdf_xml): # Iterate through all joints in the robot model to find mimic joints for joint in robot.joints: - if joint.mimic is not None: + if joint.mimic: # Add the mimic joint name to the list self.mimic_joint_names.append(joint.name) # Add the mimicked joint name to the list @@ -131,16 +131,11 @@ def compute_subtrees(self): This method is used to compute the sub-trees of the robot model """ - ## build subtrees: - #print("## Building subtrees ...") - # First, find all the tip joints : joints that are the leaves of the kinematic chain: tip_joints = [] for id in range(0, self.model.njoints): if len(self.model.subtrees[id]) == 1: tip_joints += [id] - #print("Total number of kinematic trees: ", len(tip_joints) - #print("Tip joints : ", tip_joints) self.subtrees = np.array([], dtype=object) cont = 0 for i, jointID in enumerate(tip_joints): @@ -502,7 +497,7 @@ def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarr # user selects the joint later - if configuration["configurations"] is not None and tree["selected_joint_id"] is not None: + if configuration["configurations"] and tree["selected_joint_id"]: # Verify the configurations to check if they are valid valid_configurations = self.verify_configurations(configuration["configurations"], masses, checked_frames, tree["tree_id"], tree["selected_joint_id"]) From d6558108a1bac5376dbe206f001dd7a012d2f8ce Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 14 Aug 2025 16:56:30 +0200 Subject: [PATCH 40/44] added root joint name for a universal usage with multiple type of robot --- .../dynamic_payload_analysis_core/core.py | 36 +++++++++++++- .../menu_visual.py | 26 +++++++++- .../rviz_visualization_menu.py | 49 ++++++++++--------- 3 files changed, 85 insertions(+), 26 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 3a5c57f..8498c26 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -41,6 +41,9 @@ def __init__(self, robot_description : Union[str, Path]): # compute mimic joints self.compute_mimic_joints(robot_description) + # get the root joint name + self.root_name = self.get_root_joint_name(robot_description) + # create temporary URDF file from the robot description string with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', delete=False) as temp_file: temp_file.write(robot_description) @@ -66,6 +69,8 @@ def __init__(self, robot_description : Union[str, Path]): # compute main trees of the robot model self.compute_subtrees() + + # array to store all configurations for the robot model self.configurations = np.array([], dtype=object) @@ -73,6 +78,22 @@ def __init__(self, robot_description : Union[str, Path]): for tree in self.subtrees: self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) + + def get_root_joint_name(self, robot_description: str) -> str: + """ + Get the root joint name from the robot description XML string. + + :param robot_description: Robot description in XML format provided by /robot_description topic. + """ + try: + robot = URDF.from_xml_string(robot_description) + root_name = robot.get_root() + except Exception as e: + print(f"Error parsing URDF xml: {e}") + root_name = None + + return root_name + def compute_mimic_joints(self, urdf_xml): """ @@ -497,7 +518,7 @@ def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarr # user selects the joint later - if configuration["configurations"] and tree["selected_joint_id"]: + if configuration["configurations"] is not None and tree["selected_joint_id"] is not None: # Verify the configurations to check if they are valid valid_configurations = self.verify_configurations(configuration["configurations"], masses, checked_frames, tree["tree_id"], tree["selected_joint_id"]) @@ -1033,7 +1054,7 @@ def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: :param q: Joint configuration vector. :return: Array of joint placements with names of joint, and z offset of base link. """ - base_link_id = self.model.getFrameId("base_link") + base_link_id = self.model.getFrameId(self.root_name) offset_z = self.data.oMf[base_link_id].translation[2] # Get the z offset of the base link self.update_configuration(q) @@ -1139,6 +1160,17 @@ def get_links(self,all_frames : bool = False) -> np.ndarray: return np.array(frame_names, dtype=str) + def get_root_name(self) -> str: + """ + Get the name of the root frame of the robot model. + + :return: Name of the root frame. + """ + + if self.root_name is None: + raise ValueError("Root name is not set") + + return self.root_name def get_parent_joint_id(self, frame_name : str) -> int: """ 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 bf97e1b..b0b87dc 100755 --- a/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py +++ b/dynamic_payload_analysis_ros/dynamic_payload_analysis_ros/menu_visual.py @@ -29,7 +29,15 @@ class TorqueVisualizationType(Enum): MAX_CURRENT_TORQUE = 2 class MenuPayload(): - def __init__(self, node): + def __init__(self, node, root_joint_name : str): + """ + Initialize the menu for payload selection in Rviz. + + param node: Node to create the interactive marker server. + param root_joint_name: Name of the root joint for the interactive marker server. + This is used to set the frame_id of the interactive marker. + """ + # create server for interactive markers self.server = InteractiveMarkerServer(node, 'menu_frames') @@ -45,6 +53,9 @@ def __init__(self, node): #current managed frame self.current_frame = None + # root joint name for the interactive marker server + self.root_joint_name = root_joint_name + # flag to compute workspace self.compute_workspace = False @@ -633,6 +644,15 @@ def set_workspace_state(self, state: bool): self.compute_workspace = state + def set_root_joint_name(self, name: str): + """ + Set the root joint name for the interactive marker server. + + Args: + name (str): Name of the root joint. + """ + self.root_joint_name = name + def get_item_state(self) -> np.ndarray: """ Return array of checked frames in the menu list @@ -675,7 +695,9 @@ def make_empty_marker(self, dummyBox=True): Create interactive marker """ int_marker = InteractiveMarker() - int_marker.header.frame_id = 'base_link' + + int_marker.header.frame_id = self.root_joint_name + int_marker.pose.position.z = 2.0 int_marker.scale = 0.5 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 4fadcdc..3ec4157 100755 --- 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 @@ -48,8 +48,8 @@ def __init__(self): qos_profile=rclpy.qos.QoSProfile( durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth = 1) ) - # menu for selecting frames to apply payload - self.menu = MenuPayload(self) + # object to handle interactive markers in RViz + self.menu = None # Publisher for external force self.publisher_force = self.create_publisher(MarkerArray, '/external_forces', 10) @@ -123,6 +123,10 @@ def robot_description_callback(self, msg): self.robot_handler = TorqueCalculator(robot_description = msg.data) self.robot_handler.print_active_joint() self.robot_handler.print_frames() + + if self.menu is None: + # menu for selecting frames to apply payload + self.menu = MenuPayload(self, self.robot_handler.get_root_name()) # Add subtree to the menu for i, subtree in enumerate(self.robot_handler.get_subtrees()): @@ -170,7 +174,7 @@ def publish_payload_force(self): arrow_force = Marker() - arrow_force.header.frame_id = "base_link" + arrow_force.header.frame_id = self.robot_handler.get_root_name() arrow_force.header.stamp = Time() arrow_force.ns = "external_force" arrow_force.id = id_force @@ -270,20 +274,21 @@ def update_checked_items(self): """ Function to update the external forces based on the checked frames in the menu and joint selection in the subtree menu. """ - # create the array with only the checked frames (with external force applied) - self.checked_frames = np.array([check_frame["name"] for check_frame in self.menu.get_item_state() if check_frame['checked']]) - - if len(self.checked_frames) != 0: - # create the array with the masses of the checked frames - self.masses = np.array([check_frame["payload"] for check_frame in self.menu.get_item_state() if check_frame['checked']]) - else: - # if there are no checked frames, set the masses to None - self.masses = None + if self.menu is not None: + # create the array with only the checked frames (with external force applied) + self.checked_frames = np.array([check_frame["name"] for check_frame in self.menu.get_item_state() if check_frame['checked']]) + + if len(self.checked_frames) != 0: + # create the array with the masses of the checked frames + self.masses = np.array([check_frame["payload"] for check_frame in self.menu.get_item_state() if check_frame['checked']]) + else: + # if there are no checked frames, set the masses to None + self.masses = None - selected_joint_ids = [[joint["tree"], joint["selected_joint_id"]] for joint in self.menu.get_joint_tree_selection()] - for tree_id, joint_id in selected_joint_ids: - # set the joint tree selection in the robot based on the selected joints in the menu - self.robot_handler.set_joint_tree_selection(tree_id, joint_id) + selected_joint_ids = [[joint["tree"], joint["selected_joint_id"]] for joint in self.menu.get_joint_tree_selection()] + for tree_id, joint_id in selected_joint_ids: + # set the joint tree selection in the robot based on the selected joints in the menu + self.robot_handler.set_joint_tree_selection(tree_id, joint_id) @@ -351,7 +356,7 @@ def publish_label_torques(self, torque: np.ndarray, status_torques : np.ndarray, for i, (t, joint) in enumerate(zip(torque, joints_position)): if "gripper" not in joint['name']: marker = Marker() - marker.header.frame_id = "base_link" + marker.header.frame_id = self.robot_handler.get_root_name() marker.header.stamp = Time() marker.ns = "torque_visualization" marker.id = i @@ -417,7 +422,7 @@ def generate_workspace_markers(self): # create the label for the end point (end effector position) of the valid configuration marker_point_name = Marker() - marker_point_name.header.frame_id = "base_link" + marker_point_name.header.frame_id = self.robot_handler.get_root_name() marker_point_name.header.stamp = Time() marker_point_name.ns = f"labels_workspace_area__tree_{valid_config['tree_id']}" @@ -452,7 +457,7 @@ def generate_workspace_markers(self): # print only the points for the corrisponding tree_id of the valid configuration if self.robot_handler.verify_member_tree(valid_config["tree_id"],joint_pos["id"]): point = Marker() - point.header.frame_id = "base_link" + point.header.frame_id = self.robot_handler.get_root_name() point.header.stamp = Time() point.ns = joint_pos["name"] point.id = cont @@ -486,7 +491,7 @@ def generate_workspace_markers(self): # this is used to visualize the workspace area with the unified torques for each configuration for i, unified_config in enumerate(unified_configurations_torque): marker_point = Marker() - marker_point.header.frame_id = "base_link" + marker_point.header.frame_id = self.robot_handler.get_root_name() marker_point.header.stamp = Time() marker_point.ns = f"unified_torque_workspace_tree_{unified_config['tree_id']}" marker_point.id = i @@ -561,7 +566,7 @@ def generate_maximum_payloads_markers(self): # 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.frame_id = self.robot_handler.get_root_name() marker_point_name.header.stamp = Time() marker_point_name.ns = f"label_payloads_tree_{valid_config['tree_id']}" @@ -590,7 +595,7 @@ def generate_maximum_payloads_markers(self): point = Marker() - point.header.frame_id = "base_link" + point.header.frame_id = self.robot_handler.get_root_name() point.header.stamp = Time() point.ns = f"max_payloads_tree_{valid_config['tree_id']}" point.id = i From 317622ebd16eafd311b7a89c181a96737f169d48 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 14 Aug 2025 17:02:30 +0200 Subject: [PATCH 41/44] added IK range as node parameter --- .../rviz_visualization_menu.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 3ec4157..fc08773 100755 --- 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 @@ -38,6 +38,11 @@ def __init__(self): # add parameter for the node to set the resolution of IK calculations self.declare_parameter('resolution_ik', 0.20) + # add parameter for the node to set the range of the workspace area + self.declare_parameter('workspace_range', 2.0) + + self.range_ik = self.get_parameter('workspace_range').get_parameter_value().double_value + self.resolution_ik = self.get_parameter('resolution_ik').get_parameter_value().double_value @@ -216,7 +221,7 @@ 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_handler.get_valid_workspace(range = 2,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) + self.valid_configurations = self.robot_handler.get_valid_workspace(range = self.range_ik,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) # compute the maximum payloads for the valid configurations self.valid_configurations = self.robot_handler.compute_maximum_payloads(self.valid_configurations) From 403a838b50ae7bd2ced6417b29986062a163ee32 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 14 Aug 2025 17:32:53 +0200 Subject: [PATCH 42/44] added dependencies for the packages --- dynamic_payload_analysis_core/package.xml | 6 ++++++ dynamic_payload_analysis_core/setup.py | 7 ------- dynamic_payload_analysis_ros/package.xml | 8 ++++++++ 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/dynamic_payload_analysis_core/package.xml b/dynamic_payload_analysis_core/package.xml index faf863a..a90682b 100644 --- a/dynamic_payload_analysis_core/package.xml +++ b/dynamic_payload_analysis_core/package.xml @@ -12,6 +12,12 @@ ament_pep257 python3-pytest + python3-numpy + urdfdom_py + pinocchio + python-pathlib + python-typing + ament_python diff --git a/dynamic_payload_analysis_core/setup.py b/dynamic_payload_analysis_core/setup.py index de600e7..86b17bb 100644 --- a/dynamic_payload_analysis_core/setup.py +++ b/dynamic_payload_analysis_core/setup.py @@ -19,13 +19,6 @@ tests_require=['pytest'], install_requires=[ 'setuptools', - 'numpy', - 'urdf-parser-py>=0.0.4', - 'math', - 'typing', - 'pathlib', - 'tempfile', - 'os', ], entry_points={ 'console_scripts': [ diff --git a/dynamic_payload_analysis_ros/package.xml b/dynamic_payload_analysis_ros/package.xml index 51ba95e..756fcdc 100644 --- a/dynamic_payload_analysis_ros/package.xml +++ b/dynamic_payload_analysis_ros/package.xml @@ -12,6 +12,14 @@ ament_pep257 python3-pytest + dynamic_payload_analysis_core + rclpy + std_msgs + sensor_msgs + builtin_interfaces + visualization_msgs + interactive_markers + ament_python From 60f315c7b7038a1a837813a6b0837dfb4308a0ba Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 14 Aug 2025 18:00:09 +0200 Subject: [PATCH 43/44] fix small problem with None object --- .../rviz_visualization_menu.py | 159 +++++++++--------- 1 file changed, 81 insertions(+), 78 deletions(-) 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 fc08773..0dfe34a 100755 --- 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,53 +165,54 @@ def publish_payload_force(self): """ Publish the gravity force on the frame with id `id_force`. """ - external_force_array = MarkerArray() - - for frame in self.menu.get_item_state(): - - id_force = self.robot_handler.get_parent_joint_id(frame["name"]) + if self.menu is not None: + external_force_array = MarkerArray() - # use the selected configuration from the menu to get the right joint placement - if self.selected_configuration is not None: - joint_position = self.robot_handler.get_joint_placement(id_force,self.valid_configurations[self.selected_configuration]["config"]) - else: - joint_position = self.robot_handler.get_joint_placement(id_force,self.robot_handler.get_zero_configuration()) + for frame in self.menu.get_item_state(): + + id_force = self.robot_handler.get_parent_joint_id(frame["name"]) + + # use the selected configuration from the menu to get the right joint placement + if self.selected_configuration is not None: + joint_position = self.robot_handler.get_joint_placement(id_force,self.valid_configurations[self.selected_configuration]["config"]) + else: + joint_position = self.robot_handler.get_joint_placement(id_force,self.robot_handler.get_zero_configuration()) - arrow_force = Marker() + arrow_force = Marker() - arrow_force.header.frame_id = self.robot_handler.get_root_name() - arrow_force.header.stamp = Time() - arrow_force.ns = "external_force" - arrow_force.id = id_force - arrow_force.type = Marker.ARROW + arrow_force.header.frame_id = self.robot_handler.get_root_name() + arrow_force.header.stamp = Time() + arrow_force.ns = "external_force" + arrow_force.id = id_force + arrow_force.type = Marker.ARROW - # add the arrow if the frame is checked or delete it if not - if frame["checked"]: - arrow_force.action = Marker.ADD - else: - arrow_force.action = Marker.DELETE - - arrow_force.scale.x = 0.20 # Length of the arrow - arrow_force.scale.y = 0.05 # Width of the arrow - arrow_force.scale.z = 0.05 # Height of the arrow - arrow_force.color.a = 1.0 # Alpha - arrow_force.color.r = 0.0 - arrow_force.color.g = 0.0 # Green - arrow_force.color.b = 1.0 - - # Set the position of the arrow at the joint placement - arrow_force.pose.position.x = joint_position["x"] - arrow_force.pose.position.y = joint_position["y"] - arrow_force.pose.position.z = joint_position["z"] - # Set the direction of the arrow downwards - arrow_force.pose.orientation.x = 0.0 - arrow_force.pose.orientation.y = 0.7071 - arrow_force.pose.orientation.z = 0.0 - arrow_force.pose.orientation.w = 0.7071 + # add the arrow if the frame is checked or delete it if not + if frame["checked"]: + arrow_force.action = Marker.ADD + else: + arrow_force.action = Marker.DELETE + + arrow_force.scale.x = 0.20 # Length of the arrow + arrow_force.scale.y = 0.05 # Width of the arrow + arrow_force.scale.z = 0.05 # Height of the arrow + arrow_force.color.a = 1.0 # Alpha + arrow_force.color.r = 0.0 + arrow_force.color.g = 0.0 # Green + arrow_force.color.b = 1.0 + + # Set the position of the arrow at the joint placement + arrow_force.pose.position.x = joint_position["x"] + arrow_force.pose.position.y = joint_position["y"] + arrow_force.pose.position.z = joint_position["z"] + # Set the direction of the arrow downwards + arrow_force.pose.orientation.x = 0.0 + arrow_force.pose.orientation.y = 0.7071 + arrow_force.pose.orientation.z = 0.0 + arrow_force.pose.orientation.w = 0.7071 + + external_force_array.markers.append(arrow_force) - external_force_array.markers.append(arrow_force) - - self.publisher_force.publish(external_force_array) + self.publisher_force.publish(external_force_array) @@ -219,26 +220,27 @@ def workspace_calculation(self): """ Callback for timer to compute the valid workspace area. """ - # 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_handler.get_valid_workspace(range = self.range_ik,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) + if self.menu is not None: + # 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_handler.get_valid_workspace(range = self.range_ik,resolution= self.resolution_ik, masses = self.masses, checked_frames = self.checked_frames) - # compute the maximum payloads for the valid configurations - self.valid_configurations = self.robot_handler.compute_maximum_payloads(self.valid_configurations) - - # insert the valid configurations in the menu - self.menu.insert_dropdown_configuration(self.valid_configurations) + # compute the maximum payloads for the valid configurations + self.valid_configurations = self.robot_handler.compute_maximum_payloads(self.valid_configurations) + + # insert the valid configurations in the menu + self.menu.insert_dropdown_configuration(self.valid_configurations) - # clear all the workspace area markers - self.clear_workspace_area_markers() + # clear all the workspace area markers + self.clear_workspace_area_markers() - # set the workspace state to False to stop the computation - self.menu.set_workspace_state(False) + # set the workspace state to False to stop the computation + self.menu.set_workspace_state(False) - # if there are valid configurations, publish the workspace area - if self.valid_configurations is not None: - # publish the workspace area - self.publish_workspace_area_maximum_payload_area() + # if there are valid configurations, publish the workspace area + if self.valid_configurations is not None: + # publish the workspace area + self.publish_workspace_area_maximum_payload_area() @@ -248,31 +250,32 @@ def publish_selected_configuration(self): Timer to publish the selected configuration. This will publish the joint states of the selected configuration in the menu. """ - # get the selected configuration from the menu - self.selected_configuration = self.menu.get_selected_configuration() - - # if there is a selected configuration, publish the joint states based on the valid configurations calculated previously - if self.selected_configuration is not None: - configs = self.robot_handler.get_position_for_joint_states(self.valid_configurations[self.selected_configuration]["config"]) - joint_state = JointState() - joint_state.header.stamp = self.get_clock().now().to_msg() - - joint_state.name = [joint["joint_name"] for joint in configs] - joint_state.position = [joint["q"] for joint in configs] + if self.menu is not None: + # get the selected configuration from the menu + self.selected_configuration = self.menu.get_selected_configuration() - self.publisher_joint_states.publish(joint_state) - - else: - if self.robot_handler is not None: - # if there is no selected configuration, publish the joint states with zero positions + # if there is a selected configuration, publish the joint states based on the valid configurations calculated previously + if self.selected_configuration is not None: + configs = self.robot_handler.get_position_for_joint_states(self.valid_configurations[self.selected_configuration]["config"]) joint_state = JointState() joint_state.header.stamp = self.get_clock().now().to_msg() + + joint_state.name = [joint["joint_name"] for joint in configs] + joint_state.position = [joint["q"] for joint in configs] + + self.publisher_joint_states.publish(joint_state) - joint_state.name = self.robot_handler.get_joints().tolist() - zero_config = self.robot_handler.get_position_for_joint_states(self.robot_handler.get_zero_configuration()) - joint_state.position = [joint["q"] for joint in zero_config] + else: + if self.robot_handler is not None: + # if there is no selected configuration, publish the joint states with zero positions + joint_state = JointState() + joint_state.header.stamp = self.get_clock().now().to_msg() - self.publisher_joint_states.publish(joint_state) + joint_state.name = self.robot_handler.get_joints().tolist() + zero_config = self.robot_handler.get_position_for_joint_states(self.robot_handler.get_zero_configuration()) + joint_state.position = [joint["q"] for joint in zero_config] + + self.publisher_joint_states.publish(joint_state) def update_checked_items(self): From ff81d3bb8dffaa205668f83b56f679787906e7c1 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Sat, 16 Aug 2025 11:27:17 +0200 Subject: [PATCH 44/44] fix dependencies name for urdf parser library --- dynamic_payload_analysis_core/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamic_payload_analysis_core/package.xml b/dynamic_payload_analysis_core/package.xml index a90682b..72f3082 100644 --- a/dynamic_payload_analysis_core/package.xml +++ b/dynamic_payload_analysis_core/package.xml @@ -13,7 +13,7 @@ python3-pytest python3-numpy - urdfdom_py + urdf_parser_py pinocchio python-pathlib python-typing