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
