From c227c00bf8d69c2a1a9afaaef70b472c1ff53175 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 21 Aug 2025 18:59:40 +0200 Subject: [PATCH 1/3] added publisher for analyzed points --- .../dynamic_payload_analysis_core/core.py | 18 +++++- .../rviz_visualization_menu.py | 55 +++++++++++++++++++ 2 files changed, 72 insertions(+), 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 8498c26..05ecb32 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -69,7 +69,8 @@ def __init__(self, robot_description : Union[str, Path]): # compute main trees of the robot model self.compute_subtrees() - + # array to store all analyzed points + self.analyzed_points = np.array([], dtype=object) # array to store all configurations for the robot model self.configurations = np.array([], dtype=object) @@ -422,11 +423,17 @@ def compute_all_configurations(self, range : int, resolution : int, end_joint_id # Create an array to store all configurations configurations = [] + + # restore analyzed points array + self.analyzed_points = np.array([], dtype=object) # Iterate over the range to compute all configurations 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): + + self.analyzed_points = np.append(self.analyzed_points, {"position" : [x, y, z]}) + target_position = pin.SE3(np.eye(3), np.array([x, y, z])) new_q = self.compute_inverse_kinematics(q, target_position, end_joint_id) @@ -873,6 +880,15 @@ def get_zero_acceleration(self) -> np.ndarray: return a0 + def get_analyzed_points(self) -> np.ndarray: + """ + Get the analyzed points during the computation of all configurations. + + :return: Array of analyzed points. + """ + + return self.analyzed_points + def get_random_configuration(self) -> tuple[np.ndarray, np.ndarray]: """ Get a random configuration for configuration and velocity vectors. 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 0dfe34a..84a00ed 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 @@ -68,6 +68,9 @@ 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 cloud of all analyzed points + self.publisher_analyzed_points = self.create_publisher(MarkerArray, '/analyzed_points', 10) + # subscription to joint states self.joint_states_subscription = self.create_subscription(JointState, '/joint_states', self.joint_states_callback, 10) @@ -103,6 +106,9 @@ def __init__(self): # variable to store if there is a selected configuration from the workspace menu to visualize self.selected_configuration = None + # variable to store analyzed points of the workspace area + self.marker_analyzed_points = None + # timer to compute the valid workspace area self.timer_workspace_calculation = self.create_timer(1.0, self.workspace_calculation) # timer to publish the selected configuration in joint states @@ -113,6 +119,8 @@ def __init__(self): 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) + # timer to publish all analyzed points in the workspace area + self.timer_publish_analyzed_points = self.create_timer(2, self.publish_analyzed_points) self.get_logger().info('Robot description subscriber node started') @@ -161,6 +169,53 @@ def update_payload_selection(self): self.links = links # Update the links to the current ones + + def publish_analyzed_points(self): + """ + Publish the analyzed points in the workspace area. + This will publish all the points in the workspace area where the robot can reach with the current configuration. + """ + if self.menu is not None: + + if len(self.robot_handler.get_analyzed_points()) == 0: + return + else: + if self.marker_analyzed_points is None: + + self.marker_analyzed_points = MarkerArray() + + # publish the analyzed points + for i, analyzed_point in enumerate(self.robot_handler.get_analyzed_points()): + point = Marker() + point.header.frame_id = self.robot_handler.get_root_name() + point.header.stamp = Time() + point.ns = f"analyzed_points_ns" + point.id = i + point.type = Marker.SPHERE + point.action = Marker.ADD + point.scale.x = 0.01 + point.scale.y = 0.01 + point.scale.z = 0.01 + point.pose.position.x = analyzed_point['position'][0] + point.pose.position.y = analyzed_point['position'][1] + point.pose.position.z = analyzed_point['position'][2] + point.pose.orientation.w = 1.0 + point.color.a = 1.0 # Alpha + point.color.r = 1.0 # Red + point.color.g = 1.0 # Green + point.color.b = 1.0 # Blue + + self.marker_analyzed_points.markers.append(point) + + self.publisher_analyzed_points.publish(self.marker_analyzed_points) + + else: + # update with current time + for marker in self.marker_analyzed_points.markers: + marker.header.stamp = Time() + + self.publisher_analyzed_points.publish(self.marker_analyzed_points) + def publish_payload_force(self): """ Publish the gravity force on the frame with id `id_force`. From 72a5f0c663547f9147217687fd9bd436488688fa Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 28 Aug 2025 08:51:59 +0200 Subject: [PATCH 2/3] change method to publis analyzed ws markers in transient local --- .../rviz_visualization_menu.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 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 84a00ed..3d33889 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 @@ -69,7 +69,7 @@ def __init__(self): self.publisher_maximum_payloads = self.create_publisher(MarkerArray, '/maximum_payloads', 10) # Publisher for point cloud of all analyzed points - self.publisher_analyzed_points = self.create_publisher(MarkerArray, '/analyzed_points', 10) + self.publisher_analyzed_points = self.create_publisher(MarkerArray, '/analyzed_points', qos_profile=rclpy.qos.QoSProfile( durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth = 1)) # subscription to joint states self.joint_states_subscription = self.create_subscription(JointState, '/joint_states', self.joint_states_callback, 10) @@ -119,8 +119,6 @@ def __init__(self): 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) - # timer to publish all analyzed points in the workspace area - self.timer_publish_analyzed_points = self.create_timer(2, self.publish_analyzed_points) self.get_logger().info('Robot description subscriber node started') @@ -170,7 +168,7 @@ def update_payload_selection(self): self.links = links # Update the links to the current ones - def publish_analyzed_points(self): + def publisher_analyzed_point_markers(self): """ Publish the analyzed points in the workspace area. This will publish all the points in the workspace area where the robot can reach with the current configuration. @@ -282,6 +280,9 @@ def workspace_calculation(self): # compute the maximum payloads for the valid configurations self.valid_configurations = self.robot_handler.compute_maximum_payloads(self.valid_configurations) + + # publish the analyzed points in the workspace area + self.publisher_analyzed_point_markers() # insert the valid configurations in the menu self.menu.insert_dropdown_configuration(self.valid_configurations) From 433aba409ebdce75c72f3467836cd9f67a913a35 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 3 Sep 2025 09:04:20 +0200 Subject: [PATCH 3/3] change durability policy --- .../dynamic_payload_analysis_ros/rviz_visualization_menu.py | 4 ++-- 1 file changed, 2 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 3d33889..20c55a3 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 @@ -63,10 +63,10 @@ def __init__(self): self.publisher_rviz_torque = self.create_publisher(MarkerArray, '/torque_visualization', 10) # Pusblisher for point cloud workspace area - self.publisher_workspace_area = self.create_publisher(MarkerArray, '/workspace_area', 10) + self.publisher_workspace_area = self.create_publisher(MarkerArray, '/workspace_area', qos_profile=rclpy.qos.QoSProfile( durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth = 1)) # Pusblisher for point cloud of maximum payloads in the workspace area - self.publisher_maximum_payloads = self.create_publisher(MarkerArray, '/maximum_payloads', 10) + self.publisher_maximum_payloads = self.create_publisher(MarkerArray, '/maximum_payloads', qos_profile=rclpy.qos.QoSProfile( durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth = 1)) # Publisher for point cloud of all analyzed points self.publisher_analyzed_points = self.create_publisher(MarkerArray, '/analyzed_points', qos_profile=rclpy.qos.QoSProfile( durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth = 1))