7/27/2019 E5 Lecture 7

1/36

Lecture7:EvenMoreMatLab,&ForwardandInverseKinematics

ProfessorErikCheeveroursewe page:http://www.swarthmore.edu/NatSci/echeeve1/Class/e5/E5Index.html

7/27/2019 E5 Lecture 7

2/36

emem ery Thursday 10/22: 2nd MatLab lab is due (Solving the T

.

y Thursda 10 29: 3rd MatLab lab is due Animatin the Tpuzzle this weeks lab)

,available (Trotter 201) from 8:00-10:00 p.m. specificallyfor E5. Dont wait until the last minute!

y Start thinking about your project (last 3 weeks of class).Some ideas are at Pro ects link on left side of courseweb page page (including last years projects).

7/27/2019 E5 Lecture 7

3/36

vo un eer

oppor un y

Professor Macken works with kids from Chester onng neer ng pro ec s r ges n e a , so ar cars n

the spring).

You must have Wednesday afternoons after 3:45 free.

It begins this Wednesday and goes 4 weeks.

on ac ro essor ac en, nmac en , x

7/27/2019 E5 Lecture 7

4/36

vo un eer

oppor un y

7/27/2019 E5 Lecture 7

5/36

e

gp c ure

y Today:

y

Control of flow in MatLaby Forward kinematics for Robot arm

y nverse nema cs or o o arm

y Professor Lynne Molter, Electrical Engineering

- -. . . .

y Following three weeks: controlling motors, anddesigning, build, and program both a robotic arm

with laser pointer.

r w v u r .

Some ideas on Projects link on left side of courseweb page (including last years projects).

7/27/2019 E5 Lecture 7

6/36

ControlofflowinMatLaby

So far we have used MatLab as a fancy calculatory The real power comes when we can have MatLab:

y Per orm operations many times consecutive y:y for loop

y ifthenelse

y ifthenelseifthenelseif else

y while loop

7/27/2019 E5 Lecture 7

7/36

Theforloop(1)

Output:1

Syntax:for variable=start:finish,

3

4

commands

end

Example:5

6

or = : ,disp(i) 8

9en10

7/27/2019 E5 Lecture 7

8/36

Theforloop(2)

Syntax:for variable=start:increment:finish,

comman s

end

Example: Output:

,disp(i)

end

5

9

7/27/2019 E5 Lecture 7

9/36

or

oopexamp e

First define a shape:%% Define a shape

theta=0:10:360; %theta is spaced around a circle (0 to 360).r=1; %The radius of our circle.

%Define a circular magenta patch.

= *

y=r*sind(theta);

myShape=patch(x,y,'m');

axis(20*[-1 1 -1 1],'square'); grid on;

7/27/2019 E5 Lecture 7

10/36

or

oopexamp e

Make the shape move:%% Move it across the screen

T=0.25; %Delay between images

for x0=-15:2.5:15,

' ', ,

pause(T); %Wait T seconds

end

7/27/2019 E5 Lecture 7

11/36

or

oopexamp e

Make the shape move (another method):%% Move it across the screen (different technique)

T=0.25; %Delay between imagesx0=linspace(-15,15,20);

= ,

set(myShape,'Xdata',x0(i)+x); %Translate by x0

pause(T); %Wait T seconds

end

7/27/2019 E5 Lecture 7

12/36

or

oopexamp e

Make the shape move (another method):%% Move it across the screen (different technique)

T=0.25; %Delay between imagesx0=linspace(-15,15,20);

= ,

set(myShape,'Xdata',x0(i)+x); %Translate by x0

pause(T); %Wait T seconds

end

Note: This week in lab

you will use theRotTrans function (from

parts of the T puzzle asthey move into place.

7/27/2019 E5 Lecture 7

13/36

or

oopexamp e

Make the shape move by giving it velocity:%% Move it by giving it a velocity in x

vx=5; %velocity in meters/sec.dt=0.25; %time step

=-

for t=0:dt:4,

set(myShape,'Xdata',x+x0); %Translate by x0

pause(dt); %Wait dt seconds

x0=x0+vx*dt; %update x position

end

7/27/2019 E5 Lecture 7

14/36

or

oopexamp e

Give it velocity in two directions:ove y g v ng a ve oc y n x an y

vx=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.y0

x0=x0+vx*dt; %update x position

y0=y0+vy*dt; %update y position

end

7/27/2019 E5 Lecture 7

15/36

or

oopexamp e

Add gravity:grav y

vx=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, y0

x0=x0+vx*dt; %update x postion

y0=y0+vy*dt; %update y position

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

end

7/27/2019 E5 Lecture 7

16/36

a ng

ec s ons

n

a a :

ifSyntax:

,

statements

end

expression has to evaluate to TRUE or FALSE

xamp e:if a>b,

disp('a is greater than b');

end

if (a>b) & (a>0),

sp 'a> an a s pos t ve' ;

end

7/27/2019 E5 Lecture 7

17/36

7/27/2019 E5 Lecture 7

18/36

a ng

ec s ons:

if elseSyntax:

,

statements

else

statements

end

xamp e:if a>b,

disp('a is greater than b');

else

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

7/27/2019 E5 Lecture 7

19/36

MakingDecisions:

if elseif else

Syntax:,

statements

elseif expression2,

statements

else

statements

end

Example:if a>b,

sp a s grea er an ;elseif a

7/27/2019 E5 Lecture 7

20/36

7/27/2019 E5 Lecture 7

21/36

Examp e:ifelseifelse (2)

p=0.7; %coefficient of restitution

for t=0:dt:15,

set myS ape,'X ata',x+x ,'Y ata',y+y ; Trans ate y x , y

pause(dt); %Wait dt secondsx0=x0+vx*dt; %update x position

y0=y0+vy*dt; %update y position

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

if x0>=15-r, %check for right wall

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

x0=15-r;e se x = -r, op

vy=-vy*p;

y0=15-r;

elseif y0

7/27/2019 E5 Lecture 7

22/36

a ng

ec s ons:

whileSyntax:

,

statements

end

xamp e:i=1;

while i

7/27/2019 E5 Lecture 7

23/36

Examp e:while

while (abs(vx)>2) | (abs(vy)>2) | (y0=15-r, %check for right wall

vx=-vx*p; %if hit, reverse (and decrease) velociy

x0=15-r;

elseif x0=15-r, %top

vy=-vy*p;

y = -r;

elseif y0

7/27/2019 E5 Lecture 7

24/36

e

aserpo nter

pro ect

Given a simple robotic arm with a laser, howcan you get it to point to a specified location

.

To start, we must understand the geometry of

the problem.

7/27/2019 E5 Lecture 7

25/36

r grev ew

as c

unct ons

0x0

( )= A0y sin

=A

( ) =A

0y

sin

( ) = 00

ytan

x

Inverse functions

= A0acos

= 0

yasin

=

0

0

y

atan x But, be careful with arctangent

7/27/2019 E5 Lecture 7

26/36

Trigreview

arctangents

Consider a point in the first quadrant, (x0,y0).

>> atan(6/3) * 180/pi

ans = 63.4349

>> atan2(6,3)*180/pi

ans = 63.4349

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

>> atan(-6/-3) * 180/pi

ans = 63.4349

>> atan2(-6,-3) * 180/pi

ans = -116.5651

7/27/2019 E5 Lecture 7

27/36

r grev ew

yt agoras

2 2 2

0 0x y , Pythagoras' therom+ =A

=0

( ) ( )2 2 2 2 2cos sin + =A A A

( ) ( )2 2cos sin 1+ =

www.gbbservices.com/images/cos2sin201.jpg

7/27/2019 E5 Lecture 7

28/36

Spherical

CartesianCoordinates

=

( )= A rcos= = Ax cos r cos cos

( ) ( ) ( )= = Ay sin r cos sin

Given r, and we can

Given x and y, we could

, .(Forward kinematics)

in an wit somedifficulty.(Inverse kinematics)

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

but this is not the

problem we want tosolve.

7/27/2019 E5 Lecture 7

29/36

2D

Polar

Cartesianw/

offset

( )=

0

x 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

7/27/2019 E5 Lecture 7

30/36

ForwardKinematics

(1)

, .We want to know x, y, z

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

= + + 2 2 2 2

r x y z

z r sin=

7/27/2019 E5 Lecture 7

31/36

7/27/2019 E5 Lecture 7

32/36

InverseKinematics

(1)

So usin forward kinematics we can determine x and z iven the

( ) ( ) ( )y cos r cos sin= ( ) ( ) ( )x sin r cos cos= +

angles 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) andwe know (determined by geometry of robot).

.

7/27/2019 E5 Lecture 7

33/36

InverseKinematics

(2)

We know x, y, z and .

Solving for . (this is relatively easy)

We know: ( )z r sin=

z =r

and we have solved for oneof our two angles.

7/27/2019 E5 Lecture 7

34/36

InverseKinematics

(3)

We know x, y, z and .

( ) ( )= + Ax sin cosStart with:

Solving for . (this is harder)

( ) ( )= Ay cos sin

Multiply x by sin() and y by cos() and add (to get rid of):( ) ( ) ( ) ( ) = + A2xsin sin cos sin

( ) ( ) ( ) ( ) = A2ycos cos sin cos

( ) ( ) ( ) ( ) ( ) + = + A

2

xsin ycos sin cos sin ( ) ( ) ( )+ A

2

cos sin cos

( ) ( ) ( ) ( )( ) + = + 2 2xsin ycos sin cos

Were almost done

( ) ( ) + = xsin ycos

7/27/2019 E5 Lecture 7

35/36

InverseKinematics

(4)

( ) ( ) + = xsin ycos

continued from previous

Use trigonometric identity:

( ) ( ) ( )( ) + = + +2 2

asin bcos a b sin atan2 b,a= = = a x b y

( ) ( ) ( )( ) + = + + = 2 2xsin ycos x y sin atan2 y,x

( )( ) + = +2 2sin atan2 y,x

x y

+ =

+ 2 2

a an y,x as nx y

so ( ) = + 2 2

asin atan2 y,xx y

7/27/2019 E5 Lecture 7

36/36

Whatifweaddathirdjoint?

Are there any difficulties to the forward kinematics problem?

Inverse kinematics is enerall harder than forward kinematics.

Are there any difficulties to the reverse kinematics problem?