def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega, theta)
I = np.identity(3)
if np.array_equal(R, I):
v = trans / np.norm(trans)
else:
omega1 = np.array([[omega[0]], [omega[1]], [omega[2]]])
A = ((I - kfs.rotation_3d(omega, theta)).dot(kfs.skew_3d(omega)) +
(omega1).dot(omega1.T) * theta)
v = (np.linalg.inv(A)).dot(trans)
v = np.array([[v[0]], [v[1]], [v[2]]])
return v
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega, theta)
g = np.zeros([4, 4])
g[0:3, 0:3] = R
#rotation matrix
g[0, 3] = trans[0]
g[1, 3] = trans[1]
g[2, 3] = trans[2]
#three elements of the trans vector, use specific elements since trans doesn't transpose due to not being the right kind of nd array
g[3, 3] = 1
#bottom right element to 1
return g
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
trans_transposed = np.array([[trans[0]], [trans[1]], [trans[2]]])
omega_transposed = np.array([[omega[0]], [omega[1]], [omega[2]]])
omega_new = np.transpose(omega_transposed)
#computer omega_transposed, omega_new, that can both be fed into np.dot
#NOTE: order in the np.dot(omega_transposes,omega_new) is backwards because the omega that's given to us it a row, not a collum, vector
A = (np.dot(
(np.eye(3) - kfs.rotation_3d(omega, theta)), kfs.skew_3d(omega)) +
np.dot(omega_transposed, omega_new) * theta)
v = np.dot(np.linalg.inv(A), trans_transposed)
#YOUR CODE HERE
return v
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
A_1 = np.eye(3) - kfs.rotation_3d(omega, theta)
#print A_1
A_1 = A_1.dot(kfs.skew_3d(omega))
#print A_1
A_2 = np.outer(omega, omega.T)*theta
#print A_2
A = A_1 + A_2
#print A
#print np.linalg.inv(A)
v = np.dot(np.linalg.inv(A), trans)
#print v
return np.array([v]).T
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
A_1 = np.eye(3) - kfs.rotation_3d(omega, theta)
#print A_1
A_1 = A_1.dot(kfs.skew_3d(omega))
#print A_1
A_2 = np.outer(omega, omega.T) * theta
#print A_2
A = A_1 + A_2
#print A
#print np.linalg.inv(A)
v = np.dot(np.linalg.inv(A), trans)
#print v
return np.array([v]).T
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
#print omega, "omega"
w = omega.reshape(3, 1)
#print w, "w"
if theta == 0:
#print "t=-0"
v = trans / np.linalg.norm(trans)
else:
#print "print"
what = kfs.skew_3d(omega)
#print "must"
#print theta
R = kfs.rotation_3d(omega, theta)
#print "be"
A = (np.eye(3) - R).dot(what) + np.dot(w, w.T) * theta
#print "a"
v = np.linalg.inv(A).dot(trans)
#print "3vector"
return v.reshape(3, 1)
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega,theta)
g = np.zeros([4,4])
g[0:3,0:3] = R
#rotation matrix
g[0,3] = trans[0]
g[1,3] = trans[1]
g[2,3] = trans[2]
#three elements of the trans vector, use specific elements since trans doesn't transpose due to not being the right kind of nd array
g[3,3] = 1
#bottom right element to 1
return g
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
trans_transposed = np.array([ [trans[0]] , [trans[1]] , [trans[2]] ])
omega_transposed = np.array([ [omega[0]] , [omega[1]] , [omega[2]] ])
omega_new = np.transpose(omega_transposed)
#computer omega_transposed, omega_new, that can both be fed into np.dot
#NOTE: order in the np.dot(omega_transposes,omega_new) is backwards because the omega that's given to us it a row, not a collum, vector
A = ( np.dot((np.eye(3) - kfs.rotation_3d(omega,theta)) , kfs.skew_3d(omega)) + np.dot(omega_transposed,omega_new )*theta )
v = np.dot(np.linalg.inv(A),trans_transposed)
#YOUR CODE HERE
return v
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
omega_hat = np.array([[0, -omega[2], omega[1]],
[omega[2], 0, -omega[0]],
[-omega[1], omega[0], 0]])
omega_hat2 = np.dot(omega_hat,omega_hat)
norm_omega = math.sqrt(omega[0]*omega[0]+omega[1]*omega[1]+omega[2]*omega[2])
# Rodrigues' Formula
#R = np.identity(3)+(omega_hat/norm_omega)*math.sin(theta) + (omega_hat2/(norm_omega*norm_omega))*(1-math.cos(theta))
R = kfs.rotation_3d(omega,theta)
g = np.array([[R[0,0],R[0,1],R[0,2],trans[0]],
[R[1,0],R[1,1],R[1,2],trans[1]],
[R[2,0],R[2,1],R[2,2],trans[2]],
[0, 0, 0, 1 ]])
return g
def make_g_d(pos):
"""
Constructs the g_d configuration, with the rotation the same as it already was
Args:
pos - (3,) ndarray: the desired end effector position for the UR5
Returns:
g_0 - (4, 4) ndarray: the UR5 starting condition
"""
x1 = float(pos[0])
y1 = float(pos[1])
theta1 = np.arctan2(y1, x1)
# print rad2deg(theta1)
x0 = float(qe[0])
y0 = float(qe[1])
theta0 = np.arctan2(y0, x0)
# print rad2deg(theta0)
theta = theta1 - theta0
# print rad2deg(theta)
R = kfs.rotation_3d(np.array([0, 0, 1]), theta)
R0 = rpy(rx, ry, rz)
R = np.dot(R0, R)
pos = np.array([pos]).T
g_d = np.hstack([R, pos])
g_d = np.vstack([g_d, np.array([0, 0, 0, 1])])
g_d = np.round(g_d, 4)
return g_d
def rpy(x, y, z):
"""
Constructs the rotation matrix for roll, pitch, yaw
Args:
x: roll
y: pitch
z: yaw
Returns:
R - (3,3) ndarray: Rotation matrix
"""
Rx = kfs.rotation_3d(np.array([1, 0, 0]), x)
Ry = kfs.rotation_3d(np.array([0, 1, 0]), y)
Rz = kfs.rotation_3d(np.array([0, 0, 1]), z)
R = np.dot(Rz, np.dot(Ry, Rx))
return R
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
if np.array_equal(kfs.rotation_3d(omega, theta), np.eye(3)):
v = trans / np.norm(trans)
v = v.reshape(3, 1)
else:
A = np.dot(
(np.eye(3) - kfs.rotation_3d(omega, theta)), kfs.skew_3d(
omega)) + omega.reshape(3, 1).T * omega.reshape(3, 1) * theta
v = np.dot(np.linalg.inv(A), trans)
v = v.reshape(3, 1)
return v
def create_rbt(omega, theta, p):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega, theta)
g = np.array([[R[0][0], R[0][1], R[0][2], p[0]], [R[1][0], R[1][1], R[1][2], p[1]], [R[2][0], R[2][1], R[2][2], p[2]], [0, 0, 0, 1]])
return g
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = rotation_3d(omega, theta)
g = np.vstack(( np.hstack(( R, np.array([trans]).T )), np.array([[0, 0, 0, 1]])))
return g
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
rot = kfs.rotation_3d(omega, theta)
g = np.zeros((4,4))
g[:3,:3] = rot
g[3,3] = 1
g[:3,3] = trans
return g
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega, theta)
R_p = np.array([[R[0][0], R[0][1], R[0][2], trans[0]],
[R[1][0], R[1][1], R[1][2], trans[1]],
[R[2][0], R[2][1], R[2][2], trans[2]], [0, 0, 0, 1]])
return R_p
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega, theta)
p = trans.reshape((3, 1))
g = np.vstack((np.hstack((R, p)), np.array([0, 0, 0, 1])))
return g
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
g = np.eye(4)
R = kfs.rotation_3d(omega, theta)
g[:3, :3] = R
g[:3, 3] = trans
return g
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
A = (
np.dot((np.eye(3) - kfs.rotation_3d(omega, theta)), kfs.skew_3d(omega))
+ np.dot(convert_1d_to_nd(omega), convert_1d_to_nd(omega).T) * theta
)
v = np.dot(inv(A), trans)
v = convert_1d_to_nd(v)
return v
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
#YOUR CODE HERE
R = (kfs.rotation_3d(omega,theta)).tolist()
for i in range(len(R)):
R[i].append(trans[i])
R.append([0,0,0,1])
return np.array(R)
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
R = kfs.rotation_3d(omega, theta)
v = np.zeros((3,1))
if R.all() != np.eye(3).all():
A = np.dot((np.eye(3) - R), kfs.skew_3d(omega)) + np.outer(omega, omega.T)*theta
v = np.dot(np.linalg.inv(A), trans)
else:
v = trans/np.linalg.norm(trans)
v = v.reshape((3,1))
return v
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
R = kfs.rotation_3d(omega,theta)
omega_hat = kfs.skew_3d(omega)
omegaT = np.array([[omega[0]],[omega[1]],[omega[2]]])
A = np.dot((np.eye(3)-R),omega_hat)+omega*omegaT*theta
v = np.dot(inv(A),trans)
v = np.array([[v[0]], [v[1]], [v[2]]])
#if (R == np.eye(3)):
# v =
return v
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
from prelab3 import rotation_3d, skew_3d
r = rotation_3d(omega, theta)
if (r == np.eye(3)).all():
v = np.array([trans / np.linalg.norm(trans)]).T
else:
w = np.array([omega]).T
A = (np.eye(3) - r).dot(skew_3d(omega)) + w.dot(w.T) * theta
v = np.linalg.inv(A).dot(np.array([trans]).T)
return v
def find_v(omega, theta, trans):
"""
Finds the linear velocity term of the twist (v,omega) given omega, theta and translation
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray of 3x1 list : the translation component of the rigid body transform
Returns:
v - (3,1) ndarray : the linear velocity term of the twist (v,omega)
"""
#YOUR CODE HERE
R = create_rbt(omega, theta, trans)
what = kfs.skew_3d(omega)
A1 = np.eye(3) - kfs.rotation_3d(omega, theta)
A2 = np.matrix([omega]).T*np.matrix([omega])
A = np.dot(A1, what) + A2*theta
if np.count_nonzero(A):
v = np.dot(np.linalg.inv(A), trans).T
else:
v = trans/np.linalg.norm(trans).T
return v
def create_rbt(omega, theta, trans):
"""
Creates a rigid body transform using omega, theta, and the translation component.
g = [R,p; 0,1], where R = exp(omega * theta), p = trans
Args:
omega - (3,) ndarray : the axis you want to rotate about
theta - scalar value
trans - (3,) ndarray or 3x1 array: the translation component of the rigid body motion
Returns:
g - (4,4) ndarray : the rigid body transform
"""
rotation = kfs.rotation_3d(omega, theta)
translation = convert_1d_to_nd(trans)
g = np.concatenate((rotation, translation), axis=1)
g = np.concatenate((g, np.array([[0, 0, 0, 1]])), axis=0)
"""print(g)
v = np.cross(-omega, trans)
xi = np.array([v[0], v[1], v[2], omega[0], omega[1], omega[2]])
g2 = kfs.homog_3d(xi, theta)
print(g2)"""
return g
1.3683108612304689,
-0.1223349676940918,
]
)
baxt_trans = np.array([0.656, 0.724, 0.374])
baxt_w = np.array([-3.096, 0.139, -2.389])
baxt_w_norm = baxt_w / np.linalg.norm(baxt_w)
baxt_theta = np.linalg.norm(baxt_w)
baxt_homo_transform = np.zeros((4, 4))
baxt_homo_transform[0:3, 3] = baxt_trans[0:3]
baxt_rot = skeleton.rotation_3d(baxt_w_norm, baxt_theta)
baxt_homo_transform[0:3, 0:3] = baxt_rot[0:3, 0:3]
baxt_homo_transform[3, 3] = 1
# print("What we think what Baxter thinks the homogenous transform is")
# print(baxt_homo_transform)
def task1(angles):
return skeleton.prod_exp(t, angles)
def task2(angles):
np_angles = np.array(angles)
print(np_angles)
for j in range(box_size_z):
if box[k, i, j] == 1:
ax.scatter(k/100., i/100., j/100., c='k', marker='o', s=0.5)
if box[k, i, j] == -1:
ax.scatter(k/100., i/100., j/100., c='gray', marker='o', s=0.5)
## Choosing the next best view ##
print("## Computing the score for N rotations of the occupancy grid ##")
for rotation_number in [1, 2, 3, 4]:
## choosing a rotation about the world's z axis ##
rotation_angle = np.pi / rotation_number # add some randomness
rotation_matrix = rotation_3d(np.array([0,0,1]), rotation_angle)
camera_center_in_cube_center_frame = camera_center - np.array([0.5*box_size_x/100., 0.5*box_size_y/100., 0])
camera_center_in_cube_center_frame_after_rotation = np.matmul(rotation_matrix, camera_center_in_cube_center_frame)
# ax.scatter([camera_center_in_cube_center_frame_after_rotation[0]], [camera_center_in_cube_center_frame_after_rotation[1]], [camera_center_in_cube_center_frame_after_rotation[2]])
unknown_points_we_can_probably_see = 0
occupied_points_we_can_probably_see = 0
score_dict = {}
## Plotting the occupancy grid ##
for k in range(box_size_x):
for i in range(box_size_y):
for j in range(box_size_z):
if box[k, i, j] == -1:
# we want to check if the camera can see it now
tested_theta = np.array([
-0.13920875634155275, 0.9690923616394044, -0.05675728908691407,
-0.9046651686218262, 0.07133010655517578, 1.3683108612304689,
-0.1223349676940918
])
baxt_trans = np.array([0.656, 0.724, 0.374])
baxt_w = np.array([-3.096, 0.139, -2.389])
baxt_w_norm = baxt_w / np.linalg.norm(baxt_w)
baxt_theta = np.linalg.norm(baxt_w)
baxt_homo_transform = np.zeros((4, 4))
baxt_homo_transform[0:3, 3] = baxt_trans[0:3]
baxt_rot = skeleton.rotation_3d(baxt_w_norm, baxt_theta)
baxt_homo_transform[0:3, 0:3] = baxt_rot[0:3, 0:3]
baxt_homo_transform[3, 3] = 1
#print("What we think what Baxter thinks the homogenous transform is")
#print(baxt_homo_transform)
def task1(angles):
return skeleton.prod_exp(t, angles)
def task2(angles):
np_angles = np.array(angles)
print(np_angles)
