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

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

parents c4c4af15 13e15d91
Branches
No related tags found
No related merge requests found
%% Point-mass LQR with infinite horizon
%% Point-mass discrete LQR with infinite horizon
%%
%% Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
%% Written by Sylvain Calinon <https://calinon.ch>
%% Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch/>
%% Written by Ante Marić <ante.maric@idiap.ch> and Sylvain Calinon <https://calinon.ch>
%%
%% This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
%% License: GPL-3.0-only
......@@ -21,12 +21,15 @@ param.rfactor = 4E-2; %Control cost in LQR
%Control cost matrix
R = eye(param.nbVarPos) * param.rfactor;
%Target and desired covariance
%Target and desired covariance (for precision matrix on static features)
param.Mu = [randn(param.nbVarPos,1); zeros(param.nbVarPos*(param.nbDeriv-1),1)];
[Ar,~] = qr(randn(param.nbVarPos));
xCov = Ar * diag(rand(param.nbVarPos,1)) * Ar' * 1E-1;
%Target and desired covariance (for precision matrix on static and dynamic features)
%param.Mu = [randn(param.nbVarPos,1); zeros(param.nbVarPos*(param.nbDeriv-1),1)];
%xCov = diag([rand(2,1); rand(2,1) / param.dt]) * 1E-1;
%% Discrete dynamical System settings
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -44,7 +47,12 @@ B = kron(B1d, eye(param.nbVarPos)); %Discrete nD
%% discrete LQR with infinite horizon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = blkdiag(inv(xCov), zeros(param.nbVarPos*(param.nbDeriv-1))); %Precision matrix
% Precision matrix on static features
Q = blkdiag(inv(xCov), zeros(param.nbVarPos*(param.nbDeriv-1)));
% Precision matrix on static and dynamic features
% Q = inv(xCov);
P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
......@@ -59,7 +67,7 @@ for n=1:param.nbRepros
x = [ones(param.nbVarPos,1)+randn(param.nbVarPos,1)*5E-1; zeros(param.nbVarPos*(param.nbDeriv-1),1)];
for t=1:param.nbData
r(n).Data(:,t) = x;
u = L * (param.Mu - x); %Compute acceleration (with only feedback terms)
u = L * (param.Mu - x); %Compute control input (with only feedback terms)
x = A * x + B * u;
end
end
......@@ -110,7 +118,7 @@ function X = solveAlgebraicRiccati_eig_discrete(A, G, Q)
n = size(A,1);
%Symplectic matrix (see https://en.wikipedia.org/wiki/Algebraic_Riccati_equation)
%Z = [A+B*(R\B')/A'*Q, -B*(R\B')/A'; -A'\Q, A'^-1];
%Z = [A+B*(R\B')/A'*Q, -B*(R\B')/A'; -A'\Q, A'^-1];
Z = [A+G/A'*Q, -G/A'; -A'\Q, inv(A')];
%Since Z is symplectic, if it does not have any eigenvalues on the unit circle,
......
'''
Point-mass LQR with infinite horizon
Point-mass discrete LQR with infinite horizon
Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
Written by Jérémy Maceiras <jeremy.maceiras@idiap.ch> and
Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch/>
Written by Ante Marić <ante.maric@idiap.ch>, Jérémy Maceiras <jeremy.maceiras@idiap.ch> and
Sylvain Calinon <https://calinon.ch>
This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
......@@ -11,83 +11,119 @@ License: GPL-3.0-only
import numpy as np
import copy
from scipy.linalg import solve_discrete_are as solve_algebraic_riccati_discrete
from scipy.stats import multivariate_normal
from math import factorial
import matplotlib.pyplot as plt
# Plot a 2D Gaussians
def plot_gaussian(mu, sigma):
w, h = 100, 100
std = [np.sqrt(sigma[0, 0]), np.sqrt(sigma[1, 1])]
x = np.linspace(mu[0] - 3 * std[0], mu[0] + 3 * std[0], w)
y = np.linspace(mu[1] - 3 * std[1], mu[1] + 3 * std[1], h)
x, y = np.meshgrid(x, y)
x_ = x.flatten()
y_ = y.flatten()
xy = np.vstack((x_, y_)).T
normal_rv = multivariate_normal(mu, sigma)
z = normal_rv.pdf(xy)
z = z.reshape(w, h, order='F')
plt.contourf(x, y, z.T,levels=1,colors=["white","red"],alpha=.5)
import matplotlib.patches as patches
def plot_gaussian(Mu, Sigma, color, valAlpha=1):
nbDrawingSeg = 100
darkcolor = color * 0.7
t = np.linspace(-np.pi, np.pi, nbDrawingSeg)
D, V = np.linalg.eig(Sigma)
R = np.real(V @ np.sqrt(np.diag(D)))
X = R @ np.array([np.cos(t), np.sin(t)]) + Mu.reshape(-1, 1)
if valAlpha < 1: # Plot with alpha transparency
patch = patches.Polygon(X.T, closed=True, facecolor=color, edgecolor=color, alpha=valAlpha, zorder=0)
plt.gca().add_patch(patch)
plt.plot(Mu[0], Mu[1], '.', markersize=10, color=darkcolor, zorder=1)
else: # Plot without transparency
patch = patches.Polygon(X.T, closed=True, facecolor=color, edgecolor=darkcolor, zorder=0)
plt.gca().add_patch(patch)
plt.plot(Mu[0], Mu[1], '.', markersize=10, color=darkcolor, zorder=1)
plt.axis('equal')
def solve_algebraic_riccati_eig_discrete(A, G, Q):
n = A.shape[0]
# Symplectic matrix (see https://en.wikipedia.org/wiki/Algebraic_Riccati_equation)
Z = np.block([
[A + G @ np.linalg.inv(A).T @ Q, -G @ np.linalg.inv(A).T],
[-np.linalg.inv(A).T @ Q, np.linalg.inv(A).T]
])
# Since Z is symplectic, if it does not have any eigenvalues on the unit circle,
# then exactly half of its eigenvalues are inside the unit circle.
eigvals, eigvecs = np.linalg.eig(Z)
U = []
for j in range(2 * n):
if np.abs(eigvals[j]) < 1: # inside unit circle
U.append(eigvecs[:, j])
U = np.array(U).T
X = np.real(U[n:, :] @ np.linalg.inv(U[:n, :]))
return X
# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
param.nbData = 200 # Number of datapoints
param.nbRepros = 4 #Number of reproductions
param.nbVarPos = 2 #Dimension of position data (here: x1,x2)
param.nbDeriv = 2 #Number of static & dynamic features (D=2 for [x,dx])
param.nbVar = param.nbVarPos * param.nbDeriv #Dimension of state vector in the tangent space
param.dt = 1e-2 #Time step duration
param.rfactor = 4e-2 #Control cost in LQR
param = lambda: None # Lazy way to define an empty class in python
param.nbData = 200 # Number of datapoints
param.nbRepros = 4 # Number of reproductions
param.nbVarPos = 2 # Dimension of position data (here: x1,x2)
param.nbDeriv = 2 # Number of static & dynamic features (D=2 for [x,dx])
param.nbVar = param.nbVarPos * param.nbDeriv # Dimension of state vector in the tangent space
param.dt = 1e-2 # Time step duration
param.rfactor = 4e-2 # Control cost in LQR
# Control cost matrix
R = np.identity(param.nbVarPos) * param.rfactor
# Target and desired covariance
param.Mu = np.hstack(( np.random.uniform(size=param.nbVarPos) , np.zeros(param.nbVarPos) ))
Ar,_ = np.linalg.qr(np.random.uniform(size=(param.nbVarPos,param.nbVarPos)))
# Target and desired covariance (for precision matrix on static features)
param.Mu = np.hstack((np.random.uniform(size=param.nbVarPos), np.zeros(param.nbVarPos)))
Ar, _ = np.linalg.qr(np.random.uniform(size=(param.nbVarPos, param.nbVarPos)))
xCov = Ar @ np.diag(np.random.uniform(size=param.nbVarPos)) @ Ar.T * 1e-1
## Target and desired covariance (for precision matrix on static and dynamic features)
# param.Mu = np.hstack((np.random.randn(param.nbVarPos), np.zeros(param.nbVarPos * (param.nbDeriv - 1))))
# xCov = np.diag(np.hstack((np.random.rand(2), np.random.rand(2) / param.dt))) * 1e-1
# Discrete dynamical System settings
# ===================================
A1d = np.zeros((param.nbDeriv,param.nbDeriv))
B1d = np.zeros((param.nbDeriv,1))
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)
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.kron(A1d,np.identity(param.nbVarPos))
B = np.kron(B1d,np.identity(param.nbVarPos))
A = np.kron(A1d, np.identity(param.nbVarPos))
B = np.kron(B1d, np.identity(param.nbVarPos))
# Discrete LQR with infinite horizon
# ===================================
Q = np.zeros((param.nbVar,param.nbVar))
Q[:param.nbVarPos,:param.nbVarPos] = np.linalg.inv(xCov) # Precision matrix
P = solve_algebraic_riccati_discrete(A,B,Q,R)
L = np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A # Feedback gain (discrete version)
# Precision matrix on static features
Q = np.zeros((param.nbVar, param.nbVar))
Q[:param.nbVarPos, :param.nbVarPos] = np.linalg.inv(xCov) # Precision matrix
## Precision matrix on static and dynamic features
# Q = np.linalg.inv(xCov)
P = solve_algebraic_riccati_eig_discrete(A, B @ np.linalg.inv(R) @ B.T, (Q + Q.T) / 2)
L = np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A # Feedback gain (discrete version)
# Test ratio between kp and kv
if param.nbDeriv > 1:
kp = np.linalg.eigvals(L[:, :param.nbVarPos])
kv = np.linalg.eigvals(L[:, param.nbVarPos:])
ratio = kv / (2 * np.sqrt(kp)) # Corresponds to ideal damping ratio
print("Ratio:", ratio)
reproducitons = []
reproductions = []
for i in range(param.nbRepros):
xt = np.zeros(param.nbVar)
xt[:param.nbVarPos] = 1+np.random.uniform(param.nbVarPos)*2
xt[:param.nbVarPos] = 1 + np.random.uniform(param.nbVarPos) * 2
xs = [copy.deepcopy(xt)]
for t in range(param.nbData):
u = L @ (param.Mu - xt)
xt = A @ xt + B @ u
xs += [copy.deepcopy(xt)]
reproducitons += [ np.asarray(xs) ]
reproductions += [np.asarray(xs)]
# Plots
# ======
......@@ -95,20 +131,20 @@ for i in range(param.nbRepros):
plt.figure()
plt.title("Position")
for r in reproducitons:
plt.plot(r[:,0],r[:,1],c="black")
plot_gaussian(param.Mu[:param.nbVarPos],xCov)
for r in reproductions:
plt.plot(r[:, 0], r[:, 1], c="black")
plot_gaussian(param.Mu[:param.nbVarPos], xCov[:2, :2], color=np.array([.8, 0, 0]), valAlpha=0.3)
plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
fig,axs = plt.subplots(5)
fig, axs = plt.subplots(5)
for r in reproducitons:
axs[0].plot(r[:,0],c="black",alpha=.4,linestyle="dashed")
axs[1].plot(r[:,1],c="black",alpha=.4,linestyle="dashed")
axs[2].plot(r[:,2],c="black",alpha=.4,linestyle="dashed")
axs[3].plot(r[:,3],c="black",alpha=.4,linestyle="dashed")
axs[4].plot(np.linalg.norm(r[:,2:4],axis=1),c="black",alpha=.4,linestyle="dashed")
for r in reproductions:
axs[0].plot(r[:, 0], c="black", alpha=.4, linestyle="dashed")
axs[1].plot(r[:, 1], c="black", alpha=.4, linestyle="dashed")
axs[2].plot(r[:, 2], c="black", alpha=.4, linestyle="dashed")
axs[3].plot(r[:, 3], c="black", alpha=.4, linestyle="dashed")
axs[4].plot(np.linalg.norm(r[:, 2:4], axis=1), c="black", alpha=.4, linestyle="dashed")
axs[0].set_ylabel("$x_1$")
axs[1].set_ylabel("$x_2$")
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment