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]
R = np.array([[0.0076, 0.0001, -1.0000],
[-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
gst0 = np.vstack((np.hstack((R, np.array([q[:,7]]).T)), [0, 0, 0, 1]))
xi = np.vstack((-np.cross(q[:, :7].T, w.T).T, w))
g = kfs.prod_exp(xi, thetas).dot(gst0)
return g
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
# YOUR CODE HERE
twistMats = np.zeros((6, 7))
for i in range(7):
twistMats[:3, i] = np.cross(-w[:3, i], q[:3, i])
twistMats[3:, i] = w[:3, i]
gst_zero = np.eye(4)
gst_zero[:3, :3] = R
gst_zero[:3, 3] = q[:3, 7]
return np.matmul(kfs.prod_exp(twistMats, thetas), gst_zero)
def lab3():
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]
R = np.array([[0.0076, 0.0001, -1.0000],[-0.7040, 0.7102, -0.0053],[0.7102, 0.7040, 0.0055]]).T
t = np.zeros((6,7))
for i in range(0,7):
t[:,i] = twist(q[0:3,i],w[0:3,i])
theta = np.array([0,0,0,0,0,0])
trans = kfs.prod_exp(t,theta)
twist_arm = np.ndarray((4,4))
twist_arm[0:3,0:3] = R
twist_arm[0:3,3] = q[0:3,7]
twist_arm[3,3] = 1
trans = np.dot(trans,twist_arm)
print(trans)
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 callback(message):
#Print the contents of the message to the console
#print(rospy.get_name() + ": I heard %s" % message.data)
joint_angle = [
message.position[4], message.position[5], message.position[2],
message.position[3], message.position[6], message.position[7],
message.position[8]
]
print("Left Arm Position:\n")
print(joint_angle)
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
# YOUR CODE HERE
#base = np.array([0,0,0])
#g_st(0)
g_st0 = np.ndarray((4, 4))
g_st0[0:3, 0:3] = R
g_st0[3, 0:3] = np.array([0, 0, 0])
g_st0[0:3, 3] = q[0:3, 7]
g_st0[3, 3] = 1
#twists
xi = np.ndarray((6, 7))
for i in range(0, 7):
xi[0:3, i] = np.cross(-w[0:3, i], q[0:3, i])
xi[3:6, i] = w[0:3, i]
xi_hat = kfs.prod_exp(xi, joint_angle)
g = np.matmul(xi_hat, g_st0)
print("Transformation matrix:\n")
print(g)
def gst(theta, gst0, w, q):
#theta = array of 7 joint angles
multiplication = 1
xi_array = []
for i in range(len(theta)):
q_i = q[0:3, i]
w_i = w[0:3, i]
qw_cross = np.cross(-(w_i), q_i)
xi_i = np.vstack((qw_cross.reshape(3, 1), w_i.reshape(3, 1)))
xi_array.append(xi_i[:, 0])
xi_array = np.array(xi_array)
#print("xi_array.shape")
#print(xi_array.shape)
return np.dot(kfs.prod_exp(xi_array.T, theta), gst0)
def callback(message):
#Print the contents of the message to the console
# left arm angles
theta = [
message.position[4], message.position[5], message.position[2],
message.position[3], message.position[6], message.position[7],
message.position[8]
]
print('Angles list (left hand)\n')
print(theta)
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
t = np.zeros((6, 7))
for i in range(0, 7):
t[:, i] = twist(q[0:3, i], w[0:3, i])
trans = kfs.prod_exp(t, theta)
twist_arm = np.ndarray((4, 4))
twist_arm[0:3, 0:3] = R
twist_arm[0:3, 3] = q[0:3, 7]
twist_arm[3, 3] = 1
trans = np.dot(trans, twist_arm)
print(trans)
rotation = trans[0:3, 0:3]
print('Calculated RPY angles')
print rotationMatrixToEulerAngles(rotation)
def lab3(theta):
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
xi = np.ndarray((6, 7))
v_1 = -1 * np.cross(w[0:3, 0], q[0:3, 0])
v_2 = -1 * np.cross(w[0:3, 1], q[0:3, 1])
v_3 = -1 * np.cross(w[0:3, 2], q[0:3, 2])
v_4 = -1 * np.cross(w[0:3, 3], q[0:3, 3])
v_5 = -1 * np.cross(w[0:3, 4], q[0:3, 4])
v_6 = -1 * np.cross(w[0:3, 5], q[0:3, 5])
v_7 = -1 * np.cross(w[0:3, 6], q[0:3, 6])
xi[0:6, 0] = np.concatenate((v_1, w[0:3, 0]))
xi[0:6, 1] = np.concatenate((v_2, w[0:3, 1]))
xi[0:6, 2] = np.concatenate((v_3, w[0:3, 2]))
xi[0:6, 3] = np.concatenate((v_4, w[0:3, 3]))
xi[0:6, 4] = np.concatenate((v_5, w[0:3, 4]))
xi[0:6, 5] = np.concatenate((v_6, w[0:3, 5]))
xi[0:6, 6] = np.concatenate((v_7, w[0:3, 6]))
g_1 = kfs.prod_exp(xi, theta)
q = np.array([[0.7957], [0.9965], [0.3058]])
h = np.hstack((R, q))
g = np.vstack((h, np.array([0, 0, 0, 1])))
return np.dot(g_1, g)
def fk_helper(theta):
"""
Runs the forward kinematics of the UR5
Args:
theta - (4,) ndarray: Desired joint states
Returns:
g_d - (4,4) ndarray: output configuration
"""
xi = make_xi()
g_0 = make_g_0()
g_we = kfs.prod_exp(xi, theta)
g_d = np.dot(g_we, g_0)
return g_d
def fk(theta):
"""
Tests the forward kinematics of the UR5
Args:
theta - (5,) ndarray: Desired joint states
Returns:
g_d - output position
"""
xi = make_xi()
g_0 = make_g_0()
g_we = kfs.prod_exp(xi, theta)
g_d = np.dot(g_we, g_0)
return g_d
def lab3():
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
# YOUR CODE HERE
base = np.array([0, 0, 0])
#g_st(0)
g_st0 = np.ndarray((4, 4))
g_st0[0:3, 0:3] = R
g_st0[3, 0:3] = np.array([0, 0, 0])
g_st0[0:3, 3] = q[0:3, 7]
g_st0[3, 3] = 1
#twists
xi = np.ndarray(6, 7)
for i in range(0, 7):
xi[0:3, i] = np.cross(-w[0:3, i], q[0:3, i])
xi[3:6, i] = w[0:3, i]
xi_hat = kfs.prod_exp(xi, base)
g = np.matmul(xi_hat, g_st0)
return g
def scara_fk(theta):
"""
An example implementation of a forward kinematics map.
Feel free to use this as a template for your own implementations
in this file.
This function implements the forward kinematics map of the
SCARA manipulator, following Example 3.1 from MLS (page 87).
We take L0 = L1 = L2 = 1
Arguments:
theta: numpy.ndarray of size (4,), the values of the joint angles.
theta[i] is the value of the ith joint, at which the
FK map should be computed.
Returns:
- g (numpy.ndarray of shape (4,4)): the 4x4 configuration of the
end effector when the joints have been placed at the angles
specified in theta.
- xi_array (numpy.ndarray of shape (6, N)): an array with the twists
stacked in its columns. This is used by the autograder to assign
partial credit.
"""
# Specify all twists.
xi_1 = [0, 0, 0, 0, 0, 1]
xi_2 = [1, 0, 0, 0, 0, 1]
xi_3 = [2, 0, 0, 0, 0, 1]
xi_4 = [0, 0, 1, 0, 0, 0]
# Specify end effector configuration at theta = 0.
gst0 = np.array([[1, 0, 0, 0], [0, 1, 0, 2], [0, 0, 1, 1], [0, 0, 0, 1]],
dtype=np.float64)
# Stack twists into an array that prod_exp can accept.
xi_array = np.array([xi_1, xi_2, xi_3, xi_4], dtype=np.float64).T
# Use product of exponentials formula to compute forward kinematics.
g = np.matmul(prod_exp(xi_array, theta), gst0)
# Return the required quantities.
return g, xi_array
def lab3(theta):
q = np.ndarray((3, 8))
w = np.ndarray((3, 7))
#print w
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]
R = np.array([[0.0076, 0.0001, -1.0000], [-0.7040, 0.7102, -0.0053],
[0.7102, 0.7040, 0.0055]]).T
# YOUR CODE HERE
gst_0 = np.ndarray((4, 4))
gst_0[0:3, 0:3] = R
gst_0[0:3, 3] = np.transpose(q[0:3, 7])
gst_0[3][3] = 1
#print "gst_", gst_0
xi = np.ndarray((6, 7))
for i in range(0, 7):
xi[0:3, i] = -np.cross(w[0:3, i], q[0:3, i])
xi[3:6, i] = w[0:3, i]
# print "xi", i,xi[0:6, i]
g = kfs.prod_exp(xi, theta)
g_final = g.dot(gst_0)
return g_final
def forward_kinematics_map(theta): gst0 = np.array([[1, 0, 0, 0.7957], [0, 1, 0, 0.9965], [0, 0, 1, 0.3058], [0, 0, 0, 1]]) w1 = np.array([-0.0059,0.0113,0.9999]).T q1 = np.array([0.0635,0.2598,0.1188]).T w2 = np.array([-0.7077,0.7065,-0.0122]).T q2 = np.array([0.1106,0.3116,0.3885]).T w3 = np.array([0.7065,0.7077,-0.0038]).T q3 = np.array([0.1827,0.3838,0.3881]).T w4 = np.array([-0.7077,0.7065,-0.0122]).T q4 = np.array([0.3682,0.5684,0.3181]).T w5 = np.array([0.7065,0.7077,-0.0038]).T q5 = np.array([0.4417,0.6420,0.3177]).T w6 = np.array([-0.7077,0.7065,-0.0122]).T q6 = np.array([0.6332,0.8337,0.3067]).T w7 = np.array([0.7065,0.7077,-0.0038]).T q7 = np.array([0.7152,0.9158,0.3063]).T t1 = np.zeros((6, 1)) temp = np.cross(w1, q1) for i in range(3): t1[i,0] = -temp[i] t1[i+3,0] = w1[i] t2 = np.zeros((6, 1)) temp = np.cross(w2, q2) for i in range(3): t2[i,0] = -temp[i] t2[i+3,0] = w2[i] t3 = np.zeros((6, 1)) temp = np.cross(w3, q3) for i in range(3): t3[i,0] = -temp[i] t3[i+3,0] = w3[i] t4 = np.zeros((6, 1)) temp = np.cross(w4, q4) for i in range(3): t4[i,0] = -temp[i] t4[i+3,0] = w4[i] t5 = np.zeros((6, 1)) temp = np.cross(w5, q5) for i in range(3): t5[i,0] = -temp[i] t5[i+3,0] = w5[i] t6 = np.zeros((6, 1)) temp = np.cross(w6, q6) for i in range(3): t6[i,0] = -temp[i] t6[i+3,0] = w6[i] t7 = np.zeros((6, 1)) temp = np.cross(w7, q7) for i in range(3): t7[i,0] = -temp[i] t7[i+3,0] = w7[i] twists = np.array([t1.T[0], t2.T[0], t3.T[0], t4.T[0], t5.T[0], t6.T[0], t7.T[0]]).T return np.dot(kfs.prod_exp(twists, theta), gst0)
def task1(angles):
return skeleton.prod_exp(t, angles)
def task1(angles):
return skeleton.prod_exp(t, angles)
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
x = float(raw_input('Enter x coordinate'))
y = float(raw_input('Enter y coordinate'))
z = float(raw_input('Enter z coordinate'))
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = x
request.ik_request.pose_stamped.pose.position.y = y
request.ik_request.pose_stamped.pose.position.z = z
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
s0 = float(raw_input("s0"))
s1 = float(raw_input("s1"))
e0 = float(raw_input("e0"))
e1 = float(raw_input("e1"))
w1 = float(raw_input("w1"))
w2 = float(raw_input("w2"))
w3 = float(raw_input("w3"))
theta = [s0, s1, e0, e1, w1, w2, w3]
print('Angles list (left hand)\n')
print(theta)
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]
R = np.array([[0.0076, 0.0001, -1.0000],[-0.7040, 0.7102, -0.0053],[0.7102, 0.7040, 0.0055]]).T
t = np.zeros((6,7))
for i in range(0,7):
t[:,i] = twist(q[0:3,i],w[0:3,i])
trans = kfs.prod_exp(t,theta)
twist_arm = np.ndarray((4,4))
twist_arm[0:3,0:3] = R
twist_arm[0:3,3] = q[0:3,7]
twist_arm[3,3] = 1
trans = np.dot(trans,twist_arm)
print(trans)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
