# 投影片 1

Mechanics

Nov 14, 2013 (4 years and 7 months ago)

87 views

Data Driven Function
Approximation

Given X
t

what is the joint angle P

x

Y

x
t

P

solution
position

forward
)
cos(
P
l
x
t

Plot1d

clear all

fstr=input('input a function: x.^2+cos(x) :','s');

fx=inline(fstr);

range=2*pi;

x=linspace(
-
range,range);

y=fx(x);

max_y=max(abs(y));

plot(x,y/max_y);

hold on;

Plot a function that maps joint angle P to tool position X
t

Black box

Black box

P

X
t

range=2*pi;

linspace(
-
range,range);

observation

N=input('keyin sample size:');

x=rand(1,N)*2*range
-
range;

n=rand(1,N)*0.1
-
0.05;

y=fx(x)/max_y+n;

figure

plot(x,y,'.');

Paired data

Input

)
;
(

i
x
f
Desired output

Network output

error

Network function

Weight sum of hyper
-
tangent functions

}
{
}
{
}
{
)
tanh(
)
;
(
0
1
m
m
m
M
m
m
m
m
i
r
b
a
r
b
x
a
r
x
f

Network

}
{
}
{
}
{
)
tanh(
)
;
(
0
1
m
m
m
M
m
m
m
m
i
r
b
a
r
b
x
a
r
x
f

tanh

tanh

tanh

…….

x

1

a
2

a
1

a
M

b
2

b
1

b
M

1

r
1

r
2

r
M

r
M+1

Procedure 1: MLP evaluation

1.
Input r,a,b and x,M

2.
y=r(M+1)

3.
Set M to the length of a

4.
For m=1:M

a.

5.
Return y

Matlab code

function y=eval_MLP(x,r,a,b,M)

y=r(M+1);

for m=1:M

y=y+r(m)*tanh(x.*a(m)+b(m));

end

return

eval_MLP.m

Mean square error

Given

Find to minimize

,...,
1

),
,
(
n
i
y
x
i
i

)
)
;
(
(
1
)
(
2
1

n
i
i
i
x
f
y
n
E

Procedure 2:

Mean square error

1.
Input x,y,a,b,r

2.
Set E to zero

3.
Set n to the length of x

4.
For i=1:n

a)
Calculate the square error of approximating
y(i) by f(x;r,a,b)

b)

5.
Return E

Matlab code

function E=mean_square_error2(x,y,a,b,r)

E=0;

M=length(a);

n=length(x);

for i=1:n

s_err=(y(i)
-
eval_MLP(x(i),r,a,b,M)).^2;

E=E+s_err;

end

E=E/n;

return

mean_square_error2.m

Matlab code

1

eval_MLP.m

Evaluate an MLP function

2

mean_square
_error2.m

Calculate E

3

learn_MLP.m

Seek parameters

Application

Black box

plot1d.m

P

X
t

observation

Create paired data

sampling.m

Input

)
;
(

i
x
f
Desired output

Network output

error

Leaning

learning.m

Matlab code

Function approximation for an arbitrary target function

fa1d.m

x

Y

x
t

P

l
x
P
l
x
(P)
t
t
arccos
cos

Inverse kinematics

Construction of inverse kinematics

Procedure

1.
Input a 1d forward kinematics

2.
Sampling to form paired data

3.
Input
-
output swapping

4.
Rescaling

5.
Learning

Black box

plot1d.m

P

X
t

observation

Create paired data

sampling.m

Data preparation

-4
-3
-2
-1
0
1
2
3
4
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
-4
-3
-2
-1
0
1
2
3
4
-1.5
-1
-0.5
0
0.5
1
1.5
temp=x;

x=y;

y=temp;

swapping

Rescaling

max_y=max(y);

y=y/max_y;

plot(x,y,'.')

-1.5
-1
-0.5
0
0.5
1
1.5
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Learning inverse kinematics

Input

)
;
(

i
x
f
Desired output

Network output

error

Leaning

learning.m

-1.5
-1
-0.5
0
0.5
1
1.5
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Matlab code

fa1d_inv.m

MSE for training data 0.336461

ME for training data 0.490247

Unacceptable training errors

-1.5
-1
-0.5
0
0.5
1
1.5
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Multiple outputs

Inverse kinematics

Conflicts of I/O
relation

Two distinct
outcomes for the
same input

First order derivative

Forward kinematics should be
characterized by two outputs for
construction of inverse kinematics

Black box

plot1d.m

P

X
t

Two observations

Derivative

Symbolic differentiation

function demo_diff()

% input a string to specify a function

% plot its derivative

ss=input('function of x:','s');

fx=inline(ss);

x=sym('x');

ss=['diff(' ss ')'];

ss1=eval([sprintf(ss)]);

fx1=inline(ss1)

x=linspace(
-
pi,pi);

plot(x,fx(x),'b');hold on;

plot(x,fx1(x),'r');

return

demo_diff.m

Data preparation

Black box

plot1dd.m

P

X
t

Two observations

Derivative

D
t

Reverse kinematics

Reverse Kinematics

P

X
t

Derivative

D
t

plot3(y1,y2,ix)

Reverse kinematics

Matlab code

Segmentation

Procedure

1.
Input paired data,(x,y)

2.
Sort data by y

Write a matlab function to divide paired
data to four segments

Matlab code

multiple solutions

l
x
P
l
x
(P)
P
l
x
t
t
t
arccos
cos
solution
position

forward
)
cos(

o
t
x
l
45
71
arccos0.70
P
feet

7071
.
0
foot

one

:

x

Y

(x
t
y
t
)

P
1

P
2

1
l
2
l
Forward kinematics of two
-

)
sin(
)
sin(
)
cos(
)
cos(
2
2
1
1
2
2
1
1
P
l
P
l
y
P
l
P
l
x
t
t

2
1
,
P
P
t
t
y
x
,
t
t
y
x
,
Inverse kinematics of two
-

2
2
2
t
t
y
x
B

)
atan2(
1
t
t
y
x
q

2
1
1
1
2
2
2
2
1
2
)
2
acos(
q
q
P
B
l
l
B
l
q

)
2
acos(
2
1
2
2
2
2
1
l
l
B
l
l

)
(
1
2

P
P
2
1
,
P
P
t
t
y
x
,
Reconstruction of forward
Kinematics

Learning forward kinematics

learn_MLP.m

eval_MLP2.m

fa2d.m

Forward Kinematics for planar robot

)
sin(
)
sin(
)
sin(
)
cos(
)
cos(
)
cos(
3
3
2
2
1
1
3
3
2
2
1
1
P
l
P
l
P
l
y
P
l
P
l
P
l
x
t
t

2
P
1
P
3
P
1
l
3
l
)
,
(
t
t
y
x
2
2
3
2
1
2
1
1
180
180
180
2

P
P
P
P
P
)
sin(
)
sin(
)
sin(
)
cos(
)
cos(
)
cos(
3
3
2
2
1
1
3
3
2
2
1
1
P
l
P
l
P
l
y
P
l
P
l
P
l
x
t
t

t
t
y
P
l
P
l
P
l
F
x
P
l
P
l
P
l
F

)
sin(
)
sin(
)
sin(
)
,
(
)
cos(
)
cos(
)
cos(
)
,
(
3
3
2
2
1
1
2
1
2
3
3
2
2
1
1
2
1
1

Inverse kinematics

At the position level, the problem is
stated as,

"Given the desired position of the robot's
hand, what must be the angles at all of
the robots joints? "

Plot

Exercise

Write matlab functions to implement
forward and inverse kinematics of a two
-

Example

l1=1;l2=1;

p1=pi/3;p2=pi/5;

[x,y]=fkin(p1,p2,l1,l2)

[p1,p2]=inverse_kin(x,y,l1,l2);

p1/pi

p2/pi

Nonlinear system

0
0
4
2
1
2
2
2
1

x
x
x
x
A set of nonlinear functions

0
)
,
(
0
4
)
,
(
2
1
2
1
2
2
2
2
1
2
1
1

x
x
x
x
f
x
x
x
x
f
x=linspace(
-
2,2);

plot(x,x); hold on;

plot(x,sqrt(4
-
x.^2))

plot(x,
-
sqrt(4
-
x.^2))

function F = myfun(x)

F(1) = x(1).^2 + x(2)^2
-
4;

F(2) = x(1)
-

x(2);

return

Least square of nonlinear
functions

i
i
x
x
x
x
f
2
2
1
,
)
,
(
min
2
1
function demo_lsq()

x0= ones(1,2)*2;

x = lsqnonlin(@myfun,x0)

y=myfun(x);

sum(y.^2)

return

function F = myfun(x)

F(1) = x(1).^2 + x(2)^2
-
4;

F(2) = x(1)
-

x(2);

return

Demo_lsq

0
12
)
,
(
0
24
2
)
,
(
2
2
2
1
2
2
2
2
1
2
1
1
2
1

x
x
x
x
f
x
x
x
x
f
x=linspace(
-
5,5);

plot(x,sqrt(24
-
2*x.^2),'r');hold on;

plot(x,
-
sqrt(24
-
2*x.^2),'r')

plot(x,sqrt(12+x.^2),'b')

plot(x,
-
sqrt(12+x.^2),'b')

Demo_lsq_2c

source code

Nonlinear systems

)
sin(
)
sin(
)
cos(
)
cos(
2
2
1
1
2
2
1
1
P
l
P
l
y
P
l
P
l
x
t
t

t
t
y
P
l
P
l
P
P
F
x
P
l
P
l
P
P
F

)
sin(
)
sin(
)
,
(
)
cos(
)
cos(
)
,
(
2
2
1
1
2
1
2
2
2
1
1
2
1
1
)
sin(
)
sin(
2
)
cos(
)
cos(
2
2
1
2
1
P
P
P
P

2
)
sin(
)
sin(
)
,
(
2
)
cos(
)
cos(
)
,
(
2
1
2
1
2
2
1
2
1
1

P
P
P
P
F
P
P
P
P
F
Exercise

Write a malab function to solve the
following nonlinear system

2
)
sin(
)
sin(
)
,
(
2
)
cos(
)
cos(
)
,
(
2
1
2
1
2
2
1
2
1
1

P
P
P
P
F
P
P
P
P
F