Java程序辅导

Lecture 7: Even More MatLab, &
Forward and Inverse Kinematics
Professor Erik Cheever
C   b    ourse we page:
http://www.swarthmore.edu/NatSci/echeeve1/Class/e5/E5Index.html
R bemem er…
y Thursday 10/22: 2nd MatLab lab is due (Solving the T 
puzzle).
y Thursday 10/29: 3rd MatLab lab is due (Animating the T 
puzzle – this week’s lab)
y 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!
y Start thinking about your project (last 3 weeks of class). 
Some ideas are at “Projects” link on left side of course 
web page page (including last year’s projects).
A l t t it vo un eer oppor un y
Professor Macken works with kids from Chester on 
E i i  j t  (b id  i  th  f ll  l   i  ng 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. 
C t t P f  M k  k 1  8073on ac ro essor ac en, nmac en , x
A l t t it vo un eer oppor un y
Th bi i te  g p c ure…
y Today:
y Control of flow in MatLab
y Forward kinematics for Robot arm
I  ki ti  f  R b t y nverse nema cs or o o arm
y Professor Lynne Molter, Electrical Engineering
y Read sections 4 3 4-5 and 5 2 1-3 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   – .
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:
f ly Per orm operations many times consecutive y:
y for… loop
y Make decisions:
y if…then…else…
y if…then…elseif…then…elseif… … …else…
y while… loop
The for… loop (1)
Output:
1
2
Syntax:
for variable=start:finish,
3
4
…commands…
end
Example:
f i 1 10
5
6
7or = : ,
disp(i)
d
8
9en 10
The for… loop (2)
Syntax:
for variable=start:increment:finish,
d…comman s…
end
Example:
for i=1:4:10
Output:
1 ,
disp(i)
end
5
9
F l l (0)or  oop examp 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.
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)or  oop examp e 
Make the shape move:
%%  Move it across the screen
T=0.25;                         %Delay between images
for x0=-15:2.5:15,
set(myShape 'Xdata' x0+x); %Translate by x0, ,     
pause(T);                   %Wait T seconds
end
F l l (2)or  oop examp e 
Make the shape move (another method):
%%  Move it across the screen (different technique)
T=0.25;                           %Delay between images
x0=linspace(-15,15,20);
for i=1:length(x0) ,
set(myShape,'Xdata',x0(i)+x);  %Translate by x0
pause(T);                      %Wait T seconds
end
F l l (2)or  oop examp e 
Make the shape move (another method):
%%  Move it across the screen (different technique)
T=0.25;                           %Delay between images
x0=linspace(-15,15,20);
for 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)or  oop examp 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
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)or  oop examp e 
Give it velocity in two directions:
%% M it b i i it l it i d  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
pause(dt); %Wait dt seconds                               
x0=x0+vx*dt;       %update x position
y0=y0+vy*dt;       %update y position
end
F l l (5)or  oop examp e 
Add gravity:
%% Add it   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
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 ifa ng  ec s ons  n  a a : 
Syntax:
if expression ,
… statements … 
end
E le
expression has to evaluate to TRUE or FALSE
xamp :
if a>b,
disp('a is greater than b');
end
if (a>b) & (a>0),
di (' b d i iti ')sp a>  an  a s pos ve ;
end
L i l iog ca  express ons
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 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  FALSEa   w en a s grea er an or s grea er an zero, o erw se
(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 care u  i  a or  are matrices, surprising resu ts can occur!
Note: this list is not exhaustive, check you text for more.
M ki D i i if la ng  ec s ons:    … e se
Syntax:
if expression ,
… statements … 
else
… statements
end
E lexamp :
if a>b,
disp('a is greater than b');
else
disp('a is less than or equal to b');
end
kMa ing Decisions: if … elseif … else
Syntax:
if expression1 ,
… statements … 
elseif expression2,
… statements …
else
… statements …
end
Example:
if a>b,
di (' i t th b')sp a s grea er an ;
elseif a=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
l if 0 15 % h k f lli i ith l ft ide se x <=- +r, c ec  or co s on w  e  s e...
vx=-vx;
x0=-15+r;
end
if 0> 15 % h k f t y = -r,     c ec  or op
vy=-vy;
y0=15-r;
elseif y0<=-15+r, %check for bottom
vy=-vy;
y0=-15+r;
end
end
l (2)Examp e: if…elseif…else 
p=0.7;             %coefficient of restitution
for t=0:dt:15,
( h d 0 d 0) % l b 0 0set myS ape,'X ata',x+x ,'Y ata',y+y ;   Trans ate y x , y
pause(dt);                                %Wait dt seconds
x0=x0+vx*dt;   %update x position
y0=y0+vy*dt;   %update y position
9 8*dt % d t l itvy=vy- . ;  up a e y ve oc y
if x0>=15-r,   %check for right wall
vx=-vx*p;     %...if hit, reverse (and decrease) velociy
x0=15-r;
l if 0 15 %l ft lle se x <=- +r, e  wa
vx=-vx*p;
x0=-15+r;
end
if 0> 15 %t y = -r,      op
vy=-vy*p;
y0=15-r;
elseif y0<=-15+r, %bottom
*vy=-vy p;
y0=-15+r;
end
end
M ki D i i hila ng  ec s ons:  w e…
Syntax:
while expression ,
… statements … 
end
E lxamp e:
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
lExamp e: 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 t ;                                Wa t t secon s
x0=x0+vx*dt;   %update x position
vy=vy-9.8*dt;  %update y velocity
if x0>=15-r,   %check for right wall
* %if hit ( d d ) l ivx=-vx p;      , reverse an  ecrease  ve oc y
x0=15-r;
elseif x0<=-15+r, %left wall
vx=-vx*p;
0 15x =- +r;
end
if y0>=15-r,      %top
vy=-vy*p;
0 15y = -r;
elseif y0<=-15+r, %bottom
vy=-vy*p;
y0=-15+r;
den
end
Th l i je  aser po nter pro ect
l b h l hGiven a simp e ro otic arm wit  a aser, ow 
can you get it to point to a specified location 
on the wall.
To start, we must understand the geometry of 
the problem.
T i i b i f ir g rev ew – as c  unct ons
( )= θAx cos ( )θ 0xcos0
( )= θA0y sin
= A
( )θ = A0
ysin
( )θ = 0
0
ytan
x
Inverse functions
⎛ ⎞xθ = ⎜ ⎟⎝ ⎠A
0acos
⎛ ⎞θ = ⎜ ⎟⎝ ⎠A
0yasin
⎛ ⎞θ = ⎜ ⎟⎝ ⎠
0
0
yatan
x But, be careful with arctangent…
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
T i i P hr g rev ew – yt agoras
2 2 2
0 0x y ,   Pythagoras' therom+ = A
( )x cos θ= A ( )0y sin θ= A0
( ) ( )2 2 2 2 2cos θ sin θ+ =A A A
( ) ( )2 2cos θ sin θ 1+ =
www.gbbservices.com/images/cos2sin201.jpg
Spherical → Cartesian Coordinates
( )θz r sin=
( )= θA r cos
( ) ( ) ( )= φ = θ φAx cos r cos cos
( ) ( ) ( )= φ = θ φAy 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
, .
(Forward kinematics)
in  an  φ wit  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 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)
( )0y cos= ρ φ
( )0x x cos= + θA
We know θ, φ and ρ.
We want to know x, y, z
( ) ( )x sin cos= ρ φ + φA
( ) ( ) ( )x sin r cos cos= ρ φ + θ φ
( )0y y sin= − θA
N t i i thi i f
( ) ( )y cos sin= ρ φ − φA
( ) ( ) ( )
o e s gn n s express on or 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= ρ φ + θ φ
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) 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= θ
zarcsin⎛ ⎞θ = ⎜ ⎟So: r⎝ ⎠
…and we have solved for one 
of our two angles.
Inverse Kinematics (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 ℓ):
( ) ( ) ( ) ( )φ = ρ φ + φ φA2x sin sin cos sin
( ) ( ) ( ) ( )φ = ρ φ − φ φA2y cos cos sin cos
( ) ( ) ( ) ( ) ( )φ + φ = ρ φ + φ φA2x sin ycos sin cos sin ( ) ( ) ( )+ ρ φ − φ φA2cos 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 2a an y,x as n x y so… ( )φ = −⎜ ⎟⎜ ⎟+⎝ ⎠2 2asin atan2 y,xx 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?