Commit 1d9a4686 authored by Emmanuel PIGNAT's avatar Emmanuel PIGNAT
Browse files

changing name

parent bdd8269e
......@@ -392,7 +392,7 @@ def plot_mixture_linear_system(model, mode='glob', nb_sub=20, gmm=True, min_alph
def plot_gmm(Mu, Sigma, dim=None, color=[1, 0, 0], alpha=0.5, linewidth=1, markersize=6,
ax=None, empty=False, edgecolor=None, edgealpha=None, priors=None,
border=False, nb=1, swap=True, center=True):
border=False, nb=1, swap=True, center=True, zorder=20):
''' This function displays the parameters of a Gaussian Mixture Model (GMM).
Inputs -----------------------------------------------------------------
......@@ -470,7 +470,7 @@ def plot_gmm(Mu, Sigma, dim=None, color=[1, 0, 0], alpha=0.5, linewidth=1, marke
if priors is not None: a *= priors[i]
polygon = plt.Polygon(points.transpose().tolist(), facecolor=c, alpha=a,
linewidth=linewidth, zorder=20, edgecolor=edgecolor)
linewidth=linewidth, zorder=zorder, edgecolor=edgecolor)
if edgealpha is not None:
plt.plot(points[0,:], points[1,:], color=edgecolor)
......@@ -482,7 +482,7 @@ def plot_gmm(Mu, Sigma, dim=None, color=[1, 0, 0], alpha=0.5, linewidth=1, marke
i].reshape(
[2, 1])
polygon_2 = plt.Polygon(points.transpose().tolist(), facecolor=c, alpha=a / 2.,
linewidth=linewidth, zorder=15, edgecolor=edgecolor)
linewidth=linewidth, zorder=zorder-5, edgecolor=edgecolor)
# Set properties
# polygon.set_alpha(0.3)
# polygon.set_color(color)
......
......@@ -191,7 +191,7 @@ class LQR(object):
_v = [None for i in range(self._horizon)]
_K = [None for i in range(self._horizon-1)]
_Kv = [None for i in range(self._horizon-1)]
_Q = [None for i in range(self._horizon-1)]
_Q = [None for i in range(self._horizon-1)]
# _S = np.empty((self._horizon, self.xi_dim, self.xi_dim))
# _v = np.empty((self._horizon, self.xi_dim))
# _K = np.empty((self._horizon-1, self.u_dim, self.xi_dim))
......@@ -213,11 +213,11 @@ class LQR(object):
_S[t] = self.A.T.dot(_S[t+1]).dot(AmBK) + Q
_v[t] = AmBK.T.dot(_v[t+1]) + Q.dot(z)
self._S = _S
self._v = _v
self._K = _K
self._Kv = _Kv
self._Q = _Q
self._S = np.array(_S)
self._v = np.array(_v)
self._K = np.array(_K)
self._Kv = np.array(_Kv)
self._Q = np.array(_Q)
self._ds = None
self._cs = None
......
......@@ -11,6 +11,58 @@ plt.style.use('ggplot')
import scipy.sparse as ss
def quaternion_from_matrix(matrix, isprecise=False):
"""Return quaternion from rotation matrix.
If isprecise is True, the input matrix is assumed to be a precise rotation
matrix and a faster algorithm is used.
"""
M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
if isprecise:
q = np.empty((4,))
t = np.trace(M)
if t > M[3, 3]:
q[0] = t
q[3] = M[1, 0] - M[0, 1]
q[2] = M[0, 2] - M[2, 0]
q[1] = M[2, 1] - M[1, 2]
else:
i, j, k = 1, 2, 3
if M[1, 1] > M[0, 0]:
i, j, k = 2, 3, 1
if M[2, 2] > M[i, i]:
i, j, k = 3, 1, 2
t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
q[i] = t
q[j] = M[i, j] + M[j, i]
q[k] = M[k, i] + M[i, k]
q[3] = M[k, j] - M[j, k]
q *= 0.5 / np.sqrt(t * M[3, 3])
else:
m00 = M[0, 0]
m01 = M[0, 1]
m02 = M[0, 2]
m10 = M[1, 0]
m11 = M[1, 1]
m12 = M[1, 2]
m20 = M[2, 0]
m21 = M[2, 1]
m22 = M[2, 2]
# symmetric matrix K
K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
[m01 + m10, m11 - m00 - m22, 0.0, 0.0],
[m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
[m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
K /= 3.0
# quaternion is eigenvector of K that corresponds to largest eigenvalue
w, V = np.linalg.eigh(K)
q = V[:, np.argmax(w)]
if q[0] < 0.0:
np.negative(q, q)
return q
def get_canonical(nb_dim, nb_deriv=2, dt=0.01):
A1d = np.zeros((nb_deriv, nb_deriv))
......@@ -345,6 +397,35 @@ def plot_model_time(model, demos, figsize=(10, 2), dim_idx=[1], demo_idx=0):
plt.show()
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> np.allclose(M, np.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> np.allclose(M, np.diag([1, -1, -1, 1]))
True
"""
q = np.array(quaternion, dtype=np.float64, copy=True)
q = q[[3, 0, 1, 2]]
n = np.dot(q, q)
if n < np.finfo(float).eps * 4.0:
return np.identity(4)
q *= np.sqrt(2.0 / n)
q = np.outer(q, q)
return np.array([
[1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
[q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
[q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
[0.0, 0.0, 0.0, 1.0]])
def plot_demos_3d(demos, figsize=(15, 5), angle=[60, 45]):
nb_samples = len(demos)
fig = plt.figure(1, figsize=figsize)
......
Markdown is supported
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