Skip to content
Snippets Groups Projects
Commit 0052a286 authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

Merge branch 'master' of gitlab.idiap.ch:rli/robotics-codes-from-scratch

parents 49831881 c8f48299
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
python/IK_manipulator_manipulability.py
+ 23
− 250
View file @ 0052a286
......@@ -73,7 +73,6 @@ x[0] = x[0] + np.pi
## Inverse kinematics (IK)
# ===============================
ax.scatter(fh[0], fh[1], color='r', marker='.', s=10**2) #Plot target
for t in range(param.nbData):
f = fkin(x, param) # Forward kinematics (for end-effector)
......@@ -83,72 +82,15 @@ for t in range(param.nbData):
f_rob = fkin0(x, param) # Forward kinematics (for all articulations, including end-effector)
ax.plot(f_rob[0,:], f_rob[1,:], color=str(1-t/param.nbData), linewidth=2) # Plot robot
### MANIPULABILITY ###
J = J[:2,:]
center = np.array([f[0], f[1]]) # end-effector position
print(f"Center: {center}")
length, width, height = 1.8,1.5,1 # max joint velocities
size = np.array([length, width, height])
refell = 130 * np.identity(2) # reference ellipsoid
# Choice of the Jacobian matrix
J1 = False
J2 = False
J3 = False
J4 = False
diffJac = [J1, J2, J3, J4]
# 1. Robot manipulator
if J1 == True:
theta = 5*m.pi/6
U = np.array([[m.cos(theta), -m.sin(theta)],[m.sin(theta), m.cos(theta)]])
J = U.T @ J
print(J)
# 2. Bounded joint-space
if J2 == True:
jminlim = -np.ones(param.nbVarX)
jmaxlim = np.ones(param.nbVarX)
J = np.diag(1 - np.heaviside(x - jminlim,0)*np.heaviside(jmaxlim - x, 0))[:2,:]
print(J)
# 3. Bounded task-space
if J3 == True:
tminlim = -np.ones(2)
tmaxlim = np.ones(2)
J = np.diag(1 - np.heaviside(f[:2] - tminlim,0)*np.heaviside(tmaxlim - f[:2], 0)) @ J
print(J)
# 4. Object boundaries
if J4 == True:
theta = m.pi/4
U = np.array([[m.cos(theta), -m.sin(theta)],[m.sin(theta), m.cos(theta)]])
tminlim = -np.ones(2)
tmaxlim = 2*np.ones(2)
J = np.diag(1 - np.heaviside(U.T@(f[:2] - fh[:2]) - tminlim,0)*np.heaviside(tmaxlim - (U.T@(f[:2] - fh[:2])), 0)) @ J
print(J)
# Boundaries in joint-velocity space
# 1. Rectangular cuboid
showedges = False # Shows the mapping of the cube's edges
# 2. Ellipse
ellBound = True
# 3. Superellipsoid
superBound = False
superVolume = False # Returns the fraction of the rectangular cuboid's volume covered by the superellipsoid
# 1. Rectangular cuboid
# initialize a rectangular cuboid for polytope
cube = np.zeros((2 ** param.nbVarX, param.nbVarX))
vertex = np.zeros(param.nbVarX)
# These two loops store the numbers 0 to 7 in binary (which can be seen as the coordinates of a cube)
......@@ -159,12 +101,10 @@ for count1 in range(2 ** param.nbVarX):
# Rescaling so that the center of the cube is located at the origin
cube = cube * 2 - 1
for i in range(len(size)):
cube[:,i] = cube[:,i] * size[i]
# Computation of the manipulability polytope
# Computation of the manipulability polytope (blue)
polytope = np.zeros((2 ** param.nbVarX,2))
for count in range(2 ** param.nbVarX):
polytope[count] = J @ cube[count] + center
......@@ -173,17 +113,13 @@ xpoints = polytope[:,0]
ypoints = polytope[:,1]
polytope = np.array([xpoints, ypoints]).T
if not any(diffJac) == True:
hull = scipy.spatial.ConvexHull(polytope)
# vertices of the covex hull (might come in handy)
vertices = np.zeros((len(hull.vertices),2))
for i in range(len(hull.vertices)):
vertices[i] = polytope[hull.vertices[i]]
cube_norms = np.linalg.norm(vertices, axis = 1)
for simplex in hull.simplices:
plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'b--')
hull = scipy.spatial.ConvexHull(polytope)
vertices = np.zeros((len(hull.vertices),2))
for i in range(len(hull.vertices)):
vertices[i] = polytope[hull.vertices[i]]
ax.plot(xpoints, ypoints, "kx")
for simplex in hull.simplices:
plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'b--')
def norm(vec, coeff, exp):
......@@ -199,189 +135,26 @@ def sample(npoints, coeff, exp):
vecs[count] = vecs[count] / norm(vecs[count], coeff, exp)
return vecs
# 2. Ellipsoid
if ellBound == True:
num_iter = 1000
# coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
coeff = size # if one wants the superellipsoid to be contained in the cuboid
exp = 2
# plot the manipulability ellipsoid (red)
num_iter = 1000
coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
# coeff = size # if one wants the superellipsoid to be contained in the cuboid
ell_jvlim = sample(num_iter, coeff, 2)
ell_tvlim = np.zeros((num_iter,2))
ell_jvlim = sample(num_iter, coeff, 2)
for count in range(len(ell_jvlim)):
ell_tvlim[count] = J @ ell_jvlim[count] + center
ell_tvlim = np.zeros((num_iter,2))
ell_x, ell_y = ell_tvlim.T
for count in range(len(ell_jvlim)):
ell_tvlim[count] = J @ ell_jvlim[count] + center
A = np.diag(coeff ** 2)
Q = J @ A @ J.T
eigenvals, eigenvecs = np.linalg.eig(Q)
# Sort Eigenvalues and EigenVectors
idx = eigenvals.argsort()[::-1]
eigenvals = eigenvals[idx]
eigenvecs = eigenvecs[idx]
ell_x, ell_y = ell_tvlim.T
# the sqrt of the eigenvalues give the length of the semi-axes
print(f"Ellipsoid eigenvalues: {eigenvals}")
vec1, vec2 = eigenvecs.T
vec1 = vec1 * m.sqrt(eigenvals[0]) + center
vec2 = vec2 * m.sqrt(eigenvals[1]) + center
polytope = np.array([ell_x, ell_y]).T
hull = scipy.spatial.ConvexHull(polytope)
for simplex in hull.simplices:
plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'r--')
if not any(diffJac) == True:
polytope = np.array([ell_x, ell_y]).T
hull = scipy.spatial.ConvexHull(polytope)
# vertices of the covex hull (might come in handy)
#vertex = np.zeros((len(hull.vertices),2))
#for i in range(len(hull.vertices)):
# vertex[i] = polytope[hull.vertices[i]]
#ax.plot(vertex[:,0], vertex[:,1], "gv")
for simplex in hull.simplices:
plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'r--')
# 3. Superellipsoid (rigorously this is not the most general form of a superellipsoid)
if superBound == True:
num_iter = 1000
# coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
coeff = size # if one wants the superellipsoid to be contained in the cuboid
exp = 4 # exp = 2 for an ellipse, exp = 4 for squircle, exp --> infty for rectangular cuboid
if superVolume == True:
vol = scipy.special.gamma(1/exp + 1)**param.nbVarX/scipy.special.gamma(param.nbVarX/exp + 1)
print(f"fraction of the rectangular cuboid's volume: {vol}")
jvlim = sample(num_iter, coeff, exp)
tvlim = np.zeros((num_iter,2))
for count in range(len(jvlim)):
tvlim[count] = J @ jvlim[count] + center
xpoints, ypoints = tvlim.T
# Idea: approximate whatever shape I get with an ellipsoid, so that the reasoning on the eigenvalues apply!
# Note: it does not just give the same ellipsoid as if exp = 2
tvmax = tvlim[np.argmax(np.linalg.norm(tvlim-center, axis = 1))]
cov_mat = np.cov(tvlim.T)
eigenvals, eigenvecs = np.linalg.eig(cov_mat)
idx = eigenvals.argsort()[::-1]
eigenvals = eigenvals[idx]
eigenvecs = eigenvecs[:,idx]
eigenvecs = eigenvecs * np.sqrt(eigenvals)
ratio = np.linalg.norm(tvmax-center)/m.sqrt(eigenvals[0])
eigenvecs *= ratio
vec1, vec2 = eigenvecs.T[0],eigenvecs.T[1]
'''
# Manipulability matrix
Q = eigenvecs @ np.array([[eigenvals[0],0],[0,eigenvals[1]]]) @ np.linalg.inv(eigenvecs)
# Riemannian distance
A = np.linalg.inv(scipy.linalg.sqrtm(refell)) @ Q @ np.linalg.inv(scipy.linalg.sqrtm(refell))
d = np.linalg.norm(scipy.linalg.logm(A))
print(f"Riemannian distance: {d}")
'''
print(f"Superellipsoid eigenvalues: {(ratio * np.sqrt(eigenvals))**2}")
phi = np.linspace(0, 2*m.pi,200)
x = np.zeros((len(phi),2))
for i in range(len(phi)):
x[i] = center + vec1 * m.cos(phi[i]) + vec2 * m.sin(phi[i])
super_norms = np.linalg.norm(x,axis = 1)
if not any(diffJac) == True:
ax.plot(x[:,0], x[:,1], "g1", label = "superellipsoid")
vec1 += center
vec2 += center
ax.plot([center[0], tvmax[0]], [center[1],tvmax[1]])
ax.plot([center[0], vec1[0]], [center[1],vec1[1]])
ax.plot([center[0], vec2[0]], [center[1],vec2[1]])
# Plots
showhull = True # to show the convex hull of the superellipsoid
showpoints = False # to show the image of all the sampled points
if showhull == True and not any(diffJac) == True:
polytope = np.array([xpoints, ypoints]).T
hull = scipy.spatial.ConvexHull(polytope)
# vertices of the covex hull (might come in handy)
#vertex = np.zeros((len(hull.vertices),2))
#for i in range(len(hull.vertices)):
# vertex[i] = polytope[hull.vertices[i]]
#ax.plot(vertex[:,0], vertex[:,1], "gv")
for simplex in hull.simplices:
plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'g--')
if showpoints == True:
ax.plot(xpoints, ypoints, "kx")
#fig = plt.figure()
#ax2 = fig.add_subplot(projection='3d')
#ax2.scatter(cube[:,0], cube[:,1], cube[:,2], c = "blue", label = "rectangular cuboid")
#if ellBound == True:
# ax2.scatter(ell_jvlim[:,0], ell_jvlim[:,1], ell_jvlim[:,2], c = "red", label = "ellipsoid")
#
#if superBound == True:
# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "green", label = "superellipsoid")
#legend = ax2.legend(loc='upper right')
#if showedges == True:
# num_points = 50
# jvlim, tvlim = np.zeros((num_points,3)), np.zeros((num_points,2))
#
# edges = np.vstack((np.unique(cube[:,:2], axis = 0), np.unique(cube[:,1:3], axis = 0), np.unique(cube[:,0:3:2], axis = 0)))
# for edge in edges[:4]:
# for count in range(num_points):
# z = (random.random() * 2 - 1) * height
# jvlim[count] = np.array([edge[0],edge[1],z])
# tvlim[count] = J @ jvlim[count] + center
# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "blue")
# ax.plot(tvlim[:,0], tvlim[:,1], "bx")
# for edge in edges[4:8]:
# for count in range(num_points):
# x = (random.random() * 2 - 1) * length
# jvlim[count] = np.array([x,edge[0],edge[1]])
# tvlim[count] = J @ jvlim[count] + center
# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "red")
# ax.plot(tvlim[:,0], tvlim[:,1], "rx")
#
# for edge in edges[8:]:
# for count in range(num_points):
# y = (random.random() * 2 - 1) * width
# jvlim[count] = np.array([edge[0],y,edge[1]])
# tvlim[count] = J @ jvlim[count] + center
# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "green")
# ax.plot(tvlim[:,0], tvlim[:,1], "gx")
#
#ax.axis('off')
ax.axis('equal')
#ax2.axis('equal')
#plt.title(f"Length: {length}, width: {width}, height: {height}, p = {exp}")
plt.show()
python/LQR_probabilistic.py 0 → 100644
+ 66
− 0
View file @ 0052a286
import numpy as np
from math import factorial
import matplotlib.pyplot as plt
from scipy.linalg import solve_continuous_are
from scipy.stats import multivariate_normal
# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
param.nbDeriv = 2 # Order of the dynamical system
param.nbVarPos = 2 # Number of position variable
param.nbVar = param.nbVarPos * param.nbDeriv # Dimension of state vector
param.nbData = 100 # Number of datapoints
param.rfactor = 1e-7 # Control weight term
R = np.eye((param.nbData-1) * param.nbVarPos) * param.rfactor # Control cost matrix
Q = np.zeros((param.nbVar * param.nbData, param.nbVar * param.nbData)) # Task precision for augmented state
xd = np.zeros([param.nbVar, param.nbData])
target = np.random.uniform(size=param.nbVarPos)
xd[:, param.nbData-1] = np.concatenate((target, np.zeros(param.nbVarPos)))
xd = xd.T.flatten()
Q[param.nbVar*(param.nbData-1):param.nbVar*param.nbData,param.nbVar*(param.nbData-1):param.nbVar*param.nbData] = 10.0 * np.eye(param.nbVar)
A1d = np.zeros((param.nbDeriv,param.nbDeriv))
B1d = np.zeros((param.nbDeriv,1))
for i in range(param.nbDeriv):
A1d += np.diag( np.ones(param.nbDeriv-i), i ) * param.dt**i * 1/factorial(i)
B1d[param.nbDeriv-i-1] = param.dt**(i+1) * 1/factorial(i+1)
A = np.eye(param.nbVar)
A[:param.nbVar, :param.nbVar] = np.kron(A1d, np.identity(param.nbVarPos))
B = np.zeros((param.nbVar, param.nbVarPos))
B[:param.nbVar] = np.kron(B1d, np.identity(param.nbVarPos))
Su = np.zeros((param.nbVar*param.nbData, param.nbVarPos * (param.nbData-1)))
Sx = np.kron(np.ones((param.nbData,1)), np.eye(param.nbVar,param.nbVar))
M = B
for i in range(1,param.nbData):
Sx[i*param.nbVar:param.nbData*param.nbVar,:] = np.dot(Sx[i*param.nbVar:param.nbData*param.nbVar,:], A)
Su[param.nbVar*i:param.nbVar*i+M.shape[0],0:M.shape[1]] = M
M = np.hstack((np.dot(A,M),B)) # [0,nb_state_var-1]
x0 = np.random.uniform(size=param.nbVar)
mean = Sx @ x0 + Su @ np.linalg.inv(Su.T @ Q @ Su + R) @ Su.T @ Q @ (xd - Sx @ x0)
cov = Su @ (Su.T @ Q @ Su + R) @ Su.T
trajectories = multivariate_normal.rvs(mean=mean, cov=cov, size=100)
plt.figure()
for k in range(100):
trajectory = trajectories[k, :]
trajectory = trajectory.reshape((param.nbData, param.nbVar))
plt.plot(trajectory[:, 0], trajectory[:, 1], alpha=0.5)
plt.scatter(x0[0], x0[1], color='green', label='Initial State')
plt.scatter(target[0], target[1], color='red', label='Target State')
plt.legend()
plt.grid()
plt.show()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment