From f2ecebf1c63dd6fc0cf72fc968cf421fede70bb7 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 26 Aug 2025 15:52:17 +0200 Subject: [PATCH 01/15] removed not necessary type in the init function --- .../dynamic_payload_analysis_core/core.py | 12 ++++++------ 1 file changed, 6 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 8498c26..96ec344 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -26,12 +26,11 @@ class TorqueCalculator: - def __init__(self, robot_description : Union[str, Path]): + def __init__(self, robot_description : str): """ Initialize the Torques_calculator with the URDF model or XML format provided by robot_description topic. - :param urdf_path: Path to the URDF file of the robot. - :param robot_description: Robot description in XML format provided by /robot_description topic or path of URDF file. + :param robot_description: Robot description in XML format provided by /robot_description topic. """ # Load the robot model from path or XML string @@ -56,8 +55,8 @@ def __init__(self, robot_description : Union[str, Path]): os.unlink(temp_urdf_path) - elif isinstance(robot_description, Path): - self.model = pin.buildModelFromUrdf(str(robot_description.resolve())) + else: + raise ValueError("robot_description must be a string containing the URDF XML or the path to the URDF file") # create data for the robot model self.data = self.model.createData() @@ -69,7 +68,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) From f8cfd918a464190a53d18ef5cc1485fb9348fe3b Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 26 Aug 2025 15:52:38 +0200 Subject: [PATCH 02/15] added first version of test script for core library --- .../test/test_core.py | 56 +++++++++++++++++++ .../test/test_xml.urdf | 1 + 2 files changed, 57 insertions(+) create mode 100644 dynamic_payload_analysis_core/test/test_core.py create mode 100644 dynamic_payload_analysis_core/test/test_xml.urdf diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py new file mode 100644 index 0000000..99d36c6 --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -0,0 +1,56 @@ +from dynamic_payload_analysis_core.core import TorqueCalculator +import unittest +import os + + +class TestTorqueCalculator(unittest.TestCase): + + def __init__(self, methodName = "runTest"): + super().__init__(methodName) + + test_dir = os.path.dirname(__file__) + urdf_path = os.path.join(test_dir, "test_xml.urdf") + with open(urdf_path, "r") as f: + self.xml_string = f.read() + + self.robot_handler = TorqueCalculator(robot_description=self.xml_string) + + + def test_init_torque_calculator(self): + self.assertIsNotNone(self.robot_handler.model, "Model should be initialized") + self.assertIsNotNone(self.robot_handler.geom_model,"Geom model should be initialized") + self.assertIsNotNone(self.robot_handler.geom_data, "Model data should be initialized") + print("✅ Initialization assertions passed") + + def test_get_root_name(self): + root_name = self.robot_handler.get_root_joint_name(self.xml_string) + self.assertEqual(self.robot_handler.get_root_name(), "base_footprint") + self.assertEqual(root_name, self.robot_handler.get_root_name()) + print("✅ Root name assertion passed") + + def test_subtree(self): + subtrees = self.robot_handler.get_subtrees() + + self.assertEqual(len(subtrees), 7) + self.assertEqual(subtrees[0]["tip_link_name"], "wheel_front_left_link") + self.assertEqual(subtrees[1]["tip_link_name"], "wheel_front_right_link") + self.assertEqual(subtrees[2]["tip_link_name"], "wheel_rear_left_link") + self.assertEqual(subtrees[3]["tip_link_name"], "wheel_rear_right_link") + self.assertEqual(subtrees[4]["tip_link_name"], "gripper_left_outer_finger_left_link") + self.assertEqual(subtrees[5]["tip_link_name"], "gripper_right_outer_finger_left_link") + self.assertEqual(subtrees[6]["tip_link_name"], "head_2_link") + print("✅ Subtrees assertion passed") + + def test_compute_mimic_joints(self): + mimic_joints = self.robot_handler.mimic_joint_names + self.assertEqual(len(mimic_joints), 10) + self.assertIn("gripper_left_inner_finger_left_joint", mimic_joints) + self.assertIn("gripper_right_inner_finger_left_joint", mimic_joints) + self.assertNotIn("arm_1_joint", mimic_joints) + print("✅ Mimic joints assertion passed") + + def + + +if __name__ == "__main__": + unittest.main() \ No newline at end of file diff --git a/dynamic_payload_analysis_core/test/test_xml.urdf b/dynamic_payload_analysis_core/test/test_xml.urdf new file mode 100644 index 0000000..59620d2 --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_xml.urdf @@ -0,0 +1 @@ +0 0 0 0 0 010true818.01-2.3561944901923452.3561944901923450.0525.00.001gaussian0.00.01~/out:=scan_rear_rawsensor_msgs/LaserScanbase_rear_laser_linkGazebo/DarkGrey0 0 0 0 0 010true818.01-2.3561944901923452.3561944901923450.0525.00.001gaussian0.00.01~/out:=scan_front_rawsensor_msgs/LaserScanbase_front_laser_linkGazebo/DarkGrey1100.0gaussian0.02e-40.00000750.00000080.01.7e-20.10.001~/out:=base_imuGazebo/BlackGazebo/Black011000000.01000.00.00.01 0 01.00.001Gazebo/Grey011000000.01000.00.00.01 0 01.00.001Gazebo/Grey011000000.01000.00.00.01 0 01.00.001Gazebo/Grey011000000.01000.00.00.01 0 01.00.001Gazebo/GreyGazebo/White100000000.010.00.10.11 0 010.00.00050cmd_vel:=mobile_base_controller/cmd_vel_unstampedodom:=mobile_base_controller/odom1001000truefalseodombase_footprint0.00010.00010.01Gazebo/DarkGreyGazebo/DarkGrey1~/image_raw:=robot_face/image_raw480640Gazebo/DarkGrey0.90.9Gazebo/White0.90.911000110 0 01e+1311.2112585008840648640480RGB_INT80.1100gaussian0.00.00713011.48702052269916881280720L_INT80.1100gaussian0.00.0513001.48702052269916881280720L_INT80.1100gaussian0.00.0513001.487020522699168812807200.1100gaussian0.00.1001300head_front_camera30.030.030.0depth/image_rawcolor/image_rawinfra1/image_rawinfra2/image_rawhead_front_camera_color_optical_framehead_front_camera_depth_optical_framehead_front_camera_infra1_ir_optical_framehead_front_camera_infra2_ir_optical_frame0.210.0Truedepth/color/points0.259.0Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.911111111Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/DarkGrey0.90.91111Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.9Gazebo/DarkGrey0.90.911111111Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/White0.90.9Gazebo/DarkGrey0.90.91111robot_control/RobotControlcan-11transmission_interface/SimpleTransmission1.0-11transmission_interface/SimpleTransmission1.0-11transmission_interface/SimpleTransmission1.0-11transmission_interface/SimpleTransmission1.0robot_control/RobotControldynamixeltransmission_interface/SimpleTransmission0.01transmission_interface/SimpleTransmission0.01robot_control/RobotControlethercattruetruetransmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.00.15gripper_left_finger_joint1gripper_left_finger_joint1gripper_left_finger_joint-1gripper_left_finger_joint1gripper_left_finger_joint-1transmission_interface/SimpleTransmission1transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.0transmission_interface/SimpleTransmission0.01.00.15gripper_right_finger_joint1gripper_right_finger_joint1gripper_right_finger_joint-1gripper_right_finger_joint1gripper_right_finger_joint-1transmission_interface/SimpleTransmission1transmission_interface/SimpleTransmission1.00.0 \ No newline at end of file From 81e2fa39b8853ac3bf2c3d5e928afdcd301945f6 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 26 Aug 2025 17:52:28 +0200 Subject: [PATCH 03/15] fix description and add methods for testing --- .../dynamic_payload_analysis_core/core.py | 22 ++++- .../test/test_core.py | 94 ++++++++++++++++++- .../Universal_Robots_ROS2_Description | 1 + .../franka_description | 1 + talos_robot | 1 + tiago_pro_robot | 1 + 6 files changed, 114 insertions(+), 6 deletions(-) create mode 160000 examples_robot_description_packages/Universal_Robots_ROS2_Description create mode 160000 examples_robot_description_packages/franka_description create mode 160000 talos_robot create mode 160000 tiago_pro_robot 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 96ec344..666610f 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -230,7 +230,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: + def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Union[str | np.ndarray], q : np.ndarray) -> list: """ 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. @@ -338,9 +338,10 @@ 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, joint_id : str) -> np.ndarray: """ Compute the inverse kinematics for the robot model with joint limits consideration. - :param q: current joint configuration vector. + + :param q: Current joint configuration vector. :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. - :param end_effector_name: Name of the end effector joint. + :param joint_id: Id of the end effector joint. :return: Joint configuration vector that achieves the desired end effector position. """ @@ -450,7 +451,7 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, :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", "tree_id","selected_joint_id" }]. """ - + # TODO : get the tree_id from the sub tree array instead of passing it as parameter valid_configurations = [] # check valid configurations for left arm @@ -467,7 +468,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 + # 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 @@ -654,6 +655,17 @@ def get_links_from_tree(self, joint_ids: np.ndarray | int) -> np.ndarray: return np.array(frames, dtype=object) + def get_end_effector_position_array(self, x: float, y: float, z: float) -> np.ndarray: + """ + Get the end effector position as a array. + + :param x: X position of the end effector. + :param y: Y position of the end effector. + :param z: Z position of the end effector. + :return: Numpy array representing the end effector position. + """ + return pin.SE3(np.eye(3), np.array([x, y, z])) + def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: """ Get the maximum torques for each joint in all valid configurations. diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py index 99d36c6..7dffdbc 100644 --- a/dynamic_payload_analysis_core/test/test_core.py +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -1,6 +1,7 @@ from dynamic_payload_analysis_core.core import TorqueCalculator import unittest import os +import numpy as np class TestTorqueCalculator(unittest.TestCase): @@ -24,6 +25,7 @@ def test_init_torque_calculator(self): def test_get_root_name(self): root_name = self.robot_handler.get_root_joint_name(self.xml_string) + self.assertIsInstance(root_name, str, "Root name should be a string") self.assertEqual(self.robot_handler.get_root_name(), "base_footprint") self.assertEqual(root_name, self.robot_handler.get_root_name()) print("✅ Root name assertion passed") @@ -31,6 +33,7 @@ def test_get_root_name(self): def test_subtree(self): subtrees = self.robot_handler.get_subtrees() + self.assertIsInstance(subtrees, np.ndarray, "Subtrees should be a list") self.assertEqual(len(subtrees), 7) self.assertEqual(subtrees[0]["tip_link_name"], "wheel_front_left_link") self.assertEqual(subtrees[1]["tip_link_name"], "wheel_front_right_link") @@ -49,8 +52,97 @@ def test_compute_mimic_joints(self): self.assertNotIn("arm_1_joint", mimic_joints) print("✅ Mimic joints assertion passed") - def + def test_create_external_force(self): + ext_forces = self.robot_handler.create_ext_force(masses = 1, frame_name = "arm_left_3_link", q = self.robot_handler.get_zero_configuration()) + self.assertIsInstance(ext_forces, list, "External forces should be a numpy array") + self.assertNotEqual(ext_forces[8], [0,0,0], "External force should not be zero") + print("✅ External force assertion passed") + def test_compute_inverse_dynamics(self): + standard_config = self.robot_handler.get_zero_configuration() + + torques = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) + + self.assertIsNotNone(torques, "Torques should not be None") + self.assertIsInstance(torques, np.ndarray, "Torques should be a list") + self.assertTrue(all(isinstance(torque, float) for torque in torques), "All torques should be floats") + + # add external forces to the end-effector + external_forces = self.robot_handler.create_ext_force(masses=1, frame_name="arm_left_3_link", q=standard_config) + torques_with_ext = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration(), external_forces) + + np.testing.assert_raises(AssertionError, np.testing.assert_array_equal, torques, torques_with_ext) + print("✅ Inverse dynamics assertion passed") + + def test_update_configuration(self): + zero_config = self.robot_handler.get_zero_configuration() + new_config = zero_config + 5 + + self.robot_handler.update_configuration(new_config) + joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = new_config) # just to ensure the configuration is updated internally + + self.assertIsInstance(joint_placement, dict, "Joint placement should be a dict") + + self.robot_handler.update_configuration(zero_config) + updated_joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = zero_config) + + self.assertNotEqual(joint_placement["x"], updated_joint_placement["x"], "Joint placement should differ after configuration update") + self.assertNotEqual(joint_placement["y"], updated_joint_placement["y"], "Joint placement should differ after configuration update") + self.assertNotEqual(joint_placement["z"], updated_joint_placement["z"], "Joint placement should differ after configuration update") + + print("✅ Update configuration assertion passed") + + def test_compute_inverse_kinematics(self): + initial_config = self.robot_handler.get_zero_configuration() + + end_effector_pos = self.robot_handler.get_end_effector_position_array(-0.4, -0.6, 0.6) # reachable position + computed_config = self.robot_handler.compute_inverse_kinematics(q = initial_config, end_effector_position = end_effector_pos, joint_id= 25) + + self.assertIsNotNone(computed_config, "Computed configuration should not be None if IK is successful") + self.assertIsInstance(computed_config, np.ndarray, "Computed configuration should be a numpy array") + + end_effector_pos_not_reachable = self.robot_handler.get_end_effector_position_array(4, 6, 6) # not reachable position + computed_config_not_reachable = self.robot_handler.compute_inverse_kinematics(q = initial_config, end_effector_position = end_effector_pos_not_reachable, joint_id= 25) + + self.assertIsNone(computed_config_not_reachable, "Computed configuration should be None if IK is not successful") + + print("✅ Inverse kinematics assertion passed") + + + def test_compute_valid_workspace(self): + computed_configurations = self.robot_handler.compute_all_configurations(range = 1, resolution = 0.3, end_joint_id= 25) # used 1 meter range and 0.3 resolution to speed up the test + len_configs = len(computed_configurations) + self.assertIsInstance(computed_configurations, np.ndarray, "Computed configuration should be a numpy array") + + self.assertTrue(np.all(computed_configurations[round(len_configs/2)]["end_effector_pos"] <= np.array([1, 1, 1])), "End effector positions should be within the specified range") + self.assertTrue(np.all(computed_configurations[round(len_configs/3)]["end_effector_pos"] <= np.array([1, 1, 1])), "End effector positions should be within the specified range") + + verified_configs = self.robot_handler.verify_configurations(computed_configurations, masses= None, checked_frames= None, tree_id=5, selected_joint_id=25) + + self.assertIsInstance(verified_configs, np.ndarray, "Verified configuration should be a numpy array") + + self.assertEqual(verified_configs[0]["tree_id"], 5, "Tree ID should match the provided tree ID") + self.assertEqual(verified_configs[0]["selected_joint_id"], 25, "Selected joint ID should match the provided joint ID") + + + # should be None because there is no selected joint id inside the object + valid_workspace = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + self.assertIsInstance(valid_workspace, np.ndarray, "Valid workspace should be a numpy array") + self.assertEqual(len(valid_workspace), 0, "Valid workspace length should be zero if there is no selected joint id") + + # modify selected joint id to test valid workspace + self.robot_handler.subtrees[5]["selected_joint_id"] = 25 + valid_workspace = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + self.assertEqual(len(valid_workspace), len(verified_configs), "Valid workspace length should match the verified configurations length") + + print("✅ Compute valid workspace assertion passed") + + + + + if __name__ == "__main__": unittest.main() \ No newline at end of file diff --git a/examples_robot_description_packages/Universal_Robots_ROS2_Description b/examples_robot_description_packages/Universal_Robots_ROS2_Description new file mode 160000 index 0000000..17f88f5 --- /dev/null +++ b/examples_robot_description_packages/Universal_Robots_ROS2_Description @@ -0,0 +1 @@ +Subproject commit 17f88f5513ef817ea3c65b07f33dbea3f3340e68 diff --git a/examples_robot_description_packages/franka_description b/examples_robot_description_packages/franka_description new file mode 160000 index 0000000..4b8948e --- /dev/null +++ b/examples_robot_description_packages/franka_description @@ -0,0 +1 @@ +Subproject commit 4b8948e061c2ef8b2ea15d658cf35981c683f864 diff --git a/talos_robot b/talos_robot new file mode 160000 index 0000000..6152887 --- /dev/null +++ b/talos_robot @@ -0,0 +1 @@ +Subproject commit 6152887e3f258cca58231391ae0f380baede43ff diff --git a/tiago_pro_robot b/tiago_pro_robot new file mode 160000 index 0000000..c62d9f8 --- /dev/null +++ b/tiago_pro_robot @@ -0,0 +1 @@ +Subproject commit c62d9f885a5a48a145228c13acb16e027691993d From afe50cfcdad1b3aefb27853c020dfaa40fffca69 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 26 Aug 2025 20:55:25 +0200 Subject: [PATCH 04/15] fix old descriptions --- .../dynamic_payload_analysis_core/core.py | 12 +++++++----- .../rviz_visualization_menu.py | 2 +- 2 files changed, 8 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 666610f..a530e33 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -499,7 +499,7 @@ def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarr :param resolution (int): Resolution of the grid to compute configurations. :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"}]. + :return: Array of valid configurations that achieve the desired end effector position in format: [{"config", "end_effector_pos, "tau", "tree_id"}]. """ # create the array to store all current valid configurations valid_current_configurations = np.array([], dtype=object) @@ -535,7 +535,8 @@ def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarr 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" } + + :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "tree_id", "max_payload" } """ for config in configs: config["max_payload"] = self.find_max_payload_binary_search(config, payload_min=0.0, payload_max=15, resolution=0.01) @@ -546,6 +547,7 @@ def compute_maximum_payloads(self, configs : np.ndarray): 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). :param payload_min: Minimum payload to test. :param payload_max: Maximum payload to test. @@ -666,7 +668,7 @@ def get_end_effector_position_array(self, x: float, y: float, z: float) -> np.nd """ return pin.SE3(np.eye(3), np.array([x, y, z])) - def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: + def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray: """ Get the maximum torques for each joint in all valid configurations. @@ -715,9 +717,9 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda def get_maximum_payloads(self, valid_configs : np.ndarray) -> np.ndarray: """ - Get the maximum payloads for all configuration in the left and right arm. + Get the maximum payloads for all configuration in the corrisponding tree. - :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "arm", "max_payload"}]. + :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "tree_id", "max_payload"}]. :return: Tuple of arrays of maximum payloads for left and right arms. """ max_payloads = np.array([], dtype=float) 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..2033a3a 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 @@ -566,7 +566,7 @@ def generate_maximum_payloads_markers(self): # Create a MarkerArray to visualize the maximum payloads marker_max_payloads = MarkerArray() - # get the maximum payloads for each arm based on the valid configurations + # get the maximum payloads for each tree based on the valid configurations max_payloads = self.robot_handler.get_maximum_payloads(self.valid_configurations) # Iterate through the valid configurations and create markers From d2e766466aefc84a9ba366e6cb04aa20e6a3632c Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Tue, 26 Aug 2025 20:55:46 +0200 Subject: [PATCH 05/15] add new test for core library --- .../test/test_core.py | 123 +++++++++++++++++- 1 file changed, 116 insertions(+), 7 deletions(-) diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py index 7dffdbc..e1410b5 100644 --- a/dynamic_payload_analysis_core/test/test_core.py +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -30,9 +30,10 @@ def test_get_root_name(self): self.assertEqual(root_name, self.robot_handler.get_root_name()) print("✅ Root name assertion passed") - def test_subtree(self): + def test_subtrees(self): subtrees = self.robot_handler.get_subtrees() + self.assertIsNotNone(subtrees, "Subtrees should not be None") self.assertIsInstance(subtrees, np.ndarray, "Subtrees should be a list") self.assertEqual(len(subtrees), 7) self.assertEqual(subtrees[0]["tip_link_name"], "wheel_front_left_link") @@ -126,22 +127,130 @@ def test_compute_valid_workspace(self): # should be None because there is no selected joint id inside the object - valid_workspace = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - self.assertIsInstance(valid_workspace, np.ndarray, "Valid workspace should be a numpy array") - self.assertEqual(len(valid_workspace), 0, "Valid workspace length should be zero if there is no selected joint id") + self.assertIsInstance(valid_configurations, np.ndarray, "Valid workspace should be a numpy array") + self.assertEqual(len(valid_configurations), 0, "Valid workspace length should be zero if there is no selected joint id") # modify selected joint id to test valid workspace self.robot_handler.subtrees[5]["selected_joint_id"] = 25 - valid_workspace = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - self.assertEqual(len(valid_workspace), len(verified_configs), "Valid workspace length should match the verified configurations length") + self.assertEqual(len(valid_configurations), len(verified_configs), "Valid workspace length should match the verified configurations length") print("✅ Compute valid workspace assertion passed") + def test_compute_maximum_payloads(self): + self.robot_handler.subtrees[5]["selected_joint_id"] = 25 + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + max_payload = self.robot_handler.find_max_payload_binary_search(config = valid_configurations[0], payload_min=0.0, payload_max=10.0, resolution=0.01) + + self.assertIsInstance(max_payload, float, "Max payload should be a float") + self.assertGreaterEqual(max_payload, 0.0, "Max payload should be non-negative") + self.assertLessEqual(max_payload, 10.0, "Max payload should be within the specified range") + + max_payload = self.robot_handler.find_max_payload_binary_search(config = valid_configurations[0], payload_min=0.0, payload_max=0, resolution=0.01) + self.assertEqual(max_payload, 0.0, "Max payload should be zero if payload_max is zero") + + configs_with_payloads = self.robot_handler.compute_maximum_payloads(valid_configurations) + + self.assertIsInstance(configs_with_payloads, np.ndarray, "Configurations with payloads should be a numpy array") + self.assertIn("max_payload", configs_with_payloads[0], "Each configuration should have a max_payload key") + self.assertIsInstance(configs_with_payloads[0]["max_payload"], float, "Max payload should be a float") + + print("✅ Compute maximum payloads assertion passed") + + def test_compute_forward_dynamics_aba_method(self): + zero_config = self.robot_handler.get_zero_configuration() + zero_velocity = self.robot_handler.get_zero_velocity() + zero_acceleration = self.robot_handler.get_zero_acceleration() + tau = np.zeros(self.robot_handler.model.nv) + + computed_acceleration = self.robot_handler.compute_forward_dynamics_aba_method(q=zero_config, qdot=zero_velocity, tau=tau) - + self.assertIsNotNone(computed_acceleration, "Computed acceleration should not be None") + self.assertIsInstance(computed_acceleration, np.ndarray, "Computed acceleration should be a numpy array") + print("✅ Compute forward dynamics ABA method assertion passed") + + def test_compute_jacobian(self): + zero_config = self.robot_handler.get_zero_configuration() + jacobian = self.robot_handler.compute_jacobian(q=zero_config, frame_name= "arm_left_3_link") + + self.assertIsNotNone(jacobian, "Jacobian should not be None") + self.assertIsInstance(jacobian, np.ndarray, "Jacobian should be a numpy array") + self.assertEqual(jacobian.shape, (6, self.robot_handler.model.nv), "Jacobian shape should be (6, nv)") + + print("✅ Compute Jacobian assertion passed") + + def test_verify_member_tree(self): + flag = self.robot_handler.verify_member_tree(tree_id=5, joint_id=25) + + self.assertTrue(flag, "Joint ID should be in the specified tree ID") + self.assertIsInstance(flag, bool, "Flag should be a boolean") + + flag = self.robot_handler.verify_member_tree(tree_id=5, joint_id=2) + + self.assertFalse(flag, "Joint ID should not be in the specified tree ID") + + print("✅ Verify member tree assertion passed") + + def test_get_links_from_tree(self): + link = self.robot_handler.get_links_from_tree(joint_ids=25) + links = self.robot_handler.get_links_from_tree(joint_ids=[25, 26, 24]) + + self.assertIsInstance(links, np.ndarray, "Links should be a numpy array") + self.assertEqual(len(links), 3, "There should be 3 links in the subtree") + + self.assertIn("arm_right_7_link", link, "Link should be in the subtree") + self.assertIsNotNone(link, "Link should not be None") + self.assertIsInstance(link, np.ndarray, "Link should be a numpy array") + + print("✅ Get links from tree assertion passed") + + def test_get_maximum_torques(self): + #modify selected joint id to test valid workspace + self.robot_handler.subtrees[5]["selected_joint_id"] = 25 + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + max_torques = self.robot_handler.get_maximum_torques(valid_configurations) + + self.assertIsNotNone(max_torques, "Max torques should not be None") + self.assertIsInstance(max_torques, np.ndarray, "Max torques should be a numpy array") + + # get selected subtrees only + selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees() if subtree["selected_joint_id"] is not None] + + self.assertEqual(len(max_torques), len(selected_subtrees), "Max torques length should match the number of selected subtrees") + self.assertIn("max_values", max_torques[0], "Each max torque entry should have a max_values key") + self.assertEqual(max_torques[0]["tree_id"], selected_subtrees[0]["tree_id"], "Tree ID should match the selected subtree's tree ID") + + print("✅ Get maximum torques assertion passed") + + def test_get_maximum_payloads(self): + #modify selected joint id to test valid workspace + self.robot_handler.subtrees[5]["selected_joint_id"] = 25 + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + valid_configurations = self.robot_handler.compute_maximum_payloads(valid_configurations) + + max_payloads = self.robot_handler.get_maximum_payloads(valid_configurations) + + self.assertIsNotNone(max_payloads, "Max payloads should not be None") + self.assertIsInstance(max_payloads, np.ndarray, "Max payloads should be a numpy array") + self.assertGreater(len(max_payloads), 0, "Max payloads length should be greater than zero") + self.assertIn("max_payload", max_payloads[0], "Each max payload entry should have a max_payload key") + self.assertIsInstance(max_payloads[0]["max_payload"], float, "Max payload values should be a float") + + # get selected subtrees only + selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees() if subtree["selected_joint_id"] is not None] + + self.assertEqual(len(max_payloads), len(selected_subtrees), "Max payloads length should match the number of selected subtrees") + self.assertEqual(max_payloads[0]["tree_id"], selected_subtrees[0]["tree_id"], "Tree ID should match the selected subtree's tree ID") + + print("✅ Get maximum payloads assertion passed") + + if __name__ == "__main__": From 336d5abbef0358518bb2c1a4b42bcaa46eb68767 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 27 Aug 2025 08:21:41 +0200 Subject: [PATCH 06/15] removed wrong files added before --- .../Universal_Robots_ROS2_Description | 1 - examples_robot_description_packages/franka_description | 1 - 2 files changed, 2 deletions(-) delete mode 160000 examples_robot_description_packages/Universal_Robots_ROS2_Description delete mode 160000 examples_robot_description_packages/franka_description diff --git a/examples_robot_description_packages/Universal_Robots_ROS2_Description b/examples_robot_description_packages/Universal_Robots_ROS2_Description deleted file mode 160000 index 17f88f5..0000000 --- a/examples_robot_description_packages/Universal_Robots_ROS2_Description +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 17f88f5513ef817ea3c65b07f33dbea3f3340e68 diff --git a/examples_robot_description_packages/franka_description b/examples_robot_description_packages/franka_description deleted file mode 160000 index 4b8948e..0000000 --- a/examples_robot_description_packages/franka_description +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4b8948e061c2ef8b2ea15d658cf35981c683f864 From 5dbccabd0c5e6c775c12c97e08a653280da428fb Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 27 Aug 2025 08:24:23 +0200 Subject: [PATCH 07/15] remove wrong tracked files --- talos_robot | 1 - tiago_pro_robot | 1 - 2 files changed, 2 deletions(-) delete mode 160000 talos_robot delete mode 160000 tiago_pro_robot diff --git a/talos_robot b/talos_robot deleted file mode 160000 index 6152887..0000000 --- a/talos_robot +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6152887e3f258cca58231391ae0f380baede43ff diff --git a/tiago_pro_robot b/tiago_pro_robot deleted file mode 160000 index c62d9f8..0000000 --- a/tiago_pro_robot +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c62d9f885a5a48a145228c13acb16e027691993d From e74ca21b654ebaa4106b2acc9510b964dad713dc Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 27 Aug 2025 09:58:22 +0200 Subject: [PATCH 08/15] fix old description and type hints --- .../dynamic_payload_analysis_core/core.py | 26 +++++++++++-------- 1 file changed, 15 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 9bd7135..77091fe 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -767,10 +767,10 @@ def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = else: norm_tau.append(0.0) - return norm_tau + return np.array(norm_tau, dtype=float) - def get_normalized_payload(self, payload : np.ndarray, target_payload : float) -> np.ndarray: + def get_normalized_payload(self, payload : float, target_payload : float) -> float: """ Normalize the torques vector to a unified scale. @@ -783,12 +783,11 @@ def get_normalized_payload(self, payload : np.ndarray, target_payload : float) - return norm_payload - def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.ndarray | np.ndarray: + def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.ndarray: """ Get a unified sum of torques for all possible configurations of the robot model. - :param q: Joint configuration vector. - :param valid_configs: Array of + :param valid_configs: Array of valid configurations """ torques_sum = np.array([], dtype=float) @@ -905,7 +904,8 @@ def get_analyzed_points(self) -> np.ndarray: def get_random_configuration(self) -> tuple[np.ndarray, np.ndarray]: """ Get a random configuration for configuration and velocity vectors. - :return: Random configuration vectors. + + :return: Random configuration vectors for joint positions and velocities. """ q_limits_lower = self.model.lowerPositionLimit q_limits_upper = self.model.upperPositionLimit @@ -1019,6 +1019,8 @@ def set_position(self, pos_joints : list[float], name_positions : list[str] ) -> :param pos_joints: List of joint positions provided by jointstate publisher. :param name_positions: List of joint names in the order provided by jointstate publisher. + + :return: Joint configuration vector in pinocchio format. """ q = np.zeros(self.model.nq) @@ -1050,7 +1052,8 @@ def get_position_for_joint_states(self, q : np.ndarray) -> np.ndarray: Example: continuous joints (wheels) are represented as two values in the configuration vector but in the joint state publisher they are represented as one value (angle). - :param q : Joint configuration provided by pinocchio library + :param q: Joint configuration provided by pinocchio library + :return: Joint positions in the format of joint state publisher. """ @@ -1073,7 +1076,7 @@ def get_position_for_joint_states(self, q : np.ndarray) -> np.ndarray: cont += self.model.joints[i].nq - return config + return np.array(config, dtype=object) def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: @@ -1097,15 +1100,15 @@ 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: + def get_joint_name(self, joint_id: int) -> str: """ Get the name of the joint by its ID. - :param id_joint: ID of the joint to get the name for. + :param joint_id: ID of the joint to get the name for. :return: Name of the joint. """ - return self.model.names[id_joint] + return self.model.names[joint_id] def get_joint_placement(self, joint_id : int, q : np.ndarray) -> dict: """ @@ -1166,6 +1169,7 @@ def get_frames(self) -> np.ndarray: def get_links(self,all_frames : bool = False) -> np.ndarray: """ 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. """ From 0d913e3e8e807c1d7de04fd10d8afa47f8b97d86 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 27 Aug 2025 09:58:35 +0200 Subject: [PATCH 09/15] added new test for core functions --- .../test/test_core.py | 221 ++++++++++++++++++ 1 file changed, 221 insertions(+) diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py index e1410b5..8851df6 100644 --- a/dynamic_payload_analysis_core/test/test_core.py +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -249,9 +249,230 @@ def test_get_maximum_payloads(self): self.assertEqual(max_payloads[0]["tree_id"], selected_subtrees[0]["tree_id"], "Tree ID should match the selected subtree's tree ID") print("✅ Get maximum payloads assertion passed") + + + def test_get_normalized_torques(self): + standard_config = self.robot_handler.get_zero_configuration() + # get torques for standard configuration + torques = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) + + normalized_torques = self.robot_handler.get_normalized_torques(torques) + + self.assertIsNotNone(normalized_torques, "Normalized torques should not be None") + self.assertIsInstance(normalized_torques, np.ndarray, "Normalized torques should be a numpy array") + self.assertEqual(len(normalized_torques), len(torques), "Normalized torques length should match the torques length") + self.assertTrue(all(0 <= nt <= 1.0 for nt in normalized_torques), "All normalized torques should be between 0 and 1") + + self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + max_torques = self.robot_handler.get_maximum_torques(valid_configurations) + normalized_torques_with_max = self.robot_handler.get_normalized_torques(torques, target_torque= max_torques, tree_id=5) + + self.assertIsNotNone(normalized_torques_with_max, "Normalized torques with max should not be None") + self.assertIsInstance(normalized_torques_with_max, np.ndarray, "Normalized torques with max should be a numpy array") + self.assertEqual(len(normalized_torques_with_max), len(torques), "Normalized torques with max length should match the torques length") + + print("✅ Get normalized torques assertion passed") + + def test_get_normalized_payload(self): + norm_payload = self.robot_handler.get_normalized_payload(payload=5,target_payload=10) + + self.assertIsNotNone(norm_payload, "Normalized payload should not be None") + self.assertIsInstance(norm_payload, float, "Normalized payload should be a float") + self.assertGreaterEqual(norm_payload, 0.0, "Normalized payload should be non-negative") + self.assertEqual(norm_payload, 0.5, "Normalized payload should be 0.5 for payload 5 and target payload 10") + + print("✅ Get normalized payload assertion passed") + + def test_get_unified_configurations_torque(self): + self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + valid_configurations = self.robot_handler.compute_maximum_payloads(valid_configurations) + + unified_configs = self.robot_handler.get_unified_configurations_torque(valid_configurations) + + self.assertIsNotNone(unified_configs, "Unified configurations should not be None") + self.assertIsInstance(unified_configs, np.ndarray, "Unified configurations should be a numpy array") + self.assertIn("norm_tau", unified_configs[0], "Each unified configuration should have a normalized_torque key") + self.assertEqual(len(unified_configs), len(valid_configurations), "Unified configurations length should match the valid configurations length") + + print("✅ Get unified configurations torque assertion passed") + + def test_check_zero(self): + zero_config = np.zeros(self.robot_handler.model.nv) + non_zero_config = zero_config + 1 + + self.assertTrue(self.robot_handler.check_zero(zero_config), "Zero configuration should be identified as zero") + self.assertFalse(self.robot_handler.check_zero(non_zero_config), "Non-zero configuration should not be identified as zero") + + print("✅ Check zero assertion passed") + + def test_get_zero_config_vel_acc(self): + zero_config = self.robot_handler.get_zero_configuration() + zero_velocity = self.robot_handler.get_zero_velocity() + zero_acceleration = self.robot_handler.get_zero_acceleration() + + self.assertIsNotNone(zero_config, "Zero configuration should not be None") + self.assertIsNotNone(zero_velocity, "Zero velocity should not be None") + self.assertIsNotNone(zero_acceleration, "Zero acceleration should not be None") + + self.assertIsInstance(zero_config, np.ndarray, "Zero configuration should be a numpy array") + self.assertIsInstance(zero_velocity, np.ndarray, "Zero velocity should be a numpy array") + self.assertIsInstance(zero_acceleration, np.ndarray, "Zero acceleration should be a numpy array") + + self.assertTrue(all(v == 0.0 for v in zero_config), "All elements in zero configuration should be zero") + self.assertTrue(all(v == 0.0 for v in zero_velocity), "All elements in zero velocity should be zero") + self.assertTrue(all(v == 0.0 for v in zero_acceleration), "All elements in zero acceleration should be zero") + + print("✅ Get zero configuration, velocity, and acceleration assertion passed") + + def test_get_analyzed_points(self): + analyzed_points = self.robot_handler.get_analyzed_points() + + self.assertIsInstance(analyzed_points, np.ndarray, "Analyzed points should be a numpy array") + self.assertEqual(len(analyzed_points), 0, "Analyzed points length should be zero initially") + + self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) + valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + + analyzed_points = self.robot_handler.get_analyzed_points() + + self.assertGreater(len(analyzed_points), 0, "Analyzed points length should be greater than zero after computing valid workspace") + + print("✅ Get analyzed points assertion passed") + + def test_get_random_configuration(self): + q, qdot = self.robot_handler.get_random_configuration() + self.assertIsNotNone(q, "Random configuration should not be None") + self.assertIsInstance(q, np.ndarray, "Random configuration should be a numpy array") + self.assertIsNotNone(qdot, "Random velocity should not be None") + self.assertIsInstance(qdot, np.ndarray, "Random velocity should be a numpy array") + print("✅ Get random configuration assertion passed") + + def test_check_joint_limits(self): + within_limits_config = self.robot_handler.get_zero_configuration() + out_of_limits_config = within_limits_config + 100 # Assuming this will exceed joint limits + + self.assertIsInstance(self.robot_handler.check_joint_limits(within_limits_config), np.ndarray, "Return value should be a numpy array of booleans") + self.assertTrue(np.all(self.robot_handler.check_joint_limits(within_limits_config)), "Configuration within limits should return True") + self.assertFalse(np.all(self.robot_handler.check_joint_limits(out_of_limits_config)), "Configuration out of limits should return False") + + print("✅ Check joint limits assertion passed") + + def test_check_effort_limits(self): + low_tau = self.robot_handler.compute_inverse_dynamics(self.robot_handler.get_zero_configuration(), self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) + high_tau = low_tau + 100 + self.assertIsInstance(self.robot_handler.check_effort_limits(low_tau,tree_id=5), np.ndarray, "Return value should be a numpy array of booleans") + self.assertTrue(np.all(self.robot_handler.check_effort_limits(low_tau,tree_id=5)), "Torques within limits should return True") + self.assertFalse(np.all(self.robot_handler.check_effort_limits(high_tau,tree_id=5)), "Torques out of limits should return False") + + print("✅ Check effort limits assertion passed") + + def test_get_position_for_joint_states(self): + q = self.robot_handler.get_zero_configuration() + + position = self.robot_handler.get_position_for_joint_states(q) + + self.assertIsNotNone(position, "Position should not be None") + self.assertIsInstance(position, np.ndarray, "Position should be a numpy array") + + print("✅ Get position for joint states assertion passed") + + def test_get_joints_placements(self): + q = self.robot_handler.get_zero_configuration() + + joint_placement, offset = self.robot_handler.get_joints_placements(q = q) + + self.assertIsNotNone(joint_placement, "Joint placement should not be None") + self.assertIsInstance(joint_placement, np.ndarray, "Joint placement should be a numpy array") + self.assertIsNotNone(offset, "Offset should not be None") + self.assertIsInstance(offset, float, "Offset should be a float") + + def test_get_joint_placement(self): + q = self.robot_handler.get_zero_configuration() + + joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = q) + + self.assertIsNotNone(joint_placement, "Joint placement should not be None") + self.assertIsInstance(joint_placement, dict, "Joint placement should be a dictionary") + self.assertIn("x", joint_placement, "Joint placement should have an 'x' key") + self.assertIn("y", joint_placement, "Joint placement should have a 'y' key") + self.assertIn("z", joint_placement, "Joint placement should have a 'z' key") + + print("✅ Get joint placement assertion passed") + + def test_get_joint_name(self): + joint_name = self.robot_handler.get_joint_name(joint_id=25) + + self.assertIsNotNone(joint_name, "Joint name should not be None") + self.assertIsInstance(joint_name, str, "Joint name should be a string") + self.assertEqual(joint_name, "arm_right_7_joint", "Joint name should match the expected value") + + print("✅ Get joint name assertion passed") + + def test_get_mass_matrix(self): + q = self.robot_handler.get_zero_configuration() + + mass_matrix = self.robot_handler.get_mass_matrix(q = q) + + self.assertIsNotNone(mass_matrix, "Mass matrix should not be None") + self.assertIsInstance(mass_matrix, np.ndarray, "Mass matrix should be a numpy array") + self.assertEqual(mass_matrix.shape, (self.robot_handler.model.nv, self.robot_handler.model.nv), "Mass matrix shape should be (nv, nv)") + + print("✅ Get mass matrix assertion passed") + + def test_get_joints(self): + joints = self.robot_handler.get_joints() + + self.assertIsNotNone(joints, "Joints should not be None") + self.assertIsInstance(joints, np.ndarray, "Joints should be a list") + self.assertGreater(len(joints), 0, "Joints list should not be empty") + self.assertIn("arm_right_1_joint", joints, "Joints list should contain 'arm_right_1_joint'") + + print("✅ Get joints assertion passed") + + def test_get_frames(self): + frames = self.robot_handler.get_frames() + + self.assertIsNotNone(frames, "Frames should not be None") + self.assertIsInstance(frames, np.ndarray, "Frames should be a list") + self.assertGreater(len(frames), 0, "Frames list should not be empty") + self.assertIn("arm_right_1_link", frames, "Frames list should contain 'arm_right_1_link'") + + print("✅ Get frames assertion passed") + + def test_get_links(self): + self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) + links = self.robot_handler.get_links(all_frames=True) + + self.assertIsNotNone(links, "Links should not be None") + self.assertIsInstance(links, np.ndarray, "Links should be a list") + self.assertGreater(len(links), 0, "Links list should not be empty") + self.assertIn("arm_right_1_link", links, "Links list should contain 'arm_right_1_link'") + + only_tip_links = self.robot_handler.get_links(all_frames=False) + + self.assertIsNotNone(only_tip_links, "Links should not be None") + self.assertIsInstance(only_tip_links, np.ndarray, "Links should be a list") + self.assertGreater(len(only_tip_links), 0, "Links list should not be empty") + self.assertLess(len(only_tip_links), len(links), "Tip links list should be smaller than all links list") + self.assertIn("arm_right_7_link", only_tip_links, "Tip links list should contain 'arm_right_7_link'") + + print("✅ Get links assertion passed") + + def test_get_parent_joint_id(self): + parent_id = self.robot_handler.get_parent_joint_id(frame_name="arm_right_7_link") + + self.assertIsNotNone(parent_id, "Parent joint ID should not be None") + self.assertIsInstance(parent_id, int, "Parent joint ID should be an integer") + self.assertEqual(parent_id, 24, "Parent joint ID should match the expected value") + + print("✅ Get parent joint ID assertion passed") + if __name__ == "__main__": unittest.main() \ No newline at end of file From 2240eeb3f7a88a58810642b150c9f05a2d0d3380 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Wed, 27 Aug 2025 13:19:52 +0200 Subject: [PATCH 10/15] fix error --- dynamic_payload_analysis_core/test/test_core.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py index 8851df6..77642ea 100644 --- a/dynamic_payload_analysis_core/test/test_core.py +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -470,7 +470,7 @@ def test_get_parent_joint_id(self): self.assertIsNotNone(parent_id, "Parent joint ID should not be None") self.assertIsInstance(parent_id, int, "Parent joint ID should be an integer") - self.assertEqual(parent_id, 24, "Parent joint ID should match the expected value") + self.assertEqual(parent_id, 25, "Parent joint ID should match the expected value") print("✅ Get parent joint ID assertion passed") From 19bdef2eff87b8194a1794fd5b8c2e35530d45ea Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 28 Aug 2025 11:47:40 +0200 Subject: [PATCH 11/15] fix format to pass all tests --- .../test/test_core.py | 791 +++++++++++++----- 1 file changed, 576 insertions(+), 215 deletions(-) diff --git a/dynamic_payload_analysis_core/test/test_core.py b/dynamic_payload_analysis_core/test/test_core.py index 77642ea..c30965d 100644 --- a/dynamic_payload_analysis_core/test/test_core.py +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -1,3 +1,19 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2025 PAL Robotics S.L. All rights reserved. +# +# 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 dynamic_payload_analysis_core.core import TorqueCalculator import unittest import os @@ -6,7 +22,7 @@ class TestTorqueCalculator(unittest.TestCase): - def __init__(self, methodName = "runTest"): + def __init__(self, methodName="runTest"): super().__init__(methodName) test_dir = os.path.dirname(__file__) @@ -14,15 +30,21 @@ def __init__(self, methodName = "runTest"): with open(urdf_path, "r") as f: self.xml_string = f.read() - self.robot_handler = TorqueCalculator(robot_description=self.xml_string) - + self.robot_handler = TorqueCalculator( + robot_description=self.xml_string) def test_init_torque_calculator(self): - self.assertIsNotNone(self.robot_handler.model, "Model should be initialized") - self.assertIsNotNone(self.robot_handler.geom_model,"Geom model should be initialized") - self.assertIsNotNone(self.robot_handler.geom_data, "Model data should be initialized") + self.assertIsNotNone( + self.robot_handler.model, + "Model should be initialized") + self.assertIsNotNone( + self.robot_handler.geom_model, + "Geom model should be initialized") + self.assertIsNotNone( + self.robot_handler.geom_data, + "Model data should be initialized") print("✅ Initialization assertions passed") - + def test_get_root_name(self): root_name = self.robot_handler.get_root_joint_name(self.xml_string) self.assertIsInstance(root_name, str, "Root name should be a string") @@ -34,14 +56,23 @@ def test_subtrees(self): subtrees = self.robot_handler.get_subtrees() self.assertIsNotNone(subtrees, "Subtrees should not be None") - self.assertIsInstance(subtrees, np.ndarray, "Subtrees should be a list") + self.assertIsInstance( + subtrees, + np.ndarray, + "Subtrees should be a list") self.assertEqual(len(subtrees), 7) self.assertEqual(subtrees[0]["tip_link_name"], "wheel_front_left_link") - self.assertEqual(subtrees[1]["tip_link_name"], "wheel_front_right_link") + self.assertEqual( + subtrees[1]["tip_link_name"], + "wheel_front_right_link") self.assertEqual(subtrees[2]["tip_link_name"], "wheel_rear_left_link") self.assertEqual(subtrees[3]["tip_link_name"], "wheel_rear_right_link") - self.assertEqual(subtrees[4]["tip_link_name"], "gripper_left_outer_finger_left_link") - self.assertEqual(subtrees[5]["tip_link_name"], "gripper_right_outer_finger_left_link") + self.assertEqual( + subtrees[4]["tip_link_name"], + "gripper_left_outer_finger_left_link") + self.assertEqual( + subtrees[5]["tip_link_name"], + "gripper_right_outer_finger_left_link") self.assertEqual(subtrees[6]["tip_link_name"], "head_2_link") print("✅ Subtrees assertion passed") @@ -52,138 +83,256 @@ def test_compute_mimic_joints(self): self.assertIn("gripper_right_inner_finger_left_joint", mimic_joints) self.assertNotIn("arm_1_joint", mimic_joints) print("✅ Mimic joints assertion passed") - + def test_create_external_force(self): - ext_forces = self.robot_handler.create_ext_force(masses = 1, frame_name = "arm_left_3_link", q = self.robot_handler.get_zero_configuration()) - self.assertIsInstance(ext_forces, list, "External forces should be a numpy array") - self.assertNotEqual(ext_forces[8], [0,0,0], "External force should not be zero") + ext_forces = self.robot_handler.create_ext_force( + masses=1, + frame_name="arm_left_3_link", + q=self.robot_handler.get_zero_configuration()) + self.assertIsInstance( + ext_forces, + list, + "External forces should be a numpy array") + self.assertNotEqual( + ext_forces[8], [ + 0, 0, 0], "External force should not be zero") print("✅ External force assertion passed") def test_compute_inverse_dynamics(self): standard_config = self.robot_handler.get_zero_configuration() - torques = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) - + torques = self.robot_handler.compute_inverse_dynamics( + standard_config, + self.robot_handler.get_zero_velocity(), + self.robot_handler.get_zero_acceleration()) + self.assertIsNotNone(torques, "Torques should not be None") self.assertIsInstance(torques, np.ndarray, "Torques should be a list") - self.assertTrue(all(isinstance(torque, float) for torque in torques), "All torques should be floats") - - # add external forces to the end-effector - external_forces = self.robot_handler.create_ext_force(masses=1, frame_name="arm_left_3_link", q=standard_config) - torques_with_ext = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration(), external_forces) + self.assertTrue(all(isinstance(torque, float) + for torque in torques), "All torques should be floats") - np.testing.assert_raises(AssertionError, np.testing.assert_array_equal, torques, torques_with_ext) + # add external forces to the end-effector + external_forces = self.robot_handler.create_ext_force( + masses=1, frame_name="arm_left_3_link", q=standard_config) + torques_with_ext = self.robot_handler.compute_inverse_dynamics( + standard_config, + self.robot_handler.get_zero_velocity(), + self.robot_handler.get_zero_acceleration(), + external_forces) + + np.testing.assert_raises( + AssertionError, + np.testing.assert_array_equal, + torques, + torques_with_ext) print("✅ Inverse dynamics assertion passed") - + def test_update_configuration(self): zero_config = self.robot_handler.get_zero_configuration() new_config = zero_config + 5 self.robot_handler.update_configuration(new_config) - joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = new_config) # just to ensure the configuration is updated internally + # just to ensure the configuration is updated internally + joint_placement = self.robot_handler.get_joint_placement( + joint_id=20, q=new_config) - self.assertIsInstance(joint_placement, dict, "Joint placement should be a dict") + self.assertIsInstance( + joint_placement, + dict, + "Joint placement should be a dict") self.robot_handler.update_configuration(zero_config) - updated_joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = zero_config) - - self.assertNotEqual(joint_placement["x"], updated_joint_placement["x"], "Joint placement should differ after configuration update") - self.assertNotEqual(joint_placement["y"], updated_joint_placement["y"], "Joint placement should differ after configuration update") - self.assertNotEqual(joint_placement["z"], updated_joint_placement["z"], "Joint placement should differ after configuration update") + updated_joint_placement = self.robot_handler.get_joint_placement( + joint_id=20, q=zero_config) + + self.assertNotEqual( + joint_placement["x"], + updated_joint_placement["x"], + "Joint placement should differ after configuration update") + self.assertNotEqual( + joint_placement["y"], + updated_joint_placement["y"], + "Joint placement should differ after configuration update") + self.assertNotEqual( + joint_placement["z"], + updated_joint_placement["z"], + "Joint placement should differ after configuration update") print("✅ Update configuration assertion passed") def test_compute_inverse_kinematics(self): initial_config = self.robot_handler.get_zero_configuration() - end_effector_pos = self.robot_handler.get_end_effector_position_array(-0.4, -0.6, 0.6) # reachable position - computed_config = self.robot_handler.compute_inverse_kinematics(q = initial_config, end_effector_position = end_effector_pos, joint_id= 25) - - self.assertIsNotNone(computed_config, "Computed configuration should not be None if IK is successful") - self.assertIsInstance(computed_config, np.ndarray, "Computed configuration should be a numpy array") - - end_effector_pos_not_reachable = self.robot_handler.get_end_effector_position_array(4, 6, 6) # not reachable position - computed_config_not_reachable = self.robot_handler.compute_inverse_kinematics(q = initial_config, end_effector_position = end_effector_pos_not_reachable, joint_id= 25) - - self.assertIsNone(computed_config_not_reachable, "Computed configuration should be None if IK is not successful") + # reachable position + end_effector_pos = self.robot_handler.get_end_effector_position_array( + -0.4, -0.6, 0.6) + computed_config = self.robot_handler.compute_inverse_kinematics( + q=initial_config, end_effector_position=end_effector_pos, joint_id=25) + + self.assertIsNotNone( + computed_config, + "Computed configuration should not be None if IK is successful") + self.assertIsInstance( + computed_config, + np.ndarray, + "Computed configuration should be a numpy array") + + end_effector_pos_not_reachable = self.robot_handler.get_end_effector_position_array( + 4, 6, 6) # not reachable position + computed_config_not_reachable = self.robot_handler.compute_inverse_kinematics( + q=initial_config, end_effector_position=end_effector_pos_not_reachable, joint_id=25) + + self.assertIsNone( + computed_config_not_reachable, + "Computed configuration should be None if IK is not successful") print("✅ Inverse kinematics assertion passed") - def test_compute_valid_workspace(self): - computed_configurations = self.robot_handler.compute_all_configurations(range = 1, resolution = 0.3, end_joint_id= 25) # used 1 meter range and 0.3 resolution to speed up the test - len_configs = len(computed_configurations) - self.assertIsInstance(computed_configurations, np.ndarray, "Computed configuration should be a numpy array") - - self.assertTrue(np.all(computed_configurations[round(len_configs/2)]["end_effector_pos"] <= np.array([1, 1, 1])), "End effector positions should be within the specified range") - self.assertTrue(np.all(computed_configurations[round(len_configs/3)]["end_effector_pos"] <= np.array([1, 1, 1])), "End effector positions should be within the specified range") - - verified_configs = self.robot_handler.verify_configurations(computed_configurations, masses= None, checked_frames= None, tree_id=5, selected_joint_id=25) - - self.assertIsInstance(verified_configs, np.ndarray, "Verified configuration should be a numpy array") - - self.assertEqual(verified_configs[0]["tree_id"], 5, "Tree ID should match the provided tree ID") - self.assertEqual(verified_configs[0]["selected_joint_id"], 25, "Selected joint ID should match the provided joint ID") - - - # should be None because there is no selected joint id inside the object - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - - self.assertIsInstance(valid_configurations, np.ndarray, "Valid workspace should be a numpy array") - self.assertEqual(len(valid_configurations), 0, "Valid workspace length should be zero if there is no selected joint id") + computed_configs = self.robot_handler.compute_all_configurations( + range=1, + resolution=0.3, + end_joint_id=25) # used 1 meter range and 0.3 resolution to speed up the test + len_configs = len(computed_configs) + self.assertIsInstance( + computed_configs, + np.ndarray, + "Computed configuration should be a numpy array") + + config_number = round(len_configs / 2) + self.assertTrue(np.all(computed_configs[config_number]["end_effector_pos"] <= np.array( + [1, 1, 1])), "End effector positions should be within the specified range") + config_number = round(len_configs / 3) + self.assertTrue(np.all(computed_configs[config_number]["end_effector_pos"] <= np.array( + [1, 1, 1])), "End effector positions should be within the specified range") + + verified_configs = self.robot_handler.verify_configurations( + computed_configs, + masses=None, + checked_frames=None, + tree_id=5, + selected_joint_id=25) + + self.assertIsInstance( + verified_configs, + np.ndarray, + "Verified configuration should be a numpy array") + + self.assertEqual( + verified_configs[0]["tree_id"], + 5, + "Tree ID should match the provided tree ID") + self.assertEqual( + verified_configs[0]["selected_joint_id"], + 25, + "Selected joint ID should match the provided joint ID") + + # should be None because there is no selected joint id inside the + # object + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + + self.assertIsInstance( + valid_configurations, + np.ndarray, + "Valid workspace should be a numpy array") + self.assertEqual( + len(valid_configurations), + 0, + "Valid workspace length should be zero if there is no selected joint id") # modify selected joint id to test valid workspace self.robot_handler.subtrees[5]["selected_joint_id"] = 25 - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) - self.assertEqual(len(valid_configurations), len(verified_configs), "Valid workspace length should match the verified configurations length") + self.assertEqual( + len(valid_configurations), + len(verified_configs), + "Valid workspace length should match the verified configurations length") print("✅ Compute valid workspace assertion passed") - + def test_compute_maximum_payloads(self): self.robot_handler.subtrees[5]["selected_joint_id"] = 25 - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - - max_payload = self.robot_handler.find_max_payload_binary_search(config = valid_configurations[0], payload_min=0.0, payload_max=10.0, resolution=0.01) - - self.assertIsInstance(max_payload, float, "Max payload should be a float") - self.assertGreaterEqual(max_payload, 0.0, "Max payload should be non-negative") - self.assertLessEqual(max_payload, 10.0, "Max payload should be within the specified range") - - max_payload = self.robot_handler.find_max_payload_binary_search(config = valid_configurations[0], payload_min=0.0, payload_max=0, resolution=0.01) - self.assertEqual(max_payload, 0.0, "Max payload should be zero if payload_max is zero") - - configs_with_payloads = self.robot_handler.compute_maximum_payloads(valid_configurations) - - self.assertIsInstance(configs_with_payloads, np.ndarray, "Configurations with payloads should be a numpy array") - self.assertIn("max_payload", configs_with_payloads[0], "Each configuration should have a max_payload key") - self.assertIsInstance(configs_with_payloads[0]["max_payload"], float, "Max payload should be a float") + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + + max_payload = self.robot_handler.find_max_payload_binary_search( + config=valid_configurations[0], payload_min=0.0, payload_max=10.0, resolution=0.01) + + self.assertIsInstance( + max_payload, + float, + "Max payload should be a float") + self.assertGreaterEqual( + max_payload, 0.0, "Max payload should be non-negative") + self.assertLessEqual( + max_payload, + 10.0, + "Max payload should be within the specified range") + + max_payload = self.robot_handler.find_max_payload_binary_search( + config=valid_configurations[0], payload_min=0.0, payload_max=0, resolution=0.01) + self.assertEqual( + max_payload, + 0.0, + "Max payload should be zero if payload_max is zero") + + configs_with_payloads = self.robot_handler.compute_maximum_payloads( + valid_configurations) + + self.assertIsInstance( + configs_with_payloads, + np.ndarray, + "Configurations with payloads should be a numpy array") + self.assertIn( + "max_payload", + configs_with_payloads[0], + "Each configuration should have a max_payload key") + self.assertIsInstance( + configs_with_payloads[0]["max_payload"], + float, + "Max payload should be a float") print("✅ Compute maximum payloads assertion passed") def test_compute_forward_dynamics_aba_method(self): zero_config = self.robot_handler.get_zero_configuration() zero_velocity = self.robot_handler.get_zero_velocity() - zero_acceleration = self.robot_handler.get_zero_acceleration() tau = np.zeros(self.robot_handler.model.nv) - computed_acceleration = self.robot_handler.compute_forward_dynamics_aba_method(q=zero_config, qdot=zero_velocity, tau=tau) + computed_acceleration = self.robot_handler.compute_forward_dynamics_aba_method( + q=zero_config, qdot=zero_velocity, tau=tau) - self.assertIsNotNone(computed_acceleration, "Computed acceleration should not be None") - self.assertIsInstance(computed_acceleration, np.ndarray, "Computed acceleration should be a numpy array") + self.assertIsNotNone( + computed_acceleration, + "Computed acceleration should not be None") + self.assertIsInstance( + computed_acceleration, + np.ndarray, + "Computed acceleration should be a numpy array") print("✅ Compute forward dynamics ABA method assertion passed") def test_compute_jacobian(self): zero_config = self.robot_handler.get_zero_configuration() - jacobian = self.robot_handler.compute_jacobian(q=zero_config, frame_name= "arm_left_3_link") + jacobian = self.robot_handler.compute_jacobian( + q=zero_config, frame_name="arm_left_3_link") self.assertIsNotNone(jacobian, "Jacobian should not be None") - self.assertIsInstance(jacobian, np.ndarray, "Jacobian should be a numpy array") - self.assertEqual(jacobian.shape, (6, self.robot_handler.model.nv), "Jacobian shape should be (6, nv)") + self.assertIsInstance( + jacobian, + np.ndarray, + "Jacobian should be a numpy array") + self.assertEqual( + jacobian.shape, + (6, + self.robot_handler.model.nv), + "Jacobian shape should be (6, nv)") print("✅ Compute Jacobian assertion passed") - + def test_verify_member_tree(self): flag = self.robot_handler.verify_member_tree(tree_id=5, joint_id=25) @@ -192,110 +341,202 @@ def test_verify_member_tree(self): flag = self.robot_handler.verify_member_tree(tree_id=5, joint_id=2) - self.assertFalse(flag, "Joint ID should not be in the specified tree ID") + self.assertFalse( + flag, "Joint ID should not be in the specified tree ID") print("✅ Verify member tree assertion passed") - + def test_get_links_from_tree(self): link = self.robot_handler.get_links_from_tree(joint_ids=25) links = self.robot_handler.get_links_from_tree(joint_ids=[25, 26, 24]) - self.assertIsInstance(links, np.ndarray, "Links should be a numpy array") - self.assertEqual(len(links), 3, "There should be 3 links in the subtree") - - self.assertIn("arm_right_7_link", link, "Link should be in the subtree") + self.assertIsInstance( + links, + np.ndarray, + "Links should be a numpy array") + self.assertEqual( + len(links), + 3, + "There should be 3 links in the subtree") + + self.assertIn( + "arm_right_7_link", + link, + "Link should be in the subtree") self.assertIsNotNone(link, "Link should not be None") self.assertIsInstance(link, np.ndarray, "Link should be a numpy array") - + print("✅ Get links from tree assertion passed") - + def test_get_maximum_torques(self): - #modify selected joint id to test valid workspace + # modify selected joint id to test valid workspace self.robot_handler.subtrees[5]["selected_joint_id"] = 25 - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - - max_torques = self.robot_handler.get_maximum_torques(valid_configurations) + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + + max_torques = self.robot_handler.get_maximum_torques( + valid_configurations) self.assertIsNotNone(max_torques, "Max torques should not be None") - self.assertIsInstance(max_torques, np.ndarray, "Max torques should be a numpy array") + self.assertIsInstance( + max_torques, + np.ndarray, + "Max torques should be a numpy array") # get selected subtrees only - selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees() if subtree["selected_joint_id"] is not None] - - self.assertEqual(len(max_torques), len(selected_subtrees), "Max torques length should match the number of selected subtrees") - self.assertIn("max_values", max_torques[0], "Each max torque entry should have a max_values key") - self.assertEqual(max_torques[0]["tree_id"], selected_subtrees[0]["tree_id"], "Tree ID should match the selected subtree's tree ID") + selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees( + ) if subtree["selected_joint_id"] is not None] + + self.assertEqual( + len(max_torques), + len(selected_subtrees), + "Max torques length should match the number of selected subtrees") + self.assertIn( + "max_values", + max_torques[0], + "Each max torque entry should have a max_values key") + self.assertEqual( + max_torques[0]["tree_id"], + selected_subtrees[0]["tree_id"], + "Tree ID should match the selected subtree's tree ID") print("✅ Get maximum torques assertion passed") - + def test_get_maximum_payloads(self): - #modify selected joint id to test valid workspace + # modify selected joint id to test valid workspace self.robot_handler.subtrees[5]["selected_joint_id"] = 25 - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - valid_configurations = self.robot_handler.compute_maximum_payloads(valid_configurations) - - max_payloads = self.robot_handler.get_maximum_payloads(valid_configurations) + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + valid_configurations = self.robot_handler.compute_maximum_payloads( + valid_configurations) + + max_payloads = self.robot_handler.get_maximum_payloads( + valid_configurations) self.assertIsNotNone(max_payloads, "Max payloads should not be None") - self.assertIsInstance(max_payloads, np.ndarray, "Max payloads should be a numpy array") - self.assertGreater(len(max_payloads), 0, "Max payloads length should be greater than zero") - self.assertIn("max_payload", max_payloads[0], "Each max payload entry should have a max_payload key") - self.assertIsInstance(max_payloads[0]["max_payload"], float, "Max payload values should be a float") + self.assertIsInstance( + max_payloads, + np.ndarray, + "Max payloads should be a numpy array") + self.assertGreater( + len(max_payloads), + 0, + "Max payloads length should be greater than zero") + self.assertIn( + "max_payload", + max_payloads[0], + "Each max payload entry should have a max_payload key") + self.assertIsInstance( + max_payloads[0]["max_payload"], + float, + "Max payload values should be a float") # get selected subtrees only - selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees() if subtree["selected_joint_id"] is not None] - - self.assertEqual(len(max_payloads), len(selected_subtrees), "Max payloads length should match the number of selected subtrees") - self.assertEqual(max_payloads[0]["tree_id"], selected_subtrees[0]["tree_id"], "Tree ID should match the selected subtree's tree ID") + selected_subtrees = [subtree for subtree in self.robot_handler.get_subtrees( + ) if subtree["selected_joint_id"] is not None] + + self.assertEqual( + len(max_payloads), + len(selected_subtrees), + "Max payloads length should match the number of selected subtrees") + self.assertEqual( + max_payloads[0]["tree_id"], + selected_subtrees[0]["tree_id"], + "Tree ID should match the selected subtree's tree ID") print("✅ Get maximum payloads assertion passed") - def test_get_normalized_torques(self): standard_config = self.robot_handler.get_zero_configuration() # get torques for standard configuration - torques = self.robot_handler.compute_inverse_dynamics(standard_config, self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) + torques = self.robot_handler.compute_inverse_dynamics( + standard_config, + self.robot_handler.get_zero_velocity(), + self.robot_handler.get_zero_acceleration()) normalized_torques = self.robot_handler.get_normalized_torques(torques) - self.assertIsNotNone(normalized_torques, "Normalized torques should not be None") - self.assertIsInstance(normalized_torques, np.ndarray, "Normalized torques should be a numpy array") - self.assertEqual(len(normalized_torques), len(torques), "Normalized torques length should match the torques length") - self.assertTrue(all(0 <= nt <= 1.0 for nt in normalized_torques), "All normalized torques should be between 0 and 1") + self.assertIsNotNone( + normalized_torques, + "Normalized torques should not be None") + self.assertIsInstance( + normalized_torques, + np.ndarray, + "Normalized torques should be a numpy array") + self.assertEqual( + len(normalized_torques), + len(torques), + "Normalized torques length should match the torques length") + self.assertTrue(all(0 <= nt <= 1.0 for nt in normalized_torques), + "All normalized torques should be between 0 and 1") self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - - max_torques = self.robot_handler.get_maximum_torques(valid_configurations) - normalized_torques_with_max = self.robot_handler.get_normalized_torques(torques, target_torque= max_torques, tree_id=5) - - self.assertIsNotNone(normalized_torques_with_max, "Normalized torques with max should not be None") - self.assertIsInstance(normalized_torques_with_max, np.ndarray, "Normalized torques with max should be a numpy array") - self.assertEqual(len(normalized_torques_with_max), len(torques), "Normalized torques with max length should match the torques length") + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + + max_torques = self.robot_handler.get_maximum_torques( + valid_configurations) + normalized_torques_with_max = self.robot_handler.get_normalized_torques( + torques, target_torque=max_torques, tree_id=5) + + self.assertIsNotNone(normalized_torques_with_max, + "Normalized torques with max should not be None") + self.assertIsInstance( + normalized_torques_with_max, + np.ndarray, + "Normalized torques with max should be a numpy array") + self.assertEqual( + len(normalized_torques_with_max), + len(torques), + "Normalized torques with max length should match the torques length") print("✅ Get normalized torques assertion passed") - - def test_get_normalized_payload(self): - norm_payload = self.robot_handler.get_normalized_payload(payload=5,target_payload=10) - self.assertIsNotNone(norm_payload, "Normalized payload should not be None") - self.assertIsInstance(norm_payload, float, "Normalized payload should be a float") - self.assertGreaterEqual(norm_payload, 0.0, "Normalized payload should be non-negative") - self.assertEqual(norm_payload, 0.5, "Normalized payload should be 0.5 for payload 5 and target payload 10") + def test_get_normalized_payload(self): + norm_payload = self.robot_handler.get_normalized_payload( + payload=5, target_payload=10) + + self.assertIsNotNone(norm_payload, + "Normalized payload should not be None") + self.assertIsInstance( + norm_payload, + float, + "Normalized payload should be a float") + self.assertGreaterEqual( + norm_payload, + 0.0, + "Normalized payload should be non-negative") + self.assertEqual( + norm_payload, + 0.5, + "Normalized payload should be 0.5 for payload 5 and target payload 10") print("✅ Get normalized payload assertion passed") - + def test_get_unified_configurations_torque(self): self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) - valid_configurations = self.robot_handler.compute_maximum_payloads(valid_configurations) - - unified_configs = self.robot_handler.get_unified_configurations_torque(valid_configurations) - - self.assertIsNotNone(unified_configs, "Unified configurations should not be None") - self.assertIsInstance(unified_configs, np.ndarray, "Unified configurations should be a numpy array") - self.assertIn("norm_tau", unified_configs[0], "Each unified configuration should have a normalized_torque key") - self.assertEqual(len(unified_configs), len(valid_configurations), "Unified configurations length should match the valid configurations length") + valid_configurations = self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) + valid_configurations = self.robot_handler.compute_maximum_payloads( + valid_configurations) + + unified_configs = self.robot_handler.get_unified_configurations_torque( + valid_configurations) + + self.assertIsNotNone(unified_configs, + "Unified configurations should not be None") + self.assertIsInstance( + unified_configs, + np.ndarray, + "Unified configurations should be a numpy array") + self.assertIn( + "norm_tau", + unified_configs[0], + "Each unified configuration should have a normalized_torque key") + self.assertEqual( + len(unified_configs), + len(valid_configurations), + "Unified configurations length should match the valid configurations length") print("✅ Get unified configurations torque assertion passed") @@ -303,8 +544,12 @@ def test_check_zero(self): zero_config = np.zeros(self.robot_handler.model.nv) non_zero_config = zero_config + 1 - self.assertTrue(self.robot_handler.check_zero(zero_config), "Zero configuration should be identified as zero") - self.assertFalse(self.robot_handler.check_zero(non_zero_config), "Non-zero configuration should not be identified as zero") + self.assertTrue( + self.robot_handler.check_zero(zero_config), + "Zero configuration should be identified as zero") + self.assertFalse( + self.robot_handler.check_zero(non_zero_config), + "Non-zero configuration should not be identified as zero") print("✅ Check zero assertion passed") @@ -313,97 +558,176 @@ def test_get_zero_config_vel_acc(self): zero_velocity = self.robot_handler.get_zero_velocity() zero_acceleration = self.robot_handler.get_zero_acceleration() - self.assertIsNotNone(zero_config, "Zero configuration should not be None") + self.assertIsNotNone(zero_config, + "Zero configuration should not be None") self.assertIsNotNone(zero_velocity, "Zero velocity should not be None") - self.assertIsNotNone(zero_acceleration, "Zero acceleration should not be None") - - self.assertIsInstance(zero_config, np.ndarray, "Zero configuration should be a numpy array") - self.assertIsInstance(zero_velocity, np.ndarray, "Zero velocity should be a numpy array") - self.assertIsInstance(zero_acceleration, np.ndarray, "Zero acceleration should be a numpy array") - - self.assertTrue(all(v == 0.0 for v in zero_config), "All elements in zero configuration should be zero") - self.assertTrue(all(v == 0.0 for v in zero_velocity), "All elements in zero velocity should be zero") - self.assertTrue(all(v == 0.0 for v in zero_acceleration), "All elements in zero acceleration should be zero") + self.assertIsNotNone( + zero_acceleration, + "Zero acceleration should not be None") + + self.assertIsInstance( + zero_config, + np.ndarray, + "Zero configuration should be a numpy array") + self.assertIsInstance( + zero_velocity, + np.ndarray, + "Zero velocity should be a numpy array") + self.assertIsInstance( + zero_acceleration, + np.ndarray, + "Zero acceleration should be a numpy array") + + self.assertTrue(all(v == 0.0 for v in zero_config), + "All elements in zero configuration should be zero") + self.assertTrue(all(v == 0.0 for v in zero_velocity), + "All elements in zero velocity should be zero") + self.assertTrue(all(v == 0.0 for v in zero_acceleration), + "All elements in zero acceleration should be zero") print("✅ Get zero configuration, velocity, and acceleration assertion passed") def test_get_analyzed_points(self): analyzed_points = self.robot_handler.get_analyzed_points() - self.assertIsInstance(analyzed_points, np.ndarray, "Analyzed points should be a numpy array") - self.assertEqual(len(analyzed_points), 0, "Analyzed points length should be zero initially") + self.assertIsInstance( + analyzed_points, + np.ndarray, + "Analyzed points should be a numpy array") + self.assertEqual( + len(analyzed_points), + 0, + "Analyzed points length should be zero initially") self.robot_handler.set_joint_tree_selection(tree_id=5, joint_id=25) - valid_configurations = self.robot_handler.get_valid_workspace(range = 1, resolution= 0.3, masses = None, checked_frames = None) + self.robot_handler.get_valid_workspace( + range=1, resolution=0.3, masses=None, checked_frames=None) analyzed_points = self.robot_handler.get_analyzed_points() - self.assertGreater(len(analyzed_points), 0, "Analyzed points length should be greater than zero after computing valid workspace") + self.assertGreater( + len(analyzed_points), + 0, + "Analyzed points length should be greater than zero after computing valid workspace") print("✅ Get analyzed points assertion passed") - + def test_get_random_configuration(self): q, qdot = self.robot_handler.get_random_configuration() self.assertIsNotNone(q, "Random configuration should not be None") - self.assertIsInstance(q, np.ndarray, "Random configuration should be a numpy array") + self.assertIsInstance( + q, np.ndarray, "Random configuration should be a numpy array") self.assertIsNotNone(qdot, "Random velocity should not be None") - self.assertIsInstance(qdot, np.ndarray, "Random velocity should be a numpy array") - + self.assertIsInstance( + qdot, + np.ndarray, + "Random velocity should be a numpy array") + print("✅ Get random configuration assertion passed") - + def test_check_joint_limits(self): within_limits_config = self.robot_handler.get_zero_configuration() - out_of_limits_config = within_limits_config + 100 # Assuming this will exceed joint limits + out_of_limits_config = within_limits_config + \ + 100 # Assuming this will exceed joint limits + + self.assertIsInstance( + self.robot_handler.check_joint_limits(within_limits_config), + np.ndarray, + "Return value should be a numpy array of booleans") + self.assertTrue( + np.all( + self.robot_handler.check_joint_limits(within_limits_config)), + "Configuration within limits should return True") + self.assertFalse( + np.all( + self.robot_handler.check_joint_limits(out_of_limits_config)), + "Configuration out of limits should return False") - self.assertIsInstance(self.robot_handler.check_joint_limits(within_limits_config), np.ndarray, "Return value should be a numpy array of booleans") - self.assertTrue(np.all(self.robot_handler.check_joint_limits(within_limits_config)), "Configuration within limits should return True") - self.assertFalse(np.all(self.robot_handler.check_joint_limits(out_of_limits_config)), "Configuration out of limits should return False") - print("✅ Check joint limits assertion passed") def test_check_effort_limits(self): - low_tau = self.robot_handler.compute_inverse_dynamics(self.robot_handler.get_zero_configuration(), self.robot_handler.get_zero_velocity(), self.robot_handler.get_zero_acceleration()) + low_tau = self.robot_handler.compute_inverse_dynamics( + self.robot_handler.get_zero_configuration(), + self.robot_handler.get_zero_velocity(), + self.robot_handler.get_zero_acceleration()) high_tau = low_tau + 100 - - self.assertIsInstance(self.robot_handler.check_effort_limits(low_tau,tree_id=5), np.ndarray, "Return value should be a numpy array of booleans") - self.assertTrue(np.all(self.robot_handler.check_effort_limits(low_tau,tree_id=5)), "Torques within limits should return True") - self.assertFalse(np.all(self.robot_handler.check_effort_limits(high_tau,tree_id=5)), "Torques out of limits should return False") + self.assertIsInstance( + self.robot_handler.check_effort_limits( + low_tau, + tree_id=5), + np.ndarray, + "Return value should be a numpy array of booleans") + self.assertTrue( + np.all( + self.robot_handler.check_effort_limits( + low_tau, + tree_id=5)), + "Torques within limits should return True") + self.assertFalse( + np.all( + self.robot_handler.check_effort_limits( + high_tau, + tree_id=5)), + "Torques out of limits should return False") print("✅ Check effort limits assertion passed") - + def test_get_position_for_joint_states(self): q = self.robot_handler.get_zero_configuration() position = self.robot_handler.get_position_for_joint_states(q) self.assertIsNotNone(position, "Position should not be None") - self.assertIsInstance(position, np.ndarray, "Position should be a numpy array") - + self.assertIsInstance( + position, + np.ndarray, + "Position should be a numpy array") + print("✅ Get position for joint states assertion passed") - + def test_get_joints_placements(self): q = self.robot_handler.get_zero_configuration() - joint_placement, offset = self.robot_handler.get_joints_placements(q = q) + joint_placement, offset = self.robot_handler.get_joints_placements(q=q) - self.assertIsNotNone(joint_placement, "Joint placement should not be None") - self.assertIsInstance(joint_placement, np.ndarray, "Joint placement should be a numpy array") + self.assertIsNotNone( + joint_placement, + "Joint placement should not be None") + self.assertIsInstance( + joint_placement, + np.ndarray, + "Joint placement should be a numpy array") self.assertIsNotNone(offset, "Offset should not be None") self.assertIsInstance(offset, float, "Offset should be a float") - + def test_get_joint_placement(self): q = self.robot_handler.get_zero_configuration() - joint_placement = self.robot_handler.get_joint_placement(joint_id=20,q = q) + joint_placement = self.robot_handler.get_joint_placement( + joint_id=20, q=q) + + self.assertIsNotNone( + joint_placement, + "Joint placement should not be None") + self.assertIsInstance( + joint_placement, + dict, + "Joint placement should be a dictionary") + self.assertIn( + "x", + joint_placement, + "Joint placement should have an 'x' key") + self.assertIn( + "y", + joint_placement, + "Joint placement should have a 'y' key") + self.assertIn( + "z", + joint_placement, + "Joint placement should have a 'z' key") - self.assertIsNotNone(joint_placement, "Joint placement should not be None") - self.assertIsInstance(joint_placement, dict, "Joint placement should be a dictionary") - self.assertIn("x", joint_placement, "Joint placement should have an 'x' key") - self.assertIn("y", joint_placement, "Joint placement should have a 'y' key") - self.assertIn("z", joint_placement, "Joint placement should have a 'z' key") - print("✅ Get joint placement assertion passed") def test_get_joint_name(self): @@ -411,28 +735,41 @@ def test_get_joint_name(self): self.assertIsNotNone(joint_name, "Joint name should not be None") self.assertIsInstance(joint_name, str, "Joint name should be a string") - self.assertEqual(joint_name, "arm_right_7_joint", "Joint name should match the expected value") + self.assertEqual( + joint_name, + "arm_right_7_joint", + "Joint name should match the expected value") print("✅ Get joint name assertion passed") def test_get_mass_matrix(self): q = self.robot_handler.get_zero_configuration() - mass_matrix = self.robot_handler.get_mass_matrix(q = q) + mass_matrix = self.robot_handler.get_mass_matrix(q=q) self.assertIsNotNone(mass_matrix, "Mass matrix should not be None") - self.assertIsInstance(mass_matrix, np.ndarray, "Mass matrix should be a numpy array") - self.assertEqual(mass_matrix.shape, (self.robot_handler.model.nv, self.robot_handler.model.nv), "Mass matrix shape should be (nv, nv)") + self.assertIsInstance( + mass_matrix, + np.ndarray, + "Mass matrix should be a numpy array") + self.assertEqual( + mass_matrix.shape, + (self.robot_handler.model.nv, + self.robot_handler.model.nv), + "Mass matrix shape should be (nv, nv)") print("✅ Get mass matrix assertion passed") - + def test_get_joints(self): joints = self.robot_handler.get_joints() self.assertIsNotNone(joints, "Joints should not be None") self.assertIsInstance(joints, np.ndarray, "Joints should be a list") self.assertGreater(len(joints), 0, "Joints list should not be empty") - self.assertIn("arm_right_1_joint", joints, "Joints list should contain 'arm_right_1_joint'") + self.assertIn( + "arm_right_1_joint", + joints, + "Joints list should contain 'arm_right_1_joint'") print("✅ Get joints assertion passed") @@ -442,7 +779,10 @@ def test_get_frames(self): self.assertIsNotNone(frames, "Frames should not be None") self.assertIsInstance(frames, np.ndarray, "Frames should be a list") self.assertGreater(len(frames), 0, "Frames list should not be empty") - self.assertIn("arm_right_1_link", frames, "Frames list should contain 'arm_right_1_link'") + self.assertIn( + "arm_right_1_link", + frames, + "Frames list should contain 'arm_right_1_link'") print("✅ Get frames assertion passed") @@ -453,26 +793,47 @@ def test_get_links(self): self.assertIsNotNone(links, "Links should not be None") self.assertIsInstance(links, np.ndarray, "Links should be a list") self.assertGreater(len(links), 0, "Links list should not be empty") - self.assertIn("arm_right_1_link", links, "Links list should contain 'arm_right_1_link'") + self.assertIn( + "arm_right_1_link", + links, + "Links list should contain 'arm_right_1_link'") only_tip_links = self.robot_handler.get_links(all_frames=False) self.assertIsNotNone(only_tip_links, "Links should not be None") - self.assertIsInstance(only_tip_links, np.ndarray, "Links should be a list") - self.assertGreater(len(only_tip_links), 0, "Links list should not be empty") - self.assertLess(len(only_tip_links), len(links), "Tip links list should be smaller than all links list") - self.assertIn("arm_right_7_link", only_tip_links, "Tip links list should contain 'arm_right_7_link'") - + self.assertIsInstance( + only_tip_links, + np.ndarray, + "Links should be a list") + self.assertGreater( + len(only_tip_links), + 0, + "Links list should not be empty") + self.assertLess( + len(only_tip_links), + len(links), + "Tip links list should be smaller than all links list") + self.assertIn( + "arm_right_7_link", + only_tip_links, + "Tip links list should contain 'arm_right_7_link'") + print("✅ Get links assertion passed") def test_get_parent_joint_id(self): - parent_id = self.robot_handler.get_parent_joint_id(frame_name="arm_right_7_link") + parent_id = self.robot_handler.get_parent_joint_id( + frame_name="arm_right_7_link") self.assertIsNotNone(parent_id, "Parent joint ID should not be None") - self.assertIsInstance(parent_id, int, "Parent joint ID should be an integer") - self.assertEqual(parent_id, 25, "Parent joint ID should match the expected value") + self.assertIsInstance( + parent_id, int, "Parent joint ID should be an integer") + self.assertEqual( + parent_id, + 25, + "Parent joint ID should match the expected value") print("✅ Get parent joint ID assertion passed") + if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() From 96f96c0923a60225ea987976ac824513f51c3f29 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 28 Aug 2025 12:01:02 +0200 Subject: [PATCH 12/15] fix format to pass test --- .../dynamic_payload_analysis_core/core.py | 562 ++++++++---------- 1 file changed, 242 insertions(+), 320 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 77091fe..d2744ef 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -18,7 +18,6 @@ import numpy as np import math from typing import Union -from pathlib import Path from numpy.linalg import norm, solve import tempfile import os @@ -26,18 +25,18 @@ class TorqueCalculator: - def __init__(self, robot_description : str): + def __init__(self, robot_description: str): """ Initialize the Torques_calculator with the URDF model or XML format provided by robot_description topic. - + :param robot_description: Robot description in XML format provided by /robot_description topic. """ # Load the robot model from path or XML string if isinstance(robot_description, str): self.model = pin.buildModelFromXML(robot_description) - - # compute mimic joints + + # compute mimic joints self.compute_mimic_joints(robot_description) # get the root joint name @@ -45,11 +44,11 @@ def __init__(self, robot_description : str): # 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 + temp_file.write(robot_description) + temp_urdf_path = temp_file.name + + self.geom_model = pin.buildGeomFromUrdf(self.model, temp_urdf_path, pin.GeometryType.COLLISION) - self.geom_model = pin.buildGeomFromUrdf(self.model,temp_urdf_path,pin.GeometryType.COLLISION) - # Add collisition pairs self.geom_model.addAllCollisionPairs() @@ -57,7 +56,7 @@ def __init__(self, robot_description : str): else: raise ValueError("robot_description must be a string containing the URDF XML or the path to the URDF file") - + # create data for the robot model self.data = self.model.createData() self.geom_data = pin.GeometryData(self.geom_model) @@ -76,13 +75,12 @@ def __init__(self, robot_description : str): # create items for each tree in the robot model for tree in self.subtrees: - self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id" : None}) - + 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: @@ -94,7 +92,6 @@ def get_root_joint_name(self, robot_description: str) -> str: return root_name - def compute_mimic_joints(self, urdf_xml): """ Function to find all mimic joints with mimicked joints and ids. @@ -104,8 +101,8 @@ def compute_mimic_joints(self, urdf_xml): """ try: robot = URDF.from_xml_string(urdf_xml) - except: - print(f"Error parsing URDF xml") + except BaseException: + print("Error parsing URDF xml") self.mimic_joint_names = [] self.mimicked_joint_names = [] @@ -122,19 +119,18 @@ def compute_mimic_joints(self, urdf_xml): 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): """ Compute the static collisions for the robot model. This method is used to compute the collisions in the robot model in the zero configuration. """ - + # array to store the collision pairs collision_pairs = [] # Compute all the collisions pin.computeCollisions(self.model, self.data, self.geom_model, self.geom_data, self.get_zero_configuration(), 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] @@ -144,24 +140,23 @@ def compute_static_collisions(self): collision_pairs.append((cp.first, cp.second, k)) return collision_pairs - - + 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 """ - + tip_joints = [] for id in range(0, self.model.njoints): - if len(self.model.subtrees[id]) == 1: - tip_joints += [id] - + if len(self.model.subtrees[id]) == 1: + tip_joints += [id] + self.subtrees = np.array([], dtype=object) cont = 0 for i, jointID in enumerate(tip_joints): joint_tree_ids = self.get_filtered_subtree(jointID) - + # 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: @@ -175,15 +170,14 @@ 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_link_name": link_names[-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 - - def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: + 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. """ @@ -193,7 +187,7 @@ def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: 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: @@ -202,14 +196,13 @@ def get_filtered_subtree(self, current_tip_id : int) -> np.ndarray: # remove universe joint filtered_subtree = filtered_subtree[1:] - - return filtered_subtree + return filtered_subtree - def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : np.ndarray, extForce : np.ndarray = 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. - + :param q: Joint configuration vector. :param qdot: Joint velocity vector. :param qddot: Joint acceleration vector. @@ -223,18 +216,17 @@ def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : n tau = pin.rnea(self.model, self.data, q, qdot, qddot, extForce) else: tau = pin.rnea(self.model, self.data, q, qdot, qddot) - + if tau is None: raise ValueError("Failed to compute torques") - + return tau - - def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Union[str | np.ndarray], q : np.ndarray) -> list: + def create_ext_force(self, masses: Union[float, np.ndarray], frame_name: Union[str | np.ndarray], q: np.ndarray) -> list: """ 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. - + :param masses (float, np.ndarray) : Mass of the object to apply the force to or vector with masses related to frames names. :param frame_name(str , np.ndarray) : Frame name where the force is applied or vector of frame names where the forces is applied. :param q: Joint configuration vector. @@ -243,40 +235,36 @@ def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Unio if isinstance(masses, float): if masses < 0: raise ValueError("Mass must be a positive value") - + if frame_name is None: raise ValueError("Frame name must be provided") - - #if frame_name not in self.model.frames: + + # if frame_name not in self.model.frames: # raise ValueError(f"Frame name '{frame_name}' not found in the robot model") # assumption made : the force is applied to the joint - + # Initialize external forces array fext = [pin.Force(np.zeros(6)) for _ in range(self.model.njoints)] - + self.update_configuration(q) # Check if frame_name is a single string or an array of strings if isinstance(frame_name, str): - # Get the frame ID and joint ID from the frame name + # Get the frame ID and joint ID from the frame name frame_id = self.model.getFrameId(frame_name) joint_id = self.model.frames[frame_id].parentJoint # force expressed in the world frame (gravity force in z axis) - ext_force_world = pin.Force(np.array([0.0, 0.0, masses * -9.81]), np.array([0.0, 0.0, 0.0])) + ext_force_world = pin.Force(np.array([0.0, 0.0, masses * -9.81]), np.array([0.0, 0.0, 0.0])) - # placement of the frame in the world frame - #frame_placement = self.data.oMf[frame_id] - #print(f"Frame placement: {frame_placement}") - # Convert the external force expressed in the world frame to the orientation of the joint frame where the force is applied fext[joint_id] = self.data.oMi[joint_id].actInv(ext_force_world) # Zero out the last 3 components (torques) of the force to ensure only the force in z axis (gravity force) is applied - fext[joint_id].angular = np.zeros(3) # TODO : make it more efficient + fext[joint_id].angular = np.zeros(3) # TODO : make it more efficient else: - for mass, frame in zip(masses,frame_name): + for mass, frame in zip(masses, frame_name): frame_id = self.model.getFrameId(frame) joint_id = self.model.frames[frame_id].parentJoint @@ -289,56 +277,49 @@ def create_ext_force(self, masses : Union[float, np.ndarray] , frame_name : Unio return fext - - def update_configuration(self, q : np.ndarray): + def update_configuration(self, q: np.ndarray): """ Update the robot model configuration with the given joint configuration vector. - + :param q: Joint configuration vector. """ pin.forwardKinematics(self.model, self.data, q) pin.updateFramePlacements(self.model, self.data) - - - def compute_inverse_kinematics_optik(self, q : np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: + def compute_inverse_kinematics_optik(self, q: np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: """ Compute the inverse kinematics for the robot model using the Optik library. - + :param q: current joint configuration vector. :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. :return: Joint configuration vector that achieves the desired end effector position. """ # TODO : It doees not work with the current version of the library - + # Compute the inverse kinematics sol = self.ik_model.ik(self.ik_config, end_effector_position, q) - - return sol - + return sol - def compute_inverse_kinematics_ikpy(self, q : np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: + def compute_inverse_kinematics_ikpy(self, q: np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: """ Compute the inverse kinematics for the robot model using the ikpy library. - + :param q: current joint configuration vector. :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. :return: Joint configuration vector that achieves the desired end effector position. """ # TODO : It doees not work with the current version of the library - + # Compute the inverse kinematics sol = self.ik_model.inverse_kinematics(end_effector_position) - - return sol - + return sol - def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.ndarray, joint_id : 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. :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. :param joint_id: Id of the end effector joint. @@ -346,16 +327,16 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n """ # Set parameters for the inverse kinematics solver - eps = 1e-2 # reduce for more precision - IT_MAX = 500 # Maximum number of iterations - DT = 1e-1 + eps = 1e-2 # reduce for more precision + IT_MAX = 500 # Maximum number of iterations + DT = 1e-1 damp = 1e-12 i = 0 while True: self.update_configuration(q) - iMd = self.data.oMi[joint_id].actInv(end_effector_position) # Get the transformation from the current end effector pose to the desired pose - + iMd = self.data.oMi[joint_id].actInv(end_effector_position) # Get the transformation from the current end effector pose to the desired pose + err = pin.log(iMd).vector # compute the error in the end effector position if norm(err[:3]) < eps: success = True @@ -365,30 +346,29 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n break J = pin.computeJointJacobian(self.model, self.data, q, joint_id) # compute the Jacobian of current pose of end effector - #J = -np.dot(pin.Jlog6(iMd.inverse()), J) - J = -J[:3, :] + J = -J[:3, :] # compute the inverse kinematics v = -J^T * (J * J^T + damp * I)^-1 * err v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err[:3])) - + # Apply joint limits by clamping the resulting configuration q_new = pin.integrate(self.model, q, v * DT) # Ensure the new configuration is within joint limits q_new = np.clip(q_new, self.model.lowerPositionLimit, self.model.upperPositionLimit) - + # Check if we're hitting joint limits and reduce step size if needed if np.allclose(q_new, q, atol=1e-6): DT *= 0.5 # Reduce step size if no progress due to joint limits if DT < 1e-6: # Minimum step size threshold success = False break - + q = q_new # if not i % 10: # print(f"{i}: error = {err.T}") i += 1 - + if success: print(f"Convergence achieved! in {i} iterations") return q @@ -398,58 +378,50 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n "Warning: the iterative algorithm has not reached convergence to the desired precision" ) return None # Return None if convergence is not achieved - - - def compute_all_configurations(self, range : int, resolution : int, end_joint_id) -> 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_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. """ - + if range <= 0: raise ValueError("Range must be a positive value") - + # Get the current joint configuration q = self.get_zero_configuration() - #id_end_effector = self.model.getJointId(end_effector_name) - # Get the current position of the end effector - #end_effector_pos = self.data.oMi[id_end_effector] - # 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]}) + 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) - + if new_q is not None: q = new_q # store the valid configuration and the position of the end effector relative to that configuration - configurations.append({"config" : new_q, "end_effector_pos": target_position.translation}) - - return np.array(configurations, dtype=object) - + configurations.append({"config": new_q, "end_effector_pos": target_position.translation}) + return np.array(configurations, dtype=object) - def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, checked_frames : np.ndarray, tree_id: int, selected_joint_id: int) -> 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: 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. @@ -459,23 +431,23 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, """ # TODO : get the tree_id from the sub tree array instead of passing it as parameter valid_configurations = [] - + # check valid configurations for left arm for q in configurations: # 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) + 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()) - # Check if the torques are within the effort limits - if self.check_effort_limits(tau= tau, tree_id= tree_id).all(): + # 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 pin.computeCollisions(self.model, self.data, self.geom_model, self.geom_data, q["config"], False) @@ -489,18 +461,16 @@ def verify_configurations(self, configurations: np.ndarray, masses : np.ndarray, print(f"Collision detected between {cp.first} and {cp.second} in the left arm configuration.") valid = False break - - if valid: - 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}) + if valid: + 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}) return np.array(valid_configurations, dtype=object) - - def get_valid_workspace(self, range : int, resolution : float, 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. - + :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 masses (np.ndarray): Array of masses to apply to the robot model. @@ -511,49 +481,43 @@ def get_valid_workspace(self, range : int, resolution : float, masses : np.ndarr valid_current_configurations = np.array([], dtype=object) # compute all configurations for the selected joints of the trees - for tree,configuration in zip(self.subtrees,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: # Compute all configurations for the current tree - configuration["configurations"] = self.compute_all_configurations(range, resolution,tree["selected_joint_id"]) + 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 - - + # 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 + 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): + 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", "tree_id", "max_payload" } + + :param configs: Array of configurations , format {"config", "end_effector_pos", "tau", "tree_id", "max_payload" } """ for config in configs: config["max_payload"] = self.find_max_payload_binary_search(config, payload_min=0.0, payload_max=15, resolution=0.01) - - return configs + return configs - def find_max_payload_binary_search(self, config : np.ndarray, payload_min : float = 0.0, payload_max : float = 10.0, resolution : float = 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). :param payload_min: Minimum payload to test. :param payload_max: Maximum payload to test. @@ -576,76 +540,72 @@ def find_max_payload_binary_search(self, config : np.ndarray, payload_min : floa return max_valid - def compute_forward_dynamics_aba_method(self, q : np.ndarray, qdot : np.ndarray, tau : np.ndarray, extForce : np.ndarray = 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). - + :param q: Joint configuration vector. :param qdot: Joint velocity vector. :param tau: Joint torque vector. :param extForce: External forces vector applied to the robot model. :return: Acceleration vector. """ - + # calculate dynamics drift if extForce is not None: ddq = pin.aba(self.model, self.data, q, qdot, tau, extForce) else: ddq = pin.aba(self.model, self.data, q, qdot, tau) - + return ddq - - def compute_jacobian(self, q : np.ndarray, frame_name : str) -> np.ndarray: + def compute_jacobian(self, q: np.ndarray, frame_name: str) -> np.ndarray: """ Get the Jacobian matrix for a specific frame in the robot model. - + :param q: Joint configuration vector. :param frame_name: Name of the frame to compute the Jacobian for. :return: Jacobian matrix. """ - + # Get the frame ID frame_id = self.model.getFrameId(frame_name) joint_id = self.model.frames[frame_id].parentJoint - + # Compute the Jacobian - J = pin.computeJointJacobians(self.model, self.data, q) - + pin.computeJointJacobians(self.model, self.data, q) + J_frame = pin.getJointJacobian(self.model, self.data, joint_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) - + if J_frame is None: raise ValueError("Failed to compute Jacobian") - + 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.subtrees[tree_id]["joint_ids"] - 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.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. """ @@ -659,14 +619,13 @@ def get_links_from_tree(self, joint_ids: np.ndarray | int) -> np.ndarray: if link.parentJoint == joint_id and link.type == pin.FrameType.BODY: frames.append(link.name) break - + return np.array(frames, dtype=object) - def get_end_effector_position_array(self, x: float, y: float, z: float) -> np.ndarray: """ Get the end effector position as a array. - + :param x: X position of the end effector. :param y: Y position of the end effector. :param z: Z position of the end effector. @@ -674,34 +633,34 @@ def get_end_effector_position_array(self, x: float, y: float, z: float) -> np.nd """ return pin.SE3(np.eye(3), np.array([x, y, z])) - def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray: + def get_maximum_torques(self, valid_configs: np.ndarray) -> np.ndarray: """ 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 selected trees. """ - + # Get the number of joints num_joints = len(valid_configs[0]["tau"]) - + # 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.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: # 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 ,{"joint" : i ,"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"]]}) abs_joint_torques = np.append(abs_joint_torques, {"tree_id": tree["tree_id"], "abs_torques": abs_torques}) - + # array to store the maximum absolute torques for each joint in the current valid configurations max_torques = np.array([], dtype=float) @@ -714,17 +673,15 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray: 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}) - return max_torques - - def get_maximum_payloads(self, valid_configs : np.ndarray) -> np.ndarray: + def get_maximum_payloads(self, valid_configs: np.ndarray) -> np.ndarray: """ Get the maximum payloads for all configuration in the corrisponding tree. - + :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "tree_id", "max_payload"}]. :return: Tuple of arrays of maximum payloads for left and right arms. """ @@ -736,20 +693,17 @@ def get_maximum_payloads(self, valid_configs : np.ndarray) -> np.ndarray: max_payloads = np.append(max_payloads, {"tree_id": tree["tree_id"], "max_payload": max_payload}) return max_payloads - - - - def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = None, tree_id: int = 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. - + :param tau: Torques vector to normalize. :return: Normalized torques vector. """ if tau is None: raise ValueError("Torques vector is None") - + norm_tau = [] # if target_torque is not specified, normalize the torques vector to the effort limits of the robot model @@ -769,25 +723,23 @@ def get_normalized_torques(self, tau : np.ndarray, target_torque : np.ndarray = return np.array(norm_tau, dtype=float) - - def get_normalized_payload(self, payload : float, target_payload : float) -> float: + def get_normalized_payload(self, payload: float, target_payload: float) -> float: """ 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 - + return norm_payload - def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.ndarray: + def get_unified_configurations_torque(self, valid_configs: np.ndarray) -> np.ndarray: """ Get a unified sum of torques for all possible configurations of the robot model. - - :param valid_configs: Array of valid configurations + + :param valid_configs: Array of valid configurations """ torques_sum = np.array([], dtype=float) @@ -799,185 +751,175 @@ def get_unified_configurations_torque(self, valid_configs : np.ndarray) -> np.nd for valid_config in valid_configs: # get the joint configuration and torques vector from the valid configuration - q = valid_config["config"] + # q = valid_config["config"] tau = valid_config["tau"] - + # calculate the sum of torques for each joint configuration for torque in tau: - #if abs(torque) < 50: + # if abs(torque) < 50: sum += abs(torque) - - 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 + 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 all selected 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"]] - + 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 = 1.0 # Default value if no sums found min_value = 0.0 # Default value if no sums found - - max_min_value_torques = np.append(max_min_value_torques, {"tree_id": tree["tree_id"], "max_value": max_value, "min_value": min_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 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) + norm_tau = (tau["sum"] - min_value) / (max_value - min_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"], "tree_id" : tau["tree_id"]}) + 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 - - def check_zero(self, vec : np.ndarray) -> bool: + def check_zero(self, vec: np.ndarray) -> bool: """ Checks if the vector is zero. - + :param vec: Vector to check. :return: True if the acceleration vector is zero, False otherwise. """ - - return np.allclose(vec, np.zeros(self.model.nv), atol=1e-6) + return np.allclose(vec, np.zeros(self.model.nv), atol=1e-6) def get_zero_configuration(self) -> np.ndarray: """ Get the zero configuration of the robot model. - + :return: Zero configuration vector. """ q0 = np.zeros(self.model.nq) if q0 is None: raise ValueError("Failed to get zero configuration") - + return q0 - def get_zero_velocity(self) -> np.ndarray: """ Get the zero velocity vector of the robot model. - + :return: Zero velocity vector. """ v0 = np.zeros(self.model.nv) if v0 is None: raise ValueError("Failed to get zero velocity") - + return v0 - def get_zero_acceleration(self) -> np.ndarray: """ Get the zero acceleration vector of the robot model. - + :return: Zero acceleration vector. """ a0 = np.zeros(self.model.nv) if a0 is None: raise ValueError("Failed to get zero acceleration") - + 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. - + :return: Random configuration vectors for joint positions and velocities. """ q_limits_lower = self.model.lowerPositionLimit q_limits_upper = self.model.upperPositionLimit - + # generate random configuration vector within the limits q = np.random.uniform(q_limits_lower, q_limits_upper) if q is None: raise ValueError("Failed to get random configuration") - + # Generate random velocity vector within the limits qdot = np.random.uniform(-self.model.velocityLimit, self.model.velocityLimit) if qdot is None: raise ValueError("Failed to get random velocity") - + return q, qdot - - def check_joint_limits(self, q : np.ndarray) -> np.ndarray: + def check_joint_limits(self, q: np.ndarray) -> np.ndarray: """ Check if the joint configuration vector is within the joint limits of the robot model. - + :param q: Joint configuration vector to check. :return: Array of booleans indicating if each joint is within the limits. """ if q is None: raise ValueError("Joint configuration vector is None") - + # array to store if the joint is within the limits within_limits = np.zeros(self.model.njoints - 1, dtype=bool) # Check if the joint configuration is within the limits - for i in range(self.model.njoints - 1): + for i in range(self.model.njoints - 1): if q[i] < self.model.lowerPositionLimit[i] or q[i] > self.model.upperPositionLimit[i]: within_limits[i] = False else: within_limits[i] = True - - return within_limits + return within_limits - def check_effort_limits(self, tau : np.ndarray, tree_id : int = 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 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: raise ValueError("Torques vector is None") - + # array to store if the joint is within the limits within_limits = np.array([], dtype=bool) # if arm is not specified, check all joints if tree_id is None: # Check if the torques are within the limits - for i in range(self.model.njoints -1): + for i in range(self.model.njoints - 1): if abs(tau[i]) > self.model.effortLimit[i]: print(f"\033[91mJoint {i+2} 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") - + else: # Check if the torque of joints inside the tree is within the limits for id in self.subtrees[tree_id]["joint_ids"]: - if abs(tau[id-1]) > self.model.effortLimit[id-1]: + 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: @@ -990,18 +932,16 @@ def check_effort_limits(self, tau : np.ndarray, tree_id : int = None) -> np.ndar # 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") - return within_limits - - def set_joint_tree_selection(self, tree_id : int, joint_id: int): + 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. """ @@ -1011,26 +951,24 @@ def set_joint_tree_selection(self, tree_id : int, joint_id: int): tree["selected_joint_id"] = joint_id return - - - def set_position(self, pos_joints : list[float], name_positions : list[str] ) -> np.ndarray: + def set_position(self, pos_joints: list[float], name_positions: list[str]) -> np.ndarray: """ Convert joint positions provided by jointstate publisher to the right format of the robot model. - + :param pos_joints: List of joint positions provided by jointstate publisher. :param name_positions: List of joint names in the order provided by jointstate publisher. :return: Joint configuration vector in pinocchio format. """ - + q = np.zeros(self.model.nq) cont = 0 for i in range(1, np.size(pos_joints) + 1): - # find index in name positions list that corrisponds to the joint name + # find index in name positions list that corrisponds to the joint name # (REASON: the joint position from joint_state message are not in the same order as the joint indices in model object) - index = name_positions.index(self.model.names[i]) - + index = name_positions.index(self.model.names[i]) + if self.model.joints[i].nq == 2: # for continuous joints (wheels) q[cont] = math.cos(pos_joints[index]) @@ -1042,47 +980,45 @@ def set_position(self, pos_joints : list[float], name_positions : list[str] ) -> cont += self.model.joints[i].nq self.update_configuration(q) - + return q - - def get_position_for_joint_states(self, q : np.ndarray) -> np.ndarray: + def get_position_for_joint_states(self, q: np.ndarray) -> np.ndarray: """ Convert configuration in pinocchio format to right format of joint states publisher - Example: continuous joints (wheels) are represented as two values in the configuration vector but + Example: continuous joints (wheels) are represented as two values in the configuration vector but in the joint state publisher they are represented as one value (angle). :param q: Joint configuration provided by pinocchio library - + :return: Joint positions in the format of joint state publisher. """ - + config = [] - + current_selected_config = q - # Use this method to get the single values for joints, for example continuous joints (wheels) are represented as two values in the configuration vector but + # Use this method to get the single values for joints, for example continuous joints (wheels) are represented as two values in the configuration vector but # in the joint state publisher they are represented as one value (angle) # so we need to convert the configuration vector to the right format for joint state publisher cont = 0 for i in range(1, self.model.njoints): if self.model.joints[i].nq == 2: # for continuous joints (wheels) - config.append({ "q" : math.atan2(current_selected_config[cont+1], current_selected_config[cont]), "joint_name" : self.model.names[i] }) - + config.append({"q": math.atan2(current_selected_config[cont + 1], current_selected_config[cont]), "joint_name": self.model.names[i]}) + elif self.model.joints[i].nq == 1: # for revolute joints - config.append({"q" : current_selected_config[cont], "joint_name" : self.model.names[i]}) - + config.append({"q": current_selected_config[cont], "joint_name": self.model.names[i]}) + cont += self.model.joints[i].nq - + return np.array(config, dtype=object) - - def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: + def get_joints_placements(self, q: np.ndarray) -> np.ndarray | float: """ Get the placements of the joints in the robot model. - + :param q: Joint configuration vector. :return: Array of joint placements with names of joint, and z offset of base link. """ @@ -1090,92 +1026,85 @@ def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: offset_z = self.data.oMf[base_link_id].translation[2] # Get the z offset of the base link self.update_configuration(q) - placements = np.array([({"name" : self.model.names[i], - "type" : self.model.joints[i].shortname() , - "id" : i, - "x": self.data.oMi[i].translation[0], - "y": self.data.oMi[i].translation[1], + placements = np.array([({"name": self.model.names[i], + "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) - + return placements, offset_z - def get_joint_name(self, joint_id: int) -> str: """ Get the name of the joint by its ID. - + :param joint_id: ID of the joint to get the name for. :return: Name of the joint. """ return self.model.names[joint_id] - def get_joint_placement(self, joint_id : int, q : np.ndarray) -> 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.") - + placement = self.data.oMi[joint_id].translation - - return {"x" : placement[0], "y": placement[1], "z": placement[2]} - - def get_mass_matrix(self, q : np.ndarray) -> np.ndarray: + return {"x": placement[0], "y": placement[1], "z": placement[2]} + + def get_mass_matrix(self, q: np.ndarray) -> np.ndarray: """ Compute the mass matrix for the robot model. - + :param q: Joint configuration vector. :return: Mass matrix. """ - + mass_matrix = pin.crba(self.model, self.data, q) if mass_matrix is None: raise ValueError("Failed to compute mass matrix") - + return mass_matrix - def get_joints(self) -> np.ndarray: """ Get the array joint names of the robot model. - + :return: array Joint names . """ - - return np.array(self.model.names[1:], dtype=str) - + return np.array(self.model.names[1:], dtype=str) def get_frames(self) -> np.ndarray: """ Get the array of frame names in the robot model. - + :return: array of frame names. """ - - return np.array([frame.name for frame in self.model.frames if frame.type == pin.FrameType.BODY], dtype=str) - + return np.array([frame.name for frame in self.model.frames if frame.type == pin.FrameType.BODY], dtype=str) - def get_links(self,all_frames : bool = False) -> np.ndarray: + def get_links(self, all_frames: bool = False) -> np.ndarray: """ 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 tree in self.subtrees: @@ -1191,42 +1120,40 @@ def get_links(self,all_frames : bool = False) -> np.ndarray: frame_names.append(link_name[0]) 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: + def get_parent_joint_id(self, frame_name: str) -> int: """ Get the parent joint ID for a given frame name. - + :param frame_name: Name of the frame. :return: Joint ID. """ - + if frame_name is None: raise ValueError("Frame name must be provided") - + # Get the frame ID from the model frame_id = self.model.getFrameId(frame_name) joint_id = self.model.frames[frame_id].parentJoint - + if joint_id == -1: raise ValueError(f"Joint '{joint_id}' not found in the robot model") - + return joint_id - - def print_configuration(self, q : np.ndarray = None): + def print_configuration(self, q: np.ndarray = None): """ Print the current configuration of the robot model. """ @@ -1239,11 +1166,10 @@ def print_configuration(self, q : np.ndarray = None): print(f" Rotation:\n{placement.rotation}") print(f" Translation:\n{placement.translation}") - - def print_torques(self, tau : np.ndarray): + def print_torques(self, tau: np.ndarray): """ Print the torques vector. - + :param tau: Torques vector to print. """ if tau is None: @@ -1252,16 +1178,14 @@ def print_torques(self, tau : np.ndarray): print("Torques vector:") for i, torque in enumerate(tau): # check if the joint is a prismatic joint - if self.model.joints[i+1].shortname() == "JointModelPZ": + if self.model.joints[i + 1].shortname() == "JointModelPZ": print(f"Joint {i+2} {self.model.names[i+1]}: {torque:.4f} N") - + # for revolute joints else: print(f"Joint {i+2} {self.model.names[i+1]}: {torque:.4f} Nm") print("\n") - - def print_frames(self): """ Print the frames of the robot model. @@ -1272,20 +1196,18 @@ def print_frames(self): print(f"ID: {i:2d} | Name: {frame.name:20s}") print("\n") - - def print_acceleration(self, qddot : np.ndarray): + def print_acceleration(self, qddot: np.ndarray): """ Print the acceleration vector. - + :param a: Acceleration vector to print. """ - + print("Acceleration vector:") for i, acc in enumerate(qddot): print(f"Joint {i+2} {self.model.names[i+1]}: {acc:.6f} rad/s^2") - - print("\n") + print("\n") def print_active_joint(self): """ @@ -1293,7 +1215,7 @@ def print_active_joint(self): """ if self.model is None: raise ValueError("Model is not initialized") - + print("Active joints:") for i in range(self.model.njoints): print(f"Joint {i+1}: {self.model.names[i]}") @@ -1301,4 +1223,4 @@ def print_active_joint(self): print("\n") # placement of each joint in the kinematic tree # for name, oMi in zip(self.model.names, self.data.oMi): - # print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) \ No newline at end of file + # print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) From 26debeeff8a7e6b4e636568d5ec7f17d41ba7d20 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 28 Aug 2025 14:58:57 +0200 Subject: [PATCH 13/15] fix format --- dynamic_payload_analysis_core/setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dynamic_payload_analysis_core/setup.py b/dynamic_payload_analysis_core/setup.py index 86b17bb..e4ddfc9 100644 --- a/dynamic_payload_analysis_core/setup.py +++ b/dynamic_payload_analysis_core/setup.py @@ -14,7 +14,8 @@ 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.', + description='This package implements functionalities for dynamic payload analysis,' + 'focusing on torque calculations and external force handling.', license='Apache License 2.0', tests_require=['pytest'], install_requires=[ From a2bb38c610b88fbeeaa5321799162b55a0c33b01 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Thu, 28 Aug 2025 14:59:59 +0200 Subject: [PATCH 14/15] fix lines length to match good format --- .../dynamic_payload_analysis_core/core.py | 572 +++++++++++++----- talos_robot | 1 + tiago_pro_robot | 1 + 3 files changed, 415 insertions(+), 159 deletions(-) create mode 160000 talos_robot create mode 160000 tiago_pro_robot 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 d2744ef..260213c 100755 --- a/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py +++ b/dynamic_payload_analysis_core/dynamic_payload_analysis_core/core.py @@ -27,9 +27,11 @@ class TorqueCalculator: def __init__(self, robot_description: str): """ - Initialize the Torques_calculator with the URDF model or XML format provided by robot_description topic. + Initialize the Torques_calculator with the URDF model + or XML format provided by robot_description topic. - :param robot_description: Robot description in XML format provided by /robot_description topic. + :param robot_description: Robot description in XML + format provided by /robot_description topic. """ # Load the robot model from path or XML string @@ -47,7 +49,8 @@ def __init__(self, robot_description: str): temp_file.write(robot_description) temp_urdf_path = temp_file.name - self.geom_model = pin.buildGeomFromUrdf(self.model, temp_urdf_path, pin.GeometryType.COLLISION) + self.geom_model = pin.buildGeomFromUrdf( + self.model, temp_urdf_path, pin.GeometryType.COLLISION) # Add collisition pairs self.geom_model.addAllCollisionPairs() @@ -55,13 +58,17 @@ def __init__(self, robot_description: str): os.unlink(temp_urdf_path) else: - raise ValueError("robot_description must be a string containing the URDF XML or the path to the URDF file") + raise ValueError( + "robot_description must be a string containing " + "the URDF XML or the path to the URDF file" + ) # create data for the robot model self.data = self.model.createData() self.geom_data = pin.GeometryData(self.geom_model) - # get the default collisions in the robot model to avoid take them into account in the computations + # 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 @@ -75,13 +82,16 @@ def __init__(self, robot_description: str): # create items for each tree in the robot model for tree in self.subtrees: - self.configurations = np.append(self.configurations, {"tree_id": tree["tree_id"], "configurations": None, "selected_joint_id": None}) + 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. + :param robot_description: Robot description + in XML format provided by /robot_description topic. """ try: robot = URDF.from_xml_string(robot_description) @@ -116,8 +126,10 @@ def compute_mimic_joints(self, urdf_xml): 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] + 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): """ @@ -129,14 +141,21 @@ def compute_static_collisions(self): collision_pairs = [] # Compute all the collisions - pin.computeCollisions(self.model, self.data, self.geom_model, self.geom_data, self.get_zero_configuration(), False) + pin.computeCollisions( + self.model, + self.data, + self.geom_model, + self.geom_data, + self.get_zero_configuration(), + 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(): - print(f"Collision between {cp.first} and {cp.second} detected.") + print( + f"Collision between {cp.first} and {cp.second} detected.") collision_pairs.append((cp.first, cp.second, k)) return collision_pairs @@ -157,7 +176,8 @@ def compute_subtrees(self): for i, jointID in enumerate(tip_joints): joint_tree_ids = self.get_filtered_subtree(jointID) - # insert the sub-tree only if the tip joint is not already in the sub-trees + # 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]: @@ -168,9 +188,17 @@ def compute_subtrees(self): # 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, "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}) + 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_link_name": link_names[-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: @@ -188,10 +216,12 @@ def get_filtered_subtree(self, current_tip_id: int) -> np.ndarray: # 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 + # 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 + # 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 @@ -199,7 +229,12 @@ def get_filtered_subtree(self, current_tip_id: int) -> np.ndarray: return filtered_subtree - def compute_inverse_dynamics(self, q: np.ndarray, qdot: np.ndarray, qddot: np.ndarray, extForce: np.ndarray = 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. @@ -210,7 +245,8 @@ def compute_inverse_dynamics(self, q: np.ndarray, qdot: np.ndarray, qddot: np.nd :return: Torques vector """ - # basic equation for inverse dynamics : M(q) *a + b = tau + J(q)_t * extForce --> tau = M(q) * a + b - J(q)_t * extForce + # basic equation for inverse dynamics : M(q) *a + b = tau + J(q)_t * + # extForce --> tau = M(q) * a + b - J(q)_t * extForce if extForce: tau = pin.rnea(self.model, self.data, q, qdot, qddot, extForce) @@ -222,13 +258,20 @@ def compute_inverse_dynamics(self, q: np.ndarray, qdot: np.ndarray, qddot: np.nd return tau - def create_ext_force(self, masses: Union[float, np.ndarray], frame_name: Union[str | np.ndarray], q: np.ndarray) -> list: + def create_ext_force(self, + masses: Union[float, + np.ndarray], + frame_name: Union[str | np.ndarray], + q: np.ndarray) -> list: """ 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. + The resulting vector will contain the force applied to the + specified frame and with the local orientation of the parent joint. - :param masses (float, np.ndarray) : Mass of the object to apply the force to or vector with masses related to frames names. - :param frame_name(str , np.ndarray) : Frame name where the force is applied or vector of frame names where the forces is applied. + :param masses (float, np.ndarray) : Mass of the object to apply + the force to or vector with masses related to frames names. + :param frame_name(str , np.ndarray) : Frame name where the force + is applied or vector of frame names where the forces is applied. :param q: Joint configuration vector. :return: External force vector. """ @@ -256,12 +299,16 @@ def create_ext_force(self, masses: Union[float, np.ndarray], frame_name: Union[s joint_id = self.model.frames[frame_id].parentJoint # force expressed in the world frame (gravity force in z axis) - ext_force_world = pin.Force(np.array([0.0, 0.0, masses * -9.81]), np.array([0.0, 0.0, 0.0])) + ext_force_world = pin.Force( + np.array([0.0, 0.0, masses * -9.81]), np.array([0.0, 0.0, 0.0])) - # Convert the external force expressed in the world frame to the orientation of the joint frame where the force is applied + # Convert the external force expressed in the world frame to the + # orientation of the joint frame where the force is applied fext[joint_id] = self.data.oMi[joint_id].actInv(ext_force_world) - # Zero out the last 3 components (torques) of the force to ensure only the force in z axis (gravity force) is applied - fext[joint_id].angular = np.zeros(3) # TODO : make it more efficient + # Zero out the last 3 components (torques) of the force to ensure + # only the force in z axis (gravity force) is applied + # TODO : make it more efficient + fext[joint_id].angular = np.zeros(3) else: for mass, frame in zip(masses, frame_name): @@ -269,30 +316,40 @@ def create_ext_force(self, masses: Union[float, np.ndarray], frame_name: Union[s joint_id = self.model.frames[frame_id].parentJoint # force expressed in the world frame (gravity force in z axis) - ext_force_world = pin.Force(np.array([0.0, 0.0, mass * -9.81]), np.array([0.0, 0.0, 0.0])) - # Convert the external force expressed in the world frame to the orientation of the joint frame where the force is applied - fext[joint_id] = self.data.oMi[joint_id].actInv(ext_force_world) - # Zero out the last 3 components (torques) of the force to ensure only the force in z axis (gravity force) is applied + ext_force_world = pin.Force( + np.array([0.0, 0.0, mass * -9.81]), np.array([0.0, 0.0, 0.0])) + # Convert the external force expressed in the world frame to + # the orientation of the joint frame where the force is applied + fext[joint_id] = self.data.oMi[joint_id].actInv( + ext_force_world) + # Zero out the last 3 components (torques) of the force to + # ensure only the force in z axis (gravity force) is applied fext[joint_id].angular = np.zeros(3) return fext def update_configuration(self, q: np.ndarray): """ - Update the robot model configuration with the given joint configuration vector. + Update the robot model configuration with + the given joint configuration vector. :param q: Joint configuration vector. """ pin.forwardKinematics(self.model, self.data, q) pin.updateFramePlacements(self.model, self.data) - def compute_inverse_kinematics_optik(self, q: np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: + def compute_inverse_kinematics_optik( + self, + q: np.ndarray, + end_effector_position: np.ndarray) -> np.ndarray: """ Compute the inverse kinematics for the robot model using the Optik library. :param q: current joint configuration vector. - :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. - :return: Joint configuration vector that achieves the desired end effector position. + :param end_effector_position: Position of the end effector + in the world frame [rotation matrix , translation vector]. + :return: Joint configuration vector that achieves the desired + end effector position. """ # TODO : It doees not work with the current version of the library @@ -301,13 +358,18 @@ def compute_inverse_kinematics_optik(self, q: np.ndarray, end_effector_position: return sol - def compute_inverse_kinematics_ikpy(self, q: np.ndarray, end_effector_position: np.ndarray) -> np.ndarray: + def compute_inverse_kinematics_ikpy( + self, + q: np.ndarray, + end_effector_position: np.ndarray) -> np.ndarray: """ Compute the inverse kinematics for the robot model using the ikpy library. :param q: current joint configuration vector. - :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. - :return: Joint configuration vector that achieves the desired end effector position. + :param end_effector_position: Position of the end effector + in the world frame [rotation matrix , translation vector]. + :return: Joint configuration vector that achieves the + desired end effector position. """ # TODO : It doees not work with the current version of the library @@ -316,14 +378,21 @@ def compute_inverse_kinematics_ikpy(self, q: np.ndarray, end_effector_position: return sol - def compute_inverse_kinematics(self, q: np.ndarray, end_effector_position: np.ndarray, joint_id: 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. + Compute the inverse kinematics for the robot model with + joint limits consideration. :param q: Current joint configuration vector. - :param end_effector_position: Position of the end effector in the world frame [rotation matrix , translation vector]. + :param end_effector_position: Position of the end effector + in the world frame [rotation matrix , translation vector]. :param joint_id: Id of the end effector joint. - :return: Joint configuration vector that achieves the desired end effector position. + :return: Joint configuration vector that achieves + the desired end effector position. """ # Set parameters for the inverse kinematics solver @@ -335,9 +404,12 @@ def compute_inverse_kinematics(self, q: np.ndarray, end_effector_position: np.nd i = 0 while True: self.update_configuration(q) - iMd = self.data.oMi[joint_id].actInv(end_effector_position) # Get the transformation from the current end effector pose to the desired pose + # Get the transformation from the current end effector pose to the + # desired pose + iMd = self.data.oMi[joint_id].actInv(end_effector_position) - err = pin.log(iMd).vector # compute the error in the end effector position + # compute the error in the end effector position + err = pin.log(iMd).vector if norm(err[:3]) < eps: success = True break @@ -345,18 +417,23 @@ def compute_inverse_kinematics(self, q: np.ndarray, end_effector_position: np.nd success = False break - J = pin.computeJointJacobian(self.model, self.data, q, joint_id) # compute the Jacobian of current pose of end effector + # compute the Jacobian of current pose of end effector + J = pin.computeJointJacobian(self.model, self.data, q, joint_id) J = -J[:3, :] - # compute the inverse kinematics v = -J^T * (J * J^T + damp * I)^-1 * err + # compute the inverse kinematics v = -J^T * (J * J^T + damp * I)^-1 + # * err v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err[:3])) # Apply joint limits by clamping the resulting configuration q_new = pin.integrate(self.model, q, v * DT) # Ensure the new configuration is within joint limits - q_new = np.clip(q_new, self.model.lowerPositionLimit, self.model.upperPositionLimit) + q_new = np.clip(q_new, + self.model.lowerPositionLimit, + self.model.upperPositionLimit) - # Check if we're hitting joint limits and reduce step size if needed + # Check if we're hitting joint limits and reduce step size if + # needed if np.allclose(q_new, q, atol=1e-6): DT *= 0.5 # Reduce step size if no progress due to joint limits if DT < 1e-6: # Minimum step size threshold @@ -375,18 +452,27 @@ def compute_inverse_kinematics(self, q: np.ndarray, end_effector_position: np.nd else: print( "\n" - "Warning: the iterative algorithm has not reached convergence to the desired precision" + "Warning: the iterative algorithm has not reached " + "convergence to the desired precision" ) return None # Return None if convergence is not achieved - def compute_all_configurations(self, range: int, resolution: int, end_joint_id) -> 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_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. + :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_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. """ if range <= 0: @@ -406,30 +492,44 @@ def compute_all_configurations(self, range: int, resolution: int, end_joint_id) 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]}) + 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) + new_q = self.compute_inverse_kinematics( + q, target_position, end_joint_id) if new_q is not None: q = new_q - # store the valid configuration and the position of the end effector relative to that configuration - configurations.append({"config": new_q, "end_effector_pos": target_position.translation}) + # store the valid configuration and the position of the + # end effector relative to that configuration + configurations.append( + {"config": new_q, "end_effector_pos": target_position.translation}) return np.array(configurations, dtype=object) - def verify_configurations(self, configurations: np.ndarray, masses: np.ndarray, checked_frames: np.ndarray, tree_id: int, selected_joint_id: int) -> 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: 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 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", "tree_id","selected_joint_id" }]. + :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", "tree_id","selected_joint_id" }]. """ - # TODO : get the tree_id from the sub tree array instead of passing it as parameter + # TODO : get the tree_id from the sub tree array instead of passing it + # as parameter valid_configurations = [] # check valid configurations for left arm @@ -439,82 +539,139 @@ def verify_configurations(self, configurations: np.ndarray, masses: np.ndarray, 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"]) + 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) + 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()) + # 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 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) + 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 left arm configuration.") + if cr.isCollision() and ( + (cp.first, cp.second, k) not in self.default_collisions + ): + print( + f"Collision detected between {cp.first}" + f"and {cp.second} in the left arm configuration.") valid = False break if valid: - 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}) + 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}) return np.array(valid_configurations, dtype=object) - def get_valid_workspace(self, range: int, resolution: float, 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. + Get the valid workspace of the robot model by computing all + configurations 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 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 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", "tree_id"}]. + :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", "tree_id"}]. """ # 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.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 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: # 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["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 - - if configuration["configurations"] is not None and tree["selected_joint_id"] is not None: + # 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 + + 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) + 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): """ - Compute the maximum payload for each provided configuration and return the results with the configs updated with the maximum payload as a new value. + 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", "tree_id", "max_payload" } + :param configs: Array of configurations, + format {"config", "end_effector_pos", "tau", "tree_id", "max_payload" } """ for config in configs: - config["max_payload"] = self.find_max_payload_binary_search(config, payload_min=0.0, payload_max=15, 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: np.ndarray, payload_min: float = 0.0, payload_max: float = 10.0, resolution: float = 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. @@ -530,8 +687,14 @@ def find_max_payload_binary_search(self, config: np.ndarray, payload_min: float while high - low > resolution: mid_payload = (low + high) / 2 - 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) + 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(): max_valid = mid_payload low = mid_payload @@ -540,7 +703,12 @@ def find_max_payload_binary_search(self, config: np.ndarray, payload_min: float return max_valid - def compute_forward_dynamics_aba_method(self, q: np.ndarray, qdot: np.ndarray, tau: np.ndarray, extForce: np.ndarray = 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). @@ -575,7 +743,11 @@ def compute_jacobian(self, q: np.ndarray, frame_name: str) -> np.ndarray: # Compute the Jacobian pin.computeJointJacobians(self.model, self.data, q) - J_frame = pin.getJointJacobian(self.model, self.data, joint_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + J_frame = pin.getJointJacobian( + self.model, + self.data, + joint_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) if J_frame is None: raise ValueError("Failed to compute Jacobian") @@ -591,7 +763,8 @@ def verify_member_tree(self, tree_id: int, joint_id: int) -> bool: :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 + # Check if the joint ID is in the list of joint IDs for the specified + # tree return joint_id in self.subtrees[tree_id]["joint_ids"] def get_subtrees(self) -> np.ndarray: @@ -622,7 +795,8 @@ def get_links_from_tree(self, joint_ids: np.ndarray | int) -> np.ndarray: return np.array(frames, dtype=object) - def get_end_effector_position_array(self, x: float, y: float, z: float) -> np.ndarray: + def get_end_effector_position_array( + self, x: float, y: float, z: float) -> np.ndarray: """ Get the end effector position as a array. @@ -637,34 +811,54 @@ def get_maximum_torques(self, valid_configs: np.ndarray) -> np.ndarray: """ 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 selected trees. + :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. """ # Get the number of joints num_joints = len(valid_configs[0]["tau"]) - # array to store the absolute torques for each joint in the current valid configurations for each selected tree + # 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.subtrees 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 + # 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 + # 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, {"joint": i, "abs": [abs(config["tau"][i]) for config in valid_configs if config["tree_id"] == tree["tree_id"]]}) - - abs_joint_torques = np.append(abs_joint_torques, {"tree_id": tree["tree_id"], "abs_torques": abs_torques}) - - # array to store the maximum absolute torques for each joint in the current valid configurations + 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"] + ], + }, + ) + + abs_joint_torques = np.append( + abs_joint_torques, { + "tree_id": tree["tree_id"], "abs_torques": abs_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 + # 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 @@ -674,7 +868,9 @@ def get_maximum_torques(self, valid_configs: np.ndarray) -> np.ndarray: else: max_tau = np.append(max_tau, 0.0) - max_torques = np.append(max_torques, {"tree_id": torques["tree_id"], "max_values": max_tau}) + max_torques = np.append( + max_torques, { + "tree_id": torques["tree_id"], "max_values": max_tau}) return max_torques @@ -682,19 +878,28 @@ def get_maximum_payloads(self, valid_configs: np.ndarray) -> np.ndarray: """ Get the maximum payloads for all configuration in the corrisponding tree. - :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "tree_id", "max_payload"}]. + :param valid_configs: Array of valid configurations + with related torques in format: + [{"config", "end_effector_pos, "tau", "tree_id", "max_payload"}]. :return: Tuple of arrays of maximum payloads for left and right arms. """ max_payloads = np.array([], dtype=float) for tree in self.subtrees: - payloads = [config["max_payload"] for config in valid_configs if config["tree_id"] == tree["tree_id"]] + 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}) + max_payloads = np.append( + max_payloads, { + "tree_id": tree["tree_id"], "max_payload": max_payload}) return max_payloads - def get_normalized_torques(self, tau: np.ndarray, target_torque: np.ndarray = None, tree_id: int = 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. @@ -706,14 +911,16 @@ def get_normalized_torques(self, tau: np.ndarray, target_torque: np.ndarray = No norm_tau = [] - # if target_torque is not specified, normalize the torques vector to the effort limits of the robot model + # if target_torque is not specified, normalize the torques vector to + # the effort limits of the robot model if target_torque is None: # Normalize the torques vector 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) + 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 max_torque["max_values"][i] != 0: @@ -723,7 +930,10 @@ def get_normalized_torques(self, tau: np.ndarray, target_torque: np.ndarray = No return np.array(norm_tau, dtype=float) - def get_normalized_payload(self, payload: float, target_payload: float) -> float: + def get_normalized_payload( + self, + payload: float, + target_payload: float) -> float: """ Normalize the torques vector to a unified scale. @@ -735,7 +945,8 @@ def get_normalized_payload(self, payload: float, target_payload: float) -> float return norm_payload - def get_unified_configurations_torque(self, valid_configs: np.ndarray) -> np.ndarray: + def get_unified_configurations_torque( + self, valid_configs: np.ndarray) -> np.ndarray: """ Get a unified sum of torques for all possible configurations of the robot model. @@ -759,13 +970,17 @@ def get_unified_configurations_torque(self, valid_configs: np.ndarray) -> np.nda # if abs(torque) < 50: sum += abs(torque) - torques_sum = np.append(torques_sum, {"sum": sum, "end_effector_pos": valid_config["end_effector_pos"], "tree_id": valid_config["tree_id"]}) + 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 all selected 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"]] + 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) @@ -774,19 +989,29 @@ def get_unified_configurations_torque(self, valid_configs: np.ndarray) -> np.nda max_value = 1.0 # Default value if no sums found min_value = 0.0 # Default value if no sums found - max_min_value_torques = np.append(max_min_value_torques, {"tree_id": tree["tree_id"], "max_value": max_value, "min_value": min_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 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"]) + # 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 - norm_torques = np.append(norm_torques, {"norm_tau": norm_tau, "end_effector_pos": tau["end_effector_pos"], "tree_id": tau["tree_id"]}) + 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 @@ -861,7 +1086,8 @@ def get_random_configuration(self) -> tuple[np.ndarray, np.ndarray]: raise ValueError("Failed to get random configuration") # Generate random velocity vector within the limits - qdot = np.random.uniform(-self.model.velocityLimit, self.model.velocityLimit) + qdot = np.random.uniform(-self.model.velocityLimit, + self.model.velocityLimit) if qdot is None: raise ValueError("Failed to get random velocity") @@ -889,7 +1115,10 @@ def check_joint_limits(self, q: np.ndarray) -> np.ndarray: return within_limits - def check_effort_limits(self, tau: np.ndarray, tree_id: int = 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. @@ -908,7 +1137,9 @@ def check_effort_limits(self, tau: np.ndarray, tree_id: int = None) -> np.ndarra # Check if the torques are within the limits for i in range(self.model.njoints - 1): if abs(tau[i]) > self.model.effortLimit[i]: - print(f"\033[91mJoint {i+2} exceeds effort limit: {tau[i]} > {self.model.effortLimit[i]} \033[0m\n") + print( + f"\033[91mJoint {i+2} exceeds effort limit: " + f"{tau[i]} > {self.model.effortLimit[i]} \033[0m\n") within_limits = np.append(within_limits, False) else: within_limits = np.append(within_limits, True) @@ -917,22 +1148,17 @@ def check_effort_limits(self, tau: np.ndarray, tree_id: int = None) -> np.ndarra print("All joints are within effort limits. \n") else: - # Check if the torque of joints inside the tree is within the limits + # Check if the torque of joints inside the tree is within the + # limits 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") + print( + f"\033[91mJoint {id} exceeds effort limit: " + f"{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) - # 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") @@ -951,9 +1177,13 @@ def set_joint_tree_selection(self, tree_id: int, joint_id: int): tree["selected_joint_id"] = joint_id return - def set_position(self, pos_joints: list[float], name_positions: list[str]) -> np.ndarray: + def set_position( + self, + pos_joints: list[float], + name_positions: list[str]) -> np.ndarray: """ - Convert joint positions provided by jointstate publisher to the right format of the robot model. + Convert joint positions provided by jointstate publisher + to the right format of the robot model. :param pos_joints: List of joint positions provided by jointstate publisher. :param name_positions: List of joint names in the order provided by jointstate publisher. @@ -966,7 +1196,8 @@ def set_position(self, pos_joints: list[float], name_positions: list[str]) -> np for i in range(1, np.size(pos_joints) + 1): # find index in name positions list that corrisponds to the joint name - # (REASON: the joint position from joint_state message are not in the same order as the joint indices in model object) + # (REASON: the joint position from joint_state message are + # not in the same order as the joint indices in model object) index = name_positions.index(self.model.names[i]) if self.model.joints[i].nq == 2: @@ -986,7 +1217,8 @@ def set_position(self, pos_joints: list[float], name_positions: list[str]) -> np def get_position_for_joint_states(self, q: np.ndarray) -> np.ndarray: """ Convert configuration in pinocchio format to right format of joint states publisher - Example: continuous joints (wheels) are represented as two values in the configuration vector but + Example: continuous joints (wheels) are represented as + two values in the configuration vector but in the joint state publisher they are represented as one value (angle). :param q: Joint configuration provided by pinocchio library @@ -998,18 +1230,24 @@ def get_position_for_joint_states(self, q: np.ndarray) -> np.ndarray: current_selected_config = q - # Use this method to get the single values for joints, for example continuous joints (wheels) are represented as two values in the configuration vector but + # Use this method to get the single values for joints, for example + # continuous joints (wheels) are represented as two values in the configuration vector but # in the joint state publisher they are represented as one value (angle) - # so we need to convert the configuration vector to the right format for joint state publisher + # so we need to convert the configuration vector to the right format + # for joint state publisher cont = 0 for i in range(1, self.model.njoints): if self.model.joints[i].nq == 2: # for continuous joints (wheels) - config.append({"q": math.atan2(current_selected_config[cont + 1], current_selected_config[cont]), "joint_name": self.model.names[i]}) + config.append({"q": math.atan2( + current_selected_config[cont + 1], + current_selected_config[cont]), + "joint_name": self.model.names[i]}) elif self.model.joints[i].nq == 1: # for revolute joints - config.append({"q": current_selected_config[cont], "joint_name": self.model.names[i]}) + config.append( + {"q": current_selected_config[cont], "joint_name": self.model.names[i]}) cont += self.model.joints[i].nq @@ -1023,15 +1261,24 @@ def get_joints_placements(self, q: np.ndarray) -> np.ndarray | float: :return: Array of joint placements with names of joint, and z offset of 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 + # Get the z offset of the base link + offset_z = self.data.oMf[base_link_id].translation[2] self.update_configuration(q) - placements = np.array([({"name": self.model.names[i], - "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) + placements = np.array( + [ + { + "name": self.model.names[i], + "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, + ) return placements, offset_z @@ -1057,7 +1304,9 @@ def get_joint_placement(self, joint_id: int, q: np.ndarray) -> dict: 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.") + raise ValueError( + f"Joint ID {joint_id} is out of bounds" + f"for the robot model with {self.model.njoints} joints.") placement = self.data.oMi[joint_id].translation @@ -1093,7 +1342,9 @@ def get_frames(self) -> np.ndarray: :return: array of frame names. """ - return np.array([frame.name for frame in self.model.frames if frame.type == pin.FrameType.BODY], dtype=str) + return np.array( + [frame.name for frame in self.model.frames + if frame.type == pin.FrameType.BODY], dtype=str) def get_links(self, all_frames: bool = False) -> np.ndarray: """ @@ -1113,10 +1364,12 @@ def get_links(self, all_frames: bool = False) -> np.ndarray: 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 + # if all_frames is False, get only the links connected to the + # selected joint IDs from the subtrees for tree in self.subtrees: if tree["selected_joint_id"] is not None: - link_name = self.get_links_from_tree(tree["selected_joint_id"]) + link_name = self.get_links_from_tree( + tree["selected_joint_id"]) frame_names.append(link_name[0]) return np.array(frame_names, dtype=str) @@ -1149,7 +1402,8 @@ def get_parent_joint_id(self, frame_name: str) -> int: joint_id = self.model.frames[frame_id].parentJoint if joint_id == -1: - raise ValueError(f"Joint '{joint_id}' not found in the robot model") + raise ValueError( + f"Joint '{joint_id}' not found in the robot model") return joint_id diff --git a/talos_robot b/talos_robot new file mode 160000 index 0000000..6152887 --- /dev/null +++ b/talos_robot @@ -0,0 +1 @@ +Subproject commit 6152887e3f258cca58231391ae0f380baede43ff diff --git a/tiago_pro_robot b/tiago_pro_robot new file mode 160000 index 0000000..c62d9f8 --- /dev/null +++ b/tiago_pro_robot @@ -0,0 +1 @@ +Subproject commit c62d9f885a5a48a145228c13acb16e027691993d From c69fc23ed8f9e956fc51029a20c6ba8d446844a2 Mon Sep 17 00:00:00 2001 From: Enrico Moro Date: Fri, 29 Aug 2025 13:15:14 +0200 Subject: [PATCH 15/15] remove wrong tracked files --- talos_robot | 1 - tiago_pro_robot | 1 - 2 files changed, 2 deletions(-) delete mode 160000 talos_robot delete mode 160000 tiago_pro_robot diff --git a/talos_robot b/talos_robot deleted file mode 160000 index 6152887..0000000 --- a/talos_robot +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6152887e3f258cca58231391ae0f380baede43ff diff --git a/tiago_pro_robot b/tiago_pro_robot deleted file mode 160000 index c62d9f8..0000000 --- a/tiago_pro_robot +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c62d9f885a5a48a145228c13acb16e027691993d