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 = robot206.rotation_3d(omega, theta)
p = np.reshape(trans, (-1, 1))
g_1 = np.hstack((R, p))
g = np.vstack((g_1, np.array([0,0,0,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)
"""
#YOUR CODE HERE
omega_hat = robot206.skew_3d(omega)
A_1 = np.dot((np.eye(3)-robot206.rotation_3d(omega, theta)),omega_hat)
A_2 = np.outer(omega, omega)*theta
A = A_1 + A_2
v = np.reshape(np.dot(np.linalg.inv(A),trans), (-1,1))
return v