透過 Python 程式新增網誌文章

7月4號 -1

目標 : 推導 delta printer 的正向與逆向運動方程式, 弄懂之後, 寫網誌或整理成網頁內容

https://www.marginallyclever.com/other/samples/fk-ik-test.html

https://gist.github.com/hugs/4231272

圖源 : https://www.marginallyclever.com/other/samples/fk-ik-test.html

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119

# Original code from # http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ # License: MIT   import math   # Specific geometry for bitbeambot: # http://flic.kr/p/cYaQah e  =  26.0 f  =  69.0 re = 128.0 rf =  88.0   # Trigonometric constants s      = 165*2 sqrt3  = math.sqrt(3.0) pi     = 3.141592653 sin120 = sqrt3 / 2.0 cos120 = -0.5 tan60  = sqrt3 sin30  = 0.5 tan30  = 1.0 / sqrt3   # Forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) #   Returned {error code,theta1,theta2,theta3} def forward(theta1, theta2, theta3):     x0 = 0.0     y0 = 0.0     z0 = 0.0           t = (f-e) * tan30 / 2.0     dtr = pi / 180.0           theta1 *= dtr     theta2 *= dtr     theta3 *= dtr           y1 = -(t + rf*math.cos(theta1) )     z1 = -rf * math.sin(theta1)           y2 = (t + rf*math.cos(theta2)) * sin30     x2 = y2 * tan60     z2 = -rf * math.sin(theta2)           y3 = (t + rf*math.cos(theta3)) * sin30     x3 = -y3 * tan60     z3 = -rf * math.sin(theta3)           dnm = (y2-y1)*x3 - (y3-y1)*x2           w1 = y1*y1 + z1*z1     w2 = x2*x2 + y2*y2 + z2*z2     w3 = x3*x3 + y3*y3 + z3*z3           # x = (a1*z + b1)/dnm     a1 = (z2-z1)*(y3-y1) - (z3-z1)*(y2-y1)     b1= -( (w2-w1)*(y3-y1) - (w3-w1)*(y2-y1) ) / 2.0           # y = (a2*z + b2)/dnm     a2 = -(z2-z1)*x3 + (z3-z1)*x2     b2 = ( (w2-w1)*x3 - (w3-w1)*x2) / 2.0           # a*z^2 + b*z + c = 0     a = a1*a1 + a2*a2 + dnm*dnm     b = 2.0 * (a1*b1 + a2*(b2 - y1*dnm) - z1*dnm*dnm)     c = (b2 - y1*dnm)*(b2 - y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re)           # discriminant     d = b*b - 4.0*a*c     if d < 0.0:         return [1,0,0,0] # non-existing povar. return error,x,y,z           z0 = -0.5*(b + math.sqrt(d)) / a     x0 = (a1*z0 + b1) / dnm     y0 = (a2*z0 + b2) / dnm       return [0,x0,y0,z0]   # Inverse kinematics # Helper functions, calculates angle theta1 (for YZ-pane) def angle_yz(x0, y0, z0, theta=None):     y1 = -0.5*0.57735*f # f/2 * tg 30     y0 -= 0.5*0.57735*e # shift center to edge     # z = a + b*y     a = (x0*x0 + y0*y0 + z0*z0 + rf*rf - re*re - y1*y1) / (2.0*z0)     b = (y1-y0) / z0       # discriminant     d = -(a + b*y1)*(a + b*y1) + rf*(b*b*rf + rf)     if d<0:         return [1,0] # non-existing povar.  return error, theta       yj = (y1 - a*b - math.sqrt(d)) / (b*b + 1) # choosing outer povar     zj = a + b*yj     theta = math.atan(-zj / (y1-yj)) * 180.0 / pi + (180.0 if yj>y1 else 0.0)           return [0,theta] # return error, theta   def inverse(x0, y0, z0):     theta1 = 0     theta2 = 0     theta3 = 0     status = angle_yz(x0,y0,z0)       if status[0] == 0:         theta1 = status[1]         status = angle_yz(x0*cos120 + y0*sin120,                                    y0*cos120-x0*sin120,                                    z0,                                    theta2)     if status[0] == 0:         theta2 = status[1]         status = angle_yz(x0*cos120 - y0*sin120,                                    y0*cos120 + x0*sin120,                                    z0,                                    theta3)     theta3 = status[1]       return [status[0],theta1,theta2,theta3]

程式來源 : [gist.github.com/hugs/4231272