2013.09.21 17:56 1 (Forward kinematics) 1.1 2 (Global frame) G r (X, Y, Z) (Local frame) L r (x, y, z) 1 X Y, Z X Y, Z 1 ( ) ( ) 1.2 (Joint rotation) 3 P G r, L r G r L r Z α : G r Q L Z,α r (1) 1
G r X Y, Z L r x cos α sin α 0 y, Q Z,α sin α cos α 0 (2) z 0 0 1 2 Y β X γ G r Q Y,β L r (3) G r Q X,γ L r (4) cos β 0 sin β Q Y,β 0 1 0 (5) sin β 0 cos β Q X,γ 1 0 0 0 cos γ sin γ (6) 0 sin γ cos γ 2 Z (A) (B) Z L r Q 1 Z,α r (7) L r Q 1 Y,β r (8) L r Q 1 X,γ r (9) Q 1, Q 2,, Q n G r G Q L L r (10) G Q L Q n Q 2 Q 1 (11) G Q L 2
1 L r 2 X 30 Y 45 X Y 3 G r X Y Q Y,45 Q L X,30 r 0.53 0.84 0.13 0.0 0.15 0.99 1 2 0.76 3.27 0.85 0.52 0.081 3 1.64 (12) G r Y X Q X,30 Q L Y,45 r 0.53 0.0 0.85 0.84 0.15 0.52 1 2 3.08 1.02 0.13 0.99 0.081 3 1.86 (13) 1.3 - - X (Roll) Y (Pitch) Z (Yaw) - - G Q L Q Z,γ Q Y,β Q X,α TODO: p44 α tan 1 r32 r 33 β sin 1 (r 31 ) γ tan 1 r21 ( cos β 0) r 11 (14) (15) 1.4 3 L r G d G R L : G r G R L L r + G d (16) G R L 3 3 G r, L r, G d 3 3
1.5 : G r G T L L r (17) r 11 r 12 r 13 X 0 G T L r 21 r 22 r 23 Y 0 r 31 r 32 r 33 Z 0 0 0 0 1 ( G ) R G L d 0 1 X x X 0 G r Y Z, L r y z, G d Y 0 Z 0 1 1 1 (18) G T L (Homogeneous transformation matrix) 4 4 1.6 1.7 Denavit-Hartenberg (DH) n n + 1 (Base link) 0 n 1 n i + 1 i L 1 DH?? 1. z i i + 1 2. x i z i 1, z i 3. y i y i z i x i 1.8 Z 2 *1 *1 4
3 2 DH 1 1.8 DH a i α i d i θ i 1 l 1 0 0 θ 1 2 l 2 0 0 θ 2 B i B i 1 cos θ 2 sin θ 2 0 l2 cos θ 2 1 T 2 sin θ 2 cos θ 2 0 l2 sin θ 2 0 0 1 0 0 0 0 1 cos θ 1 sin θ 1 0 l1 cos θ 1 0 T 1 sin θ 1 cos θ 1 0 l1 sin θ 1 0 0 1 0 0 0 0 1 (19) cos(θ 1 + θ 2 ) sin(θ 1 + θ 2 ) 0 l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) 0 T 2 sin(θ 1 + θ 2 ) cos(θ 1 + θ 2 ) 0 l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) 0 0 1 0 (20) 0 0 0 1 1.8 1: 3 1 3 5
3 B 3 θ 3 l 3 2 T 3 0 T 3 0 B 3 3 r 0 0 0 r 1 2 T 3 0 T 3 0 r 1.9 Nao Nao 1 #!/usr/local/bin/python 2 # -*- coding: utf-8 -*- 3 4 import sys 5 from naoqi import ALProxy 6 7 HOST "localhost" 8 PORT 9559 9 10 def main(): 11 motionproxy ALProxy("ALMotion", HOST, PORT) 12 13 motionproxy.wakeup() 14 15 """ 16 17 """ 18 19 motionproxy.rest() 20 21 if name " main ": 22 main() Webots for Nao Nao PC Nao ALProxy localhost 9559 6
Nao ALMotion 1 motionproxy ALProxy("ALMotion", HOST, PORT) Nao wakeup() Nao rest()) StandInit 1 #!/usr/bin/python 2 3 import sys 4 from naoqi import ALProxy 5 6 postureproxy ALProxy("ALRobotPosture", "localhost", 9559) 7 8 postureproxy.gotoposture("standinit", 1.0) ALMotion ALRobotPosture 1 motionproxy postureproxy Nao StandZero 1 #!/usr/local/bin/python 2 # -*- coding: utf-8 -*- 3 4 import sys 5 from naoqi import ALProxy 6 import time 7 import math 8 9 HOST "localhost" 10 PORT 9559 11 12 def main(): 13 motionproxy ALProxy("ALMotion", HOST, PORT) 14 15 motionproxy.wakeup() 16 17 postureproxy ALProxy("ALRobotPosture", "localhost", 9559) 18 postureproxy.gotoposture("standzero", 1.0) 19 20 motionproxy.setangles("headyaw", -90.0*(2*math.pi/360.0), 1.0) 21 time.sleep(1.0) 22 motionproxy.setangles("headyaw", 90.0*(2*math.pi/360.0), 1.0) 23 time.sleep(1.0) 24 motionproxy.setangles("headyaw", 0, 1.0) 25 time.sleep(1.0) 26 27 motionproxy.rest() 7
28 29 if name " main ": 30 main() wakeup() postureproxy StandZero HeadYaw setangles() 1 motionproxy.setangles("headyaw", -90.0*(2*math.pi/360.0), 1.0) -90.0*(2*math.pi/360.0) 90 1.0 ( 0.0 1.0 ) Nao 1 time.sleep(1.0) rest() HeadYaw Nao 1 #!/usr/local/bin/python 2 # -*- coding: utf-8 -*- 3 4 import sys 5 from naoqi import ALProxy 6 import math 7 10 8 HOST "localhost" 9 PORT 9559 11 def main(): 12 motionproxy ALProxy("ALMotion", HOST, PORT) 13 14 motionproxy.wakeup() 15 16 bodynames motionproxy.getbodynames("body") 17 18 targetanglesinrad motionproxy.getangles("body", False) 19 targetanglesindeg [ x * (360.0/2*math.pi) for x in targetanglesinrad ] 20 21 for i in range(len(bodynames)): 22 print bodynames[i] + " " + str(targetanglesindeg[i]) 23 24 motionproxy.rest() 25 26 if name " main ": 27 main() getbodynames("body") getangles("body", False) (Radian) 1 targetanglesindeg [ x * (360.0/2*math.pi) for x in targetanglesinrad ] 8
(Degree) *2 for setangles 1 #!/usr/local/bin/python 2 # -*- coding: utf-8 -*- 3 4 import sys 5 from naoqi import ALProxy 6 import time 7 import math 8 9 HOST "localhost" 10 PORT 9559 11 12 def main(): 13 motionproxy ALProxy("ALMotion", HOST, PORT) 14 15 motionproxy.wakeup() 16 17 18 postureproxy ALProxy("ALRobotPosture", "localhost", 9559) 19 postureproxy.gotoposture("standzero", 1.0) 20 21 names ["RShoulderRoll", "RElbowRoll", "RWristYaw"] 22 anglesindeg [15.0, 80.0, 90.0] 23 anglesinrad [ x * (2*math.pi/360.0) for x in anglesindeg ] 24 motionproxy.setangles(names, anglesinrad, 1.0) 25 time.sleep(2.0) 26 27 motionproxy.rest() 28 29 if name " main ": 30 main() 1.10 2: Nao (Nao ) 4 ( ) *2 Python Scheme Javascript map Lisp mapcar Ruby collect 9
4 2 15 80 StandZero Z X Y 0 T 1 1 T 2 0 1 r 0 1 0 T 1, 1 T 2 0 r 0 T 1 1 T 1 2 r (21) HandOffsetX 20mm ( ShoulderOffsetY ) cos(π/2) sin(π/2) Python numpy A sin(π/2) cos(π/2) x (1, 1) y Ax ( 1, 1) 1 #!/usr/local/bin/python 2 3 import numpy as np 4 import math 10
5 6 def main(): 7 8 PI math.pi 9 A np.array([[math.cos(0.5*pi), -math.sin(0.5*pi)], 10 [math.sin(0.5*pi), math.cos(0.5*pi)]]) 11 x np.array([[1.0], 12 [1.0]]) 13 y np.dot(a, x) 14 15 print y 16 17 if name " main ": 18 main() numpy import np np.array ( N 1 ) np.dot 0 T 1 1 T 2 11
2 (Inverse kinematics) ( )?? 2 2.1 1.8 2 0 T 2 0 T 1 1 T 2 cos(θ 1 + θ 2 ) sin(θ 1 + θ 2 ) 0 l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) sin(θ 1 + θ 2 ) cos(θ 1 + θ 2 ) 0 l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) 0 0 1 0 0 0 0 1 X l1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) Y l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) (22) (23) X 2 + Y 2 l 2 1 + l 2 2 + 2l 1 l 2 cos θ 2 (24) cos θ 2 X2 + Y 2 l 2 1 l 2 2 2l 1 l 2 (25) θ 2 cos 1 X2 + Y 2 l 2 1 l 2 2 2l 1 l 2 (26) cos 1 (x) ( x 1 acos,asin ) tan 2 θ 2 1 cos θ 1 + cos θ (27) θ 2 > 0 θ 2 ±atan2 (l 1 + l 2 ) 2 (X 2 + Y 2 ) (X 2 + Y 2 ) (l 1 + l 2 ) 2 (28) θ 1 atan2 Y X + atan2 l 2 sin θ 2 l 1 + l 2 cos θ 2 (29) 12
θ 2 < 0 θ 1 atan2 Y X atan2 l 2 sin θ 2 l 1 + l 2 cos θ 2 (30) : acos,asin acos, asin asin(x) x + 1 x 3 2 3 + 1 3 x 5 2 4 5 + acos(x) π (31) 2 asin(x) x 1 x 1 x 1 acos(x) cos θ x cos 2 θ + sin 2 θ x 2 + sin 2 θ 1 ( ) θ asin 1 x 2 ( ) (32) 1 x 2 1 cos 1 (x) C acos (3) glibc double glibc/sysdeps/ieee754/dbl-64/e acos.c float flt-32/e acosf.c 2.2 2 ( ) T q T T(q) (33) q q q δq (34) T T (q ) (35) δt T T T T (q) T (q + δq) T (q ) + T q δq + O ( δq 2) (36) δq δt T δq Jδq q 13
J T q J δq J 1 δt q q + δq q + J 1 δt (37) q (0) i ( q (i+1) q (i) + J 1 q (i)) ( δt q (i)) (38) (Newton-Raphson Method) 2 X l1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) Y l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) (39) ( θ1 ) q θ 2 (40) X T Y J(q) J(q) ( Ti ) q j ( X θ 1 Y θ 1 X θ 2 Y θ 2 ) ( l1 sin θ 1 l 2 sin(θ 1 + θ 2 ) l 2 sin(θ 1 + θ 2 ) l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 2 cos(θ 1 + θ 2 ) ( ) J 1 1 l 2 cos(θ 1 + θ 2 ) l 2 sin(θ 1 + θ 2 ) l 1 l 2 sin θ 2 l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) ) (41) (42) (i+1) [ (i) (X ) ] (i) θ1 θ1 + J 1 X θ 2 θ 2 Y Y (43) l 1 l 2 1 (44) T T X Y 1 1 (45) 14
q (0) (0) θ1 π/3 θ 2 π/3 (46) δt ( 1 1) ( cos π/3 + cos(π/3 π/3) 1 sin π/3 + sin(π/3 π/3) 1) ( 3 ) 2 1 1 2 2 3 1 1 2 3 (47) 1 J 2 3 0 3 2 1 (48) J 1 2 3 3 0 3 1 ( θ1 ) (1) ( θ1 ) (0) + J 1 δt θ 2 θ 2 π/3 2 + 3 3 0 1 2 π/3 3 1 1 1 2 3 1.6245. 1.7792 (49) 1 : 1 J 2 3 0 3 2 1 ( 1 ) δt 2 1 2 3 + 1 q (1) 1.6245 1.7792 (50) 2 : 0.844 0.154 J 0.934 0.988 6.516 10 2 δt 0.155553 q (2) 1.583 1.582 (51) 3 : 15
1.00.433 10 3 J.988.999.119 10 1 δt.362 10 3 q (3) 1.570795886 1.570867014 (52) 4 : π/2 q (4) q π/2 1.000 0.0 J.99850 1.0.438 10 6 δt.711 10 4 q (4) 1.570796329 1.570796329 (53) 2.3 θ 1 ( θ 2 ) X( Y ) *3 LU Python numpy.linalg.inv() 2 Python 1 1 #!/usr/bin/python 2 3 import numpy as np 4 import math 5 6 L1 1.0 7 L2 1.0 8 Epsilon 1.0e-12 9 10 def X(theta1, theta2): 1 Python 11 return L1*math.cos(theta1) + L2*math.cos(theta1+theta2) 12 13 def Y(theta1, theta2): *3 16
14 return L1*math.sin(theta1) + L2*math.sin(theta1+theta2) 15 16 def Jacobian(q): 17 theta1 q[0] 18 theta2 q[1] 19 dxdtheta1 (X(theta1+Epsilon, theta2)-x(theta1, theta2))/epsilon 20 dxdtheta2 (X(theta1, theta2+epsilon)-x(theta1, theta2))/epsilon 21 dydtheta1 (Y(theta1+Epsilon, theta2)-y(theta1, theta2))/epsilon 22 dydtheta2 (Y(theta1, theta2+epsilon)-y(theta1, theta2))/epsilon 23 return np.array([[dxdtheta1, dxdtheta2], [dydtheta1, dydtheta2]]) 24 25 def distance(u, v): 26 return math.sqrt((u[0]-v[0])*(u[0]-v[0])+(u[1]-v[1])*(u[1]-v[1])) 27 28 def Tstar(q): 29 return np.array([[x(q[0],q[1])], [Y(q[0],q[1])]]) 30 31 def main(): 32 T np.array([[1.0], [1.0]]) 33 q np.array([[np.pi/3.0], [-np.pi/3.0]]) 34 35 while distance(t, Tstar(q)) > Epsilon: 36 th1 q[0] 37 th2 q[1] 38 deltat T - Tstar(q) 39 J Jacobian(q) 40 Jinv np.linalg.inv(j) 41 q + np.dot(jinv, deltat) 42 print q 43 44 if name " main ": 45 main() 2.4 2.4.1 2 ( ) 2 2.4.2 17
2.4.3 ( ) 2/2 θ 1 π/4, θ 2 0 2 2/2 ( ) J 1 1 l 2 cos(θ 1 + θ 2 ) l 2 sin(θ 1 + θ 2 ) l 1 l 2 sin θ 2 l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) (54) l 1 l 2 sin θ 2 (Singular configuration) : 2.5 3: 5 Nao ( ) Nao (StandInit ) ( Crouch ) StandInit (x, y, z) (x, y, z ) x x y y StandInit z 560 mm 450 mm 560 450 110 mm 10 p 0, p 10 getangles() setangles() 5 StandZero θ 1 90 Nao θ 1 0 90 StandZero 18
H 5 Nao A1: Python C Python *4 Hello world 1 #!/usr/local/bin/python 2 # -*- coding: utf-8 -*- 3 4 import sys 5 import time 6 7 def main(): 8 print "And now for something completely different..." 9 time.sleep(3.0) 10 for i in ["Graham Chapman", "John Cleese", "Terry Gilliam", 11 "Eric Idle", "Terry Jones", "Michael Palin"]: 12 print i 13 14 if name " main ": 15 main() 1. 1 *4 http://docs.python.jp/2/tutorial/index.html 19
2. 2 ( UTF8) 3. 3 4. 4 C #include<stdio.h> 5. 5 time *5 6. 6 7. 7 main C main Python (:) *6 8. 8 And now for something complete different... *7 C (;) C {} Python 9. 9 time sleep C sleep 3 10. 10 for C Python [] 11. 11 (:) 12. 12 13. 13 14. 14 import false 15. 15 main() for C range() 0 ( 1) 1 % python 2 Python 2.7.5 (v2.7.5:ab05e7dd2788, May 13 2013, 13:18:45) 3 [GCC 4.2.1 (Apple Inc. build 5666) (dot 3)] on darwin 4 Type "help", "copyright", "credits" or "license" for more information. 5 >>> range(10) 6 [0, 1, 2, 3, 4, 5, 6, 7, 8, 9] 7 >>> for i in range(10): 8... print i 9... ^D 10 0 11 1 12 2 13 3 *5 C *6 *7 Monty Python s Flying Circus ( ) The Olympic Hide-and-Seek Final (http://www.montypython.net/scripts/hideseek.php) YouTube ( ) 20
14 4 15 5 16 6 17 7 18 8 19 9 20 >>> 1 # # (C //) 2 """ 3 """ """ (C /*... */) 4 """ (/* foo */ """ foo """ ) 5 """ 21
A2: Nao Nao NAO Technical overview *8 6 *8 http://www.aldebaran-robotics.com/documentation/family/robots/index robots.html 22
7 23
8 9 24
10 25
11 26
12 27
13 28