Spaces:
Sleeping
Sleeping
| import numpy as np | |
| import robosuite.utils.transform_utils as trans | |
| from robosuite.utils.numba import jit_decorator | |
| def nullspace_torques(mass_matrix, nullspace_matrix, initial_joint, joint_pos, joint_vel, joint_kp=10): | |
| """ | |
| For a robot with redundant DOF(s), a nullspace exists which is orthogonal to the remainder of the controllable | |
| subspace of the robot's joints. Therefore, an additional secondary objective that does not impact the original | |
| controller objective may attempt to be maintained using these nullspace torques. | |
| This utility function specifically calculates nullspace torques that attempt to maintain a given robot joint | |
| positions @initial_joint with zero velocity using proportinal gain @joint_kp | |
| :Note: @mass_matrix, @nullspace_matrix, @joint_pos, and @joint_vel should reflect the robot's state at the current | |
| timestep | |
| Args: | |
| mass_matrix (np.array): 2d array representing the mass matrix of the robot | |
| nullspace_matrix (np.array): 2d array representing the nullspace matrix of the robot | |
| initial_joint (np.array): Joint configuration to be used for calculating nullspace torques | |
| joint_pos (np.array): Current joint positions | |
| joint_vel (np.array): Current joint velocities | |
| joint_kp (float): Proportional control gain when calculating nullspace torques | |
| Returns: | |
| np.array: nullspace torques | |
| """ | |
| # kv calculated below corresponds to critical damping | |
| joint_kv = np.sqrt(joint_kp) * 2 | |
| # calculate desired torques based on gains and error | |
| pose_torques = np.dot(mass_matrix, (joint_kp * (initial_joint - joint_pos) - joint_kv * joint_vel)) | |
| # map desired torques to null subspace within joint torque actuator space | |
| nullspace_torques = np.dot(nullspace_matrix.transpose(), pose_torques) | |
| return nullspace_torques | |
| def opspace_matrices(mass_matrix, J_full, J_pos, J_ori): | |
| """ | |
| Calculates the relevant matrices used in the operational space control algorithm | |
| Args: | |
| mass_matrix (np.array): 2d array representing the mass matrix of the robot | |
| J_full (np.array): 2d array representing the full Jacobian matrix of the robot | |
| J_pos (np.array): 2d array representing the position components of the Jacobian matrix of the robot | |
| J_ori (np.array): 2d array representing the orientation components of the Jacobian matrix of the robot | |
| Returns: | |
| 4-tuple: | |
| - (np.array): full lambda matrix (as 2d array) | |
| - (np.array): position components of lambda matrix (as 2d array) | |
| - (np.array): orientation components of lambda matrix (as 2d array) | |
| - (np.array): nullspace matrix (as 2d array) | |
| """ | |
| mass_matrix_inv = np.linalg.inv(mass_matrix) | |
| # J M^-1 J^T | |
| lambda_full_inv = np.dot(np.dot(J_full, mass_matrix_inv), J_full.transpose()) | |
| # Jx M^-1 Jx^T | |
| lambda_pos_inv = np.dot(np.dot(J_pos, mass_matrix_inv), J_pos.transpose()) | |
| # Jr M^-1 Jr^T | |
| lambda_ori_inv = np.dot(np.dot(J_ori, mass_matrix_inv), J_ori.transpose()) | |
| # take the inverses, but zero out small singular values for stability | |
| lambda_full = np.linalg.pinv(lambda_full_inv) | |
| lambda_pos = np.linalg.pinv(lambda_pos_inv) | |
| lambda_ori = np.linalg.pinv(lambda_ori_inv) | |
| # nullspace | |
| Jbar = np.dot(mass_matrix_inv, J_full.transpose()).dot(lambda_full) | |
| nullspace_matrix = np.eye(J_full.shape[-1], J_full.shape[-1]) - np.dot(Jbar, J_full) | |
| return lambda_full, lambda_pos, lambda_ori, nullspace_matrix | |
| def orientation_error(desired, current): | |
| """ | |
| This function calculates a 3-dimensional orientation error vector for use in the | |
| impedance controller. It does this by computing the delta rotation between the | |
| inputs and converting that rotation to exponential coordinates (axis-angle | |
| representation, where the 3d vector is axis * angle). | |
| See https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation for more information. | |
| Optimized function to determine orientation error from matrices | |
| Args: | |
| desired (np.array): 2d array representing target orientation matrix | |
| current (np.array): 2d array representing current orientation matrix | |
| Returns: | |
| np.array: 2d array representing orientation error as a matrix | |
| """ | |
| rc1 = current[0:3, 0] | |
| rc2 = current[0:3, 1] | |
| rc3 = current[0:3, 2] | |
| rd1 = desired[0:3, 0] | |
| rd2 = desired[0:3, 1] | |
| rd3 = desired[0:3, 2] | |
| error = 0.5 * (np.cross(rc1, rd1) + np.cross(rc2, rd2) + np.cross(rc3, rd3)) | |
| return error | |
| def set_goal_position(delta, current_position, position_limit=None, set_pos=None): | |
| """ | |
| Calculates and returns the desired goal position, clipping the result accordingly to @position_limits. | |
| @delta and @current_position must be specified if a relative goal is requested, else @set_pos must be | |
| specified to define a global goal position | |
| Args: | |
| delta (np.array): Desired relative change in position | |
| current_position (np.array): Current position | |
| position_limit (None or np.array): 2d array defining the (min, max) limits of permissible position goal commands | |
| set_pos (None or np.array): If set, will ignore @delta and set the goal position to this value | |
| Returns: | |
| np.array: calculated goal position in absolute coordinates | |
| Raises: | |
| ValueError: [Invalid position_limit shape] | |
| """ | |
| n = len(current_position) | |
| if set_pos is not None: | |
| goal_position = set_pos | |
| else: | |
| goal_position = current_position + delta | |
| if position_limit is not None: | |
| if position_limit.shape != (2, n): | |
| raise ValueError( | |
| "Position limit should be shaped (2,{}) " "but is instead: {}".format(n, position_limit.shape) | |
| ) | |
| # Clip goal position | |
| goal_position = np.clip(goal_position, position_limit[0], position_limit[1]) | |
| return goal_position | |
| def set_goal_orientation(delta, current_orientation, orientation_limit=None, set_ori=None): | |
| """ | |
| Calculates and returns the desired goal orientation, clipping the result accordingly to @orientation_limits. | |
| @delta and @current_orientation must be specified if a relative goal is requested, else @set_ori must be | |
| an orientation matrix specified to define a global orientation | |
| Args: | |
| delta (np.array): Desired relative change in orientation, in axis-angle form [ax, ay, az] | |
| current_orientation (np.array): Current orientation, in rotation matrix form | |
| orientation_limit (None or np.array): 2d array defining the (min, max) limits of permissible orientation goal commands | |
| set_ori (None or np.array): If set, will ignore @delta and set the goal orientation to this value | |
| Returns: | |
| np.array: calculated goal orientation in absolute coordinates | |
| Raises: | |
| ValueError: [Invalid orientation_limit shape] | |
| """ | |
| # directly set orientation | |
| if set_ori is not None: | |
| goal_orientation = set_ori | |
| # otherwise use delta to set goal orientation | |
| else: | |
| # convert axis-angle value to rotation matrix | |
| quat_error = trans.axisangle2quat(delta) | |
| rotation_mat_error = trans.quat2mat(quat_error) | |
| goal_orientation = np.dot(rotation_mat_error, current_orientation) | |
| # check for orientation limits | |
| if np.array(orientation_limit).any(): | |
| if orientation_limit.shape != (2, 3): | |
| raise ValueError( | |
| "Orientation limit should be shaped (2,3) " "but is instead: {}".format(orientation_limit.shape) | |
| ) | |
| # Convert to euler angles for clipping | |
| euler = trans.mat2euler(goal_orientation) | |
| # Clip euler angles according to specified limits | |
| limited = False | |
| for idx in range(3): | |
| if orientation_limit[0][idx] < orientation_limit[1][idx]: # Normal angle sector meaning | |
| if orientation_limit[0][idx] < euler[idx] < orientation_limit[1][idx]: | |
| continue | |
| else: | |
| limited = True | |
| dist_to_lower = euler[idx] - orientation_limit[0][idx] | |
| if dist_to_lower > np.pi: | |
| dist_to_lower -= 2 * np.pi | |
| elif dist_to_lower < -np.pi: | |
| dist_to_lower += 2 * np.pi | |
| dist_to_higher = euler[idx] - orientation_limit[1][idx] | |
| if dist_to_lower > np.pi: | |
| dist_to_higher -= 2 * np.pi | |
| elif dist_to_lower < -np.pi: | |
| dist_to_higher += 2 * np.pi | |
| if dist_to_lower < dist_to_higher: | |
| euler[idx] = orientation_limit[0][idx] | |
| else: | |
| euler[idx] = orientation_limit[1][idx] | |
| else: # Inverted angle sector meaning | |
| if orientation_limit[0][idx] < euler[idx] or euler[idx] < orientation_limit[1][idx]: | |
| continue | |
| else: | |
| limited = True | |
| dist_to_lower = euler[idx] - orientation_limit[0][idx] | |
| if dist_to_lower > np.pi: | |
| dist_to_lower -= 2 * np.pi | |
| elif dist_to_lower < -np.pi: | |
| dist_to_lower += 2 * np.pi | |
| dist_to_higher = euler[idx] - orientation_limit[1][idx] | |
| if dist_to_lower > np.pi: | |
| dist_to_higher -= 2 * np.pi | |
| elif dist_to_lower < -np.pi: | |
| dist_to_higher += 2 * np.pi | |
| if dist_to_lower < dist_to_higher: | |
| euler[idx] = orientation_limit[0][idx] | |
| else: | |
| euler[idx] = orientation_limit[1][idx] | |
| if limited: | |
| goal_orientation = trans.euler2mat(np.array([euler[0], euler[1], euler[2]])) | |
| return goal_orientation | |