INVERSE KINEMATICS INTRODUCTION TO ROBOTICS: DISCUSSION 3 ROBERT PETER MATTHEW 20150915 DISCUSSION 3 INVERSE KINEMATICS ADMINISTRIVA • HW1 Self Grades: Thursday 17th @1700 • HW2 Due: Thursday 17th @1700 • Discussion sections: Tues 1000-1100, Wed 1100-1200 • Office hours: Mon, Thurs 1100-1200 • DSP Students: Letters of Accommodation required asap. rpmatthew <at> berkeley.edu 2/125 DISCUSSION 3 INVERSE KINEMATICS TERMINOLOGY Forward Kinematics END EFFECTOR POSITION – Given joint positions, find end / TORQUE effector coordinates Inverse Kinematics – Given end effector coordinates, find required joint positions Forward Dynamics – Given joint torques, find end effector forces/torques Inverse Dynamics – Given a desired end effector force/torque, find required joint torques JOINT POSITION / TORQUE rpmatthew <at> berkeley.edu 3/125 DISCUSSION 2 FORWARD KINEMATICS RIGID BODY MOTION Translation of Origin Relative Rotation rpmatthew <at> berkeley.edu 4/125 DISCUSSION 2 FORWARD KINEMATICS HOMOGENEOUS COORDINATES Points Vectors rpmatthew <at> berkeley.edu 5/125 DISCUSSION 2 FORWARD KINEMATICS RIGID BODY MOTION Homogeneous Coordinates: Note: all configurations are RELATIVE. rpmatthew <at> berkeley.edu 6/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS The Kinematics of a robotic manipulator describes the relationship between the motion of the joints and the motion of the rigid bodies that make up the manipulator. = 1 1 1 = 1 1 1 = 1 1 1 1 Note: all configurations are RELATIVE. rpmatthew <at> berkeley.edu 7/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS = 1 1 1 1 = = = = = rpmatthew <at> berkeley.edu 8/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS = 1 1 1 1 = = = = = rpmatthew <at> berkeley.edu 9/125 DISCUSSION 2 FORWARD KINEMATICS EXPONENTIAL COORDINATES Rigid body motion as: solutions to coordinate transforms differential equations rpmatthew <at> berkeley.edu 11/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS The effect of multiple RBMs can be found via the composition of multiple matrix exponents. For any reference frame at a zero configuration, we can write: = 0 �11 �22 Note: all configurations are in ABSOLUTE coordinates. rpmatthew <at> berkeley.edu 12/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS = 0 �11 �22 0 3 0 0 − = 0 = 3 0 0 0 1 1 2 1 0 6 (0) = 0 3 3 1 rpmatthew <at> berkeley.edu 13/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS = 0 �11 �22 0 3 0 0 − = 0 = 3 0 0 0 1 1 2 1 0 6 (0) = 0 3 3 1 rpmatthew <at> berkeley.edu 14/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS = 0 �11 0 0 × = = 0 0 − 0 1 1 6 (0) = 0 0 3 1 rpmatthew <at> berkeley.edu 15/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS = 0 �11 0 0 × = = 0 0 − 0 1 1 6 (0) = 0 0 3 1 rpmatthew <at> berkeley.edu 16/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS = 0 �11 0 0 × = = 0 0 − 0 1 1 6 (0) = 0 0 3 1 rpmatthew <at> berkeley.edu 17/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS = 0 �11 0 0 × = = 0 0 − 0 1 1 6 (0) = 0 0 3 1 rpmatthew <at> berkeley.edu 18/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS Given a valid configuration in the workspace, find . 2/2 2/2 10 3 2 ( ) = 2/2 2/2 0 3 2 0 − 0 1 0 1 1 = 0 �11 6 (0) = 0 0 3 1 rpmatthew <at> berkeley.edu 23/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS Given a valid configuration in the workspace, find . 2/2 2/2 10 3 2 ( ) = 2/2 2/2 0 3 2 0 − 0 1 0 1 1 = 0 �11 −1 2/2 2/2 0 0 = 2/2 2/2 0 0 − 0 �11 0 0 1 1 rpmatthew <at> berkeley.edu 24/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS 2/2 2/2 0 0 = 2/2 2/2 0 0 − 0 �11 0 0 1 1 0 /4 0 0 = /4 0 0 0 0 −0 1 0 ̂11 0 0 0 0 = 0 = /4 0 11 1 /4 rpmatthew <at> berkeley.edu 25/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS Given a valid configuration in the workspace, find . 1 If the point is not in the workspace it is infeasible, and no angle can be found. 1 The closest feasible point however can be found rpmatthew <at> berkeley.edu 28/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE I Given a point , , find the corresponding joint angles , 1 2 Think of this problem in polar coordinates. Each target point , has a corresponding , For , to be in the workspace, + ≤ 1 2 rpmatthew <at> berkeley.edu 31/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE I Given a point , , find the corresponding joint angles , 1 2 Think of this problem in polar coordinates. Each target point , has a corresponding , For , to be in the workspace, + ≤ 1 2 rpmatthew <at> berkeley.edu 32/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE I Given a point , , the lengths , , are known. 1 2 Using the law of cosines: + = 2 2 2 2 1 2 − 12 Therefore = ± 2 rpmatthew <at> berkeley.edu 35/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE I Similarly: = , 푎 Using the law of cosines: + = 2 2 2 2 1 − 2 1 Therefore = ± 1 rpmatthew <at> berkeley.edu 38/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE I Given a point , , find the corresponding joint angles , = ± 1 2 = ± 1 2 While , both determine the point , , they separately 1 2 control the radial position and distance Separation is a useful IK strategy rpmatthew <at> berkeley.edu 41/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Given a valid end effector configuration, find the corresponding joint angles 1 − 6 rpmatthew <at> berkeley.edu 42/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Separation of joints: : Distance , : Polar position of 3 − : Orientation of the end 1,2, effector 4 5 6 rpmatthew <at> berkeley.edu 49/125 DISCUSSION 3 INVERSE KINEMATICS INVARIANT POINTS The points and are special: does not change with rotations about , 1 2 does not change with rotations about , , 4 5 6 These points are said to be invariant rpmatthew <at> berkeley.edu 50/125 DISCUSSION 3 INVERSE KINEMATICS INVARIANT POINTS Invariant points line on axes of rotation: = � ie: = = �11 �22 �44 �55 �66 rpmatthew <at> berkeley.edu 53/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Given a desired end effector configuration and an initial configuration find 0 1−6 = �11 �22 �33 �44 �55 �66 0 = �11 �22 �33 �44 �55 �66 1 rpmatthew <at> berkeley.edu 56/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEMS Analytical solutions for inverse kinematics: Fast, efficient Other methods (numerical) exist and will be explored in the labs. rpmatthew <at> berkeley.edu 57/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM I Rotations about a single axis Given two points and find the angle of rotation Relative coordinates of and can be found from a point on the axis of rotation = = − − rpmatthew <at> berkeley.edu 60/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM I These relative points can then be projected on the circle of revolution: = ′ = − × 풗= − ′ ′ = ′ ′ ′ ′ ′ ′ ⋅ = × , ′ ′ ′ ′ rpmatthew <at> berkeley.edu 63/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II Rotations about subsequent axes , . , = 0,0 �11 �22 1 2 Given two points and find the angles , 1 2 Find intersection point . rpmatthew <at> berkeley.edu 67/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II = �1=1 �22 −�11= =�22 −�11 �22 find a point common to both axes = = = − − − rpmatthew <at> berkeley.edu 72/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II = = −�11 �22 = = = − − − = = −�11 �22 rpmatthew <at> berkeley.edu 73/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II = = −�11 �22 , are linearly independent = + + × = 1 1 2 2 − 1 2 = 1 2 − 1 1 2 1 − 2 2 = 1 2 2 2 × 2 − 2 − − − 푎1 2 2 Two1 solutions!2 rpmatthew <at> berkeley.edu 79/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II = = = −�11+ + �22× = + + × Solve as PK I: = = −�11 = = �22 −�11 �22 rpmatthew <at> berkeley.edu 83/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III Rotations about axis to a distance = �11 − Given two points and find the angles 1 can be thought of as a ball around rpmatthew <at> berkeley.edu 85/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III Given a point on the rotational axis, the relative coordinates can be found: = = − − The projection on the plane of rotation is given by: = ′ = ′ − − rpmatthew <at> berkeley.edu 87/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III The projection of on the rotational plane can also be found: = ′2 2 2 − − This defines two angles of rotation: = ± Two solutions!0 rpmatthew <at> berkeley.edu 90/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III The two solutions have the form: = ± 0 where: = × , ′+ ′ ′ ′ 0 = cos 푎 ′ 2 ′ 2 ′2 −1 − ′ ′ rpmatthew <at> berkeley.edu 91/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III If the desired point is on the edge of the ball there is only one solution rpmatthew <at> berkeley.edu 92/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III If the desired point outside the edge of the ball there are no solutions rpmatthew <at> berkeley.edu 93/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Separation of joints: : Distance , : Polar position of 3 − : Orientation of the end 1,2, effector 4 5 6 rpmatthew <at> berkeley.edu 94/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Given a desired end effector configuration and an initial configuration find 0 1−6 = �11 �22 �33 �44 �55 �66