def joint_fk00(q): # pose = [0] * 16 # x0 = q[0] + (1 / 4) * math.pi x1 = math.cos(x0) x2 = math.sin(x0) # pose[0] = x1 pose[1] = 0 pose[2] = -x2 pose[3] = 0.069 * x1 pose[4] = x2 pose[5] = 0 pose[6] = x1 pose[7] = 0.069 * x2 pose[8] = 0 pose[9] = -1 pose[10] = 0 pose[11] = 0.270350000000000 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk01(q): # pose = [0] * 16 # x0 = math.sin(q[1]) x1 = q[0] + (1 / 4) * math.pi x2 = math.cos(x1) x3 = math.sin(x1) x4 = math.cos(q[1]) # pose[0] = -x0 * x2 pose[1] = -x3 pose[2] = x2 * x4 pose[3] = 0.069 * x2 pose[4] = -x0 * x3 pose[5] = x2 pose[6] = x3 * x4 pose[7] = 0.069 * x3 pose[8] = -x4 pose[9] = 0 pose[10] = -x0 pose[11] = 0.270350000000000 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk03(q): # pose = [0] * 16 # x0 = sin(q[3]) x1 = cos(q[1]) x2 = x0 * x1 x3 = q[0] + pi / 4 x4 = cos(x3) x5 = cos(q[3]) x6 = sin(q[2]) x7 = sin(x3) x8 = x6 * x7 x9 = sin(q[1]) x10 = cos(q[2]) x11 = x10 * x4 x12 = -x11 * x9 - x8 x13 = x10 * x7 x14 = x4 * x6 x15 = x1 * x5 x16 = 0.069 * x4 x17 = 0.069 * x7 x18 = 0.36435 * x1 x19 = x10 * x9 x20 = -x13 * x9 + x14 x21 = x1 * x10 # pose[0] = x12 * x5 - x2 * x4 pose[1] = -x13 + x14 * x9 pose[2] = x0 * x12 + x15 * x4 pose[3] = -x16 * x19 + x16 - x17 * x6 + x18 * x4 pose[4] = -x2 * x7 + x20 * x5 pose[5] = x11 + x8 * x9 pose[6] = x0 * x20 + x15 * x7 pose[7] = x16 * x6 - x17 * x19 + x17 + x18 * x7 pose[8] = x0 * x9 - x21 * x5 pose[9] = x1 * x6 pose[10] = -x0 * x21 - x5 * x9 pose[11] = -0.069 * x21 - 0.36435 * x9 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk03(q): # pose = [0] * 16 # x0 = math.sin(q[3]) x1 = q[0] + (1 / 4) * math.pi x2 = math.cos(x1) x3 = math.cos(q[1]) x4 = x2 * x3 x5 = math.cos(q[3]) x6 = math.sin(q[2]) x7 = math.sin(x1) x8 = x6 * x7 x9 = math.sin(q[1]) x10 = math.cos(q[2]) x11 = x10 * x2 x12 = -x11 * x9 - x8 x13 = x10 * x7 x14 = x2 * x6 x15 = 0.069 * x2 x16 = x3 * x7 x17 = x13 * x9 x18 = x14 - x17 x19 = x10 * x3 # pose[0] = -x0 * x4 + x12 * x5 pose[1] = -x13 + x14 * x9 pose[2] = x0 * x12 + x4 * x5 pose[3] = -x10 * x15 * x9 + x15 + 0.36435 * x4 - 0.069 * x8 pose[4] = -x0 * x16 + x18 * x5 pose[5] = x11 + x8 * x9 pose[6] = x0 * x18 + x16 * x5 pose[7] = x15 * x6 + 0.36435 * x16 - 0.069 * x17 + 0.069 * x7 pose[8] = x0 * x9 - x19 * x5 pose[9] = x3 * x6 pose[10] = -x0 * x19 - x5 * x9 pose[11] = -0.069 * x19 - 0.36435 * x9 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk02(q): # pose = [0] * 16 # x0 = sin(q[2]) x1 = q[0] + pi / 4 x2 = sin(x1) x3 = x0 * x2 x4 = sin(q[1]) x5 = cos(q[2]) x6 = cos(x1) x7 = x5 * x6 x8 = cos(q[1]) x9 = x6 * x8 x10 = x2 * x5 x11 = x0 * x6 x12 = 0.069 * x6 x13 = 0.069 * x2 x14 = x4 * x5 x15 = x2 * x8 x16 = x5 * x8 # pose[0] = -x3 - x4 * x7 pose[1] = -x9 pose[2] = -x10 + x11 * x4 pose[3] = -x0 * x13 - x12 * x14 + x12 + 0.36435 * x9 pose[4] = -x10 * x4 + x11 pose[5] = -x15 pose[6] = x3 * x4 + x7 pose[7] = x0 * x12 - x13 * x14 + x13 + 0.36435 * x15 pose[8] = -x16 pose[9] = x4 pose[10] = x0 * x8 pose[11] = -0.069 * x16 - 0.36435 * x4 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk02(q): # pose = [0] * 16 # x0 = math.sin(q[2]) x1 = q[0] + (1 / 4) * math.pi x2 = math.sin(x1) x3 = x0 * x2 x4 = math.sin(q[1]) x5 = math.cos(q[2]) x6 = math.cos(x1) x7 = x5 * x6 x8 = math.cos(q[1]) x9 = x6 * x8 x10 = x2 * x5 x11 = x0 * x6 x12 = 0.069 * x6 x13 = x10 * x4 x14 = x2 * x8 x15 = x5 * x8 # pose[0] = -x3 - x4 * x7 pose[1] = -x9 pose[2] = -x10 + x11 * x4 pose[3] = -x12 * x4 * x5 + x12 - 0.069 * x3 + 0.36435 * x9 pose[4] = x11 - x13 pose[5] = -x14 pose[6] = x3 * x4 + x7 pose[7] = x0 * x12 - 0.069 * x13 + 0.36435 * x14 + 0.069 * x2 pose[8] = -x15 pose[9] = x4 pose[10] = x0 * x8 pose[11] = -0.069 * x15 - 0.36435 * x4 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk06(q): # pose = [0] * 16 # x0 = math.sin(q[6]) x1 = math.cos(q[4]) x2 = math.cos(q[2]) x3 = q[0] + (1 / 4) * math.pi x4 = math.sin(x3) x5 = x2 * x4 x6 = math.sin(q[1]) x7 = math.sin(q[2]) x8 = math.cos(x3) x9 = x7 * x8 x10 = -x5 + x6 * x9 x11 = math.sin(q[4]) x12 = math.sin(q[3]) x13 = math.cos(q[1]) x14 = x13 * x8 x15 = math.cos(q[3]) x16 = x4 * x7 x17 = x2 * x8 x18 = -x16 - x17 * x6 x19 = -x12 * x14 + x15 * x18 x20 = x1 * x10 - x11 * x19 x21 = math.cos(q[6]) x22 = math.sin(q[5]) x23 = x14 * x15 x24 = x12 * x18 x25 = -x23 - x24 x26 = math.cos(q[5]) x27 = x10 * x11 x28 = x1 * x19 x29 = x27 + x28 x30 = x22 * x25 + x26 * x29 x31 = x25 * x26 x32 = x22 * x29 x33 = 0.069 * x8 x34 = x16 * x6 + x17 x35 = x13 * x4 x36 = x5 * x6 x37 = -x36 + x9 x38 = -x12 * x35 + x15 * x37 x39 = x1 * x34 - x11 * x38 x40 = x15 * x35 x41 = x12 * x37 x42 = -x40 - x41 x43 = x11 * x34 x44 = x1 * x38 x45 = x43 + x44 x46 = x22 * x42 + x26 * x45 x47 = x26 * x42 x48 = x22 * x45 x49 = x13 * x7 x50 = x13 * x2 x51 = x12 * x6 - x15 * x50 x52 = x1 * x49 - x11 * x51 x53 = x15 * x6 x54 = x12 * x50 x55 = x53 + x54 x56 = x11 * x49 x57 = x1 * x51 x58 = x56 + x57 x59 = x22 * x55 + x26 * x58 x60 = x26 * x55 x61 = x22 * x58 # pose[0] = x0 * x20 + x21 * x30 pose[1] = -x0 * x30 + x20 * x21 pose[2] = -x31 + x32 pose[ 3] = 0.36435 * x14 - 0.069 * x16 - x2 * x33 * x6 + 0.37429 * x23 + 0.37429 * x24 + 0.01 * x27 + 0.01 * x28 - 0.229525 * x31 + 0.229525 * x32 + x33 pose[4] = x0 * x39 + x21 * x46 pose[5] = -x0 * x46 + x21 * x39 pose[6] = -x47 + x48 pose[ 7] = x33 * x7 + 0.36435 * x35 - 0.069 * x36 + 0.069 * x4 + 0.37429 * x40 + 0.37429 * x41 + 0.01 * x43 + 0.01 * x44 - 0.229525 * x47 + 0.229525 * x48 pose[8] = x0 * x52 + x21 * x59 pose[9] = -x0 * x59 + x21 * x52 pose[10] = -x60 + x61 pose[ 11] = -0.069 * x50 - 0.37429 * x53 - 0.37429 * x54 + 0.01 * x56 + 0.01 * x57 - 0.36435 * x6 - 0.229525 * x60 + 0.229525 * x61 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk05(q): # pose = [0] * 16 # x0 = math.sin(q[5]) x1 = math.cos(q[3]) x2 = q[0] + (1 / 4) * math.pi x3 = math.cos(x2) x4 = math.cos(q[1]) x5 = x3 * x4 x6 = x1 * x5 x7 = math.sin(q[3]) x8 = math.sin(q[2]) x9 = math.sin(x2) x10 = x8 * x9 x11 = math.sin(q[1]) x12 = math.cos(q[2]) x13 = x12 * x3 x14 = -x10 - x11 * x13 x15 = x14 * x7 x16 = -x15 - x6 x17 = math.cos(q[5]) x18 = math.sin(q[4]) x19 = x12 * x9 x20 = x3 * x8 x21 = x11 * x20 - x19 x22 = x18 * x21 x23 = math.cos(q[4]) x24 = x1 * x14 - x5 * x7 x25 = x23 * x24 x26 = x22 + x25 x27 = 0.069 * x3 x28 = x4 * x9 x29 = x1 * x28 x30 = x11 * x19 x31 = x20 - x30 x32 = x31 * x7 x33 = -x29 - x32 x34 = x10 * x11 + x13 x35 = x18 * x34 x36 = x1 * x31 - x28 * x7 x37 = x23 * x36 x38 = x35 + x37 x39 = x1 * x11 x40 = x12 * x4 x41 = x40 * x7 x42 = x39 + x41 x43 = x4 * x8 x44 = x18 * x43 x45 = -x1 * x40 + x11 * x7 x46 = x23 * x45 x47 = x44 + x46 # pose[0] = x0 * x16 + x17 * x26 pose[1] = -x18 * x24 + x21 * x23 pose[2] = x0 * x26 - x16 * x17 pose[ 3] = -0.069 * x10 - x11 * x12 * x27 + 0.37429 * x15 + 0.01 * x22 + 0.01 * x25 + x27 + 0.36435 * x5 + 0.37429 * x6 pose[4] = x0 * x33 + x17 * x38 pose[5] = -x18 * x36 + x23 * x34 pose[6] = x0 * x38 - x17 * x33 pose[ 7] = x27 * x8 + 0.36435 * x28 + 0.37429 * x29 - 0.069 * x30 + 0.37429 * x32 + 0.01 * x35 + 0.01 * x37 + 0.069 * x9 pose[8] = x0 * x42 + x17 * x47 pose[9] = -x18 * x45 + x23 * x43 pose[10] = x0 * x47 - x17 * x42 pose[ 11] = -0.36435 * x11 - 0.37429 * x39 - 0.069 * x40 - 0.37429 * x41 + 0.01 * x44 + 0.01 * x46 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk04(q): # pose = [0] * 16 # x0 = math.sin(q[4]) x1 = math.cos(q[2]) x2 = q[0] + (1 / 4) * math.pi x3 = math.sin(x2) x4 = x1 * x3 x5 = math.sin(q[1]) x6 = math.sin(q[2]) x7 = math.cos(x2) x8 = x6 * x7 x9 = -x4 + x5 * x8 x10 = x0 * x9 x11 = math.cos(q[4]) x12 = math.sin(q[3]) x13 = math.cos(q[1]) x14 = x13 * x7 x15 = math.cos(q[3]) x16 = x3 * x6 x17 = x1 * x7 x18 = -x16 - x17 * x5 x19 = -x12 * x14 + x15 * x18 x20 = x11 * x19 x21 = x14 * x15 x22 = x12 * x18 x23 = 0.069 * x7 x24 = x16 * x5 + x17 x25 = x0 * x24 x26 = x13 * x3 x27 = x4 * x5 x28 = -x27 + x8 x29 = -x12 * x26 + x15 * x28 x30 = x11 * x29 x31 = x15 * x26 x32 = x12 * x28 x33 = x13 * x6 x34 = x0 * x33 x35 = x1 * x13 x36 = x12 * x5 - x15 * x35 x37 = x11 * x36 x38 = x15 * x5 x39 = x12 * x35 # pose[0] = x10 + x20 pose[1] = -x21 - x22 pose[2] = -x0 * x19 + x11 * x9 pose[ 3] = -x1 * x23 * x5 + 0.01 * x10 + 0.36435 * x14 - 0.069 * x16 + 0.01 * x20 + 0.37429 * x21 + 0.37429 * x22 + x23 pose[4] = x25 + x30 pose[5] = -x31 - x32 pose[6] = -x0 * x29 + x11 * x24 pose[ 7] = x23 * x6 + 0.01 * x25 + 0.36435 * x26 - 0.069 * x27 + 0.069 * x3 + 0.01 * x30 + 0.37429 * x31 + 0.37429 * x32 pose[8] = x34 + x37 pose[9] = x38 + x39 pose[10] = -x0 * x36 + x11 * x33 pose[ 11] = 0.01 * x34 - 0.069 * x35 + 0.01 * x37 - 0.37429 * x38 - 0.37429 * x39 - 0.36435 * x5 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk06(q): # pose = [0] * 16 # x0 = sin(q[6]) x1 = cos(q[4]) x2 = cos(q[2]) x3 = q[0] + pi / 4 x4 = sin(x3) x5 = x2 * x4 x6 = sin(q[1]) x7 = sin(q[2]) x8 = cos(x3) x9 = x7 * x8 x10 = -x5 + x6 * x9 x11 = sin(q[4]) x12 = sin(q[3]) x13 = cos(q[1]) x14 = x12 * x13 x15 = cos(q[3]) x16 = x4 * x7 x17 = x2 * x8 x18 = -x16 - x17 * x6 x19 = -x14 * x8 + x15 * x18 x20 = x1 * x10 - x11 * x19 x21 = cos(q[6]) x22 = sin(q[5]) x23 = x13 * x15 x24 = x23 * x8 x25 = x12 * x18 x26 = -x24 - x25 x27 = cos(q[5]) x28 = x10 * x11 x29 = x1 * x19 x30 = x28 + x29 x31 = x22 * x26 + x27 * x30 x32 = x26 * x27 x33 = x22 * x30 x34 = 0.069 * x8 x35 = 0.069 * x4 x36 = 0.36435 * x13 x37 = x2 * x6 x38 = x16 * x6 + x17 x39 = -x5 * x6 + x9 x40 = -x14 * x4 + x15 * x39 x41 = x1 * x38 - x11 * x40 x42 = x23 * x4 x43 = x12 * x39 x44 = -x42 - x43 x45 = x11 * x38 x46 = x1 * x40 x47 = x45 + x46 x48 = x22 * x44 + x27 * x47 x49 = x27 * x44 x50 = x22 * x47 x51 = x13 * x7 x52 = x13 * x2 x53 = x12 * x6 - x15 * x52 x54 = x1 * x51 - x11 * x53 x55 = x15 * x6 x56 = x12 * x52 x57 = x55 + x56 x58 = x11 * x51 x59 = x1 * x53 x60 = x58 + x59 x61 = x22 * x57 + x27 * x60 x62 = x27 * x57 x63 = x22 * x60 # pose[0] = x0 * x20 + x21 * x31 pose[1] = -x0 * x31 + x20 * x21 pose[2] = -x32 + x33 pose[ 3] = 0.37429 * x24 + 0.37429 * x25 + 0.01 * x28 + 0.01 * x29 - 0.229525 * x32 + 0.229525 * x33 - x34 * x37 + x34 - x35 * x7 + x36 * x8 pose[4] = x0 * x41 + x21 * x48 pose[5] = -x0 * x48 + x21 * x41 pose[6] = -x49 + x50 pose[ 7] = x34 * x7 - x35 * x37 + x35 + x36 * x4 + 0.37429 * x42 + 0.37429 * x43 + 0.01 * x45 + 0.01 * x46 - 0.229525 * x49 + 0.229525 * x50 pose[8] = x0 * x54 + x21 * x61 pose[9] = -x0 * x61 + x21 * x54 pose[10] = -x62 + x63 pose[ 11] = -0.069 * x52 - 0.37429 * x55 - 0.37429 * x56 + 0.01 * x58 + 0.01 * x59 - 0.36435 * x6 - 0.229525 * x62 + 0.229525 * x63 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk05(q): # pose = [0] * 16 # x0 = sin(q[5]) x1 = cos(q[1]) x2 = cos(q[3]) x3 = x1 * x2 x4 = q[0] + pi / 4 x5 = cos(x4) x6 = x3 * x5 x7 = sin(q[3]) x8 = sin(q[2]) x9 = sin(x4) x10 = x8 * x9 x11 = sin(q[1]) x12 = cos(q[2]) x13 = x12 * x5 x14 = -x10 - x11 * x13 x15 = x14 * x7 x16 = -x15 - x6 x17 = cos(q[5]) x18 = sin(q[4]) x19 = x12 * x9 x20 = x5 * x8 x21 = x11 * x20 - x19 x22 = x18 * x21 x23 = cos(q[4]) x24 = x1 * x7 x25 = x14 * x2 - x24 * x5 x26 = x23 * x25 x27 = x22 + x26 x28 = 0.069 * x5 x29 = 0.069 * x9 x30 = 0.36435 * x1 x31 = x11 * x12 x32 = x3 * x9 x33 = -x11 * x19 + x20 x34 = x33 * x7 x35 = -x32 - x34 x36 = x10 * x11 + x13 x37 = x18 * x36 x38 = x2 * x33 - x24 * x9 x39 = x23 * x38 x40 = x37 + x39 x41 = x11 * x2 x42 = x1 * x12 x43 = x42 * x7 x44 = x41 + x43 x45 = x1 * x8 x46 = x18 * x45 x47 = x11 * x7 - x2 * x42 x48 = x23 * x47 x49 = x46 + x48 # pose[0] = x0 * x16 + x17 * x27 pose[1] = -x18 * x25 + x21 * x23 pose[2] = x0 * x27 - x16 * x17 pose[ 3] = 0.37429 * x15 + 0.01 * x22 + 0.01 * x26 - x28 * x31 + x28 - x29 * x8 + x30 * x5 + 0.37429 * x6 pose[4] = x0 * x35 + x17 * x40 pose[5] = -x18 * x38 + x23 * x36 pose[6] = x0 * x40 - x17 * x35 pose[ 7] = x28 * x8 - x29 * x31 + x29 + x30 * x9 + 0.37429 * x32 + 0.37429 * x34 + 0.01 * x37 + 0.01 * x39 pose[8] = x0 * x44 + x17 * x49 pose[9] = -x18 * x47 + x23 * x45 pose[10] = x0 * x49 - x17 * x44 pose[ 11] = -0.36435 * x11 - 0.37429 * x41 - 0.069 * x42 - 0.37429 * x43 + 0.01 * x46 + 0.01 * x48 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose
def joint_fk04(q): # pose = [0] * 16 # x0 = sin(q[4]) x1 = cos(q[2]) x2 = q[0] + pi / 4 x3 = sin(x2) x4 = x1 * x3 x5 = sin(q[1]) x6 = sin(q[2]) x7 = cos(x2) x8 = x6 * x7 x9 = -x4 + x5 * x8 x10 = x0 * x9 x11 = cos(q[4]) x12 = sin(q[3]) x13 = cos(q[1]) x14 = x12 * x13 x15 = cos(q[3]) x16 = x3 * x6 x17 = x1 * x7 x18 = -x16 - x17 * x5 x19 = -x14 * x7 + x15 * x18 x20 = x11 * x19 x21 = x13 * x15 x22 = x21 * x7 x23 = x12 * x18 x24 = 0.069 * x7 x25 = 0.069 * x3 x26 = 0.36435 * x13 x27 = x1 * x5 x28 = x16 * x5 + x17 x29 = x0 * x28 x30 = -x4 * x5 + x8 x31 = -x14 * x3 + x15 * x30 x32 = x11 * x31 x33 = x21 * x3 x34 = x12 * x30 x35 = x13 * x6 x36 = x0 * x35 x37 = x1 * x13 x38 = x12 * x5 - x15 * x37 x39 = x11 * x38 x40 = x15 * x5 x41 = x12 * x37 # pose[0] = x10 + x20 pose[1] = -x22 - x23 pose[2] = -x0 * x19 + x11 * x9 pose[ 3] = 0.01 * x10 + 0.01 * x20 + 0.37429 * x22 + 0.37429 * x23 - x24 * x27 + x24 - x25 * x6 + x26 * x7 pose[4] = x29 + x32 pose[5] = -x33 - x34 pose[6] = -x0 * x31 + x11 * x28 pose[ 7] = x24 * x6 - x25 * x27 + x25 + x26 * x3 + 0.01 * x29 + 0.01 * x32 + 0.37429 * x33 + 0.37429 * x34 pose[8] = x36 + x39 pose[9] = x40 + x41 pose[10] = -x0 * x38 + x11 * x35 pose[ 11] = 0.01 * x36 - 0.069 * x37 + 0.01 * x39 - 0.37429 * x40 - 0.37429 * x41 - 0.36435 * x5 + 0.27035 pose[12] = 0 pose[13] = 0 pose[14] = 0 pose[15] = 1 # pose = offset_and_reshape(pose, 0.0640273121782, 0.259027312178, 0.129626) return pose