1. ME451 Kinematics and Dynamics of Machine Systems Newton-Raphson Method October 04, 2013 Radu Serban University of Wisconsin-Madison

2. 2 Before we get started… Last Time: Position, Velocity, Acceleration Analysis. The Implicit Function Theorem. We can tell if a solution exists and if it is unique… but don’t know how to find it Today: Numerical solution of systems of nonlinear equations Newton-Raphson method Assignments: No book problems until midterm Matlab 4 – due October 9, Learn@UW (11:59pm) Adams 2 – due October 9, Learn@UW (11:59pm) Midterm Exam Friday, October 11 – regular class hours and place Review session: Wednesday, October 9, 6:30pm in ME1152

3. 3 Kinematic Analysis: Stages

4. 4 Kinematic Analysis

5. 5 Implicit Function Theorem (IFT)

6. 6 IFT: Implications for Position Analysis

7. Newton-Raphson Method for Nonlinear Equations 4.5 Isaac Newton (1642 – 1727)

8. 8 Solving a Nonlinear System The most important numerical algorithm in Kinematic Analysis Relied upon heavily by any multibody simulation engine (including ADAMS), used almost in all analysis modes Kinematics Dynamics Equilibrium Problem: Finding the solution of (systems of) equations such as:

9. 9 Newton-Raphson Method Real function of one real variable

10. 10 Newton-Raphson Method Geometric Interpretation

11. 11 Newton-Raphson Method Taylor series derivation

12. 12 Newton-Raphson Method Approximation Error and Convergence Rate

13. 13 Newton-Raphson Method Algorithm

14. 14 Newton-Raphson Method Example (scalar nonlinear equation) Initial guess Iteration 023.583853.06783 12.00000 - 3.06783 / 3.58385 = 1.143992.701940.37753 21.14399 - 0.37753 / 2.70194 = 1.004262.545240.01084 31.00426 - 0.01084 / 2.54524 = 1.000002.540310.00001 41.00000 - 0.00001 / 2.54031 = 1.00000

15. 15 Newton-Raphson Systems of nonlinear equations

16. 16 Newton-Raphson Method Example (system of nonlinear equations) Initial guess: x = 1.00000 y = 0.00000 Iteration 1: x = 1.61371 y = 0.61371 Iteration 2: x = 1.51227 y = 0.43236 Iteration 3: x = 1.50417 y = 0.40853 Iteration 4: x = 1.50396 y = 0.40810 Iteration 5: x = 1.50396 y = 0.40810 Iteration 6: x = 1.50396 y = 0.40810 Final residual: -1.37668e-14 +7.99361e-15 % Use Newton method to solve the nonlinear system % x-exp(y)=0 % ln(1+x)-cos(y)=0 % provide initial guess x = 1; y = 0; fprintf('Initial guess: x = %.5f y = %.5f\n', x, y); % perform a specified number of N-R iterations nit = 6; for it = 1:nit residual = [ x-exp(y) ; log(1+x)-cos(y) ]; jacobian = [1 -exp(y) ; 1/(1+x) sin(y)]; correction = jacobian\residual; x = x - correction(1); y = y - correction(2); fprintf('Iteration %i: x = %.5f y = %.5f\n', it, x, y); end fprintf('\nFinal residual: %+g\n', residual(1)); fprintf(' %+g\n', residual(2));

17. 17 [handout] Newton-Raphson: Position Analysis of Mechanism

18. 18 % Problem 3.5.6 - perform Position Analysis at time=0. % Numerical solution found using Newton's method. % Get a starting point (an initial guess) t = 0; q = [0.2 ; 0.2 ; pi/4]; fprintf('Initial guess: q = %.5f %.5f %.5f\n', q); % Perform a fixed number of Newton iterations for it = 1:5 residual = [ q(1) - 0.6 + 0.25*sin(q(3)) q(2) - 0.25*cos(q(3)) q(1)^2 + q(2)^2 - (t/10+0.4)^2]; jacobian = [ 1, 0, 0.25*cos(q(3)) 0, 1, 0.25*sin(q(3)) 2*q(1), 2*q(2), 0]; correction = jacobian\residual; q = q - correction; fprintf('Iteration %i: q = %.5f %.5f %.5f\n', it, q); end A first implementation Works but there’s a problem… Initial guess: q = 0.20000 0.20000 0.78540 Iteration 1: q = 0.42322 0.17678 0.78540 Iteration 2: q = 0.38125 0.13480 1.02284 Iteration 3: q = 0.38131 0.12156 1.06348 Iteration 4: q = 0.38125 0.12103 1.06543 Iteration 5: q = 0.38125 0.12103 1.06544 After 4 iterations it doesn’t make sense to keep iterating, the solution is already very good Question: how can we ensure we’re not iterating more than needed?

19. 19 % Problem 3.5.6 - perform Position Analysis at time=0. % Numerical solution found using Newton's method. % Get a starting point (an initial guess) t = 0; q = [0.2 ; 0.2 ; pi/4]; fprintf('Initial guess: q = %.5f %.5f %.5f\n', q); % Perform Newton iterations % as long as the correction is too large. normCorrection = 1.0; tolerance = 1.E-8; it = 0; while normCorrection > tolerance it = it + 1; residual = [ q(1) - 0.6 + 0.25*sin(q(3)) q(2) - 0.25*cos(q(3)) q(1^2+ q(2)^2 - (t/10+0.4)^2]; jacobian = [ 1, 0, 0.25*cos(q(3)) 0, 1, 0.25*sin(q(3)) 2*q(1), 2*q(2), 0]; correction = jacobian\residual; q = q - correction; normCorrection = norm(correction); fprintf('Iteration %i: q = %.5f %.5f %.5f\n', it, q); fprintf(' norm correction = %g\n', normCorrection); end A better implementation There still is a potential problem… Initial guess: q = 0.20000 0.20000 0.78540 Iteration 1: q = 0.42322 0.17678 0.78540 norm correction = 0.224428 Iteration 2: q = 0.38125 0.13480 1.02284 norm correction = 0.244744 Iteration 3: q = 0.38131 0.12156 1.06348 norm correction = 0.0427423 Iteration 4: q = 0.38125 0.12103 1.06543 norm correction = 0.00202878 Iteration 5: q = 0.38125 0.12103 1.06544 norm correction = 3.93443e-06 Iteration 6: q = 0.38125 0.12103 1.06544 norm correction = 1.51104e-11 Question: why do we use the correction as a stopping criteria and not the residual? Question: what if we cannot meet the tolerance criteria?

20. 20 % Problem 3.5.6 - perform Position Analysis at time=0. % Numerical solution found using Newton's method. % Get a starting point (an initial guess) t = 0; q = [0.2 ; 0.2 ; pi/4]; fprintf('Initial guess: q = %.5f %.5f %.5f\n', q); % Perform Newton iterations % as long as the correction is too large, % but do not exceed a preset number of iterations. normCorrection = 1.0; tolerance = 1.E-8; max_iterations = 20; for it = 1:max_iterations residual = [ q(1) - 0.6 + 0.25*sin(q(3)) q(2) - 0.25*cos(q(3)) q(1)^2 + q(2)^2 - (t/10+0.4)^2]; jacobian = [ 1, 0, 0.25*cos(q(3)) 0, 1, 0.25*sin(q(3)) 2*q(1), 2*q(2), 0]; correction = jacobian\residual; normCorrection = norm(correction); q = q - correction; fprintf('Iteration %i: q = %.5f %.5f %.5f\n', it, q); fprintf(' norm correction = %g\n', normCorrection); if normCorrection <= tolerance break; end An even better implementation Initial guess: q = 0.20000 0.20000 0.78540 Iteration 1: q = 0.42322 0.17678 0.78540 norm correction = 0.224428 Iteration 2: q = 0.38125 0.13480 1.02284 norm correction = 0.244744 Iteration 3: q = 0.38131 0.12156 1.06348 norm correction = 0.0427423 Iteration 4: q = 0.38125 0.12103 1.06543 norm correction = 0.00202878 Iteration 5: q = 0.38125 0.12103 1.06544 norm correction = 3.93443e-06 Iteration 6: q = 0.38125 0.12103 1.06544 norm correction = 1.51104e-11 Question: any ideas on how we could modify this for even better performance?

21. 21 A generic implementation % Problem 3.5.6 - perform Position Analysis at time=0. % Numerical solution found using Newton's method. % Get a starting point (an initial guess) t = 0; q = getInitialGuess(); fprintf('Initial guess: q = %.5f %.5f %.5f\n', q); % Perform Newton iterations as long as % the correction is too large, but do not % exceed a preset number of iterations. normCorrection = 1.0; tolerance = 1.E-8; max_iterations = 20; for it = 1:max_iterations residual = getPhi(q, t); jacobian = getJacobian(q, t); correction = jacobian\residual; normCorrection = norm(correction); q = q - correction; fprintf('Iteration %i: q = %.5f %.5f %.5f\n', it, q); fprintf(' norm correction = %g\n', normCorrection); if normCorrection <= tolerance break; end function Phi = getPhi(q, t) % Calculate the constraint violation at the % specified G.C. and time. Phi = [ q(1) - 0.6 + 0.25*sin(q(3)) q(2) - 0.25*cos(q(3)) q(1)^2 + q(2)^2 - (t/10+0.4)^2]; function Jac = getJacobian(q, t) % Calculate the constraint Jacobian at the % specified G.C. and time. Jac = [ 1, 0, 0.25*cos(q(3)) 0, 1, 0.25*sin(q(3)) 2*q(1), 2*q(2), 0]; function q0 = getInitialGuess() % Provide initial guess for G.C. q0= [0.2 ; 0.2 ; pi/4];

22. 22 Newton-Raphson Method Possible issues

23. 23 Newton-Raphson Method Position Analysis

24. 24 Position Analysis: Final Form function out = PositionAnalysis(timePoints, tol, max_it) %PositionAnalysis Perform kinematic position analysis. % out = PositionAnalysis(timePoints, tol, max_it) uses % Newton's method to solve the kinematic position analysis % problem at each time in the specified array 'timePoints' % and returns a matrix whose k-th column is the mechanism % configuration at time timePoints(k). % % The user must provide 3 functions: 'getInitialGuess()', % 'getPhi()' and 'getJac()'. % Get initial guess at first time. q = getInitialGuess(timePoints(1)); % Allocate space for output out = zeros(length(q), length(timePoints)); % Loop over all times for t = 1 : length(timePoints) % Perform at most max_it N-R iterations. Stop if % the correction norm is below the tolerance. for it = 1:max_it Phi= getPhi(q, timePoints(t)); Jac = getJacobian(q, timePoints(t)); corr = Jac \ Phi; q = q - corr; if norm(corr) <= tol break; end % Store results in output out(:,t) = q; end function Phi = getPhi(q, t) % Calculate the constraint violation at the % specified G.C. and time. Phi = [ q(1) - 0.6 + 0.25*sin(q(3)) q(2) - 0.25*cos(q(3)) q(1)^2 + q(2)^2 - (t/10+0.4)^2]; function Jac = getJacobian(q, t) % Calculate the constraint Jacobian at the % specified G.C. and time. Jac = [ 1, 0, 0.25*cos(q(3)) 0, 1, 0.25*sin(q(3)) 2*q(1), 2*q(2), 0]; function q0 = getInitialGuess(t0) % Provide initial guess for G.C. q0= [0.2 ; 0.2 ; pi/4];

25. 25 Kinematic Analysis

26. 26 Velocity Analysis (1)

27. 27 Observations: Note that as long as the constraint Jacobian is nonsingular, you can solve this linear system and recover the velocity The reason velocity analysis is easy is that, unlike for position analysis where you have to solve a nonlinear system, now you are dealing with a linear system, which is easy to solve Note that the velocity analysis comes after the position analysis is completed, and you are in a new configuration of the mechanism in which you are about to find out its velocity Velocity Analysis (2)

28. 28 Acceleration Analysis (1)

29. 29 Acceleration Analysis (2)