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
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 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
#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 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 lab3(thetas):
q = np.ndarray((3, 8))
w = np.ndarray((3, 7))
q[0:3, 0] = [0.0635, 0.2598, 0.1188]
q[0:3, 1] = [0.1106, 0.3116, 0.3885]
q[0:3, 2] = [0.1827, 0.3838, 0.3881]
q[0:3, 3] = [0.3682, 0.5684, 0.3181]
q[0:3, 4] = [0.4417, 0.6420, 0.3177]
q[0:3, 5] = [0.6332, 0.8337, 0.3067]
q[0:3, 6] = [0.7152, 0.9158, 0.3063]
q[0:3, 7] = [0.7957, 0.9965, 0.3058]
w[0:3, 0] = [-0.0059, 0.0113, 0.9999]
w[0:3, 1] = [-0.7077, 0.7065, -0.0122]
w[0:3, 2] = [0.7065, 0.7077, -0.0038]
w[0:3, 3] = [-0.7077, 0.7065, -0.0122]
w[0:3, 4] = [0.7065, 0.7077, -0.0038]
w[0:3, 5] = [-0.7077, 0.7065, -0.0122]
w[0:3, 6] = [0.7065, 0.7077, -0.0038]
w0xq0 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][0], w[1][0], w[2][0]])),
np.array([q[0][0], q[1][0], q[2][0]]))
xi_0 = [w0xq0[0], w0xq0[1], w0xq0[2], w[0][0], w[1][0], w[2][0]]
w1xq1 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][1], w[1][1], w[2][1]])),
np.array([q[0][1], q[1][1], q[2][1]]))
xi_1 = [w1xq1[0], w1xq1[1], w1xq1[2], w[0][1], w[1][1], w[2][1]]
w2xq2 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][2], w[1][2], w[2][2]])),
np.array([q[0][2], q[1][2], q[2][2]]))
xi_2 = [w2xq2[0], w2xq2[1], w2xq2[2], w[0][2], w[1][1], w[2][2]]
w3xq3 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][3], w[1][3], w[2][3]])),
np.array([q[0][3], q[1][3], q[2][3]]))
xi_3 = [w3xq3[0], w3xq3[1], w3xq3[2], w[0][3], w[1][1], w[2][3]]
w4xq4 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][4], w[1][4], w[2][4]])),
np.array([q[0][4], q[1][4], q[2][4]]))
xi_4 = [w4xq4[0], w4xq4[1], w4xq4[2], w[0][4], w[1][1], w[2][4]]
w5xq5 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][5], w[1][5], w[2][5]])),
np.array([q[0][5], q[1][5], q[2][5]]))
xi_5 = [w5xq5[0], w5xq5[1], w5xq5[2], w[0][5], w[1][1], w[2][5]]
w6xq6 = -1 * np.matmul(kfs.skew_3d(np.array([w[0][6], w[1][6], w[2][6]])),
np.array([q[0][6], q[1][6], q[2][6]]))
xi_6 = [w6xq6[0], w6xq6[1], w6xq6[2], w[0][6], w[1][1], w[2][6]]
xi_array = np.array([xi_0, xi_1, xi_2, xi_3, xi_4, xi_5, xi_6],
dtype=np.float64).T
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
gOfZero = np.array([[R[0][0], R[0][1], R[0][2], q[0][7]],
[R[1][0], R[1][1], R[1][2], q[1][7]],
[R[2][0], R[2][1], R[2][2], q[2][7]], [0, 0, 0, 1]])
g = np.matmul(kfs.prod_exp(xi_array, thetas), gOfZero)
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 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
w_hat = kfs.skew_3d(omega)
omg = omega.tolist()
omg = np.array([[omg[0]], [omg[1]], [omg[2]]])
A = (np.eye(3)-linalg.expm(w_hat*theta)).dot(w_hat) + (omg.dot(omg.T))*theta
v = ((linalg.inv(A)).dot(trans)).tolist()
v = np.array([[v[0]], [v[1]], [v[2]]])
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
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 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
