Human Kinematics
Kinematic representation! Iterative methods! Optimization methods
Kinematics Forward kinematics! given a joint configuration, what is the position of an end point on the structure?! Inverse kinematics! given the position for an end point on the structure, what angles do the joints need be to achieve that end point?
Quiz Which function is inverse kinematics? θ φ σ θ φ σ p θ, φ, σ = f(p) p p = f(θ, φ, σ)
Why inverse? More intuitive control! Maintain environment constraints! Calculate desired joint angles for control
Motion representation Global position and orientation are determined by 3 translational and 3 rotational DOFs Each joint can have up to 3 DOFs 1 DOF: knee 2 DOF: wrist 3 DOF: arm
Configurations q = x, y, z,,,, th, th, th, kn, x, y, z,,, θ thigh,φ thigh,σ thigh θ knee θ ankle,φ ankle
Constraints x, y, z,,, θ thigh,φ thigh,σ thigh d 0 Position constraint d 1 θ knee C(q) =h(q) p = 0 d 2 p θ ankle,φ ankle h(q) h(q) =T(x, y, z)r(,, )T(d 0 )R( th, th, th )T(d 1 )R( kn )T(d 2 )R( an, an )x a point on foot in world coordinates a point on foot in local coordinates
Solutions Closed form solutions can only be found for fairly simple mechanisms! Numerical solutions! No solution! Single solution! Multiple solution
Under-specified problem Multiple solutions! Mostly bad! How do we find the optimal solution?! Heuristics (move the outermost links first)! Closest to the current configuration! Energy minimization! Natural looking motion (whatever it means)
Kinematic representation! Iterative methods! Optimization methods
Iterative method Use inverse of Jacobian to iteratively step all the joint angles towards the goal! Girard and Maciejewski, Computational modeling for the computer animation of legged figures, SIGGRAPH 85
Jacobian matrix Constraint (m) i DOF (n) j C i q j Jacobian is a m by n matrix that relates differential changes of q to changes of C Jacobian maps the velocity in joint space to velocities in Cartesian space Jacobian depends on current state
IK and the Jacobian J = C q C = J q q = J 1 C q new = q + tj 1 C Linearize about current q
Invert Jacobian But Jacobian is most likely non-square Compute the pseudo inverse Jacobian J + C = J q J T C = J T J q (J T J) 1 J T C =(J T J) 1 J T J q J + C = q J + =(J T J) 1 J T
Under-constrained Where #DOFs is greater than #constraints, the problem is under-constrained and J T J is not invertible J @q = @C @q = J T (JJ T ) 1 @C
Problems Pseudo inverse returns the least square minimum norm solution! Singularities cause deficient Jacobian! Linearization about current state causes error
Kinematic representation! Iterative methods! Optimization methods
Optimization method Find a solution that optimizes some numeric metric and satisfies constraints! Numeric metric! A function of q that measures the quantity to be minimized! Also called objective function
Optimization method Solve for joint configuration q min q subject to G(q) C(q) =0
Constraint derivatives We need a direction to move joints in such way that the handle moves towards the goal! The derivative of the constraint, i.e. Jacobian, gives directions where handles move if joints move h(q) p C(q) =h(q) p = 0 C(q) q = h(q) q
Constraint derivatives C(q) =h(q) p = 0 h(q) =T(x, y, z)r(θ 0,φ 0,σ 0 )TR(θ 1 )TR(θ 2,φ 2 )h k q =[x, y, z, θ 0,φ 0,σ 0,θ 1,θ 2,φ 2 ] C(q) q = h(q) q x, y, z, θ 0,φ 0,σ 0 h(q) = T(x, y, z)r(θ 0,φ 0,σ 0 )T R(θ 1) TR(θ 2,φ 2 )h k θ 1 θ 1 θ 1 p Need to know how to compute derivatives for each transformation θ 2,φ 2 h(q) h k : local coordinate of h
Quiz What is the derivative for the translation matrix?!!!! 2 6 4 1 0 0 3 0 1 0 5 0 0 1 2 0 0 0 1 3 7 5 What is the derivative for a rotation matrix about z axis? 2 3 6 4 cos sin 0 0 sin cos 0 0 0 0 1 0 0 0 0 1 7 5
Constraint derivatives h(q) What is the most efficient way to compute the? q Compute the gradient for the DOF on the outermost link first x, y, z, θ 0,φ 0,σ 0 h = T 0 (x, y, z)r 0 (θ 0,φ 0,σ 0 )T 1 R 1 (θ 1 )T 2 R 2 (θ 2,φ 2 )h k θ 1 p θ 2,φ 2 h(q) h k : local coordinate of h
Quiz What s the dimension of a system with n dofs and m position constraints?
Optimization method Solve for joint configuration q min q subject to G(q) C(q) =0
Objective functions Joint velocity! Power consumption! Similarity to the rest pose! Similarity to the natural pose
Unconstrained optimization Treat each constraint as a separate metric and minimize weighted sum of all metrics! Also called penalty methods! each spring pulls on constraint with force proportional to violation of the constraint
Unconstrained optimization Softening constraints by adding them to the objective function Minimize F (q) =G(q)+ i w i C i (q) 2
Gradient descent For each iteration, move in the direction of negative gradient @F @q = @G @q 2 X i w i @C i @q T C i Update the state q new = q α F q
Optimization loop while F (q) > compute @F @q = @G(q) @q +2X i w i ( @C i @q )T C i update q = q @F @q
Search step size The step size in each search iteration can be determined arbitrarily, through a binary search, or solved by another optimization process Step size: α where min α F (q + α q) q new = q + α q
Unconstrained optimization Pros:! simple and fast, no linear system to solve! near-singular configurations is less of a problem! Cons:! can t maintain constraints exactly! constraints fight against each other and the objective function
Constrained optimization Treat constraints as hard constraints! Use Lagrangian formulation to solve a nonlinear optimization L(q,λ)=G(q) λ C min q,λ L(q,λ)
Constrained method Pros! Enforce constraints exactly! Quadratic convergence! Cons! Large system of equations! Near-singular configurations cause instability
Leg example L0 hip T 0h T h1 R(q 0 )R(q 1 )R(q 2 ) L1 knee T 1k R(q 3 ) L2 T k2 ankle L3 R(q 4 )R(q 5 ) T 2a T a3 h 0
Leg example node0 hip q 0, q 1, q 2 q = {q 0, q 1, q 2, q 3, q 4, q 5 } node1 knee node2 q 3 To evaluate gradients of the obj function at each iteration, we need to evaluate constraint and compute Jacobian. C(q) = T 0h R 0 R 1 R 2 T h1 T 1k R 3 T k2 T 2a R 4 R 4 T a3 h 0 - p How to compute J(q)? q 4, q 5 node3 ankle h 0 p
Compute Jacobian T 0h T h1 R(q 0 )R(q 1 )R(q 2 ) Consider the derivative of C with respect to q 5 joint T 1k R(q 3 ) T k2 T 2a R(q 4 )R(q 5 ) T a3 h 0 node @C = T 0h R(q 0 )R(q 1 )R(q 2 )T h1 T 1k R(q 3 )T k2 T 2a R(q 4 ) @R T a3 h 0 @q 5 @q 5 node->getparentbodynode()->gettransform() joint->gettransformfromparentbodynode() joint->gettransform(0) joint->gettransformderivative(1) joint->gettransformfromchildbodynode().inverse()