lecture6 dynamics

8/11/2019 Lecture6 Dynamics

1/53

ME 328: Medical RoboticsSpring 2013

Lecture 6:Robot dynamics and simulation

Allison Okamura and Troy AdebarStanford University


2/53

Robot dynamics


3/53

equations of motiondescribe the relationship between forces/torques and

motion (in joint space or workspace variables)

two possible goals:

1. Given motion variables (e.g. or ), what joint torques ( ) or end-effector forces ( ) would

have been the cause? (this is inverse dynamics)

~ ,

~ ,

~ ~

x, ~ x,

~ x

~ ~

f

2. Given joint torques ( ) or end-effector forces( ), what motions (e.g. or ) would

result? (this is forward dynamics)

~ ,

~ ,

~ ~

x, ~ x,

~ x

~ ~

f


4/53

developing equations of motion

using Lagranges equationThe Lagrangian is L = T V

where is the kinetic energy of the system and isthe potential energy of the system

T V


5/53

developing equations of motion

using Lagranges equationThe Lagrangian is L = T V

where is the kinetic energy of the system and isthe potential energy of the system

T V

Lagranges equation is ddt

L q j

L q j

= Q j

where , and is the generalizedvelocity and is the nonconservative generalizedforce corresponding to the generalized coordinate

j = 1 , 2 , . . . , n q j = q j t

j

q j


6/53

what are generalized coordinates? equations of motion can be formalized in anumber of different coordinate systems

independent coordinates are necessary todescribe the motion of a system having degreesof freedom

any set of independent coordinates is calledgeneralized coordinates:

these coordinates may be lengths, angles, etc.

q 1 , q 2 , . . . q n


7/53


8/53

where does Lagranges

equation come from?Hamiltons principle of least action: a system moves from q (t 1 ) to q (t 2 )in such a way that the following integral takes on the least possible value.S = R t 2t 1 L (q, q, t )dt

The calculus of variations is used to obtain Lagranges equations of mo-tion. Were concerned with minimizingR t 2

t 1f (y(t ), y(t ); t ) dt

The minimization leads to the equation f y

ddt

f y = 0


9/53

using Lagranges equation toderive equations of motion

ddt (

L qj )

L qj = Q j , where j = 1 , 2, . . . , n

L = T V

Substitute: L qj =

T qj

V qj

Last term is zero because P.E. is not dependent on velocities L qj = T qj

L qj =

T qj

V qj

ddt (

T qj )

T qj +

V qj = Q j , where j = 1 , 2, . . . , n

Q j is a nonconservative generalized force corresponding to coordinate q j ,e.g. damping

For a conservative system,ddt (

T qj )

T qj +

V qj = 0, where j = 1 , 2, . . . , n


10/53

adding dissipation

since the left side of Lagranges equation only includesterms for potential and kinetic energy, any dissipative

terms (e.g., damping) must be added on the right handside (where are now the input forces/torques only)j

ddt

L q j

L q j

= Q j R q j

where R =1

2 Xj

bj q 2

j

is a simplied form of Rayleighs dissipation function


11/53

example: double pendulum

Velocity of m 1 : v1 = l1 1Velocity of m 2 : v2 = ( v22 x + v22 y )

12

v2 x = l1 1 cos( 1 ) + l2 2 cos( 2 )v2 y = l1 1 sin( 1 ) + l2 2 sin( 2 )


12/53


Kinetic energy:T = 12 m 1 (l1 1 )2 + 12 m 2 (( l1 1 cos( 1 )+ l2 2 cos( 2 ))

2 +( l1 1 sin( 1 )+ l2 2 sin( 2 )) 2 )

Potential energyV = m 1 gl 1 (1 cos( 1 )) + m 2 g(l1 (1 cos( 1 )) + l2 (1 cos( 2 )))

The Lagrangian is:L = T V L = 12 (m 1 + m 2 )l

21

21 +

12 m 2 l

22

22 + m 2 l1 l2 1 2 cos( 1 2 )+( m 1 + m 2 )gl 1 cos( 1 )+

m 2 gl 2 cos( 2 )


13/53


For 1 : L

1= m 1 l21 1 + m 2 l21 1 + m 2 l1 l2 2 cos( 1 2 )

ddt

L

1 = ( m 1 + m 2 )l21 1 + m 2 l1 l2 2 cos( 1 2 ) m 2 l1 l2 2 sin( 1 2 )( 1 2 )

L 1 = l1 g(m 1 + m 2 )sin( 1 ) m 2 l1 l2 1 2 sin( 1 2 )

Thus, the di ff erential equation for 1 becomes:(m 1 + m 2 )l21 1 + m 2 l1 l2 2 cos( 1 2 ) + m 2 l1 l2 22 sin( 1 2 ) + l1 g(m 1 +m 2 ) sin( 1 ) = 0

Divide through by l1 and this simplies to:(m 1 + m 2 )l1 1 + m 2 l2 2 cos( 1 2 )+ m 2 l2 22 sin( 1 2 )+ g(m 1 + m 2 ) sin( 1 ) =0


14/53


Similarly, for 2 : L 2

= m 2 l22 2 + m 2 l1 l2 1 cos( 1 2 )

ddt

L

2 = m 2 l2 2 + m 2 l1 l2 1 cos( 1 2 ) m 2 l1 l2 1 sin( 1 2 )( 1 2 )

L

2 = m 2 l1 l2

1

2 sin(

1

2 )

l2 m 2 g sin

2

Thus, the di ff erential equation for 2 becomes:m 2 l22 2 + m 2 l1 l2 1 cos( 1 2 ) m 2 l1 l2 21 sin( 1 2 ) + l2 m 2 g sin 2 = 0

Divide through by l2 and this simplies to:m 2 l2 2 + m 2 l1 1 cos( 1 2 ) m 2 l1 21 sin( 1 2 ) + m 2 g sin 2 = 0

So, we have developed a very complicated set of coupled equations of motionfrom a very simple system!


15/53

but robots are not point masses...

links could be represented by solid cylinders

I z =mr 2

2

I x = I y =

1

12m 3 r 2 + h 2

http://en.wikipedia.org/wiki/List_of_moments_of_inertia

moment of inertia tensor:

moments of inertia about center:


16/53

what do you do if the rotation is notabout the center?

If the new axis of rotation is parallel to the originalaxis of rotation, use the parallel axis theorem:

I new = I cm + mR2

cm

R

moment of inertia of the object about anaxis passing through its center of mass

objects massperpendicular distance between the two axes


17/53

and what if the new axis of rotation isnot parallel to the original?

the moment of inertia of a continuous solid bodyrotating about a known axis is calculated by

radius vector (from origin to volume element of interest)

objects mass density at

shortest distance between a point at and the axisof rotation

I =

Z V

(~ r )d (~ r )dV (~ r )

(~ r )

~

r

~

r

d (~

r ) ~

r

integrated over the volume of the bodyV

... but to reduce the complexity of this weeks assignment, we willapproximate the moment of inertia by assuming that we do have parallel axes


18/53

considering kinetic energy of solids

sum translational and rotational components

T =1

2mv 2 +

1

2I 2

http://hyperphysics.phy-astr.gsu.edu/hbase/rke.html


19/53

for robot dynamics review

http://see.stanford.edu/materials/aiircs223a/

handout7_Dynamics.pdf

http://cs.stanford.edu/groups/manips/images/stories/teaching/cs223a/handouts/dynamics.pdf

... and many dynamics and robotics textbooks


20/53

questions


21/53

questions

how do you think an RCM robot comparesto a typical serial chain manipulator interms of its dynamics?


22/53

questions



23/53

questions


does the da Vinci have haptic feedback?(and how do the system dynamics affectthis capability?)


24/53

Dynamic simulation


25/53

controller on one end,system dynamics on the other

a controller computesthe desired force

this force and externally appliedloads result in robot motion

e.g. f = k p*(x-x d)

e.g., solve for x in f = m x + bx


26/53

controller on one end,system dynamics on the other

a controller computesthe desired force

this force and externally appliedloads result in robot motion

e.g. f = k p*(x-x d)

e.g., solve for x in f = m x + bx

in Assignment 3,you will model and

simulate the effects of

system dynamics


27/53

simulating equations of motion

we will use Matlabs ODE solver tools in order tosimulate robot dynamic behavior

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

you can do a simple calculation yourself,

i.e. integration using the trapezoidal rule, or use a handyand more accurate/robust Matlab function(Simulink is also an option)

you can show observe the resulting behavior


28/53

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

Solver Problem Type Order of Accuracy When to Useode45 Nonstiff Medium Most of the time. This should be the rst

solver you try.ode23 Nonstiff Low For problems with crude error tolerances or

for solving moderately sti ff problems.ode113 Nonstiff Low to high For problems with stringent error tolerances

or for solving computationally intensive prob-lems.

ode15s Stiff Low to medium If ode45 is slow because the problem is sti ff .ode23s Stiff Low If using crude error tolerances to solve sti ff

systems and the mass matrix is constant.ode23t Moderately Sti ff Low For moderately sti ff problems if you need a

solution without numerical damping.ode23tb Stiff Low If using crude error tolerances to solve sti ff

systems.

ode45 is based on an explicit Runge-Kutta (4,5) formula, the Dormand-Prince pair. It is a one-stepsolver - in computing y(t n ), 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.


29/53

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 ff erential equationstspan is a vector specifying the interval of integration, [t0,tf]y0 is a vector of initial conditions

Example: First order system

Lets say that we want to solve the function

y + 2 y = 0

for the initial condition y(0) = 10.

y + 2y = 0


30/53

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

ydot = -2*y; % end of subprogram

And the code to apply initial condition and solve the system is:tspan = [0,5]; % time duration for calculationy0 = 10; % initial condition

% Note that the equation of motion is in a% subprogram named my_func

[t,y] = ode45(my_func,tspan,y0);

% plot the responsefigureplot(t,y)title(First Order Response)xlabel(Time (s))ylabel(Position (m))

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

1

2

3

4

5

6

7

8

9

10First Order Response

Time (s)

P o s i

t i o n

( m )


31/53

trajectory generation


32/53

trajectory generation

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


33/53

teleoperation autonomy


34/53

teleoperation autonomyuser provides thetrajectory at each

control loop

surgical planningspecies the start/

end points anddesired via points


35/53

teleoperation autonomyuser provides thetrajectory at each

control loop

surgical planningspecies the start/

end points anddesired via points

for autonomous robots, we typically specify thetrajectory based on start point, end point, via points ,

motion (e.g., velocity) constraints,and/or time constraints


36/53

Trajectory generation goal

move a manipulator from an initial pose to anal pose in a smooth manner


37/53

Trajectory generation goal

move a manipulator from an initial pose to anal pose in a smooth manner

what does smooth mean?

at least C1 continuous position prole

at least C0 continuous velocity prolepossibly continuous acceleration prole


38/53

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

p (0) p ( t f )p (t ) =x (t )y (t )z (t )


39/53

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

robot graphic courtesy Fidel Hernandez

p (0) p ( t f )p (t ) =x (t )y (t )z (t )


40/53

a smooth path

cubic polynomial: p (t ) =x (t )y (t )z (t )

= a 0 + a 1 t + a 2 t 2 + a 3 t 3

or

x ( t ) = a x 0 + a x 1 t + a x 2 t 2 + a x 3 t 3

y (t ) = a y 0 + a y 1 t + a y 2 t2

+ a y 3 t3

z ( t ) = a z 0 + a z 1 t + a z 2 t 2 + a z 3 t 3

derivatives: p (t ) =x (t )

y (t )z (t ) =

a 1 + 2 a 2 t + 3 a 3 t2

p (t ) =x (t )y (t )

z (t )

= 2 a 2 + 6 a 3 t


41/53

constraintsposition and velocity at initial and nal times:

with three equations and four unknowns, you cansolve for the coefcients in terms of and

and the time

p (0) = p 0p (t f ) = p f p (0) = 0

p (t

f ) = 0

a 0 f

you can pick the time based on how quickly youwant to move between to the two points, or based

on a maximum velocity constraint

t f

t f

l j


42/53

example trajectory

positioncubic polynomialC1 continuous

velocityquadratic polynomial

C0 continuous

accelerationquadratic polynomial

not continuous


43/53

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 trajectoryusing the master of the teleoperator

di i


44/53

discussion

di i


45/53

discussion

why is a smooth trajectory important?

di i


46/53

discussion

why is a smooth trajectory important?why might you want a smoother trajectory?

di i


47/53

discussion


how could you compute this?

di i


48/53

discussion



would you want to do trajectory generation in

di i


49/53

discussion



would you want to do trajectory generation inCartesian space or joint space?

di i


50/53

discussion




how might via points be useful?

di i


51/53

discussion




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

di i


52/53

discussion

how would you compute based on adened maximum velocity?

what kind of trajectory would you want for a

robot that inserts a needle into solid tissue?

t f

bloodbot (Imperial College of London)


53/53

Assignment 3

Problem 1: Commentary on seminars

Problem 2: Read papers, answer questionsProblem 3: Modeling and simulation of medical

robot dynamics

Problem 4: Effects of dynamics on robot control

Due Wednesday April 24 at 4 pm

lecture6 dynamics

Documents