def SetInit2Start(self,T0_TSY):
T_rh = None
T_lf = None
# Defines a box in which to have the end-effector manipulate
if self.use_manipbox :
# hand_box = matrix( self.manipbox_dim + [-1000,1000,-1000,1000,-1000,1000] )
hand_box = matrix( self.manipbox_dim + [-pi,pi,-pi,pi,-pi,pi] )
T_rh = dot(T0_TSY, MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0])))) # T0_TSY
T_lh = dot(T0_TSY, MakeTransform(rodrigues([pi/2,0,0]),transpose(matrix([0,0,0])))) # T0_TSY
else :
hand_box = matrix([-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000])
T_rh = self.robotManips[1].GetEndEffectorTransform()
T_lh = self.robotManips[0].GetEndEffectorTransform()
# Define Task Space Region strings
# Left Hand
TSRStringLH1 = SerializeTSR(0,'NULL',T_lh,eye(4),hand_box)
TSRStringLH0 = SerializeTSR(0,'NULL',self.robotManips[0].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Right Hand
TSRStringRH1 = SerializeTSR(1,'NULL',T_rh,eye(4),hand_box)
TSRStringRH0 = SerializeTSR(1,'NULL',self.robotManips[1].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Define TSR for this path
# Left Foot
TSRStringLF1 = SerializeTSR(2,'NULL', self.robotManips[2].GetEndEffectorTransform(), eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Right Foot
TSRStringRF1 = SerializeTSR(3,'NULL', self.robotManips[3].GetEndEffectorTransform(), eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Head
TSRStringH = SerializeTSR(4,'NULL', self.robotManips[4].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# We defined Task Space Regions. Now let's concatenate them.
self.TSRChainStringFeetandHead_init2start_bh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
self.TSRChainStringFeetandHead_init2start_lh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
self.TSRChainStringFeetandHead_init2start_rh = SerializeTSRChain(0,0,1,1,TSRStringLH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
def extr_TSR(self,
window_points,
ee_trans,
hold,
stud_offset=[],
plot=False,
draw_axes=False,
verbose=False,
eigenvalue_threshold=0.0001):
eig_pairs = self.run_PCA(window_points)
matrix_w = np.hstack(
(eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1),
eig_pairs[2][1].reshape(3, 1)))
if verbose: print 'New coordinates:\n', matrix_w
transformed = matrix_w.T.dot(window_points.T)
if plot:
fig = plt.figure(2)
#fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection='3d')
plt.plot(transformed.T[:, 0],
transformed.T[:, 1],
transformed.T[:, 2],
'o',
markersize=7,
color='blue',
alpha=0.5)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Transformed samples with class labels')
#plt.show(block=False)
plt.draw()
# print np.min(transformed.T[:, 0])
# print np.max(transformed.T[:, 0])
# print np.min(transformed.T[:, 1])
# print np.max(transformed.T[:, 1])
# print np.min(transformed.T[:, 2])
# print np.max(transformed.T[:, 2])
# Path TSR frame
TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :]))
change_axes = MakeTransform(rodrigues([0, pi / 2, 0]), matrix([
0, 0, 0
])) * MakeTransform(rodrigues([0, 0, pi / 2]), matrix([0, 0, 0]))
TSR_0_w_path = TSR_0_w_path * change_axes
# Path EE offset
EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0]))
# Path B_w
# print eig_pairs[0][0]
# print eig_pairs[1][0]
# print eig_pairs[2][0]
# print eigenvalue_threshold
# TODO fix the frames from extraction to execution
z_min = np.min(transformed.T[:, 0]) - transformed[0, 0] - 0. if (
eig_pairs[0][0] < eigenvalue_threshold) else -1000
z_max = np.max(transformed.T[:, 0]) - transformed[0, 0] + 0. if (
eig_pairs[0][0] < eigenvalue_threshold) else 1000
x_min = np.min(transformed.T[:, 1]) - transformed[1, 0] - 0. if (
eig_pairs[1][0] < eigenvalue_threshold) else -1000
x_max = np.max(transformed.T[:, 1]) - transformed[1, 0] + 0. if (
eig_pairs[1][0] < eigenvalue_threshold) else 1000
y_min = np.min(transformed.T[:, 2]) - transformed[2, 0] - 0.2 if (
eig_pairs[2][0] < eigenvalue_threshold) else -1000
y_max = np.max(transformed.T[:, 2]) - transformed[2, 0] + 0.2 if (
eig_pairs[2][0] < eigenvalue_threshold) else 1000
B_w_path = mat([
x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000,
-1000, 1000
])
#print window_points[0, :]
#print transformed[:, 0]
#print B_w_path
#sys.stdin.readline()
# Goal TSR frame
TSR_0_w_goal = ee_trans[-1]
# Goal EE offset
#EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0]))
EE_offset_goal = stud_offset[-1] # for the screw task the offset
# Goal B_w
B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0])
if draw_axes:
#draw TSR_frame
self.drawingHandles.append(
misc.DrawAxes(self.env, TSR_0_w_path, 0.3))
self.drawingHandles.append(
misc.DrawAxes(self.env, TSR_0_w_goal, 0.3))
new_z_axis = eig_pairs[0][1]
plt.show(block=False)
return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path
], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def run_PCA(window_points,
ee_trans,
hold,
stud_offset=[],
plot=False,
draw_axes=False,
verbose=False,
eigenvalue_threshold=0.001):
# 1. PCA calculations
mean_x = np.mean(window_points[:, 0])
mean_y = np.mean(window_points[:, 1])
mean_z = np.mean(window_points[:, 2])
mean_vector = np.array([[mean_x], [mean_y], [mean_z]])
if verbose: print 'Mean Vector:\n', mean_vector
dim = 3
scatter_matrix = np.zeros((dim, dim))
for i in range(window_points.shape[0]):
scatter_matrix += (window_points[i, :].reshape(dim, 1) -
mean_vector).dot(
(window_points[i, :].reshape(dim, 1) -
mean_vector).T)
if verbose: print 'Scatter Matrix:\n', scatter_matrix
cov_mat = np.cov(
[window_points[:, 0], window_points[:, 1], window_points[:, 2]])
if verbose: print 'Covariance Matrix:\n', cov_mat
# eigenvectors and eigenvalues for the from the scatter matrix
eig_val_sc, eig_vec_sc = np.linalg.eigh(scatter_matrix)
# eigenvectors and eigenvalues for the from the covariance matrix
eig_val_cov, eig_vec_cov = np.linalg.eig(cov_mat)
for i in range(len(eig_val_sc)):
eigvec_sc = eig_vec_sc[:, i] #.reshape(1, 3).T
eigvec_cov = eig_vec_cov[:, i] #.reshape(1, 3).T
assert eigvec_sc.all() == eigvec_cov.all(
), 'Eigenvectors are not identical'
if verbose:
print 'Eigenvector {}: \n{}'.format(i + 1, eig_vec_sc[:, i])
if verbose:
print 'Eigenvalue {} from scatter matrix: {}'.format(
i + 1, eig_val_sc[i])
if verbose:
print 'Eigenvalue {} from covariance matrix: {}'.format(
i + 1, eig_val_cov[i])
if verbose: print 'Scaling factor: ', eig_val_sc[i] / eig_val_cov[i]
if verbose: print 40 * '-'
# check eigenvector-eigenvalue calculations
for i in range(len(eig_val_sc)):
eigv = eig_vec_sc[:, i].reshape(1, dim).T
np.testing.assert_array_almost_equal(scatter_matrix.dot(eigv),
eig_val_sc[i] * eigv,
decimal=6,
err_msg='',
verbose=True)
if plot:
fig = plt.figure(1)
#fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection='3d')
plt.plot(window_points[:, 0],
window_points[:, 1],
window_points[:, 2],
'o',
markersize=8,
color='green',
alpha=0.2)
plt.plot([mean_x], [mean_y], [mean_z],
'o',
markersize=10,
color='red',
alpha=0.5)
first = True
for v in eig_vec_sc.T:
c = 'b'
if first:
c = 'r'
first = False
a = Arrow3D([window_points[0, 0], v[0] + window_points[0, 0]],
[window_points[0, 1], v[1] + window_points[0, 1]],
[window_points[0, 2], v[2] + window_points[0, 2]],
mutation_scale=1,
lw=3,
arrowstyle="-|>",
color=c)
ax.add_artist(a)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Eigenvectors')
plt.draw()
# test if the eigenvectors are unit vectors
# for ev in eig_vec_sc:
# np.testing.assert_array_almost_equal(1.0, np.linalg.norm(ev))
# Make a list of (eigenvalue, eigenvector) tuples
eig_pairs = [(np.abs(eig_val_sc[i]), eig_vec_sc[:, i])
for i in range(len(eig_val_sc))]
# Sort the (eigenvalue, eigenvector) tuples from high to low
eig_pairs.sort()
eig_pairs.reverse()
matrix_w = np.hstack(
(eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1),
eig_pairs[2][1].reshape(3, 1)))
if verbose: print 'New coordinates:\n', matrix_w
transformed = matrix_w.T.dot(window_points.T)
if plot:
fig = plt.figure(2)
ax = fig.add_subplot(111, projection='3d')
plt.plot(transformed.T[:, 0],
transformed.T[:, 1],
transformed.T[:, 2],
'o',
markersize=7,
color='blue',
alpha=0.5)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Transformed samples with class labels')
plt.draw()
# 2. TSR calculations from PCA results
# Path TSR frame
TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :]))
change_axes = MakeTransform(rodrigues(
[0, pi / 2, 0]), matrix([0, 0, 0])) * MakeTransform(
rodrigues([0, 0, pi / 2]), matrix([0, 0, 0]))
TSR_0_w_path = TSR_0_w_path * change_axes
# check for valid rotation matrix (det = 1) and fix
if numpy.linalg.det(TSR_0_w_path[0:3][:, 0:3]) != 1:
TSR_0_w_path[0:3, 0] *= -1
# Path EE offset
EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0]))
# Path B_w
if verbose:
print "eigen pairs"
print eig_pairs[0][0]
print eig_pairs[1][0]
print eig_pairs[2][0]
# print eigenvalue_threshold
# TODO fix the frames from extraction to execution
z_min = np.min(transformed.T[:, 0]) - transformed[0, 0] - 0. if (
abs(eig_pairs[0][0]) < eigenvalue_threshold) else -1000
z_max = np.max(transformed.T[:, 0]) - transformed[0, 0] + 0. if (
abs(eig_pairs[0][0]) < eigenvalue_threshold) else 1000
x_min = np.min(transformed.T[:, 1]) - transformed[1, 0] - 0. if (
abs(eig_pairs[1][0]) < eigenvalue_threshold) else -1000
x_max = np.max(transformed.T[:, 1]) - transformed[1, 0] + 0. if (
abs(eig_pairs[1][0]) < eigenvalue_threshold) else 1000
y_min = np.min(transformed.T[:, 2]) - transformed[2, 0] - 0. if (
abs(eig_pairs[2][0]) < eigenvalue_threshold) else -1000
y_max = np.max(transformed.T[:, 2]) - transformed[2, 0] + 0. if (
abs(eig_pairs[2][0]) < eigenvalue_threshold) else 1000
B_w_path = mat([
x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000,
-1000, 1000
])
if verbose:
print "boundaries:"
print transformed
print B_w_path
# Goal TSR frame
#TSR_0_w_goal = ee_trans[-1] # not gonna be used
TSR_0_w_goal = ee_trans[0]
# Goal EE offset
#EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0]))
EE_offset_goal = ee_trans[
-1] #MakeTransform(eye(3), matrix([0, -0.05, -0.002])) # for the unscrew nut task (took from AppraochAction)
#EE_offset_goal = stud_offset[-1] # for the screw task the offset
# Goal B_w
B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0])
if draw_axes:
#draw TSR_frame
#drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.035, linewidth=1.0))
drawingHandles.append(
misc.DrawAxes(env, TSR_0_w_path, 0.15, linewidth=1.0))
#drawingHandles.append(misc.DrawAxes(env, TSR_0_w_goal, 0.3))
new_z_axis = eig_pairs[0][1]
plt.show(block=False)
return new_z_axis, [TSR_0_w_path, EE_offset_path,
B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def extr_TSR(self, window_points, ee_trans, hold, stud_offset=[], plot = False, draw_axes = False, verbose = False, eigenvalue_threshold = 0.0001):
eig_pairs = self.run_PCA(window_points)
matrix_w = np.hstack((eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1)))
if verbose: print 'New coordinates:\n', matrix_w
transformed = matrix_w.T.dot(window_points.T)
if plot:
fig = plt.figure(2)
#fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection='3d')
plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2],
'o', markersize=7, color='blue', alpha=0.5)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Transformed samples with class labels')
#plt.show(block=False)
plt.draw()
# print np.min(transformed.T[:, 0])
# print np.max(transformed.T[:, 0])
# print np.min(transformed.T[:, 1])
# print np.max(transformed.T[:, 1])
# print np.min(transformed.T[:, 2])
# print np.max(transformed.T[:, 2])
# Path TSR frame
TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :]))
change_axes = MakeTransform(rodrigues([0, pi/2, 0]), matrix([0, 0, 0]))*MakeTransform(rodrigues([0, 0, pi/2]), matrix([0, 0, 0]))
TSR_0_w_path = TSR_0_w_path*change_axes
# Path EE offset
EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0]))
# Path B_w
# print eig_pairs[0][0]
# print eig_pairs[1][0]
# print eig_pairs[2][0]
# print eigenvalue_threshold
# TODO fix the frames from extraction to execution
z_min = np.min(transformed.T[:, 0])-transformed[0, 0]-0. if (eig_pairs[0][0] < eigenvalue_threshold) else -1000
z_max = np.max(transformed.T[:, 0])-transformed[0, 0]+0. if (eig_pairs[0][0] < eigenvalue_threshold) else 1000
x_min = np.min(transformed.T[:, 1])-transformed[1, 0]-0. if (eig_pairs[1][0] < eigenvalue_threshold) else -1000
x_max = np.max(transformed.T[:, 1])-transformed[1, 0]+0. if (eig_pairs[1][0] < eigenvalue_threshold) else 1000
y_min = np.min(transformed.T[:, 2])-transformed[2, 0]-0.2 if (eig_pairs[2][0] < eigenvalue_threshold) else -1000
y_max = np.max(transformed.T[:, 2])-transformed[2, 0]+0.2 if (eig_pairs[2][0] < eigenvalue_threshold) else 1000
B_w_path = mat([x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000])
#print window_points[0, :]
#print transformed[:, 0]
#print B_w_path
#sys.stdin.readline()
# Goal TSR frame
TSR_0_w_goal = ee_trans[-1]
# Goal EE offset
#EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0]))
EE_offset_goal = stud_offset[-1] # for the screw task the offset
# Goal B_w
B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0])
if draw_axes:
#draw TSR_frame
self.drawingHandles.append(misc.DrawAxes(self.env, TSR_0_w_path, 0.3))
self.drawingHandles.append(misc.DrawAxes(self.env, TSR_0_w_goal, 0.3))
new_z_axis = eig_pairs[0][1]
plt.show(block = False)
return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def run_PCA(window_points, ee_trans, hold, stud_offset=[], plot = False, draw_axes = False, verbose = False, eigenvalue_threshold = 0.001):
# 1. PCA calculations
mean_x = np.mean(window_points[:, 0])
mean_y = np.mean(window_points[:, 1])
mean_z = np.mean(window_points[:, 2])
mean_vector = np.array([[mean_x], [mean_y], [mean_z]])
if verbose: print 'Mean Vector:\n', mean_vector
dim = 3
scatter_matrix = np.zeros((dim, dim))
for i in range(window_points.shape[0]):
scatter_matrix += (window_points[i, :].reshape(dim, 1)
- mean_vector).dot((window_points[i, :].reshape(dim, 1) - mean_vector).T)
if verbose: print 'Scatter Matrix:\n', scatter_matrix
cov_mat = np.cov([window_points[:, 0], window_points[:, 1], window_points[:, 2]])
if verbose: print 'Covariance Matrix:\n', cov_mat
# eigenvectors and eigenvalues for the from the scatter matrix
eig_val_sc, eig_vec_sc = np.linalg.eigh(scatter_matrix)
# eigenvectors and eigenvalues for the from the covariance matrix
eig_val_cov, eig_vec_cov = np.linalg.eig(cov_mat)
for i in range(len(eig_val_sc)):
eigvec_sc = eig_vec_sc[:, i]#.reshape(1, 3).T
eigvec_cov = eig_vec_cov[:, i]#.reshape(1, 3).T
assert eigvec_sc.all() == eigvec_cov.all(), 'Eigenvectors are not identical'
if verbose: print 'Eigenvector {}: \n{}'.format(i+1, eig_vec_sc[:, i])
if verbose: print 'Eigenvalue {} from scatter matrix: {}'.format(i+1, eig_val_sc[i])
if verbose: print 'Eigenvalue {} from covariance matrix: {}'.format(i+1, eig_val_cov[i])
if verbose: print 'Scaling factor: ', eig_val_sc[i]/eig_val_cov[i]
if verbose: print 40 * '-'
# check eigenvector-eigenvalue calculations
for i in range(len(eig_val_sc)):
eigv = eig_vec_sc[:, i].reshape(1, dim).T
np.testing.assert_array_almost_equal(scatter_matrix.dot(eigv),
eig_val_sc[i] * eigv, decimal=6,
err_msg='', verbose=True)
if plot:
fig = plt.figure(1)
#fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(111, projection='3d')
plt.plot(window_points[:, 0], window_points[:, 1],
window_points[:, 2],
'o', markersize=8, color='green', alpha=0.2)
plt.plot([mean_x], [mean_y],
[mean_z],
'o', markersize=10, color='red', alpha=0.5)
first = True
for v in eig_vec_sc.T:
c = 'b'
if first:
c = 'r'
first = False
a = Arrow3D([window_points[0, 0], v[0]+window_points[0, 0]], [window_points[0, 1], v[1]+window_points[0, 1]],
[window_points[0, 2], v[2]+window_points[0, 2]],
mutation_scale=1, lw=3, arrowstyle="-|>", color=c)
ax.add_artist(a)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Eigenvectors')
plt.draw()
# test if the eigenvectors are unit vectors
# for ev in eig_vec_sc:
# np.testing.assert_array_almost_equal(1.0, np.linalg.norm(ev))
# Make a list of (eigenvalue, eigenvector) tuples
eig_pairs = [(np.abs(eig_val_sc[i]), eig_vec_sc[:, i]) for i in range(len(eig_val_sc))]
# Sort the (eigenvalue, eigenvector) tuples from high to low
eig_pairs.sort()
eig_pairs.reverse()
matrix_w = np.hstack((eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1)))
if verbose: print 'New coordinates:\n', matrix_w
transformed = matrix_w.T.dot(window_points.T)
if plot:
fig = plt.figure(2)
ax = fig.add_subplot(111, projection='3d')
plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2],
'o', markersize=7, color='blue', alpha=0.5)
ax.set_xlabel('x_values')
ax.set_ylabel('y_values')
ax.set_zlabel('z_values')
plt.title('Transformed samples with class labels')
plt.draw()
# 2. TSR calculations from PCA results
# Path TSR frame
TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :]))
change_axes = MakeTransform(rodrigues([0, pi/2, 0]), matrix([0, 0, 0]))*MakeTransform(rodrigues([0, 0, pi/2]), matrix([0, 0, 0]))
TSR_0_w_path = TSR_0_w_path*change_axes
# check for valid rotation matrix (det = 1) and fix
if numpy.linalg.det(TSR_0_w_path[0:3][:,0:3]) != 1:
TSR_0_w_path[0:3, 0] *= -1;
# Path EE offset
EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0]))
# Path B_w
if verbose:
print "eigen pairs"
print eig_pairs[0][0]
print eig_pairs[1][0]
print eig_pairs[2][0]
# print eigenvalue_threshold
# TODO fix the frames from extraction to execution
z_min = np.min(transformed.T[:, 0])-transformed[0, 0]-0. if (abs(eig_pairs[0][0]) < eigenvalue_threshold) else -1000
z_max = np.max(transformed.T[:, 0])-transformed[0, 0]+0. if (abs(eig_pairs[0][0]) < eigenvalue_threshold) else 1000
x_min = np.min(transformed.T[:, 1])-transformed[1, 0]-0. if (abs(eig_pairs[1][0]) < eigenvalue_threshold) else -1000
x_max = np.max(transformed.T[:, 1])-transformed[1, 0]+0. if (abs(eig_pairs[1][0]) < eigenvalue_threshold) else 1000
y_min = np.min(transformed.T[:, 2])-transformed[2, 0]-0. if (abs(eig_pairs[2][0]) < eigenvalue_threshold) else -1000
y_max = np.max(transformed.T[:, 2])-transformed[2, 0]+0. if (abs(eig_pairs[2][0]) < eigenvalue_threshold) else 1000
B_w_path = mat([x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000])
if verbose:
print "boundaries:"
print transformed
print B_w_path
# Goal TSR frame
#TSR_0_w_goal = ee_trans[-1] # not gonna be used
TSR_0_w_goal = ee_trans[0]
# Goal EE offset
#EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0]))
EE_offset_goal = ee_trans[-1] #MakeTransform(eye(3), matrix([0, -0.05, -0.002])) # for the unscrew nut task (took from AppraochAction)
#EE_offset_goal = stud_offset[-1] # for the screw task the offset
# Goal B_w
B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0])
if draw_axes:
#draw TSR_frame
#drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.035, linewidth=1.0))
drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.15, linewidth=1.0))
#drawingHandles.append(misc.DrawAxes(env, TSR_0_w_goal, 0.3))
new_z_axis = eig_pairs[0][1]
plt.show(block = False)
return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]