Ch. 3: Inverse Kinematics Ch. 4: Velocity Kinematics Inverse - PDF document

Ch. 3: Inverse Kinematics Ch. 4: Velocity Kinematics Inverse orientation kinematics • Now that we can solve for the position of the wrist center (given kinematic decoupling), we can use the desired orientation of the end effector to solve for the last three joint angles end effector to solve for the last three joint angles 1

Inverse orientation: spherical wrist • Previously, we said that the forward kinematics of the spherical wrist were identical to a ZYZ Euler angle transformation: wrist were identical to a ZYZ Euler angle transformation: ⎡ − − − ⎤ c c c s s c c s s c c s c s d 4 5 6 4 6 4 5 6 4 6 4 5 4 5 6 ⎢ ⎥ + − + s c c c s s c s c c s s s s d ⎢ ⎥ = = 4 5 6 4 6 4 5 6 4 6 4 5 4 5 6 3 T A A A ⎢ ⎥ 6 4 5 6 − s c s c c c d 5 6 5 6 5 5 6 ⎢ ⎥ ⎣ 0 0 0 1 ⎦ Inverse orientation: spherical wrist • The inverse orientation problem reduces to finding a set of Euler angles ( θ 4 , θ 5 , θ 6 ) that satisfy: angles ( θ θ θ ) that satisfy: ⎡ − − − ⎤ c c c s s c c s s c c s 4 5 6 4 6 4 5 6 4 6 4 5 ⎢ ⎥ = + − + 3 R s c c c s s c s c c s s ⎢ ⎥ 6 4 5 6 4 6 4 5 6 4 6 4 5 ⎢ ⎥ − ⎣ s c s c c ⎦ 5 6 5 6 5 2

Inverse orientation: spherical wrist (Non-singular case) Thus there are two values for θ 5 . Using the first ( s 5 > 0): • Using the second value for θ 5 ( s 5 < 0): • Inverse orientation: spherical wrist (singular case) In the singular case, θ 5 = 0 • − − − ⎡ ⎤ c c c s s c c s s c c s 4 5 6 4 6 4 5 6 4 6 4 5 ⎢ ⎥ = + − + 3 R s c c c s s c s c c s s ⎢ ⎥ 6 4 5 6 4 6 4 5 6 4 6 4 5 ⎢ − ⎥ ⎣ s c s c c ⎦ 5 6 5 6 5 3

Inverse Kinematics: general procedure 1. Find q 1 , q 2 , q 3 such that the position of the wrist center is: ⎡ 0 ⎤ ⎢ ⎢ ⎥ ⎥ = − o o inverse position o d 6 R 0 ⎢ ⎥ c kinematics ⎢ ⎥ 1 ⎣ ⎦ 0 2. Using q 1 , q 2 , q 3 , determine R 3 3. Find Euler angles corresponding to the rotation matrix: ( ) ( ) R inverse orientation − 1 T = = 3 0 0 R R R R kinematics 6 3 3 Example: RRR arm with spherical wrist 0 from the • For the DH parameters below, we can derive R 3 forward kinematics: ⎡ ⎡ − ⎤ ⎤ c c c c c c s s s s 1 23 1 23 1 ⎢ ⎥ = − − 0 R s c s s c ⎢ ⎥ 3 1 23 1 23 1 ⎢ ⎥ ⎣ s c 0 ⎦ 23 23 3 is given as follows: • We know that R 6 ⎡ − − − ⎤ c c c s s c c s s c c s 4 5 6 4 6 4 5 6 4 6 4 5 ⎢ ⎥ = + − + 3 R s c c c s s c s c c s s ⎢ ⎢ ⎥ ⎥ 6 6 4 4 5 5 6 6 4 4 6 6 4 4 5 5 6 6 4 4 6 6 4 4 5 5 ⎢ − ⎥ ⎣ s c s c c ⎦ 5 6 5 6 5 α i θ i link a i d i • To solve the inverse orientation kinematics: θ 1 1 0 90 d 1 θ 2 2 a 2 0 0 θ 3 3 a 3 0 0 4

Example: RRR arm with spherical wrist • Euler angle solutions can be applied. Taking the third column of 0 ) T R ( R 3 = + + c s c c r s c r s r 4 5 1 23 13 1 23 23 23 33 = − − + s s c s r s s r c r 4 5 1 23 13 1 23 23 23 33 = − c s r c r 5 1 13 1 23 Example: elbow manipulator with spherical wrist • Derive complete inverse kinematics solution p α i θ i link a i d i θ 1 1 0 90 d 1 θ 2 2 a 2 0 0 θ 3 3 a 3 0 0 θ 4 4 0 -90 0 θ 5 5 0 0 0 0 such that: θ 3 • we are given H = T 6 6 0 0 d 6 ⎡ ⎤ ⎡ ⎤ o r r r x 11 12 13 ⎢ ⎥ ⎢ ⎥ = = o o , R r r r ⎢ ⎥ ⎢ ⎥ y 21 22 23 ⎢ ⎥ ⎢ ⎥ ⎣ o ⎦ ⎣ r r r ⎦ z 31 32 33 5

Example: elbow manipulator with spherical wrist • First, we find the wrist center: , − ⎡ ⎤ ⎡ ⎤ x o d r c x 6 13 ⎢ ⎥ ⎢ ⎥ = − y o d r ⎢ ⎥ ⎢ ⎥ c y 6 23 ⎢ ⎥ ⎢ − ⎥ z o d r ⎣ ⎦ ⎣ ⎦ c z 6 33 • Inverse position kinematics: • Where d is the shoulder offset (if any) and Example: elbow manipulator with spherical wrist • Inverse orientation kinematics: – Now that we know θ 1 , θ 2 , θ 3 , we know R 3 0 . need to find R 3 6 : ( ) R T 6 = 3 0 R R 3 Solve for θ 4 , θ 5 , θ 6 , Euler angles: • ( ( ) ) θ θ = + + + + − − + + atan atan 2 2 , , c c c c r r s s c c r r s s r r c c s s r r s s s s r r c c r r 4 4 1 1 23 23 13 13 1 1 23 23 23 23 23 23 33 33 1 1 23 23 13 13 1 1 23 23 23 23 23 23 33 33 ⎛ ( ) ⎞ θ = − ± − − ⎜ 2 ⎟ atan 2 , 1 s r c r s r c r ⎝ ⎠ 5 1 13 1 23 1 13 1 23 ( ) θ = − + − atan 2 , s r c r s r c r 6 1 11 1 21 1 12 1 22 6

Example: inverse kinematics of SCARA manipulator 0 : • We are given T 4 g ⎡ ⎡ ⎤ ⎤ R R o o 4 = 0 T ⎢ ⎥ 4 ⎣ 0 1 ⎦ + − + ⎡ ⎤ c c s s s c c s 0 a c a c 12 4 12 4 12 4 12 4 1 1 2 12 ⎢ ⎥ − − − + s c c s c c s s 0 a s a s ⎢ ⎥ = 12 4 12 4 12 4 12 4 1 1 2 12 ⎢ ⎥ − − − 0 0 1 d d 3 4 ⎢ ⎥ ⎣ 0 0 0 1 ⎦ α i θ i link a i d i θ 1 1 a 1 0 0 θ 2 2 a 2 180 0 3 0 0 d 3 0 θ 4 4 0 0 d 4 Example: inverse kinematics of SCARA manipulator 0 , R must have the following form: • Thus, given the form of T 4 − ⎡ ⎤ c s 0 α α ⎢ ⎥ = R s c 0 ⎢ ⎢ ⎥ ⎥ α α α α ⎢ ⎥ ⎣ 0 0 1 ⎦ α = θ + θ − θ = Where α is defined as: • 1 2 4 7

Example: number of solutions • How many solutions to the inverse position kinematics of a planar 3-link arm? Example: number of solutions • What if now we describe the desired position and orientation of the end effector? 8

Velocity Kinematics • Now we want to relate end-effector linear and angular velocities with the joint velocities • First we will discuss angular velocities about a fixed axis First we will discuss angular velocities about a fixed axis • Second we discuss angular velocities about arbitrary (moving) axes • We will then introduce the Jacobian • Finally, we use the Jacobian to discuss numerous aspects of manipulators: – Singular configurations g g – Dynamics – Joint/end-effector forces and torques Angular velocity: fixed axis • When a rigid body rotates about a fixed axis, every point moves in a circle 9

Angular velocity: arbitrary axis • Skew-symmetric matrices – Definition: a matrix S is skew symmetric if: + S = S T S T S 0 0 – i.e. i e Angular velocity: arbitrary axis • Example: – Let i , j , k be defined as follows: ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 0 0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ˆ = ˆ = ˆ = i 0 , j 1 , k 0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ 0 ⎦ ⎣ 0 ⎦ ⎣ 1 ⎦ – Then we can define the skew symmetric matrices S ( i ), S ( j ), S ( k ): − ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 0 0 0 0 1 0 1 0 ( ) ( ) ( ) ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ˆ = − ˆ = ˆ = S i 0 0 1 , S j 0 0 0 , S k 1 0 0 , ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ ⎥ ⎢ ⎣− ⎢ ⎥ ⎥ ⎢ ⎢ ⎥ ⎥ ⎣ ⎣ 0 0 1 1 0 0 ⎦ ⎦ ⎣ 1 1 0 0 0 0 ⎦ ⎦ ⎣ ⎣ 0 0 0 0 0 0 ⎦ ⎦ 10

Angular velocity: arbitrary axis • Properties of skew-symmetric matrices 1. The operator S is linear ( ( ) ) ( ) ( ) ( ) ( ) α α + + β β = α α + + β β ∀ ∀ ∈ ∈ α α β β ∈ ∈ 3 S S a a b b S S a a S S b b a a , , b b , , , , , , R R R R 2. The operator S is known as the cross product operator ( ) = × ∀ ∈ 3 S a p a p , a, p R – This can be seen by the definition of the cross product: Angular velocity: arbitrary axis • Properties of skew-symmetric matrices 3. For ( ) ∈ ∈ 3 R SO 3 , a R – This can be shown for an R as follows: Thi b h f R f ll T = ( ) ( ) RS a R S Ra 4 For any 4. For any ( ) ∈ ∈ n S so n , x R = x T Sx 0 11

Angular velocity: arbitrary axis ⎡ ⎤ ( ) d ( ) ( ) d θ = θ T θ = SR R R R R ⎢ ⎥ θ θ ⎣ ⎦ d d • Derivative of a rotation matrix Angular velocity: arbitrary axis • Example: let R = R x, θ , then using the previous results we have: 12

Angular velocity: arbitrary axis • Now consider that we have an angular velocity about an arbitrary axis bit i • Further, let R = R ( t ) Next class… • Derivation of the Jacobian 13