Skip to content
Snippets Groups Projects
Commit 485b62d2 authored by Hakan GIRGIN's avatar Hakan GIRGIN
Browse files

Uploading python version of demo_OC_LQT_nullspace01.m

parent 5f699061
Branches
Tags
No related merge requests found
'''
Linear Quadratic tracker applied on a via point example
Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
Written by Jeremy Maceiras <jeremy.maceiras@idiap.ch>,
Sylvain Calinon <https://calinon.ch>
This file is part of RCFS.
RCFS is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License version 3 as
published by the Free Software Foundation.
RCFS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with RCFS. If not, see <http://www.gnu.org/licenses/>.
'''
import numpy as np
from math import factorial
import matplotlib.pyplot as plt
from scipy.linalg import block_diag
# Parameters
# ===============================
dt = 1e-1 # Time step length
nbPoints = 1 # Number of targets
nbDeriv = 1 # Order of the dynamical system
nbVarPos = 2 # Number of position variable
nbData = 50 # Number of datapoints
rfactor = 1e-4 # Control weight term
nbRepros = 60 # Number of stochastic reproductions
nb_var = nbVarPos * nbDeriv # Dimension of state vector
# Dynamical System settings (discrete)
# =====================================
A1d = np.zeros((nbDeriv,nbDeriv))
B1d = np.zeros((nbDeriv,1))
for i in range(nbDeriv):
A1d += np.diag( np.ones(nbDeriv-i) ,i ) * dt**i * 1/factorial(i)
B1d[nbDeriv-i-1] = dt**(i+1) * 1/factorial(i+1)
A = np.kron(A1d,np.identity(nbVarPos))
B = np.kron(B1d,np.identity(nbVarPos))
# Build Sx and Su transfer matrices
Su = np.zeros((nb_var*nbData,nbVarPos * (nbData-1)))
Sx = np.kron(np.ones((nbData,1)),np.eye(nb_var,nb_var))
M = B
for i in range(1, nbData):
Sx[i*nb_var:nbData*nb_var,:] = np.dot(Sx[i*nb_var:nbData*nb_var,:], A)
Su[nb_var*i:nb_var*i+M.shape[0],0:M.shape[1]] = M
M = np.hstack((np.dot(A,M),B)) # [0,nb_state_var-1]
# Cost function settings
# =====================================
R = np.identity( (nbData-1) * nbVarPos ) * rfactor # Control cost matrix
t_list = np.stack([nbData-1]) # viapoint time list
mu_list = np.stack([np.array([20,10.])]) # viapoint list
Q_list = np.stack([np.eye(nb_var)*1e3]) # viapoint precision list
mus = np.zeros((nbData, nb_var))
Qs = np.zeros((nbData, nb_var, nb_var))
for i,t in enumerate(t_list):
mus[t] = mu_list[i]
Qs[t] = Q_list[i]
muQ = mus.flatten()
Q = block_diag(*Qs)
# Change here off block diagonals of Q
# Batch LQR Reproduction
# =====================================
## Precomputation of basis functions to generate structured stochastic u through Bezier curves
# Building Bernstein basis functions
def build_phi_bernstein(nb_data, nb_fct):
t = np.linspace(0,1,nb_data)
phi = np.zeros((nb_data,nb_fct))
for i in range(nb_fct):
phi[:,i] = factorial(nb_fct-1) / (factorial(i) * factorial(nb_fct-1-i)) * (1-t)**(nb_fct-1-i) * t**i
return phi
nbRBF = 10
H = build_phi_bernstein(nbData-1, nbRBF)
J = Q @ Su
N = np.eye(J.shape[-1]) - np.linalg.pinv(J) @ J # nullspace operator of LQT
# Principal Task
x0 = np.zeros(nb_var)
u1 = np.linalg.solve(Su.T @ Q @ Su + R, Su.T @ Q @ (muQ - Sx @ x0))
x1 = (Sx @ x0 + Su @ u1).reshape((-1,nb_var))
# Secondary Task
repr_x = np.zeros((nbRepros, nbData, nb_var))
for n in range(nbRepros):
w = np.random.randn(nbRBF, nbVarPos) * 1E1 # Random weights
u2 = H @ w # Reconstruction of control signals by a weighted superposition of basis functions
u = u1 + N @ u2.flatten()
repr_x[n] = np.reshape(Sx @ x0 + Su @ u, (-1, nb_var)) # Reshape data for plotting
# Plotting
# =========
plt.figure()
plt.title("2D Trajectory")
plt.scatter(x1[0,0],x1[0,1],c='black',s=100)
for mu in mu_list:
plt.scatter(mu[0], mu[1],c='red',s=100)
plt.plot(x1[:,0] , x1[:,1], c='black')
for i in range(nbRepros):
plt.plot(repr_x[i, :, 0], repr_x[i, :, 1], c='blue', alpha=0.1, zorder=0)
plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
fig,axs = plt.subplots(2,1)
for i,t in enumerate(t_list):
axs[0].scatter(t, mu_list[i][0],c='red')
for i in range(nbRepros):
axs[0].plot(repr_x[i, :, 0], c='blue', alpha=0.1, zorder=0)
axs[0].plot(x1[:,0], c='black')
axs[0].set_ylabel("$x_1$")
axs[0].set_xticks([0,nbData])
axs[0].set_xticklabels(["0","T"])
for i,t in enumerate(t_list):
axs[1].scatter(t, mu_list[i][1],c='red')
for i in range(nbRepros):
axs[1].plot(repr_x[i, :, 1], c='blue', alpha=0.1, zorder=0)
axs[1].plot(x1[:,1], c='black')
axs[1].set_ylabel("$x_2$")
axs[1].set_xlabel("$t$")
axs[1].set_xticks([0,nbData])
axs[1].set_xticklabels(["0","T"])
plt.show()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment