Inverse Kinematics Set goal configuration of end effector calculate interior joint angles Analytic approach – when linkage is simple enough, directly calculate joint angles in configuration that satifies goal Numeric approach – complex linkages At each time slice, determine joint movements that take you in direction of goal position (and orientation)

Forward Kinematics - review Articulated linkage – hierarchy of joint-link pairs Pose – linkage is a specific configuration Pose Vector – vector of joint angles for linkage Degrees of Freedom (DoF) – of joint or of whole figure Types of joints: revolute, prismatic Tree structure – arcs & nodes Recursive traversal – concatenate arc matrices Push current matrix leaving node downward Pop current matrix traversing back up to node

Inverse Kinematics End Effector L1 q3 q2 L3 L2 q1 Goal

Inverse Kinematics Underconstrained – if fewer constraints than DoFs Many solutions Overconstrained – too many constraints No solution Reachable workspace – volume the end effector can reach Dextrous workspace – volume end effector can reach in any orientation

Inverse Kinematics - Analytic Given arm configuration (L1, L2, …) Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, y1,y2, y3] Analytically compute goal configuration (q1,q2) Interpolate pose vector from initial to goal

Analytic Inverse Kinematics q2 L2 L1 Goal (X,Y) q1

Analytic Inverse Kinematics Multiple solutions Goal (X,Y)

Analytic Inverse Kinematics 180- q2 L1 (X,Y) q1 qT

Analytic Inverse Kinematics 180- q2 L1 (X,Y) q1 Y qT X

Law of Cosines C A a B

Analytic Inverse Kinematics (X,Y) 180- q2 q1 qT Y X

Analytic Inverse Kinematics

Iterative Inverse Kinematics When linkage is too complex for analytic methods At each time step, determine changes to joint angles that take the end effector toward goal position and orientation Need to recompute at each time step

Inverse Jacobian Method d2=EF-J2 End Effector q2 a2 x d2 - Compute instantaneous effect of each joint - Linear approximation to curvilinear motion - Find linear combination to take end effector towards goal position

Instantaneous linear change in end effector for ith joint Inverse Jacobian Method Instantaneous linear change in end effector for ith joint = (EF - Ji) x ai

Inverse Jacobian Method What is the change in orientation of end effector induced by joint i that has axis of rotation a i and position Ji? Angular velocity

Inverse Jacobian Method Solution only valid for an instantaneous step Angular affect is really curved, not straight line Once a step is taken, need to recompute solution

Inverse Jacobian Method Set up equations yi: state variable xi : system parameter fi : relate system parameters to state variable Inverse Jacobian Method - Mathematics

Inverse Jacobian Method - Mathematics Matrix Form

Inverse Jacobian Method - Mathematics Use chain rule to differentiate equations to relate changes in system parameters to changes in state variables

Inverse Jacobian Method - Mathematics Matrix Form

Inverse Jacobian Method Change in position (and orientation) of end effector Change in joint angles Linear approximation that relates change in joint angle to change in end effector position (and orientation)

Inverse Jacobian Method

Inverse Jacobian Method

Inverse Jacobian Method = (S - J1) x a1 = w1

The Matrices

The Matrices N DoFs V – desired linear and angular velocities 3x1, 6x1 J – Jacobian Matrix of partials 3xN, 6xN N x 1 q – change to joint angles (unknowns)

Pseudo Inverse of the Jacobian

Solving using the Pseudo Inverse LU decomposition

Adding a Control Term A solution of this form When put into this formula Like this After some manipulation, you can show that it… …doesn’t affect the desired configuration But it can be used to bias The solution vector

Form of the Control Term Desired angles and corresponding gains are input Bias to desired angles (not the same as hard joint limits) ‘z’ is H differentiated Where the deviation is large, you bump up the solution vector in such a way that you don’t disturb the desired effect

Some Algebraic Manipulation Include this in equation Isolate vector of unknown Rearrange to isolate the inverse

Solving the Equations LU decomp.

Control Term Use to bias to desired mid-angle Does not enforce joint angles Does not address “human-like” or “natural” motion Only kinematic control – no forces involved

Other ways to numerically IK Jacobian transpose Alternate Jacobian – use goal position HAL – human arm linkage Damped Least Squares CCD

Jacobian Transpose Use projection of effect vector onto desired movement

Jacobian Transpose S

Jacobian Transpose …

Alternate Jacobian G Use the goal postion instead of the end-effector!!?? !? …

Damped Least Squares G substitution Solve

Hueristic Human-Like Linkage (HAL) 7 DoF linkage RA (q1,q2 ,q3) , RB ( q4), RC(q5,q6 ,q7) 3 DoF G Decompose into simpler subproblems 1 DoF Set hand position and rotation based on relative position of Goal to shoulder 3 DoF Fix wrist position – use as Goal

Hueristic Human-Like Linkage (HAL) Set q4 based on distance between shoulder and wrist w e Assume axis of elbow is perpendicular to plane defined by s, e, w use law of cosines

Hueristic Human-Like Linkage (HAL) q4 w e Elbow lies on circle defined by w, s & q4 Determine elbow position based on heuristics For example: project forearm straight from hand orientation if arm intersects torso or a shoulder angle exceeds joint limit (or exceeds comfort zone) – Clamp to inside of limits

Hueristic Human-Like Linkage (HAL) q4 w e From e and s, determine RA From e and w and hand orientation, determine RB

Cyclic-Coordinate Descent Traverse linkage from distal joint inwards Optimally set one joint at a time Update end effector with each joint change At each joint, minimize difference between end effector and goal Easy if only trying to match position; heuristic if orientation too Use weighted average of position and orientation.

Cyclic-Coordinate Descent .

Cyclic-Coordinate Descent Rotational joint: .

Cyclic-Coordinate Descent Rotational joint: .

Cyclic-Coordinate Descent Rotational joint: .

Cyclic-Coordinate Descent Rotational joint: .

Cyclic-Coordinate Descent Translational joint: ..

IK w/ constraints Basic idea: Chris Welman, “Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001. Basic idea: Constraints are geometric, e.g., point-to-point, point-to-plan, specific orientation, etc. Assume starting out in satisfied configuration Forces are applied to system Detect, and cancel out, force components that would violate constraints.

IK w/ constraints Point-on-a-plane constraint Fa Fc Ft Given: geometric constraints & applied forces Determine: what constraints will be violated & what (minimal) forces are needed to counteract the components of the applied forces responsible for the violations.

Constraints To maintain constraints, need: Notation: .

Constraints IK Jacobian . Generalized force Constraint Jacobian

example constraint Geometric constraint on point that is function of pose Usually sparse .

Computing the constraint force Applied force Yet to be determined constraint force To counteract ga’s affect on constraints: g should lie in the nullspace of JcK .

Computing the constraint force The system is usually underconstrained Restrict gc to move the system in a direction it may not go Solve linear system to find Lagrange multiplier vector. .

Solving for Lagrange Multipliers gradient Shortest distance from A to B passing through a point P Direction of gradients are equal A Constrain P to lie on g B g Points on ellipse are set of points for which sum of distances to foci is equal to some constant

Solving for Lagrange Multipliers Use truncated SVD with backsubstitution on diagonal Nullspace basis. Range basis

Solving for Lagrange Multipliers

Feedback term Spring that penalizes deviation from constraints.

Implementation Handles on skeletons Point handle orientation handle Center-of-mass handle Each handle must know how to its value from q Each handle must know how to compute the Jacobian.

Constraints on handles Constraining a point handle to a location Constraining a point handle to a plane Constraining a point handle to a line Constraining an orientation handle to an orientation .

Dataflow approach Constraint function block Knows how to compute Its function in term of x Knows its Jacobian wrt x ..

Example network Jc C c1 c2 h1 h2 h3 h4 .. q