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 05ecb32..260213c 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,19 +25,20 @@ 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. + 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 @@ -46,24 +46,29 @@ def __init__(self, robot_description : Union[str, Path]): # 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() 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() 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 @@ -77,14 +82,16 @@ def __init__(self, robot_description : Union[str, Path]): # 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) @@ -95,7 +102,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. @@ -105,8 +111,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 = [] @@ -120,50 +126,58 @@ 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): """ 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) - + 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 - - + 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 + + # 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]: @@ -174,17 +188,24 @@ 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: + 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. """ @@ -194,23 +215,29 @@ 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 + + # 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 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. @@ -218,145 +245,171 @@ def compute_inverse_dynamics(self, q : np.ndarray , qdot : np.ndarray, qddot : n :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) 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) -> 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. - - :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. + 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. :return: External force vector. """ 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 + # 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): + for mass, frame in zip(masses, frame_name): frame_id = self.model.getFrameId(frame) 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): + 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 - + # 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. + :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 end_effector_name: Name of the end effector joint. - :return: Joint configuration vector that achieves the desired end effector position. + 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. + :return: Joint configuration vector that achieves + the desired end effector position. """ # 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 - - err = pin.log(iMd).vector # compute the error in the 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) + + # compute the error in the end effector position + err = pin.log(iMd).vector if norm(err[:3]) < eps: success = True break @@ -364,194 +417,264 @@ def compute_inverse_kinematics(self, q : np.ndarray, end_effector_position: np.n success = False 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, :] + # 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) - - # Check if we're hitting joint limits and reduce step size if needed + 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 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: 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) - + 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) - + # 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 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"]) + 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(): + 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}) + 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. + 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. - :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"}]. + :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"]: + 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"]) - # 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): + 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" } + 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" } """ 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 + 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. + :param config: Configuration dictionary (must contain 'config' key). :param payload_min: Minimum payload to test. :param payload_max: Maximum payload to test. @@ -564,8 +687,14 @@ def find_max_payload_binary_search(self, config : np.ndarray, payload_min : floa 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 @@ -574,76 +703,82 @@ 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) - - J_frame = pin.getJointJacobian(self.model, self.data, joint_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) - + 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"] + # 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. """ @@ -657,42 +792,73 @@ 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_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | 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. + + :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: """ 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] - - # create an array to store the absolute torques for each joint in the current valid configurations for each selected tree + 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 + # 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 @@ -701,52 +867,60 @@ def get_maximum_torques(self, valid_configs : np.ndarray) -> np.ndarray | np.nda 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 + 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 left and right arm. - - :param valid_configs: Array of valid configurations with related torques in format: [{"config", "end_effector_pos, "tau", "arm", "max_payload"}]. + 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. """ 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. - + :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 + # 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: @@ -754,28 +928,29 @@ 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. - + :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 | 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) @@ -787,208 +962,212 @@ 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"]] - + 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) + # 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 - - 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. + + :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) + 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") + 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) - + 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 + # 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") + if abs(tau[id - 1]) > self.model.effortLimit[id - 1]: + 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") - 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. """ @@ -998,24 +1177,29 @@ 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. + + :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 - # (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]) - + # 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]) + if self.model.joints[i].nq == 2: # for continuous joints (wheels) q[cont] = math.cos(pos_joints[index]) @@ -1027,138 +1211,151 @@ 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 + :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 + # 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 config - - def get_joints_placements(self, q : np.ndarray) -> np.ndarray | float: + return np.array(config, dtype=object) + + 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. """ 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 - - 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: + 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.") - + 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 - - 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: @@ -1167,49 +1364,50 @@ 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) - 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") - + 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. """ @@ -1222,11 +1420,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: @@ -1235,16 +1432,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. @@ -1255,20 +1450,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): """ @@ -1276,7 +1469,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]}") @@ -1284,4 +1477,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)) diff --git a/dynamic_payload_analysis_core/setup.py b/dynamic_payload_analysis_core/setup.py index 7f43d9a..b0537e3 100644 --- a/dynamic_payload_analysis_core/setup.py +++ b/dynamic_payload_analysis_core/setup.py @@ -18,7 +18,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=[ 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..c30965d --- /dev/null +++ b/dynamic_payload_analysis_core/test/test_core.py @@ -0,0 +1,839 @@ +#!/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 +import numpy as np + + +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.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") + + 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") + 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 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) + # 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.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() + + # 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_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) + + 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() + 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") + + 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) + 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, + 25, + "Parent joint ID should match the expected value") + + print("✅ Get parent joint ID assertion passed") + + +if __name__ == "__main__": + unittest.main() 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 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 20c55a3..4645957 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 @@ -622,7 +622,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