Bio-Robotics and Human-Mechatronics (T.

Matsumaru) Lab 2015

ロボット工学 Schedule – 2015 Fall semester

• 30 Sep. Wed 09h00-10h30, S104 01. What robot is?
• 07 Oct. Wed 09h00-10h30, S104 02. Robot history
Robotics •
•
•
14 Oct. Wed 09h00-10h30, S104 03. Robot configuration
21 Oct. Wed 09h00-10h30, S104 04. Robot mechanism
28 Oct. Wed 09h00-10h30, S104 05. Actuator
440111370F • 04 Nov. Wed 09h00-10h30, S104 06. Sensor (1)
• 11 Nov. Wed 09h00-10h30, S104 07. Sensor (2)
• 18 Nov. Wed 09h00-10h30, S104 08. Reduction mech. and driving part design
• 25 Nov. Wed 09h00-10h30, S104 09. Fundamental theory of manipulator (1)
• 02 Dec. Wed 09h00-10h30, S104 10. Fundamental theory of manipulator (2)
松丸隆文 •
•
09 Dec. Wed 09h00-10h30, S104 11. Fundamental theory of manipulator (3)
16 Dec. Wed 09h00-10h30, S104 12. Fundamental theory of manipulator (4)
• 23 Dec. Wed (winter break)
Takafumi Matsumaru •
•
30 Dec. Wed (winter break)
06 Jan. Wed (winter break)
• 13 Jan. Wed 09h00-10h30, S104 13. Manipulator control (1)
• 20 Jan. Wed 09h00-10h30, S104 14. Manipulator control (2)
• 27 Jan. Wed 09h00-10h30, S104 15. Conclusion
1 2
• 03 Feb. Wed (supplement/makeup class, during 30 Jan. and 04 Feb.)

Kinematics 運動学 and Dynamic 動

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Chapter 10 力学 on manipulator ﾏﾆﾋﾟｭﾚｰﾀ

Basic principles of manipulator : vector approach for kinetics • Kinematics（運動学）
– part II : joint space to work space at position, speed, accele. • Joint angle (rotational) / length (prismatic)
1. Direct kinematics（順運動学） (cont.) End-tip (link) position and orientation
1. Practice (I) – Stanford manipulator • Joint (rotational / prismatic) speed / acceleration
1. Setting of local coordinate system End-tip (link) speed / acceleration (translation and rotation)
2. Joint position / orientation of Stanford manipulator
2. Postural (orientation of local coordinate system) change rate Direct kinematics
3. Translational speed of origin of local coordinate system on link （順運動学）
4. ex.) Rotation / translation speed on articulated manipulator
1. Reference posture of manipulator
Joint space Work space
2. Rotational speed vector and Translational speed vector （関節空間） （作業空間）
5. Acceleration of origin of local coordinate system on link
6. Practice (2) – (pseudo) polar coordinate type manipulator
Inverse kinematics
1. Configuration and orientation vector and position vector of each joint （逆運動学）
2. Translational speed and Translational acceleration • Dynamics（動力学）
7. Practice (3) – (pseudo) cylindrical coordinate type manipulator
• Relation between movement and applied force/torque
1. Configuration and orientation vector and position vector of each joint
• Equation of motion（運動方程式）
2. Translational speed and Translational acceleration 3 4
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Local coordinate system
Cartesian coordinate system indicating link orientation
# coordinate system Oi ( X i , Yi , Z i ) • Rotational joint • Prismatic joint
unit vector along X-axis X i = ( X i1 , X i2 , X i3 )T • Rotation around Z-axis: θi • Linear motion along X-axis: Ｓｉ
unit vector along Y-axis Yi = (Yi1 , Yi2 , Yi3 )T θi
Z i-1
Si Xi-1
unit vector along Z-axis Z i = (Z i1 , Z i2 , Z i3 )T Zi
Zi-1 Zi
Xi-1 Yi-1
Zi Xi
Zi+1 Xi
Xi+1 Zi
Xi Yi Yi-1 Yi
Xi-1 Zi-1
Yi Link: Li Link: Li-1 Link: Li Link: Li-1
Yi Yi+1 Coordinate: Oi Coordinate: Oi-1 Coordinate: Oi Coordinate: O i-1
Xi Yi-1
Any of coordinate axes Any of coordinate axis
Cartesian coordinate: Oi Link: L i+1 Link: Li Link: Li-1 corresponds with joint rotational axis corresponds with movement direction
(three unit vectors
at right angles each other) Coordinate: Oi+1 Coordinate: O i Coordinate: Oi-1
5 6

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Coordinate transformation Coordinate transformation
at rotation joint (1/4) at rotation joint (2/4)
θi Zi =Zi-1’ # Coordinate transformation at Z-axis rotation joint
Xi-1’ (relationship between coordinate Oi and coordinate Oi-1’)
Xi-1’ Zi-1’
(describe axes in coordinate Oi using axes in coordinate Oi-1’)
Zi Z i-1 θi Yi-1’ when
X i = (X i ,1 , X i ,2 , X i ,3 )T
Xi-1 Yi
Yi-1’
Xi
cos θi X i −1 ' = (cos θi X i −1,1 ' , cos θi X i −1,2 ' , cos θi X i −1,3 ')T
Yi
Xi Yi Xi
Yi-1 θi sinθi θi
Yi-1’
Coordinate: Oi Coordinate: Oi-1’ Coordinate: Oi-1 Rotation joint around Z-axis cosθi Xi-1’  X i = cos θi X i −1 '+ sin θiYi −1 '

Yi-1’ Yi = − sin θi X i −1 '+ cos θiYi −1 '
Yi 
Transform matrix: Ai Transform matrix: Bi-1
cosθi  Z i = Z i −1 '
θi
Decided with Parallel translation based therefore
rotational axis θi on link geometric structure
Xi  cos θi − sin θi 0 
 
sinθi Xi-1’ ( X i , Yi , Z i ) = ( X i −1 ' , Yi −1 ' , Z i −1 ') sin θi cos θi 0 
Corresponds with coordinate O i when θi = 0  0
7
 0 8 1 
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Coordinate transformation
at rotation joint (3/4) Calculate each joint position (1/2)
# Coordinate transform matrix at rotation angle θi: Ai Joint position --- origin position of local coordinate system Oi fixed to link.
# In case of rotation joint
( X i , Yi , Zi ) = ( X i−1 ' , Yi −1 ' , Zi −1 ')Ai 　･･･(a1)
Origin position Pi-1’ of
1 0 0  1 0 0  coordinate system Oi-1’ Displacement vector
    Li-1 = lx Xi-1 + ly Yi-1 + lz Zi-1
Ai = Axi =  0 cos θi − sin θi  　
=  0 ci − si ･･･X − axis Origin position Pi of Zi
 0 sin θ cos θi   0 si
 ci  coordinate system Oi Origin position Pi-1 of
 i
Zi-1 coordinate system Oi-1
 cos θi 0 sin θi  Xi Yi
  Xi-1
Ai = Ayi =  0 1 0  　･･･Y − axis i th link
 − sin θ 0 cos θ  Yi-1
 i i θi i-1 th link
 cos θi − sin θi 0 
 
Ai = Azi =  sin θi cos θi 0  　･･･Z − axis
(
Pi = Pi −1 + Li −1 = Pi −1 + l x X i −1 + l yYi −1 + l z Z i −1 　　　　･･･(a7) )
 0 0 1  i-1 th link vector
 9 10

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Procedure of direct kinematics about
Calculate each joint position (2/2) position (from joints to end-tip)
Joint position --- origin position of local coordinate system Oi fixed to link. (1) Understand robot structure/configuration（構造） --- movable portions = joints
# In case of prismatic joint operation.
(2) Settle reference posture（基準姿勢） --- posture/appearance when all joint values
Displacement vector Origin position Pi-1 of are zero, for example, all links and joints are aligned on a line.
Li-1 = lx Xi-1 + ly Yi-1 + lz Zi-1 coordinate system Oi-1 (3) Draw mechanistic model（機構ﾓﾃﾞﾙ） --- link-joint model using symbols.
(4) Set local coordinate system（ﾛｰｶﾙ座標系） on each link from base to end-tip in
Origin position Pi-1’ of Zi-1 order --- paying attention to origin position of each coordinate systems, direction
coordinate system Oi-1’ Xi-1 of coordinate axes, etc.
(5) Declare link vector on each link in local coordinate system.
Zi-1’
Translation Si along Xi-axis (6) Describe relationship between neighboring local coordinate systems（隣り合った
Si Xi Yi-1 ﾛｰｶﾙ座標系どおしの関係） --- for example, rotation around X-axis and telescopic
Zi along Y-axis, in order to understand coordinate transformation（座標変換） by
Origin position Pi of Xi-1’ Yi-1’ each joint.
coordinate system Oi Yi i-1 th link (7) Develop transformation matrixes from each local coordinate system to base
Xi coordinate system（それぞれのﾛｰｶﾙ座標系からﾍﾞｰｽ座標系への変換行列）.
i th link (changed amount) (8) Add link-vectors from basement to joint position in order to calculate joint
( )
Pi = Pi −1 + l x X i −1 + l yYi −1 + l z Z i −1 + Si X i 　･･･(a8) position --- add all joint-vectors from basement to end-tip to calculate end-tip
position（手先の位置）.
i-1 th link vector i th link (changed amount) (9) Coordinate axes’ direction of local coordinate system put on end-tip link shows
11 12
end-tip orientation（手先の向き）.
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
ex.) Articulated (multi-joint) manipulator ex.) Articulated (multi-joint) manipulator
（多関節マニピュレータ）(1/4) （多関節マニピュレータ）(2/4)
# Reference posture of manipulator l4 z
Posture/appearance when all θi = 0. 6 x O6
All Bi becomes unit matrix (I), if all joint y A6 (around z-axis)
coordinate systems (local coordinate z
5 x B5 = unit matrix
systems) are aligned in the same O5
direction, so that they can be y A5 (around y-axis)
negligible. z B4 = unit matrix
4 x
O4 A4 (around z-axis)
l3 y
z
x B3 = unit matrix
3 O3
y A3 (around y-axis)
l2 z
2 x B2 = unit matrix
O2
y A2 (around y-axis)
z B1 = unit matrix
Coordinate systems O1～O6 are set so that axes are x
1 O1 A1 (around z-axis)
corresponding during reference posture（基準姿 l1 y
z B0 = unit matrix
勢） -- all θi = 0 0 x
O0
all Bi become unit matrix（単位行列） 13
y 14

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Multiple rotation joint in serial link Joint position of articulated (multi-
mechanism (1/2) joint) manipulator (1/2)
# Coordinate transform from joint (local) coordinate system P7 : end-tip position vector
P7
L7 to basement (reference) coordinate system L6 (l4) z L6 : link vector (joint #6 to end-tip)
6 x
O6 P6 O6 P6 : joint #6 position vector
L5 C6 (transform from O6 to O5) C1C2C3C4C5C6 (transform from O6 to O0) L5 (0) y A6 (z-rotation) C6 L5 : link vector (joint #5 to joint #6)
5 = 0C6 z B5 =I
x
O5 P5 O5 P5 : joint #5 position vector
C5 (transform from O5 to O4) C1C2C3C4C5 (trans. from O5 to O0) y A5 (y-rotation) C5
l4 L4 (0) z L4 : link vector (joint #4 to joint #5)
4 = 0C5 x B4 =I
O4 P4 O4 P4 : joint #4 position vector
l3 C4 (transform from O4 to O3) C1C2C3C4 (trans. from O4 to O0) L3 (l3) y A4 (z-rotation) C4 L3 : link vector (joint #3 to joint #4)
= 0C4 z B3 =I
3 x
O3 P3 O3 P3 : joint #3 position vector
l2 C3 (transform from O3 to O2) C1C2C3 (trans. from O3 to O0) y A3 (y-rotation) C3
L2 (l2) z L2 : link vector (joint #2 to joint #3)
2 = 0C3 B2 =I
O2 P2 x O2 P2 : joint #2 position vector
l1 C2 (transform from O2 to O1) C1C2 (trans. from O2 to O0) y A2 (y-rotation) C
2
= 0C2 L1 (0) z B1 =I L1 : link vector (joint #1 to joint #2)
1 x
O1 P1 O1 P1 : joint #1 position vector
C1 (transform from O1 to O0) y A1 (z-rotation) C1
l0 0 L0 (l1) z B0 =I L0 : link vector (basement to joint #1)
O0 15
P0 x O0 P0 : basement position vector 16
y
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint position of articulated (multi- Joint orientation of articulated (multi-
joint) manipulator (2/2) joint) manipulator
0P7 = 0P6 + L6
P7 P7
L6 (l4) L6 = l4 Z6 = (0,0,l4)6T describe L6 (l4)
z in reference z
x 0C6
0P6 = 0P5 + L5 coordinate x 0C6
#6 z-axis rotation Z6
P6 O6 P6 O6
L5 (0) y A6 (z-rotation) C6 L5 = 0 = (0,0,0)5T L5 (0) y A6 (z-rotation) C6 (0,0,1)6T describe
z B5 =I z B5 = I in reference
x 0C5 x 0C5
P5 O5 0P5 = 0P4 + L4 P5 O5 #5 y-axis rotation Y5 coordinate
y A5 (y-rotation) C5 y A5 (y-rotation) C5
L4 (0) z L4 = 0 = (0,0,0)4T L4 (0) z (0,1,0)5T describe
x B4 =I 0C 0 P4 = 0P3 + L3 x B4 = I 0C4 in reference
P4 O4 4
P4 O4 #4 z-axis rotation Z4 coordinate

L3 (l3) y A4 (z-rotation) C4 L3 = l3Z3 = (0,0,l3)3T describe L3 (l3) y A4 (z-rotation) C4 (0,0,1)4T describe

z B3 =I in reference z B3 = I in reference
x 0C3 0P3 ＝0P2+L2 coordinate x 0C #3 y-axis rotation Y3 coordinate
P3 O3 P3 O3 3
y A3 (y-rotation) C3 describe
y A3 (y-rotation) C3
L2 (l2) z L2 = l2Z2 = (0,0,l2)2T L2 (l2) z (0,1,0)3T describe
B2 =I in reference B2 = I in reference
P2 x O2 0C2 0P2 = 0P1 + L 1 coordinate P2 x O2 0C2 #2 y-axis rotation Y2 coordinate
y A2 (y-rotation) C y A2 (y-rotation) C
L1 (0) z B1 =I 2 L1 = 0 = (0,0,0)1T L1 (0) z B1 = I 2 (0,1,0)2T describe
x 0C1 x 0C in reference
P1 O1 0P1 = 0P0 + L0 P1 O1 1 #1 z-axis rotation Z1 coordinate
y A1 (z-rotation) C1 y A1 (z-rotation) C1
L0 (l1) z B0 =I L0 = l1Z0 = (0,0,l1)0T L0 (l1) z B0 = I (0,0,1)1T describe
in reference
P0 x O0 0P0=0 17
P0 x O0 #0 coordinate
18
y y

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint position of articulated (multi- Local coordinate system and
joint) manipulator reference coordinate system (1/3)
# manipulator configuration # relationship between neighboring local (joint) coordinate systems
base joint-1 joint-2 joint-3 joint-4 joint-5 joint-6 hand
( X i , Yi , Zi ) = ( X i −1 ' , Yi −1 ' , Zi −1 ')Ai
link-0 link-1 link-2 link-3 link-4 link-5 link-6
x 0 0 0 0 0 0 0 = ( X i −1 , Yi −1 , Z i −1 )Bi −1 Ai
　　　　　　　　
y 0 0 0 0 0 0 0 　　　　　　　　 = ( X i −1 , Yi −1 , Z i −1 )C i 　･･･(a3)
z l1 0 l2 l3 0 0 l4
# relationship between reference (basement) coordinate system and
# joint position vector local (joint) coordinate system
P7 = P6 + l4 Z 6 = l1 Z 0 + l2 Z 2 + l3 Z 3 + l4 Z 6
( X i , Yi , Zi ) = ( X i −1, Yi −1, Z i −1 )Ci
P5 = P6 = P4
= ( X i −2 , Yi −2 , Z i −2 )C i −1Ci
　　　　　
P4 = P3 + l3 Z 3 = l1 Z 0 + l2 Z 2 + l3 Z 3
　　　　　　　　 :
P3 = P2 + l2 Z 2 = l1 Z 0 + l2 Z 2 = ( X1, Y1 , Z1 )C 2 ⋅ ⋅ ⋅ C i −1C i
　　　　　
P2 = P1 Add link vectors from basement in order, = ( X 0 , Y0 , Z 0 )C1C 2 ⋅ ⋅ ⋅ Ci −1Ci
　　　　　
P1 = P0 + l1 Z 0 = l1 Z 0 but,
adding in components (x, y, and z elements) =( X 0 , Y0 , Z 0 )0Ci
　　　　　
P0 = 0 must be in the same coordinate system 19 20
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Local coordinate system and Local coordinate system and
reference coordinate system (2/3) reference coordinate system (3/3)
# (cont.) relationship between reference (basement) coordinate system  c11 c12 c13 
 
and local (joint) coordinate system ( X i , Yi , Z i ) = ( X 0 , Y0 , Z 0 ) c21 c22 c23 
( X i , Yi , Z i )　
= ( X 0 , Y0 , Z 0 )C1C 2 ⋅ ⋅ ⋅ C i −1C i c 
 31 c32 c33 
=( X 0 , Y0 , Z 0 )0C i
　　　　　　  1 0 0  c11 
        
 c11 c12

c13 
  X i = X 0c11 + Y0c21 + Z 0c31　 =  0  c11 +  1  c21 +  0  c31 =  c21 
= ( X 0 , Y0 , Z 0 ) c21 c22
　　　　　　 c23    0
 0
0
 0
1
 0
c 
 31 0
c 
 31 c32 c33    c12 
  
 Yi = X 0c12 + Y0c22 + Z 0c32 =  c22 
  1   0   0     1   0   0   c11 c12 c13 
                  c 
  0  ,  1  ,  0   =   0  ,  1  ,  0   c21 c22 c23    32 0
                  c13 
  0 i  0 i  1 i    0 0  0 0  1  0  c31 c32 c33    
Z i = X 0c13 + Y0c23 + Z 0c33 =  c23 

 c 
21  33 0 22

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Setting of reference posture and local
coordinate systems Practice (1) – Stanford manipulator
Difficulty, complexity, and complication of coordinate 1. Understand robot structure/configuration.
2. Settle reference posture.
transformation calculation depend on …
3. Draw mechanistic model.
4. Describe relationship between neighboring
setting of local coordinate systems.
(1) Reference posture (state at initial value, zero) 5. Declare link vector on each link in local
(2) Local coordinate system (orientation of coordinate axes) coordinate system.
6. Develop transformation matrixes
from each local coordinate system
to base coordinate system.

-----------------------------
Format : A4 sheet
23 24
Time limit : 15-20 min.
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Setting of local coordinate system Setting of local coordinate system

(a) As shown in question : (b) devising in reference posture :
coordinate transformation is coordinate transformation is
necessary, and directions of unnecessary, directions of local
link vector are different. coordinate systems are corresponding,
X 2-direction but directions of link vectors are Y5 X 2-direction
X4 different. Y2
End-tip Y4-rotation S(t) S(t)
Y2 X4 Z1
Joint #6 x-rotation π/2 X6
Z1 End-tip
Joint #5 y-rotation Y1-direction
Y5 Y1-direction Joint #6 x-rotation X3
Trans. y-rotation (pi / 2) －l2
Joint #4 x-rotation X3 －l2 Joint #5 y-rotation
X6-direction Z0-direction
Joint #3 x-direction Z0-direction Joint #4 x-rotation
l3 l1
Joint #2 y-rotation X 6-direction l1 Joint #3 x-direction
Joint #1 z-rotation l3 Joint #2 y-rotation
X6 Joint #1 z-rotation
Basement
Basement

25 26

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Setting of local Z6
Joint position
coordinate system Z6-direction
l3
of Stanford manipulator
(c) devising in local coordinate
system : directions of link Z3
vectors are corresponding (z- Y5 Z6-direction Z6
direction) as much as l3
possible during reference Z4
posture. Y5
Z2-direction
S(t) Z3-direction Z4
End-tip
S(t)
Joint #6 z-rotation
Joint #5 y-rotation Y2 Z3
Joint #4 z-rotation Z1 0
Y2
Joint #3 z-direction Y1-direction Z1
Joint #2 y-rotation －l 2 Y1-direction
Joint #1 z-rotation －l 2
Z0-direction
Basement Z0-direction
l1 l1

27 28
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint orientation D-H (Denavit–Hartenberg)
of Stanford manipulator parameters -1
Coordinate transformations along a serial robot
Z6-direction Z6
l3 [T] transformation locating the end-link
[Z] transformation associated with the joint
[X] second transformation associated with the link
Y5
Convention Denavit-Hartenberg introduced
Z3-direction Z4 Z coord-axes are assigned to the joint axes Si
S(t) θi is the rotation around, di is the slide along the Z axis
Z3
0
Y2
Z1
Y1-direction
X coord-axes are assigned to the common normals Ai,i+1
－l 2 αi,i+1 and ri,i+1 define the physical dimensions of the link
Z0-direction
l1

29 30

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
D-H (Denavit–Hartenberg) Postural (orientation of local
parameters -2 coordinate system) change rate (1/2)
Coordinate transformations Postural change rate
from the previous coordinate system to the next coordinate system Zi = time-derivative of vector / matrix showing local coordinate
Yi i
∂ •
　 ( X i , Yi , Z i ) = ∑ ( X i , Yi , Z i )θ j 　　･･･(b1) 　
d
Oi Xi dt j =1 ∂θ j

pj When pj is rotational axis unit vector of j th rotational joint,

dθj
∂
(
　 ( X i , Yi , Z i ) = p j × X i , p j × Yi , p j × Z i 　･･(b2)
∂θ j
)
dt
* Outer product : p j = ( p1 , p2 , p3 )T
Denavit-Hartenberg matrix
X i = ( X 1 , X 2 , X 3 )T
　  p2 X 3 − p3 X 2 
 
p j × X i =  p3 X 1 − p1 X 3 
R: 3×3 submatrix describing rotation pX −p X 
T: 3×1 submatrix describing translation
31
 1 2 2 1 32
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Postural (orientation of local Translational speed of origin of local
coordinate system) change rate (2/2) coordinate system on link (1/2)
If assign eq. (b2) to eq. (b1), # In case of i th rotational joint,
•
( )
i eq. (a7) indicating rotational joint position is differentiated.
　 ( X i , Yi , Z i ) = ∑ p j × X i , p j × Yi , p j × Z i θ j 　･･･(b3)
d
dt j =1
d d
(
　 Pi = Pi −1 + ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1 　･･･(b6) 　
dt dt
)
Rotational speed vector of i th link.
i • • • •
# In case of i th prismatic joint,
ωi = ∑ p j θ j 　
= p1 θ1 + p2 θ 2 + ⋅ ⋅ ⋅ + pi θ i 　･･･(b4) eq. (a8) indicating prismatic joint (along x) position is differentiated.
j =1
If assign eq. (b4) to eq. (b3), posture change rate is
d d
(
　 Pi = Pi −1 + ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1
dt dt
)
　 ( X i , Yi , Z i ) = (ωi × X i , ωi × Yi , ωi × Z i )　･･･(b5)
d •
dt + S (ωi × X i ) + S X i 　　　･･･(b7)
　　　　　　　　　　　
•
while prismatic speed S
rotational speed (i th link is not rotated) ωi = ωi-1
33 34

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Translational speed of origin of local ex.) Rotation / translation speed
coordinate system on link (2/2) on articulated manipulator (1/2)
# Translation speed by rotation joint = geometric image of eq. (b6) # Reference posture of manipulator l4 z
Posture/appearance when all θi = 6 x O6
dt : short period 0. y A6 (around z-axis)
z
Transform All Bi becomes unit matrix (I), if 5 x B5 = unit matrix
O5
from small rotational displacement dθ all joint coordinate systems y A5 (around y-axis)
to small translational displacement dL (local coordinate systems) are z B4 = unit matrix
4 x
aligned in the same direction, O4
dL = L sin α sin dθ 　 so that they can be negligible. l3
y
z
A4 (around z-axis)
x B3 = unit matrix
dL = L sin α dθ Small displacement 3
y
O3 A3 (around y-axis)
vector dL |L| sinα l2 z
dL = L sin α ω dt
　 Rotation speed 2 x B2 = unit matrix
vector ω O2
α y A2 (around y-axis)
　
dL z
= L sin α ω A point distant |L| Rotation x B1 = unit matrix
dt　 from rotation centre center 1
y
O1 A1 (around z-axis)
dL l1 z
= ω× L 0 x B0 = unit matrix
dt 35
O0 36
y
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
ex.) Rotation / translation speed Acceleration of origin of local
on articulated manipulator (2/2) coordinate system on link (1/2)
# Rotational speed vector # Translational speed vector of origin # In case of i th rotational joint,
: from eq. (b4) of local coordinate : from eq. (b6) eq. (b6) indicating translational speed is differentiated.
• d d
P1 = P2 = 0 •
ω1 = Z1 θ1
•
dt dt
d2 d2
(
　 2 Pi = 2 Pi −1 + ωi −1× l x X i −1 + l yYi −1 + l z Z i −1 )
dt dt
P3 = ω2 × (l2 Z 2 ) ( ( ))
ω2 = ω1 + Y2 θ2 d
　　　　　　　　 + ωi −1 × ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1
• dt
ω3 = ω2 + Y3 θ3 　　　　　　　　　　　　　　　　　　･･･(b8)
P4 = P3 + ω3 × (l3 Z 3 )
d d
• dt dt
ω4 = ω3 + Z 4 θ4 • d d  i −1 • 
d d
P5 = P4
while ωi −1 =
　 ωi −1 =  ∑ p j θ j 
• dt dt  j =1 
ω5 = ω4 + Y5 θ5 dt dt  
d d i −1 •• i −1 •
•
ω6 = ω5 + Z 6 θ6 dt
P6 = P5
dt = ∑ p j θ j + ∑ ωi −1 × p j θ j
　　　　　 ( )
j =1 j =1
P7 = P6 + ω6 × (l4 Z 6 ) 37
d d
38
dt dt

Practice (2) –
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Acceleration of origin of local
coordinate system on link (2/2) (pseudo) polar coordinate type manipulator
# In case of i th prismatic joint, Show position, speed, acceleration at basement (P0), joint-1 (P1),
eq. (b7) indicating translational speed is differentiated. joint-2 (P2), and end-tip (P3) of (pseudo) polar coordinate type
•
d2
Pi =
d2
(
Pi −1 + ωi −1× l x X i −1 + l yYi −1 + l z Z i −1 ) manipulator (2 d.o.f. = rotational + rotational).
Z2
dt 2 dt 2
　　　　　　　　 ( (
+ ωi −1 × ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1 )) Coordinate system O3 (position P3)
Z3 l2 X2
translational movement P2 Y 2
 •  x-direction link l2
+ S  ωi × X i + ωi × (ωi × X i )
　　　　　　　　 Coordinate system O2 (position P2) X3 l1
θ2
  θ1
• ••
y-axis rotation θ2 P 3 Y3 Z1
P1
+ 2 S (ωi × X i ) + S X i 　　　･･･(b9)
z-direction link l1
　　　　　　　　 X1
Coordinate system O1 (position P1)
0 Y
while rotational speed / acceleration (i th link is not rotated) z-axis rotation θ1 Z0 1
• • z-direction link 0 P0
ωi = ωi-1，　
ωi = ωi-1 Coordinate system O0 (position P0) ----------------------------- X 0
39
Format : A4 sheet Y040
Time limit : 15-20 min.
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) polar Direct kinematics of (pseudo) polar
coordinate type manipulator (1/3) coordinate type manipulator (2/3)
• Manipulator configuration (link, joint) and orientation vector and position • Translational speed of origin of local coordinate system on link
vector of each joint : assign to eq. (b6)
(referring examples of articulated manipulator and Stanford manipulator) d
P0 = 0
P3 O3 0P3
= 0P2 + L2 = l1Z1 + l2X2 dt
X2-direction l2 A3 = I
B2 = I
I L2 = l2X2 = (l2,0,0)2T d
P1 =
d
dt
(
P0 + ω0 × l x X 0 + l yY0 + l z Z0 = )
X2 P2 O2 0C 0
2 P2 = 0P1 + L 1 = l1Z1 dt
構成図
Z1-direction l1
A2 = (y-rotation θ2) C2
L1 = l1Z1 = (0,0,l1)1T
d
P2 =
B1 = I dt
Z1 P1 O 1 0C 0
1 P1 = 0P1 + L1 = 0
A1 = (z-rotation θ1) C1 d
0 L0 = (0,0,0)0T P3 =
B0 = I 0P0 = 0 dt
P0 O 0
• •
• rotation speed vector of each joint ω2 = = 　　l2 θ1 (Z1 × X 2 ) − l2 θ 2 Z 2
(referring articulated manipulator) ω1 =
ω0 = 0
41 42

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) polar Practice (3) –
coordinate type manipulator (3/3) (pseudo) cylindrical coordinate type manipulator
• Translational acceleration of origin of local coordinate system on link Show position, speed, acceleration at basement (P0), joint-1 (P1),
: assign to eq. (b8) joint-2 (P2), and end-tip (P3) of (pseudo) cylindrical coordinate
d2 type manipulator (2 d.o.f. = rotational + prismatic).
　 2 P0 = 　
0
dt
Coordinate system O3 (position P3)
d2
P1 = translational movement
dt 2 x-direction link l2
d2 Coordinate system O2 (position P2)
P2 = y-axis rotation θ2
　
dt 2
z-direction link l1
d2
P3 = Coordinate system O1 (position P1)
dt 2 z-axis rotation θ1
z-direction link 0
Coordinate system O0 (position P0) -----------------------------
43
Format : A4 sheet 44
Time limit : 15-20 min.
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) cylindrical Direct kinematics of (pseudo) cylindrical
coordinate type manipulator (1/3) coordinate type manipulator (2/3)
• Manipulator configuration (link, joint) and orientation vector and position • Translational speed of origin of local coordinate system on link
vector of each joint : assign to eq. (b6) and (b7)
(referring examples of articulated manipulator and Stanford manipulator) d
P0 = 0
0P3 = dt
d
dt
P1 =
d
dt
(
P0 + ω0 × l x X 0 + l yY0 + l z Z0 = )
0P2 =
構成図 d
P2 =
dt
0P1 =
d
P3 =
0P0 =0 dt
• rotation speed vector of each joint ω2 =
(referring articulated manipulator)
ω1 = • •
= 　　　S (t ) θ1 Y1 　
+ S (t ) X 1
ω0 = 0 45 46

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Direct kinematics of (pseudo) cylindrical
coordinate type manipulator (3/3)
• Translational acceleration of origin of local coordinate system on link
2
: assign to eq. (b8) and (b9)
d
　 2 P0 = 　
0
dt
d2
P1 =
dt 2
d2
P2 =
dt 2
d2
　 2 P3 =
dt

 •• • •   •• • 2
=  S θ1 + 2 S θ1  　Y1 +  S − S θ1  X 1　
　
    47
 