Spaces:
Sleeping
Sleeping
File size: 10,007 Bytes
96da58e |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 |
import numpy as np
import robosuite.utils.transform_utils as trans
from robosuite.utils.numba import jit_decorator
@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
@jit_decorator
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
@jit_decorator
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
|