Lecture 7: Even More MatLab, &

Forward and Inverse Kinematics

Professor Erik Cheever

Course web page:  
C   b    
http://www.swarthmore.edu/NatSci/echeeve1/Class/e5/E5Index.html
 R b
Remember…
y Thursday 10/22: 2nd MatLab lab is due (Solving the T
puzzle).
puzzle)

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

y Tuesday & Wednesday,

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!

y Start thinking about your project (last 3 weeks of class).

Some ideas are at “Projects”
j link on left side of course
web page page (including last year’s projects).
 A l t t it
A volunteer opportunity
Professor Macken works with kids from Chester on
Engineering
E i i projects
j t (bridges
(b id iin th
the ffall,
ll solar
l cars in
i
the spring).

You must have Wednesday afternoons after 3:45 free.

It begins this Wednesday and goes 4 weeks.

C t t Professor
Contact P f M
Macken,
k nmacken1,
k 1 x8073
8073
A l t t it
A volunteer opportunity
Th bi i t
The big picture…
y Today:
y Control of flow in MatLab
y Forward kinematics for Robot arm
y Inverse
I kinematics
ki ti ffor Robot
R b t arm
y Professor Lynne Molter, Electrical Engineering
y Read sections 4.3.4
4 3 4-5
5 and 5.2.1
5 2 1-33 of text
y Following three weeks: controlling motors, and
designing, build, and program both a robotic arm
with laser pointer.

y Last three weeks of class – individual projects.

projects
Some ideas on “Projects” link on left side of course
web page (including last year’s projects).
Control of flow in MatLab
y So far we have used MatLab as a fancy calculator
y The real power comes when we can have MatLab:
y Perform
f operations many times consecutively:
l
y for… loop

y Make decisions:
y if…then…else…

y if…then…elseif…then…elseif… … …else…

y while… loop
The for… loop (1)
Syntax: Output:
for variable=start:finish, 1
…commands… 2
3
end 4
5
Example: 6
f
for i 1 10
i=1:10, 7
disp(i) 8
endd 9
10
The for… loop (2)
Syntax:
for variable=start:increment:finish,
…commands…
d
end

Example: Output:
for i=1:4:10,
i=1:4:10 1
disp(i) 5
end 9
F l l (0)
For loop example (0)
p
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.
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)
p move:
Make the shape
%% Move it across the screen

T=0.25; %Delay between images

for x0=-15:2.5:15,
set(myShape 'Xdata'
set(myShape, Xdata ,x0+x);
x0+x); %Translate by x0
pause(T); %Wait T seconds
end
 F l l (2)
For loop example (2)
p move ((another method):
Make the shape )
%% Move it across the screen (different technique)
T=0.25; %Delay between images
x0=linspace(-15,15,20);

for i=1:length(x0)
i=1:length(x0),
set(myShape,'Xdata',x0(i)+x); %Translate by x0
pause(T); %Wait T seconds
end
 F l l (2)
For loop example (2)
p move ((another method):
Make the shape )
%% Move it across the screen (different technique)
T=0.25; %Delay between images
x0=linspace(-15,15,20);

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

Note: This week in lab

you will use the
“RotTrans” function (from
last lab) to animate the
parts of the T puzzle as
they move into place.
F l l (3)
For loop example (3)
p move by
Make the shape yg g it velocity:
giving y
%% Move it by giving it a velocity in x
vx=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 x0
pause(dt); %Wait dt seconds
x0=x0+vx*dt; %update x position
end
F l l (4)
For loop example (4)
Give it velocity in two directions:
%% Move
M it by
b giving
i i it a velocity
l it ini x and
d 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
pause(dt); %Wait dt seconds
x0=x0+vx*dt; %update x position
y0=y0+vy*dt; %update y position
end
F l l (5)
For loop example (5)
Add gravity:
%% Add gravity
it
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
pause(dt); %Wait dt seconds
x0=x0+vx*dt; %update x postion
y0=y0+vy*dt; %update y position

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

end
M ki D i i i M tl b if
Making Decisions in Matlab: 
Syntax:
if expression,
expression
… statements …
end

expression has to evaluate to TRUE or FALSE

E
Example:
le
if a>b,
disp('a is greater than b');
end

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

di (' b and
disp('a>b d a is
i positive');
iti ')
end
 L i l i
Logical expressions
expression 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 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 when
(a>0) h a iis greater
t than
th 0 or b is
i greater
t than
th zero, otherwise
th i FALSE

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

Be careful
f l iff a or b are matrices, surprising results
l can occur!

Note: this list is not exhaustive, check you text for more.
M ki D i i
Making Decisions:   l
if … else
Syntax:
if expression,
expression
… statements …
else
… statements
end

E
Example:
le
if a>b,
disp('a is greater than b');
else
disp('a is less than or equal to b');
end
 k
Making Decisions:  if … elseif … else
Syntax:
if expression1,
expression1
… statements …
elseif expression2,
… statements …
else
… statements …
end

Example:
if a>b,
di (' i
disp('a is greater
t th
than b')
b');
elseif a<b,
disp('a is less than b');
else
disp('a is equal to b');
end
 l
Example: if…elseif…else (1)
plot([-15 -15 15 15 -15],[15 -15 -15 15 15],'b','LineWidth',2);
for t=0:dt:10,
set(myShape,'Xdata',x+x0,'Ydata',y+y0);
( h d 0 d 0) %Translate
% l by
b x0,
0 y0
0
pause(dt); %Wait dt seconds
x0=x0+vx*dt; %update x postion
y0=y0+vy*dt; %update y postion
vy=vy-9.8*dt;
9 8*dt % d t y velocity
%update l it
if x0>=15-r, %check for collision with right side
vx=-vx; %if so, reverse x velocity
x0=15-r; %reset x position so it touches wall
elseif
l if x0<=-15+r,
0 15 %
%check
h k f for collision
lli i with
ith l
left
ft side...
id
vx=-vx;
x0=-15+r;
end
if y0>=15-r,
0> 15 % h k f
%check for t
top
vy=-vy;
y0=15-r;
elseif y0<=-15+r, %check for bottom
vy=-vy;
y0=-15+r;
end
end
 l
Example: if…elseif…else
p=0.7;
(2)
%coefficient of restitution
for t=0:dt:15,
set(myShape,'Xdata',x+x0,'Ydata',y+y0);
( h d 0 d 0) %Translate
% l by
b x0,
0 y00
pause(dt); %Wait dt seconds
x0=x0+vx*dt; %update x position
y0=y0+vy*dt; %update y position
vy=vy-9.8*dt;
9 8*dt % d t y velocity
%update l it
if x0>=15-r, %check for right wall
vx=-vx*p; %...if hit, reverse (and decrease) velociy
x0=15-r;
elseif
l if x0<=-15+r,
0 15 %l
%left
ft wall
ll
vx=-vx*p;
x0=-15+r;
end
if y0>=15-r,
0> 15 %t
%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
Making Decisions:   hil
while…
Syntax:
while expression,
expression
… statements …
end

E
Example:
l
i=1;
while i<10,
disp(i);
i=i+4;
end

Output:
1
5
9 Note: this is equivalent to the for…loop used earlier
 l
Example: while…
while (abs(vx)>2) | (abs(vy)>2) | (y0<-15+2*r),
set(myShape,'Xdata',x+x0,'Ydata',y+y0); %Translate by x0, y0
pause(dt);
(d ) %Wait
% i dtd seconds
d
x0=x0+vx*dt; %update x position
vy=vy-9.8*dt; %update y velocity
if x0>=15-r, %check for right wall
vx=-vx*p;
* %if hit
hit, reverse (and
( d ddecrease)
) velociy
l i
x0=15-r;
elseif x0<=-15+r, %left wall
vx=-vx*p;
x0=-15+r;
0 15
end
if y0>=15-r, %top
vy=-vy*p;
y0=15-r;
0 15
elseif y0<=-15+r, %bottom
vy=-vy*p;
y0=-15+r;
end
d
end
Th l i j
The laser pointer project
Given a simple
l robotic
b arm withh a laser,
l how
h
can you get it to point to a specified location
on the wall.
wall

To start, we must understand the geometry of

the problem.
Ti i
Trig review – b i f i
basic functions
x0
x0 = A cos ( θ ) cos ( θ ) =
A
y
y0 = A sin ( θ ) sin ( θ ) = 0
A
y0
tan ( θ ) =
x0

Inverse functions
⎛x ⎞
θ = acos ⎜ 0 ⎟
⎝ A ⎠
⎛y ⎞
θ = asin ⎜ 0 ⎟
⎝ A ⎠
⎛y ⎞
θ = a tan ⎜ 0 ⎟ But, be careful with arctangent…
⎝ x0 ⎠
Trig review – 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
Ti i
Trig review – P h
Pythagoras
x20 + y20 = A2 , Pythagoras' therom

x0 = A cos ( θ ) y0 = A sin ( θ )

A2 cos2 ( θ ) + A2 sin2 ( θ ) = A2

cos2 ( θ ) + sin2 ( θ ) = 1

www.gbbservices.com/images/cos2sin201.jpg
 Spherical → Cartesian Coordinates
z = r sin ( θ )
A = r cos ( θ )
x = A cos ( φ ) = r cos ( θ ) cos ( φ )
y = A sin ( φ ) = r cos ( θ ) sin ( φ )
Given r, θ and φ we can
easily find x
x, y and z.
z
(Forward kinematics)
Given x and y, we could
f d θ and
find d φ with
h some
difficulty.
(Inverse kinematics)
… but this is not the
Adapted from: http://rbrundritt.spaces.live.com/blog/cns!E7DBA9A4BFD458C5!280.entry problem we want to
solve.
2‐D Polar → Cartesian w/ offset
x0 = ρ sin ( φ )

y0 = ρ cos ( φ )

Angle of “r” from horizontal

is equal to φ.
x = x0 + r cos ( φ) = ρ sin ( φ) + r cos ( φ)

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

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

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

r= x2 + y2 + z2 − ρ2

z = r sin ( θ )
Forward Kinematics (2)
x0 = ρ sin ( φ )
We know θ, φ and ρ.
We want to know x, y, z y0 = ρ cos ( φ )

x = x0 + A cos ( θ )

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

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

y = y0 − A sin ( θ )

N t sign
Note i iin thi
this expression
i ffor y

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

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

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

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

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 θ.

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

Solving for θ. (this is relatively easy)

We know: z = r sin ( θ )

⎛z⎞
So: θ = arcsin ⎜ ⎟
⎝r ⎠
…and we have solved for one
of our two angles.
Inverse Kinematics (3)
We know x, y, z and ρ.

Solving for φ. (this is harder)

Start with: x = ρ sin ( φ ) + A cos ( φ )

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

Multiply x by sin(φ) and y by cos(φ) and add (to get rid of ℓ):
x sin ( φ ) = ρ sin2 ( φ ) + A cos ( φ ) sin ( φ )

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

x sin ( φ ) + y cos ( φ ) = ρ sin2 ( φ ) + A cos ( φ ) sin ( φ ) + ρ cos2 ( φ ) − A sin ( φ ) cos ( φ )

(
x sin ( φ ) + y cos ( φ ) = ρ sin2 ( φ ) + cos2 ( φ ) )
x sin ( φ ) + y cos ( φ ) = ρ

We’re almost done…

 Inverse Kinematics (4)
… continued from previous

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

Use trigonometric identity:

(
asin ( α ) + b cos ( α ) = a2 + b2 sin α + atan2 (b,a) )
a=x b=y α=φ

x sin ( φ ) + y cos ( φ ) = ( )
x2 + y2 sin φ + atan2 ( y, x ) = ρ
ρ
sin ( φ + atan2 ( y, x ) ) =
x2 + y2
⎛ ρ ⎞ ⎛ ⎞
t 2 ( y, x ) = asin
φ + atan2 i ⎜ ⎟ so… φ = asin ⎜
ρ
⎟ − atan2 ( y, x )
⎜ x2 + y2 ⎟ ⎜ 2 ⎟
⎝ ⎠ ⎝ x +y
2
⎠
What if we add a third joint?

Are there any difficulties to the forward kinematics problem?

Are there any difficulties to the reverse kinematics problem?

Inverse kinematics is g
generally
y harder than forward kinematics.