Lecture 5: Robot Dynamics and Simulation

Home , Dynamics (mechanics), Equations of motion, Generalized coordinates

ME 328: Medical Robotics Winter 2019

Lecture 5: Robot dynamics and simulation

Allison Okamura Stanford University Robot dynamics equations of motion describe the relationship between forces/torques and motion (in joint space or workspace variables)

two possible goals: ˙ ¨ 1. Given motion variables (e.g. ✓~ , ✓~ , ✓~ or ~x, ~ x,˙ ~x¨ ), what joint torques ( ~⌧ ) or end-effector forces ( f~ ) would have been the cause? (this is inverse dynamics) ~ 2. Given joint torques ( ~⌧ ) or end-effector forces ( f ), ˙ ¨ what motions (e.g. ✓~ , ✓~ , ✓~ or ~x, ~x, ˙ ~x¨ ) would result? (this is forward dynamics) developing equations of motion using Lagrange’s equation

The Lagrangian is L = T V where T is the kinetic energy of the system and V is the potential energy of the system

d @L @L Lagrange’s equation is = Qj dt @q˙ @q ✓ j ◆ j @q where j =1 , 2 ,...,n , and q˙ = j is the generalized j @t velocity and Q j is the nonconservative generalized force corresponding to the generalized coordinate qj what are generalized coordinates?

• equations of motion can be formalized in a number of different coordinate systems

• n independent coordinates are necessary to describe the motion of a system having n degrees of freedom

• any set of n independent coordinates is called generalized coordinates: q1,q2,...qn

• these coordinates may be lengths, angles, etc. generalized forces

• When external forces act on the system, the conﬁguration changes: generalized coordinates q j change by q j , j =1, 2,...,n

• If U j is the work done in changing q j by q j , the Uj corresponding generalized force is Q j = , where j =1, 2,...,n qj

• Q j is a force/moment and q j is a linear/angular displacement. This can include dissipation (damping). d @L @L ( ) = Qj,wherej =1, 2,...,n dt @q˙j @qj where@q does Lagrange’s j whereq ˙j = @t is the generalized velocity and Qj is the nonconservative generalized forceequation corresponding to come the generalized from? coordinate qj. Where does it come from?

Hamilton’s principle of least action: a system moves from q(t1)toq(t2) in such a way that the following integral takes on the least possible value. S = t2 L(q, q,˙ t)dt t1 R The calculus of variations is used to obtain Lagrange’s equations of motion. We’re concerned with minimizing t2 f (y(t), y˙(t); t) dt t1 R The minimization leads to the equation @f d @f =0 @y dt @y˙

If there is more than one set of variables in the functional f (e.g. yi andy ˙i) then you get one equation for each set.

To use for coupled systems, we need potential and kinetic energy expressions in matrix form.

Deﬁne: xi, displacement of mass mi Fi, force applied in the direction of xi at mass mi

3 This can be rewritten in matrix form: 1 T T = 2 ˙x M ˙x where: x˙ 1(t) x˙ 2(t) ˙x = 2 . 3 . 6 7 6 x˙ (t) 7 6 n 7 M =4diag(m15,m2,...,mn)

In general, the mass matrix M might not be diagonal, for example if we use a di↵erent set of generalized coordinates, such as the relative displacements of the masses, rather than the absolute (ground-referenced) displacement. We can also use generalized coordinates:

1 T V = 2 q Kq K generalized sti↵ness matrix ! 1 T T = 2 ˙q M ˙q M generalized mass matrix ! Note: In general,usingM needLagrange’s not be diagonal equation to

Using Lagrange’sderive Equation equations to derive of Equations motion of Motion d @L @L ( ) = Qj,wherej =1, 2,...,n dt @q˙j @qj L = T V Substitute: @L = @T @V @q˙j @q˙j @q˙j Last term is zero because P.E. is not dependent on velocities @L = @T ! @q˙j @q˙j @L = @T @V @qj @qj @qj d @T @T @V ( ) + = Qj,wherej =1, 2,...,n dt @q˙j @qj @qj

Qj is a nonconservative generalized force corresponding to coordinate qj, e.g. damping For a conservative system, d ( @T ) @T + @V = 0, where j =1, 2,...,n dt @q˙j @qj @qj 5 Example: Torsional System

Let q1 = ✓1, q2 = ✓2, q3 = ✓3

Kinetic energy: 1 ˙2 1 ˙2 1 ˙2 T = 2 J1✓1 + 2 J2✓2 + 2 J3✓3

Potential energy: V = 1 k ✓2 + 1 k (✓ ✓ )2 + 1 k (✓ ✓ )2 2 1 1 2 2 2 1 2 3 3 2

External moments are M1, M2, M3

Generalized force: th If Fxk,Fyk,Fzk are the external forces acting on the k mass of the system in the x, y, z directions: Q = (F @xk + F @yk + F @zk ) j k xk @qj yk @qj zk @qj th WherePxk, yk, zk are the displacements of the k mass in the x, y, z directions.

For a torsional system F is replaced by M, moment. Q = 3 M @✓k = 3 M @✓k j k=1 k @qj k=1 k @✓j P P Q = M @✓1 + M @✓2 + M @✓3 = M 1 1 @✓1 2 @✓1 3 @✓1 1 Q = M @✓1 + M @✓2 + M @✓3 = M 2 1 @✓2 2 @✓2 3 @✓2 2

6 adding dissipation since the left side of Lagrange’s equation only includes terms for potential and kinetic energy, any dissipative terms (e.g., damping) must be added on the right hand side (and Q j are now only the input forces/torques)

d @L @L @R = Q dt @q˙ @q j @q˙ ✓ j ◆ j j 1 R = b q˙2 2 j j j X is a simpliﬁed form of Rayleigh’s dissipation function example: double pendulum (review on your own) Double Pendulum Example

Double Pendulum Example

Velocity of m1: v1 = l1✓˙1 2 2 1 Velocity of m2: v2 =(v2x + v2y)Double2 Pendulum Example v2x = l1✓˙1 cos(✓1)+l2✓˙2 cos(✓2) v2y = l1✓˙1 sin(✓1)+l2✓˙2 sin(✓2)

Kinetic energy: 1 ˙ 2 1 ˙ ˙ 2 ˙ ˙ 2 T = 2 m1(l1✓1) + 2 m2((l1✓1 cos(✓1)+l2✓2 cos(✓2)) +(l1✓1 sin(✓1)+l2✓2 sin(✓2)) )

Velocity of m1: v1 = l1✓˙1 2 2 1 Velocity of m2: v2 =(v2x + v2y) 2 v2x = l1✓˙1 cos(✓1)+l2✓˙2 cos(✓2) v2y = l1✓˙1 sin(example:✓1)+l2✓˙2 sin(✓2 ) double pendulum

Velocity of m1: v1 = l1✓˙1 2 2 1 Kinetic energy: Velocity of m2: v2 =(v2x + v2y) 2 1 ˙ 2 1 ˙ v2x = l1✓˙1 cos(˙ ✓1)+l2✓˙2 2cos(✓2) ˙ ˙ 2 T = 2 m1(l1✓1) + 2 m2((l1✓1 cos(✓1)+l2✓2 cos(✓2)) +(l1✓1 sin(✓1)+l2✓2 sin(✓2)) ) v2y = l1✓˙1 sin(✓1)+l2✓˙2 sin(✓2)

Potential energy Kinetic energy: T = 1 m (l ✓˙ )2+ 1 m ((l ✓˙ cos(✓ )+l ✓˙ cos(✓ ))2+(l ✓˙ sin(✓ )+l ✓˙ sin(✓ ))2) V = m gl (1 cos(✓ )) + m g(l (1 2 cos(1 1 ✓1 ))2 + l2 (11 1 cos(1✓ )))2 2 2 1 1 1 2 2 2 1 1 1 2 1 1 2 2

The Lagrangian is: L = T V 1 2 ˙2 1 2 ˙2 Potential˙ energy˙ PotentialL = 2 (m1+ energym2)l1✓1+ 2 m2l2✓2+m2l1l2✓1✓2 cos(✓1 ✓2)+(m1+m2)gl1 cos(✓1)+ V = m1gl1(1 cos(✓1)) + m2g(l1(1 cos(✓1)) + l2(1 cos(✓2))) Vm2=glm2 cos(gl✓(12) cos(✓ )) + m g(l (1 cos( ✓ )) + l (1 cos( ✓ ))) 1 1 1 2 1 1 2 2 For ✓1: 3 @L 2 ˙ 2 ˙ ˙ ˙ = m1l1✓1 + m2l1✓1 + m2l1l2✓2 cos(✓1 ✓2) @✓1 3 d @L 2 ¨ ¨ ˙ ˙ ˙ dt ˙ =(m1+m2)l1✓1+m2l1l2✓2 cos(✓1 ✓2) m2l1l2✓2 sin(✓1 ✓2)(✓1 ✓2) @✓1 ⇣ ⌘ @L = l1g(m1 + m2)sin(✓1) m2l1l2✓˙1✓˙2 sin(✓1 ✓2) @✓1

Thus, the di↵erential equation for ✓1 becomes: (m + m )l2✓¨ + m l l ✓¨ cos(✓ ✓ )+m l l ✓˙2 sin(✓ ✓ )+l g(m + 1 2 1 1 2 1 2 2 1 2 2 1 2 2 1 2 1 1 m2)sin(✓1)=0

Divide through by l1 and this simpliﬁes to: (m +m )l ✓¨ +m l ✓¨ cos(✓ ✓ )+m l ✓˙2 sin(✓ ✓ )+g(m +m )sin(✓ )= 1 2 1 1 2 2 2 1 2 2 2 2 1 2 1 2 1 0

Similarly, for ✓2: @L 2 ˙ ˙ ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) @✓2 d @L ¨ ¨ ˙ ˙ ˙ dt ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) m2l1l2✓1 sin(✓1 ✓2)(✓1 ✓2) @✓2 ⇣ ⌘ @L = m2l1l2✓˙1✓˙2 sin(✓1 ✓2) l2m2g sin ✓2 @✓2

Thus, the di↵erential equation for ✓2 becomes: m l2✓¨ + m l l ✓¨ cos(✓ ✓ ) m l l ✓˙2 sin(✓ ✓ )+l m g sin ✓ =0 2 2 2 2 1 2 1 1 2 2 1 2 1 1 2 2 2 2

Divide through by l2 and this simpliﬁes to: m l ✓¨ + m l ✓¨ cos(✓ ✓ ) m l ✓˙2 sin(✓ ✓ )+m g sin ✓ =0 2 2 2 2 1 1 1 2 2 1 1 1 2 2 2 So, we have developed a very complicated set of coupled equations of motion from a very simple system!

⇡ For the initial conditions ✓1 = 2 and ✓2 = ⇡, you get the response below. If you perturbed these initial conditions slightly, you would get a very di↵erent response. This is evidence of a chaotic system. If you linearized the equations or added damping, you would get a much more predictable

4 The Lagrangian is: L = T V L = 1 (m +m )l2✓˙2+ 1 m l2✓˙2+m l l ✓˙ ✓˙ cos(✓ ✓ )+(m +m )gl cos(✓ )+ 2 1 example:2 1 1 2 2 2 2 double2 1 2 1 2 pendulum1 2 1 2 1 1 m2gl2 cos(✓2)

For ✓1: @L 2 ˙ 2 ˙ ˙ ˙ = m1l1✓1 + m2l1✓1 + m2l1l2✓2 cos(✓1 ✓2) @✓1 d @L 2 ¨ ¨ ˙ ˙ ˙ dt ˙ =(m1+m2)l1✓1+m2l1l2✓2 cos(✓1 ✓2) m2l1l2✓2 sin(✓1 ✓2)(✓1 ✓2) @✓1 ⇣ ⌘ @L = l1g(m1 + m2)sin(✓1) m2l1l2✓˙1✓˙2 sin(✓1 ✓2) @✓1

Thus, the di↵erential equation for ✓1 becomes: (m + m )l2✓¨ + m l l ✓¨ cos(✓ ✓ )+m l l ✓˙2 sin(✓ ✓ )+l g(m + 1 2 1 1 2 1 2 2 1 2 2 1 2 2 1 2 1 1 m2)sin(✓1)=0

Divide through by l1 and this simpliﬁes to: (m +m )l ✓¨ +m l ✓¨ cos(✓ ✓ )+m l ✓˙2 sin(✓ ✓ )+g(m +m )sin(✓ )= 1 2 1 1 2 2 2 1 2 2 2 2 1 2 1 2 1 0

Thus, the di↵erential equation for ✓2 becomes: m l2✓¨ + m l l ✓¨ cos(✓ ✓ ) m l l ✓˙2 sin(✓ ✓ )+l m g sin ✓ =0 2 2 2 2 1 2 1 1 2 2 1 2 1 1 2 2 2 2

4 The Lagrangian is: L = T V L = 1 (m +m )l2✓˙2+ 1 m l2✓˙2+m l l ✓˙ ✓˙ cos(✓ ✓ )+(m +m )gl cos(✓ )+ 2 1 2 1 1 2 2 2 2 2 1 2 1 2 1 2 1 2 1 1 m2gl2 cos(✓2)

Thus, the di↵erential equation for ✓1 becomes: (m + m )l2✓¨ + m l l ✓¨ cos(✓ ✓ )+m l l ✓˙2 sin(✓ ✓ )+l g(m + 1 2 1 1 2 1 2 2 1 2 2 1 2 2 1 2 1 1 m2)sin(✓1)=0

Divide through by l1 and this simpliﬁes to: (m +m )l ✓¨ +m l ✓¨ cos(✓ ✓ )+m l ✓˙2 sin(✓ ✓ )+g(m +m )sin(✓ )= 1 2 example:1 1 2 2 2 1 double2 2 2 2 pendulum1 2 1 2 1 0

Thus, the di↵erential equation for ✓2 becomes: m l2✓¨ + m l l ✓¨ cos(✓ ✓ ) m l l ✓˙2 sin(✓ ✓ )+l m g sin ✓ =0 2 2 2 2 1 2 1 1 2 2 1 2 1 1 2 2 2 2

4 but robots are not point masses... links could be represented by solid cylinders

moments of inertia about center: mr2 I = z 2 1 I = I = m 3r2 + h2 x y 12 moment of inertia tensor:

http://en.wikipedia.org/wiki/List_of_moments_of_inertia what do you do if the rotation is not about the center?

If the new axis of rotation is parallel to the original axis of rotation, use the parallel axis theorem: 2 Inew = Icm + mR

Icm moment of inertia of the object about an axis passing through its center of mass m object’s mass R perpendicular distance between the two axes and what if the new axis of rotation is not parallel to the original? the moment of inertia of a continuous solid body rotating about a known axis is calculated by I = ⇢(~r)d(~r)dV (~r) ZV ~r radius vector (from origin to volume element of interest) ⇢(~r) object’s mass density at ~r d(~r) shortest distance between a point at ~r and the axis of rotation integrated over the volume V of the body

... but to reduce the complexity of this week’s assignment, we will approximate the moment of inertia by assuming that we do have parallel axes considering kinetic energy of solids

sum translational and rotational components 1 1 T = mv2 + I!2 2 2

http://hyperphysics.phy-astr.gsu.edu/hbase/rke.html for robot dynamics background

class: CS223A / ME320: Introduction to Robotics

dynamics and robotics textbooks such as John J. Craig Introduction to Robotics: Mechanics and Control

… and many others

In Assignment 3, we will give you the dynamic equations, but it helps to understand where they come from! questions

• how do you think an RCM robot compares to a typical serial chain manipulator in terms of its dynamics?

• does the da Vinci have haptic feedback? (and how do the system dynamics affect this capability?) Dynamic simulation controller on one end, system dynamics on the other a controller computes the desired force e.g. f = kp*(x-xd) desired force (in computer)

endpoint force/torque in Assignment 3, you will simulate the this force and externally applied effects of system loads result in robot motion dynamics e.g., solve for x in f = mx¨ + bx˙ simulating equations of motion

goal: given an equation of motion and applied forces, what will the resulting robot motion be?

The dynamics equations we have are coupled, non-linear ODEs. They are hard (likely impossible) to solve analytically.

Instead, we solve them using numerical integration.

you can do a simple calculation yourself, i.e. integration using the trapezoidal rule, or use a handy and more accurate/robust Matlab function (Simulink is also an option) Numerical Solutions

In this course, we have primarily solved dynamic systems analytically, invoking Matlab to facilitate plotting and data manipulation. However, Matlab can also be a powerful tool for solving ordinary di↵erential equations (ODEs) directly.

There are a series of native Matlab functions that solve ODEs given initial conditions. Di↵erent functions are appropriate for di↵erent problem types (“sti↵ness”, corresponding to large di↵erences in scales) and and order of accuracy (low to high).

Solver Problem Type Order of Accuracy When to Use ode45 Nonsti↵ Medium Most of the time. This should be the ﬁrst solver you try. ode23 Nonsti↵ Low For problems with crude error tolerances or for solving moderately sti↵ problems. ode113 Nonsti↵ Low to high For problems with stringent error tolerances or for solving computationally intensive problems. ode15s Sti↵ Low to medium If ode45 is slow because the problem is sti↵. ode23s Sti↵ Low If using crude error tolerances to solve sti↵ systems and the mass matrix is constant. ode23t Moderately Sti↵ Low For moderately sti↵ problems if you need a solution without numerical damping. ode23tb Sti↵ Low If using crude error tolerances to solve sti↵ systems. ode45 is based on an explicit Runge-Kutta (4,5) formula, the Dormand-Prince pair. It is a one-step solver - in computing y(tn), it needs only the solution at the immediately preceding time point, y(tn-1). In general, ode45 is the best function to apply as a “ﬁrst try” for most problems.

The basic syntax for these solvers is: [T,Y] = solver(odefun,tspan,y0) where: solver is one of ode45, ode23,etc. odefun is a function handle that evaluates the right side of the di↵erential equations tspan is a vector specifying the interval of integration, [t0,tf] y0 is a vector of initial conditions

Example: First order system Let’s say that we want to solve the function

y˙ +2y =0 for the initial condition y(0) = 10.

3 Numerical SolutionsNumerical Solutions

In this course, we have primarilyIn this course, solved we dynamic have primarily systems solved analytically, dynamic invoking systems Matlab analytically, to facilitate invoking Matlab to facilitate plotting and data manipulation.plotting and However, data manipulation. Matlab can also However, be a powerful Matlab tool can for also solving be a powerful ordinary tool for solving ordinary di↵erential equations (ODEs)di↵erential directly. equations (ODEs) directly.

There are a series of nativeThere Matlab are a series functions of native that solve Matlab ODEs functions given that initial solve conditions. ODEs given Di↵erent initial conditions. Di↵erent functions are appropriatefunctions for di↵erent are appropriate problem types for di (“sti↵erent↵ness”, problem corresponding types (“sti to↵ness”, large di corresponding↵erences to large di↵erences in scales) and and orderin of scales) accuracy and (low and to order high). of accuracy (low to high).

Solver Problem TypeSolverOrderProblem of Accuracy Type WhenOrder to of Use Accuracy When to Use ode45 Nonsti↵ ode45MediumNonsti↵ MostMedium of the time. ThisMost should of the be the time. first This should be the first solver you try. solver you try. ode23 Nonsti↵ Numericalode23Low SolutionsNonsti↵ ForLow problems with crudeFor error problems tolerances with or crude error tolerances or for solving moderately sti↵ problems. In this course, we have primarily solved dynamicfor systems solving analytically, moderately invoking Matlab sti↵ problems. to facilitate ode113 Nonsti↵ plottingode113 andLow data manipulation.Nonsti to high↵ However, MatlabForLow problems can also to be high a powerful with stringenttool forFor solving problems error ordinary tolerances with stringent error tolerances di↵erential equations (ODEs) directly. or for solving computationallyor for solving intensive computationally prob- intensive prob- There are a series of native Matlab functionslems. that solve ODEs given initial conditions.lems. Di↵erent functions are appropriate for di↵erent problem types (“sti↵ness”, corresponding to large di↵erences ode15s Sti↵ in scales)ode15s andLow and orderSti to↵ medium of accuracy (low toIf high).ode45Low tois medium slow becauseIf theode45 problemis slow is because sti↵. the problem is sti↵. ode23s Sti↵ ode23sLow Sti↵ IfLow using crude error tolerancesIf using to crude solve error sti↵ tolerances to solve sti↵ Solver Problem Type Order of AccuracysystemsWhen and to Use the mass matrixsystems is and constant. the mass matrix is constant. ode45 Nonsti↵ Medium Most of the time. This should be the first ode23t Moderately Stiode23t↵ Low Moderately Sti↵ForLow moderatelysolver you try. sti↵ problemsFor moderately if you need sti↵ aproblems if you need a ode23 Nonsti↵ Low solutionFor problems without with numerical crude errorsolution tolerances damping. without or numerical damping. for solving moderately sti↵ problems. ode23tb Sti↵ ode113ode23tbNonstiLow↵Sti↵ Low to high IfLow usingFor problems crude with error stringent tolerancesIf error using tolerances to crude solve error sti↵ tolerances to solve sti↵ systems.or for solving computationallysystems. intensive problems. ode15s Sti↵ Low to medium If ode45 is slow because the problem is sti↵. ode45 is based on an explicitode45ode23s Runge-KuttaisSti based↵ on an (4,5)Low explicit formula, Runge-KuttaIf the using Dormand-Prince crude (4,5) error tolerances formula, to pair. solve the It sti Dormand-Prince↵ is a one-step pair. It is a one-step systems and the mass matrix is constant. solver - in computing ysolverode23t(tn), it -Moderately needs in computing only Sti↵ Low they( solutiontn), it needs atFor the moderately only immediately the sti↵ problems solution preceding if you at need the a immediately time point, preceding time point, y(tn-1). In general, ode45y(tn-1).is the In best general, functionode45 to applyis the as bestsolution a “first function without try” numerical to for apply mostdamping. as problems. a “first try” for most problems. ode23tb Sti↵ Low If using crude error tolerances to solve sti↵ systems. The basic syntax for theseThe solvers basic syntax is: for these solvers is: ode45 is based on an explicit Runge-Kutta (4,5) formula, the Dormand-Prince pair. It is a one-step [T,Y] = solver(odefun,tspan,y0)solver[T,Y] - in computing = solver(odefun,tspan,y0)y(tn), it needs only the solution at the immediately preceding time point, y(tn-1). In general, ode45 is the best function to apply as a “first try” for most problems. where: Thewhere: basic syntax for these solvers is: solver is one of ode45[T,Y],solverode23 = solver(odefun,tspan,y0),etc.is one of ode45, ode23,etc. odefun is a function handlewhere:odefun thatis evaluates a function the handle right that side evaluates of the di↵ theerential right equations side of the di↵erential equations tspan is a vector specifyingsolvertspanis the oneis ofinterval aode45 vector, ode23 of specifying,etc. integration, the[t0,tf] interval of integration, [t0,tf] odefun is a function handle that evaluates the right side of the di↵erential equations y0 is a vector of initialtspan conditionsy0 isis a a vector vector specifying of initial the interval conditions of integration, [t0,tf] y0 is a vector of initial conditions

Example: First orderExample:Example: system First order First system order system Let’s say that we want to solve the function

Let’s say that we want toLet’s solve say the that function we want to solvey˙ +2y =0 the function for the initial condition y(0)y =˙ 10.+2y =0 y˙ +2y =0 for the initial conditionfory(0) the = initial 10. condition y(0) =3 10.

3 3 First, you need to create a function of the formy ˙ = f(t, y): function ydot = my_function(t,y)

ydot = -2*y; % end of subprogram First, you need to create a function of the formy ˙ = f(t, y): And the code to apply initial condition and solve the system is: function ydot = my_function(t,y)tspan = [0,5]; % time duration for calculation y0 = 10; % initial condition

% Note that the equation of motion is in a ydot = -2*y; % end of subprogram% subprogram named my_func

[t,y] = ode45(’my_func’,tspan,y0); And the code to apply initial condition and solve the system is: % plot the response tspan = [0,5]; %figure time duration for calculation plot(t,y) y0 = 10; % initial conditiontitle(’First Order Response’) xlabel(’Time (s)’) ylabel(’Position (m)’) % Note that the equation of motion is in a % subprogram named my_func The resulting plot is shown below: First Order Response 10 [t,y] = ode45(’my_func’,tspan,y0); 9 8

% plot the response 6 figure 5 Position (m) plot(t,y) 4 title(’First Order Response’) 3 xlabel(’Time (s)’) 2 1 ylabel(’Position (m)’) 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Time (s) The resulting plot is shown below: Example: Second order system First Order Response 10 We will now look at the response of a second order system, an inverted pendulum with the following equation of motion: g ✓¨ sin(✓)=0 9 l

4 8

5 Position (m) 4

0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Time (s)

Example: Second order system We will now look at the response of a second order system, an inverted pendulum with the following equation of motion: g ✓¨ sin(✓)=0 l

4 trajectory generation

reference: Chapter 7 of Introduction to Robotics by J. J. Craig (any edition) discussion

why would you want to generate a trajectory? would you want to do trajectory generation in Cartesian space or joint space? teleoperation autonomy

user provides the surgical planning desired position speciﬁes the start/ (trajectory) at each end points and control loop desired via points

for autonomous robots, we typically specify the trajectory based on start point, end point, via points, motion (e.g., velocity) constraints, and/or time constraints discussion

what properties might you want in a trajectory? Trajectory generation goal move a manipulator from an initial pose to a ﬁnal pose in a smooth manner

what does smooth mean? at least C1 continuous position profile at least C0 continuous velocity profile possibly continuous acceleration profile point-to-point trajectory generation

consider the problem of moving a robot end- effector from its initial 3D pose to a ﬁnal 3D pose in a certain amount of time

x(t) p(0) p(t)= y(t) p(tf ) 2 z(t) 3 4 5

robot graphic courtesy Fidel Hernandez a smooth path

x(t) 2 3 cubic polynomial: p(t)= y(t) = a0 + a1t + a2t + a3t 2 z(t) 3 4 5 2 3 x(t)=ax0 + ax1t + ax2t + ax3t 2 3 or y(t)=ay0 + ay1t + ay2t + ay3t 2 3 z(t)=az0 + az1t + az2t + az3t

x˙(t) 2 derivatives: ˙p (t)= y˙(t) = a1 +2a2t +3a3t 2 z˙(t) 3 4 5 x¨(t) ¨p (t)= y¨(t) =2a2 +6a3t 2 z¨(t) 3 4 5 constraints position and velocity at initial and ﬁnal times:

p(0) = p0 p(tf )=pf ˙p (0) = 0 ˙p (tf )=0 with four equations and four unknowns, you can solve for the coefﬁcients a in terms of p 0 and pf and the time tf

you can pick the time t f based on how quickly you want to move between to the two points, or based on a maximum velocity constraint example trajectory Example Asingle-linkrobotwitharotaryjoint •Muststartandstopatrest position •Muststartfromθ0 =15° and stop at θf =75°

•Mustdosointcubic polynomialf =3seconds C1 continuous Position

θ (t) = 15 00. + 20 00. t2 + − 44.4 t3

velocity quadraticVelocity polynomial . C0 continuousθ (t) = 40 00. t +13 33. t2

Acceleration acceleration.. linear θpolynomial(t ) = 40 00. − 26 66. t not continuous (at t = 0) now what? now that you have created a trajectory (i.e., you know the desired position of the robot at each time step), you can control the robot to follow this trajectory previously, we generated this trajectory using the “master” of the teleoperator discussion

why is a smooth trajectory important? why might you want a smoother trajectory? how could you compute this? what do the produced trajectories look like spatially? what if you design in joint space instead of Cartesian space?

how might via points be useful? how would you generate trajectories for them? discussion

how would you compute t f based on a deﬁned maximum velocity? what kind of trajectory would you want for a robot that inserts a needle into solid tissue?

bloodbot (Imperial College of London) Overview

gpd, similar to dynamic model or user input or Assignment 2 trajectory physical robot

x (t) x(t) d PD forces + plant - controller

x’(t) = f(x(t), gpd(x(t), xd(t)))

the ODE we solve numerically Assignment 3 Problem 0: Commentary on seminar Problem 1: Read papers, answer questions Problem 2: Modeling and simulation of medical robot dynamics Problem 3: Effects of dynamics on robot control To be posted tomorrow, due Thursday, Jan. 31 at 4 pm

FRIDAY’S SEMINAR IS AT 8:30 am! (in 320-105)