Data Driven Function Approximation

One link

• Given Xt what is the joint angle P

x

Yxt

P

solutionposition forward

)cos(Plxt

Plot1d

clear allfstr=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 Xt

Black box

Black boxP Xt

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;figureplot(x,y,'.');

Paired data

Adaptive function

Input );( ixf

Desired output

Network output

error

Network function

• Weight sum of hyper-tangent functions

}{}{}{

)tanh();( 01

mmm

M

mmmmi

rba

rbxarxf

Network

}{}{}{

)tanh();( 01

mmm

M

mmmmi

rba

rbxarxf

tanh

tanh

tanh

…….x

1

a2

a1

aM

b2

b1

bM

1

r1

r2

rM

rM+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:Ma. Add r(i)*tanh(x*a(i)+b(i)) to y

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)); endreturn

eval_MLP.m

Mean square error

Given Find to minimize

,...,1 ),,( niyx ii

));((1

)( 2

1

n

iii xfy

nE

Procedure 2: Mean square error

1. Input x,y,a,b,r2. Set E to zero3. Set n to the length of x4. For i=1:n

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

b) Add the error to E

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;endE=E/n;return

mean_square_error2.m

Matlab code1 eval_MLP.m Evaluate an MLP function

2 mean_square_error2.m

Calculate E

3 learn_MLP.m Seek parameters

Application

Black boxplot1d.m

P Xt

observation

Create paired datasampling.m

Input );( ixf

Desired output

Network output

error

Leaninglearning.m

Matlab code

Function approximation for an arbitrary target functionfa1d.m

x

Yxt

Pl

xP

l

x(P)

t

t

arccos

cos

Inverse kinematics

Construction of inverse kinematics

• Procedure1. Input a 1d forward kinematics

2. Sampling to form paired data

3. Input-output swapping

4. Rescaling

5. Learning

Black boxplot1d.m

P Xt

observation

Create paired datasampling.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 );( ixf

Desired output

Network output

error

Leaninglearning.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.336461ME 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 boxplot1d.m

P

Xt

Two observations

Derivative

Symbolic differentiation

function demo_diff()% input a string to specify a function% plot its derivativess=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 boxplot1dd.m

P

Xt

Two observations

Derivative

Dt

Reverse kinematics

Reverse KinematicsP

Xt

Derivative

Dt

plot3(y1,y2,ix)

Reverse kinematics

Matlab code

Segmentation

• Procedure1. 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

xP

l

x(P)

Plx

t

t

t

arccos

cos

solutionposition forward

)cos(

o

tx

l

45

71arccos0.70P

feet 7071.0

foot one :

Two links

x

Y

(xt yt)

P1

P2

1l

2l

Forward kinematics of two-link robot

)sin()sin(

)cos()cos(

2211

2211

PlPly

PlPlx

t

t

21,PP

tt yx , tt yx ,

Inverse kinematics of two-link robot

222tt yxB

)atan2(1t

t

y

xq

211

1

22

221

2 )2

acos(

qqP

Bl

lBlq

)2

acos(21

222

21

ll

Bll

)(12 PP

21,PP

tt yx ,

Reconstruction of forward Kinematics

Learning forward kinematics

learn_MLP.meval_MLP2.m

fa2d.m

Forward Kinematics for planar robot

)sin()sin()sin(

)cos()cos()cos(

332211

332211

PlPlPly

PlPlPlx

t

t

2P

1P

3P

1l

3l ),( tt yx

223

212

11

180

180

1802

PP

PP

P

)sin()sin()sin(

)cos()cos()cos(

332211

332211

PlPlPly

PlPlPlx

t

t

t

t

yPlPlPlF

xPlPlPlF

)sin()sin()sin(),(

)cos()cos()cos(),(

332211212

332211211

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-link robot

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/pip2/pi

Nonlinear system

0

04

21

2

2

2

1

xx

xx

A set of nonlinear functions

0),(

04),(

21212

2

2

2

1211

xxxxf

xxxxf

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

ixxxxf 2

21,),(min

21

function demo_lsq()x0= ones(1,2)*2;x = lsqnonlin(@myfun,x0)y=myfun(x);sum(y.^2)returnfunction F = myfun(x) F(1) = x(1).^2 + x(2)^2 -4; F(2) = x(1) - x(2);return

Demo_lsq

012),(

0242),(22

212

2

2

2

1211

21

xxxxf

xxxxf

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(

2211

2211

PlPly

PlPlx

t

t

t

t

yPlPlPPF

xPlPlPPF

)sin()sin(),(

)cos()cos(),(

2211212

2211211

)sin()sin(2

)cos()cos(2

21

21

PP

PP

2)sin()sin(),(

2)cos()cos(),(

21212

21211

PPPPF

PPPPF

Exercise

• Write a malab function to solve the following nonlinear system

2)sin()sin(),(

2)cos()cos(),(

21212

21211

PPPPF

PPPPF