Distributed under the (new) BSD License. Copyright (c) 2014, Nicolas P. Rougier
Contributors: Nicolas P. Rougier (Nicolas.Rougier@inria.fr),
Hima Mehta(hima.mehta@inria.fr)Description : The below Model of simple arm exhibits reachability capabilities of simple arm which generates reachable region depending on Humerus bone (upper arm) length and Ulna bone (lower arm) length and generates elbow and shoulder joints and actual linear space of targets . Further this experiments distributed coding on taking targets from reachable regions and analyzing errors between actual position and position after trying to obtain mapping from normally distributed normalized values.
In [1]:
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
from mpl_toolkits.axes_grid1 import make_axes_locatable
In [2]:
n = 20
# Humerus bone (upper arm) length
l1 = 1.0
# Ulna bone (lower arm) length
l2 = 1.0
# Shoulder joint
Sx = 0.0
Sy = 0.0
#Domain of reachable Targets
domain = np.zeros((2,2))
domain[0] = [-0.5,0.25]
domain[1] = [1.0,1.75]
xmin,xmax = domain[0]
ymin,ymax = domain[1]
#print xmin,xmax
#weights
Theta = np.zeros((2,n*n))
#Error
error = np.zeros((n,n))
new_pos = np.zeros((2,n*n))
In [3]:
#Forward and inverse Definitions
def forward(theta1, theta2, l1=l1, l2=l2, Sx=Sx, Sy=Sy):
# Elbow joint
Ex = Sx + l1 * np.cos(theta1)
Ey = Sy + l1 * np.sin(theta1)
# Hand Position
Hx = Ex + l2 * np.cos(theta1+theta2)
Hy = Ey + l2 * np.sin(theta1+theta2)
return Hx, Hy
def inverse(Hx, Hy, l1=l1, l2=l2, Sx=Sx, Sy=Sy):
Hx = np.array(Hx) - Sx
Hy = np.array(Hy) - Sy
# Elbow joint
c2 = (Hx*Hx + Hy*Hy - l1*l1 - l2*l2)/(2*l1*l2)
s2 = np.sqrt(1. - c2*c2)
theta2 = np.arctan2(s2, c2)
# Shoulder joint
k1 = l1 + l2*c2
k2 = l2*s2
theta1 = np.arctan2(Hy, Hx) - np.arctan2(k2, k1)
return theta1, theta2
In [4]:
#Reachable Region
T1,T2 = np.meshgrid(np.linspace(0, 2*np.pi/3, n),
np.linspace(0, 2*np.pi/3, n))
Hx, Hy = forward(T1, T2)
f = plt.figure(figsize=(11,8))
ax = f.add_subplot(111,aspect=1)
ax.scatter(Hx, Hy, s=15,c="black")
plt.title("Reachability of Arm")
Out[4]:
In [5]:
#taking A square from reachable region
Hxmin = Hx[(np.around(Hx[:],decimals=1)==xmin)][0]
Yxmin = Hy[(np.around(Hy[:],decimals=1)==ymin)][0]
Hxmax = Hx[(np.around(Hx[:],decimals=2)==xmax)][0]
Hymax = Hy[(np.around(Hy[:],decimals=2)==ymax)][0]
#print Hxmin,Yxmin,Hxmax,Hymax
f = plt.figure(figsize=(14,9))
ax = f.add_subplot(111,aspect=1)
ax.scatter(Hx, Hy, s=5)
ax.plot([Hxmin,Hxmin,Hxmax,Hxmax,Hxmin],[Yxmin, Hymax,Hymax,Yxmin,Yxmin],'-ko',ms=10,alpha=0.6)
plt.title("Square Taken from reachable region")
Out[5]:
In [6]:
def Gaussian(x,y,sigma=0.2,shape=(10,10)):
X = np.linspace(0,1,shape[0],endpoint=True)
Y = np.linspace(0,1,shape[1],endpoint=True)
X,Y = np.meshgrid(X,Y)
R = np.sqrt((X-x)**2 + (Y-y)**2)
return np.exp(-R**2/sigma**2)
#test
if 0:
G = Gaussian(0.5,0.5,shape=(50,50),sigma=0.05)
plt.imshow(G, cmap=plt.cm.gray_r,interpolation="nearest",extent=[0,1,0,1])
In [6]:
In [7]:
def Encode(x,y,sigma,domain, shape=(10,10)):
xmin,xmax = domain[0]
ymin,ymax = domain[1]
xm = (x-xmin) / (xmax-xmin)
ym = (y-ymin) / (ymax-ymin)
return Gaussian(xm,ym,sigma=sigma,shape=shape)
#test
if 0:
SMA = Encode(xmin+(xmax-xmin)/2, ymin+(ymax-ymin)/2, sigma=0.25, domain=domain)
plt.imshow(SMA)
In [8]:
xmin,xmax = domain[0]
ymin,ymax = domain[1]
xi = np.linspace(xmin,xmax, n)
yi = np.linspace(ymin,ymax, n)
X,Y = np.meshgrid(xi,yi)
target = np.zeros((1,2))
T1,T2 = inverse(X,Y) #Getting all Angles for above square
Theta[0] = T1.reshape(n*n)
Theta[1] = T2.reshape(n*n)
for i,x in enumerate(xi):
for j,y in enumerate(yi):
SMA = Encode(x,y,sigma=0.05,domain=domain,shape=(n,n))
x_, y_ = np.dot(Theta,SMA.reshape((n*n,1))) / np.sum(SMA)
pos = forward(x_,y_)
new_pos[0][i*n+j] = pos[0]
new_pos[1][i*n+j] = pos[1]
error[i][j] = np.sqrt((x-pos[0])**2 + (y-pos[1])**2)
f = plt.figure(figsize=(15,11))
ax = f.add_subplot(121,aspect=1)
plt.title("Errors")
im = plt.imshow(error, cmap=plt.cm.gray, interpolation="nearest", extent=[xmin,xmax,ymin,ymax])
divider = make_axes_locatable(ax)
cax = divider.append_axes("right", size="5%", pad=0.05)
plt.colorbar(im, cax=cax)
ax = f.add_subplot(122,aspect=1)
ax.scatter(X,Y,s=40,c="white",linewidth=2 ,alpha = 0.7)
ax.scatter(new_pos[0],new_pos[1],s=40,c="black",linewidth=2 ,alpha = 0.9)
plt.title("Actual Target vs New position")
plt.show()
f = plt.figure(figsize=(10,10))
ax = f.add_subplot(121,aspect=1)
plt.imshow(error, cmap=plt.cm.gray, interpolation="nearest", extent=[xmin,xmax,ymin,ymax])
ax.scatter(X,Y,s=30,c="white",linewidth=2 ,alpha = 0.6)
plt.title("Error for each target")
plt.show()
In [11]:
#Random Targets and Mean Error
xr = np.linspace(-0.5,0.25,100)
yr = np.linspace(1.0,1.75,100)
Xr,Yr = np.meshgrid(xr,yr)
e = np.zeros((100,100))
ep = np.zeros((100,100))
#f = plt.figure(figsize=(15,11))
#ax = f.add_subplot(111,aspect=1)
#ax.scatter(Xr,Yr)
#plt.show()
for i,k in enumerate(xr):
for j,l in enumerate(yr):
SMA = Encode(k,l,sigma=0.05,domain=domain,shape=(n,n))
x_, y_ = np.dot(Theta,SMA.reshape((n*n,1))) / np.sum(SMA)
pos = forward(x_,y_)
e[i][j] = np.sqrt((k-pos[0])**2 + (l-pos[1])**2)
ep[i][j] = e[i][j]
ep[i][j] /= (xmax-xmin)/100
f = plt.figure(figsize=(15,4))
ax = f.add_subplot(121,aspect=1)
img = ax.imshow(e,interpolation="nearest",cmap=plt.cm.gray_r,extent=[xmin,xmax,ymin,ymax])
plt.title("Error of 100 x 100 targets")
plt.colorbar(img)
ax = f.add_subplot(122,aspect=1)
img = ax.imshow(ep,interpolation="nearest",cmap=plt.cm.gray_r,extent=[xmin,xmax,ymin,ymax])
plt.colorbar(img)
plt.title("% Error")
print "Average Error of 100 x 100 Targets = ", np.average(e)
In [9]: