Lecture 7: Even More MatLab, &Forward and Inverse Kinematics

Professor Erik Cheever

C   b    Course web page:  http://www.swarthmore.edu/NatSci/echeeve1/Class/e5/E5Index.html

R bRemember…Thursday 10/22: 2nd MatLab lab is due (Solving the T puzzle)puzzle).

Thursday 10/29: 3rd MatLab lab is due (Animating the T y / ( gpuzzle – this week’s lab)

Tuesday & Wednesday 10/20 & 10/21: Wizards Tuesday & Wednesday, 10/20 & 10/21: Wizards available (Trotter 201) from 8:00-10:00 p.m. specifically for E5. Don’t wait until the last minute!

Start thinking about your project (last 3 weeks of class). Some ideas are at “Projects” link on left side of course jweb page page (including last year’s projects).

A l t t itA volunteer opportunityProfessor Macken works with kids from Chester on E i i j t (b id i th f ll l i Engineering projects (bridges in the fall, solar cars in the spring).

You must have Wednesday afternoons after 3:45 free.

It begins this Wednesday and goes 4 weeks.

C t t P f M k k 1 8073Contact Professor Macken, nmacken1, x8073

A l t t itA volunteer opportunity

Th bi i tThe big picture…Today:

Control of flow in MatLabForward kinematics for Robot armI ki ti f R b t Inverse kinematics for Robot armProfessor Lynne Molter, Electrical Engineering

Read sections 4 3 4-5 and 5 2 1-3 of textRead sections 4.3.4 5 and 5.2.1 3 of textFollowing three weeks: controlling motors, and designing, build, and program both a robotic arm with laser pointer.

Last three weeks of class individual projects Last three weeks of class – individual projects. Some ideas on “Projects” link on left side of course web page (including last year’s projects).

Control of flow in MatLabSo far we have used MatLab as a fancy calculatorThe real power comes when we can have MatLab:

f lPerform operations many times consecutively:for… loop

Make decisions:Make decisions:if…then…else…if…then…elseif…then…elseif… … …else…while… loop

The for… loop (1)Output:

12

Syntax:for variable=start:finish,

234

…commands…end

Example:f i 1 10

567for i=1:10,

disp(i)d

789end 9

10

The for… loop (2)Syntax:for variable=start:increment:finish,

d…commands…end

Example:for i=1:4:10

Output:1for i=1:4:10,

disp(i)end

159

F l l (0)For loop example (0)First define a shape:p%% Define a shapetheta=0:10:360; %theta is spaced around a circle (0 to 360).r=1; %The radius of our circle.%Define a circular magenta patch.x=r*cosd(theta);x=r*cosd(theta);y=r*sind(theta);myShape=patch(x,y,'m');axis(20*[-1 1 -1 1],'square'); grid on;

F l l (1)For loop example (1)Make the shape move:p%% Move it across the screen

T=0.25; %Delay between imagesfor x0=-15:2.5:15,

set(myShape 'Xdata' x0+x); %Translate by x0set(myShape, Xdata ,x0+x); %Translate by x0pause(T); %Wait T seconds

end

F l l (2)For loop example (2)Make the shape move (another method):p ( )%% Move it across the screen (different technique)T=0.25; %Delay between imagesx0=linspace(-15,15,20);

for i=1:length(x0)for i=1:length(x0),set(myShape,'Xdata',x0(i)+x); %Translate by x0pause(T); %Wait T seconds

end

F l l (2)For loop example (2)Make the shape move (another method):p ( )%% Move it across the screen (different technique)T=0.25; %Delay between imagesx0=linspace(-15,15,20);

for i=1:length(x0)for i=1:length(x0),set(myShape,'Xdata',x0(i)+x); %Translate by x0pause(T); %Wait T seconds

end

Note: This week in lab Note: This week in lab you will use the “RotTrans” function (from last lab) to animate the last lab) to animate the parts of the T puzzle as they move into place.

F l l (3)For loop example (3)Make the shape move by giving it velocity:p y g g y%% Move it by giving it a velocity in xvx=5; %velocity in meters/sec.dt=0.25; %time step

x0=-10;x0= 10;for t=0:dt:4,

set(myShape,'Xdata',x+x0); %Translate by x0pause(dt); %Wait dt secondsx0=x0+vx*dt; %update x position

end

F l l (4)For loop example (4)Give it velocity in two directions:%% M it b i i it l it i d%% Move it by giving it a velocity in x and yvx=5; %velocity in meters/sec.vy=10;dt=0.25; %time step

x0=-10;y0=-10;for t=0:dt:3,

set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0.y0pause(dt); %Wait dt secondspause(dt); %Wait dt secondsx0=x0+vx*dt; %update x positiony0=y0+vy*dt; %update y position

end

F l l (5)For loop example (5)Add gravity:%% Add it%% Add gravityvx=5; %velocity in meters/sec.vy=15;dt=0.2; %time step

x0=-15;y0=0;for t=0:dt:4,

set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0, y0pause(dt); %Wait dt secondspause(dt); %Wait dt secondsx0=x0+vx*dt; %update x postiony0=y0+vy*dt; %update y position

vy=vy-9.8*dt; %update y velocityend

M ki D i i i M tl b ifMaking Decisions in Matlab: ifSyntax:if expressionif expression,

… statements … end

E le

expression has to evaluate to TRUE or FALSE

Example:if a>b,

disp('a is greater than b');end

if (a>b) & (a>0),di (' b d i iti ')disp('a>b and a is positive');

end

L i l iLogical expressionsexpression has to evaluate to TRUE or FALSE

a>b TRUE when a is greater than b, otherwise FALSE

a<b TRUE when a is less than b, otherwise FALSEa<b TRUE when a is less than b, otherwise FALSE

a>=b TRUE when a is greater than or equal to b, otherwise FALSE

a==b TRUE when a is equal to b, otherwise FALSE

a~=b TRUE when a is not equal to b, otherwise FALSE

(a>0) & (b>0) TRUE when a is greater than 0 and b is greater than zero, otherwise FALSE

( >0) | (b>0) TRUE h i t th 0 b i t th th i FALSE(a>0) | (b>0) TRUE when a is greater than 0 or b is greater than zero, otherwise FALSE

(a>0) | ~(b>0)TRUE when a is greater than 0 or b is not greater than zero, otherwise FALSE

f l f b lBe careful if a or b are matrices, surprising results can occur!

Note: this list is not exhaustive, check you text for more.

M ki D i i if lMaking Decisions:  if … elseSyntax:if expressionif expression,

… statements … else

… statementsend

E leExample:if a>b,

disp('a is greater than b');else

disp('a is less than or equal to b');end

kMaking Decisions: if … elseif … elseSyntax:if expression1if expression1,

… statements … elseif expression2,

… statements …else

… statements …end

Example:if a>b,

di (' i t th b')disp('a is greater than b');elseif a<b,

disp('a is less than b');else

disp('a is equal to b');end

l (1)Example: if…elseif…else (1)plot([-15 -15 15 15 -15],[15 -15 -15 15 15],'b','LineWidth',2);for t=0:dt:10,

( h d 0 d 0) % l b 0 0set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0, y0pause(dt); %Wait dt secondsx0=x0+vx*dt; %update x postiony0=y0+vy*dt; %update y postion

9 8*dt % d t l itvy=vy-9.8*dt; %update y velocityif x0>=15-r, %check for collision with right side

vx=-vx; %if so, reverse x velocityx0=15-r; %reset x position so it touches wall

l if 0 15 % h k f lli i ith l ft idelseif x0<=-15+r, %check for collision with left side...vx=-vx;x0=-15+r;

endif 0> 15 % h k f tif y0>=15-r, %check for top

vy=-vy;y0=15-r;

elseif y0<=-15+r, %check for bottomvy=-vy;y0=-15+r;

endend

l (2)Example: if…elseif…else (2)p=0.7; %coefficient of restitutionfor t=0:dt:15,

( h d 0 d 0) % l b 0 0set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0, y0pause(dt); %Wait dt secondsx0=x0+vx*dt; %update x positiony0=y0+vy*dt; %update y position

9 8*dt % d t l itvy=vy-9.8*dt; %update y velocityif x0>=15-r, %check for right wall

vx=-vx*p; %...if hit, reverse (and decrease) velociyx0=15-r;

l if 0 15 %l ft llelseif x0<=-15+r, %left wallvx=-vx*p;x0=-15+r;

endif 0> 15 %tif y0>=15-r, %top

vy=-vy*p;y0=15-r;

elseif y0<=-15+r, %bottom*vy=-vy*p;

y0=-15+r;end

end

M ki D i i hilMaking Decisions:  while…Syntax:while expressionwhile expression,

… statements … end

E lExample:i=1;while i<10,

disp(i);disp(i);i=i+4;

end

Output:159 Note: this is equivalent to the for…loop used earlier

lExample: while…while (abs(vx)>2) | (abs(vy)>2) | (y0<-15+2*r),

set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0, y0(d ) % i d dpause(dt); %Wait dt seconds

x0=x0+vx*dt; %update x positionvy=vy-9.8*dt; %update y velocityif x0>=15-r, %check for right wall

* %if hit ( d d ) l ivx=-vx*p; %if hit, reverse (and decrease) velociyx0=15-r;

elseif x0<=-15+r, %left wallvx=-vx*p;0 15x0=-15+r;

endif y0>=15-r, %top

vy=-vy*p;0 15y0=15-r;

elseif y0<=-15+r, %bottomvy=-vy*p;y0=-15+r;

dendend

Th l i jThe laser pointer projectl b h l hGiven a simple robotic arm with a laser, how

can you get it to point to a specified location on the wallon the wall.

To start, we must understand the geometry of the problem.

T i i b i f iTrig review – basic functions( )= θx cos ( )θ 0xcos( )= θ0x cos

( )= θ0y sin

( )θ = 0cos

( )θ = 0ysin

( )θ = 0

0

ytanx

Inverse functions⎛ ⎞x⎛ ⎞

θ = ⎜ ⎟⎝ ⎠

0xacos

⎛ ⎞θ = ⎜ ⎟

⎝ ⎠0yasin⎜ ⎟

⎝ ⎠⎛ ⎞

θ = ⎜ ⎟⎝ ⎠

0

0

yatanx But, be careful with arctangent…

Trig review – arctangentsConsider a point in the first quadrant, (x0,y0).

>> atan(6/3) * 180/pi ans = 63.4349

>> atan2(6,3)*180/pians = 63.4349

Now consider a point in the third quadrant, (x0,y0).

>> atan(-6/-3) * 180/pians = 63.4349

>> atan2(-6,-3) * 180/pians = -116.5651

T i i P hTrig review – Pythagoras2 2 20 0x y , Pythagoras' therom+ =

( )x cos θ= ( )0y sin θ=( )0x cos θ= ( )0y sin θ

( ) ( )2 2 2 2 2cos θ sin θ+ =

( ) ( )2 2cos θ sin θ 1+ =

www.gbbservices.com/images/cos2sin201.jpg

Spherical → Cartesian Coordinates( )θz r sin( )= θz r sin

( )= θr cos

( ) ( ) ( )= φ = θ φx cos r cos cos( ) ( ) ( )( ) ( ) ( )= φ = θ φy sin r cos sin

Given r, θ and φ we can easily find x y and z

Given x and y, we could f d θ d h

easily find x, y and z.(Forward kinematics)

find θ and φ with some difficulty.(Inverse kinematics)

Adapted from: http://rbrundritt.spaces.live.com/blog/cns!E7DBA9A4BFD458C5!280.entry

… but this is not the problem we want to solve.

2‐D Polar → Cartesian w/ offset( )= ρ φ0x sin

( )= ρ φ0y cos

Angle of “r” from horizontal is equal to φ.

( ) ( ) ( )0x x r cos sin r cos= + φ = ρ φ + φ

( ) ( ) ( )= + φ = ρ φ + φ0y y r sin cos r sin

Forward Kinematics (1)We know θ φ and ρWe know θ, φ and ρ.We want to know x, y, z

Pythagoras tells us: 2 2 2 2 2x y z r+ + = ρ +

= + + − ρ2 2 2 2r x y z

( )z r sin= θ( )

( )0x sin= ρ φ

Forward Kinematics (2)( )0 ρ φ

( )0y cos= ρ φ

( )0x x cos= + θ

We know θ, φ and ρ.We want to know x, y, z

( )0x x cos+ θ

( ) ( )x sin cos= ρ φ + φ

( ) ( ) ( )x sin r cos cos= ρ φ + θ φ( ) ( ) ( )x sin r cos cos= ρ φ + θ φ

( )0y y sin= − θ

N t i i thi i f

( ) ( )y cos sin= ρ φ − φ

( ) ( ) ( )

Note sign in this expression for y

( ) ( ) ( )y cos r cos sin= ρ φ − θ φ

Inverse Kinematics (1)So… using forward kinematics we can determine x, y and z, given the

( ) ( ) ( )y cos r cos sin= ρ φ − θ φ( ) ( ) ( )x sin r cos cos= ρ φ + θ φ

g , y , gangles φ and θ.

But… forward kinematics is not enough. Generally with a robot, we know where we want the robot to be (x,y), and need to find the angles.

This process is called inverse kinematics.

Problem statement: we know x, y, and z (these are inputs) and we know ρ (determined by geometry of robot).

We want to find φ and θWe want to find φ and θ.

Inverse Kinematics (2)We know x, y, z and ρ.

Solving for θ. (this is relatively easy)

We know: ( )z r sin= θ

zarcsin⎛ ⎞θ = ⎜ ⎟So: arcsinr

θ = ⎜ ⎟⎝ ⎠

…and we have solved for one of our two angles.

So:

Inverse Kinematics (3)We know x, y, z and ρ.

( ) ( )= ρ φ + φx sin cosStart with:

Solving for φ. (this is harder)

( ) ( )= ρ φ − φy cos sin

Multiply x by sin(φ) and y by cos(φ) and add (to get rid of ℓ):

( ) ( ) ( ) ( )φ = ρ φ + φ φ2x sin sin cos sin

( ) ( ) ( ) ( )φ = ρ φ − φ φ2y cos cos sin cos

( ) ( ) ( ) ( ) ( )φ + φ = ρ φ + φ φ2x sin ycos sin cos sin ( ) ( ) ( )+ ρ φ − φ φ2cos sin cos

( ) ( ) ( ) ( )( )φ + φ = ρ φ + φ2 2x sin ycos sin cos

We’re almost done…

( ) ( )φ + φ = ρxsin ycos

Inverse Kinematics (4)

( ) ( )φ + φ = ρxsin ycos

… continued from previous

Use trigonometric identity:

( ) ( ) ( )( )α + α = + α +2 2asin bcos a b sin atan2 b,a

= = α = φa x b y

( ) ( ) ( )( )φ + φ = + φ + = ρ2 2x sin ycos x y sin atan2 y,x

( )( ) ρφ + =

+2 2sin atan2 y,x

x y

( )⎛ ⎞ρ

φ ⎜ ⎟t 2 i ⎛ ⎞ρ( ) ρφ + = ⎜ ⎟

⎜ ⎟+⎝ ⎠2 2

atan2 y,x asinx y

so… ( )⎛ ⎞ρ

φ = −⎜ ⎟⎜ ⎟+⎝ ⎠

2 2asin atan2 y,x

x y

What if we add a third joint?

Are there any difficulties to the forward kinematics problem?

Inverse kinematics is generally harder than forward kinematics.

Are there any difficulties to the reverse kinematics problem?

g y