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 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 berkeley.edu 3/125 DISCUSSION 2 FORWARD KINEMATICS RIGID BODY MOTION

Translation of Origin Relative Rotation

rpmatthew berkeley.edu 4/125 DISCUSSION 2 FORWARD KINEMATICS HOMOGENEOUS COORDINATES

Points Vectors

rpmatthew berkeley.edu 5/125 DISCUSSION 2 FORWARD KINEMATICS RIGID BODY MOTION

Homogeneous Coordinates:

Note: all configurations are RELATIVE.

rpmatthew 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 berkeley.edu 7/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS

= 1 1 1 1 𝒒𝒒𝐴𝐴 𝑹𝑹𝐴𝐴𝐴𝐴 𝒑𝒑𝐴𝐴𝐴𝐴 𝑹𝑹𝐵𝐵𝐶𝐶 𝒑𝒑𝐵𝐵𝐵𝐵 𝒒𝒒𝐶𝐶 =𝟎𝟎 𝟎𝟎 =

𝑹𝑹𝑨𝑨𝑨𝑨 𝑹𝑹𝒁𝒁 𝑹𝑹𝑩𝑩𝑩𝑩 𝑹𝑹𝒀𝒀 = = 𝟎𝟎 𝟑𝟑 𝒑𝒑𝑨𝑨𝑨𝑨 𝟎𝟎 𝒑𝒑𝑩𝑩𝑩𝑩 𝟎𝟎 𝟑𝟑 = 𝟎𝟎 𝟑𝟑 𝒒𝒒𝑪𝑪 𝟎𝟎 𝟎𝟎 rpmatthew berkeley.edu 8/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS

= 1 1 1 1 𝒒𝒒𝐴𝐴 𝑹𝑹𝐴𝐴𝐴𝐴 𝒑𝒑𝐴𝐴𝐴𝐴 𝑹𝑹𝐵𝐵𝐶𝐶 𝒑𝒑𝐵𝐵𝐵𝐵 𝒒𝒒𝐶𝐶 =𝟎𝟎 𝟎𝟎 =

𝑹𝑹𝑨𝑨𝑨𝑨 𝑹𝑹𝒁𝒁 𝑹𝑹𝑩𝑩𝑩𝑩 𝑹𝑹𝒀𝒀 = = 𝟎𝟎 𝟑𝟑 𝒑𝒑𝑨𝑨𝑨𝑨 𝟎𝟎 𝒑𝒑𝑩𝑩𝑩𝑩 𝟎𝟎 𝟑𝟑 = 𝟎𝟎 𝟑𝟑 𝒒𝒒𝑪𝑪 𝟎𝟎 𝟎𝟎 rpmatthew berkeley.edu 9/125 DISCUSSION 2 FORWARD KINEMATICS EXPONENTIAL COORDINATES

Rigid body motion as:

solutions to coordinate transforms differential equations

rpmatthew 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 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑒𝑒 𝑔𝑔 Note: all configurations are in ABSOLUTE coordinates.

rpmatthew berkeley.edu 12/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑔𝑔 𝜃𝜃 0 𝑒𝑒 𝑒𝑒 𝑔𝑔 3 0 0 − = 0 = 3 0 0 0 1 𝜉𝜉1 𝜉𝜉2 1 0 6 (0) = 0 3 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew berkeley.edu 13/125 DISCUSSION 2 FORWARD KINEMATICS FORWARD KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑔𝑔 𝜃𝜃 0 𝑒𝑒 𝑒𝑒 𝑔𝑔 3 0 0 − = 0 = 3 0 0 0 1 𝜉𝜉1 𝜉𝜉2 1 0 6 (0) = 0 3 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew berkeley.edu 14/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑔𝑔 0 0 × = = 0 0 −𝜔𝜔 𝑞𝑞 0 𝜉𝜉1 𝜔𝜔 1 6 (0) = 0 0 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew berkeley.edu 15/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑔𝑔 0 0 × = = 0 0 −𝜔𝜔 𝑞𝑞 0 𝜉𝜉1 𝜔𝜔 1 6 (0) = 0 0 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew berkeley.edu 16/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑔𝑔 0 0 × = = 0 0 −𝜔𝜔 𝑞𝑞 0 𝜉𝜉1 𝜔𝜔 1 6 (0) = 0 0 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew berkeley.edu 17/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS

= 0 𝜉𝜉�1𝜃𝜃1 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑔𝑔 0 0 × = = 0 0 −𝜔𝜔 𝑞𝑞 0 𝜉𝜉1 𝜔𝜔 1 6 (0) = 0 0 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎

rpmatthew 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 𝜉𝜉�1𝜃𝜃1 𝑔𝑔 𝜃𝜃 𝑒𝑒 𝑔𝑔6 (0) = 0 0 𝕀𝕀3 1 𝑔𝑔 𝟎𝟎 rpmatthew 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 𝜉𝜉�1𝜃𝜃1 −1 𝑒𝑒 2/2𝑔𝑔 𝜃𝜃 2𝑔𝑔/2 0 0 = 2/2 2/2 0 0 − 0 𝜉𝜉�1𝜃𝜃1 0 0 1 𝑒𝑒 1

𝟎𝟎

rpmatthew berkeley.edu 24/125 DISCUSSION 3 INVERSE KINEMATICS INVERSE KINEMATICS

2/2 2/2 0 0 = 2/2 2/2 0 0 − 0 𝜉𝜉�1𝜃𝜃1 0 0 1 𝑒𝑒 1

𝟎𝟎 0 /4 0 0 = /4 0 0 0 0 −𝜋𝜋0 1 0 𝜉𝜉̂1𝜃𝜃1 𝜋𝜋 0 𝟎𝟎 0 0 0 = 0 = /4 0 𝜉𝜉1𝜃𝜃1 1 /4 𝜃𝜃 𝜋𝜋

𝜋𝜋

rpmatthew 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 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 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 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 − 𝑟𝑟 𝛼𝛼 𝑎𝑎𝑎𝑎𝑎𝑎𝑎𝑎 𝑙𝑙1𝑙𝑙2 Therefore = ±

𝜃𝜃2 𝜋𝜋 𝛼𝛼

rpmatthew 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 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 berkeley.edu 41/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II

Given a valid end effector configuration, find the corresponding joint angles

𝜃𝜃1 − 𝜃𝜃6

rpmatthew 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 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 berkeley.edu 50/125 DISCUSSION 3 INVERSE KINEMATICS INVARIANT POINTS

Invariant points line on axes of rotation: = 𝜉𝜉�𝑖𝑖𝜃𝜃𝑖𝑖 𝑝𝑝 𝑒𝑒 𝑝𝑝 ie: = = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑝𝑝𝑏𝑏 𝑒𝑒 𝑒𝑒 𝑝𝑝𝑏𝑏 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤

rpmatthew berkeley.edu 53/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Given a desired end effector configuration and an initial configuration find 𝑔𝑔𝑑𝑑 0 1−6 = 𝑔𝑔 𝜃𝜃 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔𝑑𝑑 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑔𝑔0 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔1 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒

rpmatthew 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 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 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 berkeley.edu 63/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II

Rotations about subsequent axes , .

𝝎𝝎𝟏𝟏 𝝎𝝎𝟐𝟐 , = 0,0 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑔𝑔𝐴𝐴𝐴𝐴 𝜃𝜃1 𝜃𝜃2 𝑒𝑒 𝑒𝑒 𝑔𝑔𝐴𝐴𝐴𝐴 Given two points and find the angles , 𝒑𝒑 𝒒𝒒 𝜃𝜃1 𝜃𝜃2 Find intersection point .

𝒄𝒄

rpmatthew berkeley.edu 67/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II

= 𝜉𝜉�1𝜃𝜃=1 𝜉𝜉�2𝜃𝜃2 𝒒𝒒 𝑒𝑒 𝑒𝑒 𝒑𝒑 −𝜉𝜉�1𝜃𝜃1= =𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒒𝒒 𝑒𝑒 𝒑𝒑 −𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒒𝒒 𝒄𝒄 𝑒𝑒 𝒑𝒑 find a point common to both axes

𝒓𝒓 = = 𝒖𝒖 = 𝒑𝒑 − 𝒓𝒓 𝒛𝒛 𝒄𝒄 − 𝒓𝒓 𝒗𝒗 𝒒𝒒 − 𝒓𝒓

rpmatthew berkeley.edu 72/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II

= = −𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒒𝒒= 𝒄𝒄 𝑒𝑒 𝒑𝒑 = 𝒖𝒖 = 𝒑𝒑 − 𝒓𝒓 𝒛𝒛 𝒄𝒄 − 𝒓𝒓 𝒗𝒗 𝒒𝒒 − 𝒓𝒓 = = −𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒗𝒗 𝒛𝒛 𝑒𝑒 𝒖𝒖

rpmatthew berkeley.edu 73/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II

= = −𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 , 𝑒𝑒 are𝒗𝒗 linearly𝒛𝒛 independent𝑒𝑒 𝒖𝒖 𝟏𝟏 𝟐𝟐 𝝎𝝎 =𝝎𝝎 + + ×

𝒛𝒛 𝜶𝜶𝝎𝝎𝟏𝟏 𝜷𝜷𝝎𝝎𝟐𝟐 𝜸𝜸 𝝎𝝎𝟏𝟏 𝝎𝝎𝟐𝟐 = 𝑇𝑇 𝑇𝑇 1 𝑇𝑇 𝜔𝜔1 𝜔𝜔2 𝜔𝜔2 𝑢𝑢 − 𝜔𝜔1 𝑣𝑣 𝛼𝛼 𝑇𝑇 2 = 1 2 𝑇𝑇 𝜔𝜔 𝜔𝜔 𝑇𝑇 − 1 𝑇𝑇 𝜔𝜔1 𝜔𝜔2 𝜔𝜔1 𝑣𝑣 − 𝜔𝜔2 𝑢𝑢 𝛽𝛽 𝑇𝑇 2 = 1 2 2 𝜔𝜔2 𝜔𝜔× 2 − 𝑇𝑇 2 𝑢𝑢 − 𝛼𝛼 − 𝛽𝛽 − 2𝛼𝛼𝛼𝛼𝜔𝜔1 𝜔𝜔2 𝛾𝛾 2 Two𝜔𝜔1 𝜔𝜔solutions!2

rpmatthew berkeley.edu 79/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM II

= = = −𝜉𝜉�1𝜃𝜃1+ + 𝜉𝜉�2𝜃𝜃2× =𝑒𝑒 +𝒗𝒗 𝒛𝒛+ 𝑒𝑒 ×𝒖𝒖 𝒛𝒛𝟏𝟏 𝜶𝜶𝝎𝝎𝟏𝟏 𝜷𝜷𝝎𝝎𝟐𝟐 𝜸𝜸𝟏𝟏 𝝎𝝎𝟏𝟏 𝝎𝝎𝟐𝟐 𝟐𝟐 𝟏𝟏 𝟐𝟐 𝟐𝟐 𝟏𝟏 𝟐𝟐 Solve𝒛𝒛 𝜶𝜶as𝝎𝝎 PK I:𝜷𝜷𝝎𝝎 𝜸𝜸 𝝎𝝎 𝝎𝝎 = = −𝜉𝜉�1𝜃𝜃1 = = 𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒗𝒗 𝒛𝒛𝟏𝟏 𝑒𝑒 𝒖𝒖 −𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑒𝑒 𝒗𝒗 𝒛𝒛𝟐𝟐 𝑒𝑒 𝒖𝒖

rpmatthew berkeley.edu 83/125 DISCUSSION 3 INVERSE KINEMATICS PADEN-KAHAN SUBPROBLEM III

Rotations about axis to a distance 𝟏𝟏 = 𝝎𝝎 𝜹𝜹 𝜉𝜉�1𝜃𝜃1 𝜹𝜹 𝒒𝒒 − 𝑒𝑒 𝒑𝒑 Given two points and find the angles 𝒑𝒑 𝒒𝒒 𝜃𝜃1 can be thought of as a ball around 𝜹𝜹 𝒒𝒒

rpmatthew 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 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 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 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 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 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 berkeley.edu 94/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II Given a desired end effector configuration and an initial configuration find 𝑔𝑔𝑑𝑑 0 1−6 = 𝑔𝑔 𝜃𝜃 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔1 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 Consider the invariant points and : 𝑏𝑏 = 𝑝𝑝 𝑞𝑞𝑤𝑤 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑏𝑏 𝑏𝑏 𝑝𝑝 𝜉𝜉�4𝑒𝑒𝜃𝜃4 𝜉𝜉�5𝑒𝑒𝜃𝜃5 𝜉𝜉�6𝑝𝑝𝜃𝜃6 𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤

rpmatthew berkeley.edu 97/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�=2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔1 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑏𝑏 𝑏𝑏 𝑝𝑝 𝜉𝜉�4𝑒𝑒𝜃𝜃4 𝜉𝜉�5𝑒𝑒𝜃𝜃5 𝜉𝜉�6𝑝𝑝𝜃𝜃6 𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 = 𝜉𝜉�1𝜃𝜃1 =𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔1𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝑔𝑔1𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝑔𝑔1𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝑔𝑔1𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 𝑒𝑒 =𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 𝜉𝜉�3𝜃𝜃3 𝑔𝑔1𝑞𝑞𝑤𝑤 −=𝑝𝑝𝑏𝑏 𝑒𝑒 𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 𝜉𝜉�3𝜃𝜃3 PADEN𝛿𝛿 KAHAN𝑒𝑒 Subproblem𝑞𝑞𝑤𝑤 − 𝑝𝑝𝑏𝑏 III

rpmatthew berkeley.edu 105/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 berkeley.edu 106/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�=2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔1 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑒𝑒 = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝑏𝑏 𝑏𝑏 𝑝𝑝 𝜉𝜉�4𝑒𝑒𝜃𝜃4 𝜉𝜉�5𝑒𝑒𝜃𝜃5 𝜉𝜉�6𝑝𝑝𝜃𝜃6 𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 = = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 𝑔𝑔1𝑞𝑞𝑤𝑤 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑞𝑞𝑤𝑤 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2 𝜉𝜉�3𝜃𝜃3 PADEN𝑔𝑔1𝑞𝑞𝑤𝑤 KAHAN𝑒𝑒 𝑒𝑒 Subproblem𝑒𝑒 𝑞𝑞𝑤𝑤 II

rpmatthew berkeley.edu 109/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 berkeley.edu 110/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2=𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 1 𝑔𝑔 𝑒𝑒 = 𝑒𝑒 𝑒𝑒 𝑒𝑒−1 𝑒𝑒 𝑒𝑒 𝑔𝑔1 𝑔𝑔𝑑𝑑𝑔𝑔0 = −𝜉𝜉�3𝜃𝜃3 −𝜉𝜉�2𝜃𝜃2 −𝜉𝜉�1𝜃𝜃1 𝑔𝑔2 𝑔𝑔1𝑒𝑒 𝑒𝑒 𝑒𝑒 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔2 𝑒𝑒 𝑒𝑒 𝑒𝑒 Consider a point on the axis = 𝑝𝑝6 𝜉𝜉6 𝜉𝜉�6𝜃𝜃6 𝑝𝑝6 𝑒𝑒 𝑝𝑝6 = =𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔2𝑝𝑝6 𝑒𝑒 𝑒𝑒 𝑒𝑒 𝑝𝑝6 PADEN KAHAN𝜉𝜉�4 𝜃𝜃Subproblem4 𝜉𝜉�5𝜃𝜃5 II 𝑔𝑔2𝑝𝑝6 𝑒𝑒 𝑒𝑒 𝑝𝑝6

rpmatthew berkeley.edu 119/125 DISCUSSION 3 INVERSE KINEMATICS EXAMPLE II = 𝜉𝜉�1𝜃𝜃1 𝜉𝜉�2𝜃𝜃2=𝜉𝜉�3𝜃𝜃3 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 1 𝑔𝑔 𝑒𝑒 = 𝑒𝑒 𝑒𝑒 𝑒𝑒−1 𝑒𝑒 𝑒𝑒 𝑔𝑔1 𝑔𝑔𝑑𝑑𝑔𝑔0 = −𝜉𝜉�3𝜃𝜃3 −𝜉𝜉�2𝜃𝜃2 −𝜉𝜉�1𝜃𝜃1 𝑔𝑔2 𝑔𝑔1𝑒𝑒 𝑒𝑒 𝑒𝑒 = 𝜉𝜉�4𝜃𝜃4 𝜉𝜉�5𝜃𝜃5 𝜉𝜉�6𝜃𝜃6 𝑔𝑔2 𝑒𝑒 𝑒𝑒 𝑒𝑒 =−𝜉𝜉�5𝜃𝜃5 −𝜉𝜉�4𝜃𝜃4 3 2 𝑔𝑔 𝑔𝑔 𝑒𝑒 𝜉𝜉�6𝜃𝜃𝑒𝑒6 𝑔𝑔3 𝑒𝑒 Consider a point not on the axis = 𝑝𝑝𝐸𝐸 𝜉𝜉6 PADEN KAHAN 𝜉𝜉�Subproblem6𝜃𝜃6 I 𝑔𝑔3𝑝𝑝𝐸𝐸 𝑒𝑒 𝑝𝑝𝐸𝐸

rpmatthew berkeley.edu 125/125