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 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