def __init__(self, discrete_dynamics, n_x, n_u):
self.x_sym = np.array(
[sym.Variable("x_{}".format(i)) for i in range(n_x)])
self.u_sym = np.array(
[sym.Variable("u_{}".format(i)) for i in range(n_u)])
x = self.x_sym
u = self.u_sym
f = car_continuous_dynamics(x, u)
self.f_x = sym.Jacobian(f, x)
self.f_u = sym.Jacobian(f, u)
def test_jacobian_matrix(self):
p1 = sym.Polynomial(x * x + y, [x, y]) # p1 = x² + y
p2 = sym.Polynomial(2 * x + y * y, [x, y]) # p2 = 2x + y²
p1_dx = sym.Polynomial(2 * x, [x, y]) # ∂p1/∂x = 2x
p1_dy = sym.Polynomial(1.0, [x, y]) # ∂p1/∂y = 1
p2_dx = sym.Polynomial(2, [x, y]) # ∂p1/∂x = 2
p2_dy = sym.Polynomial(2 * y, [x, y]) # ∂p1/∂y = 2y
J = sym.Jacobian([p1, p2], [x, y])
numpy_compare.assert_equal(J[0, 0], p1_dx)
numpy_compare.assert_equal(J[0, 1], p1_dy)
numpy_compare.assert_equal(J[1, 0], p2_dx)
numpy_compare.assert_equal(J[1, 1], p2_dy)
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x**2], [x, y])
self._check_scalar(J[0, 0], sym.cos(y))
self._check_scalar(J[1, 0], sym.sin(y))
self._check_scalar(J[2, 0], 2 * x)
self._check_scalar(J[0, 1], -x * sym.sin(y))
self._check_scalar(J[1, 1], x * sym.cos(y))
self._check_scalar(J[2, 1], 0)
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x**2], [x, y])
numpy_compare.assert_equal(J[0, 0], sym.cos(y))
numpy_compare.assert_equal(J[1, 0], sym.sin(y))
numpy_compare.assert_equal(J[2, 0], 2 * x)
numpy_compare.assert_equal(J[0, 1], -x * sym.sin(y))
numpy_compare.assert_equal(J[1, 1], x * sym.cos(y))
numpy_compare.assert_equal(J[2, 1], sym.Expression(0))
def __init__(self, discrete_dynamics, cost_stage, cost_final, n_x, n_u):
self.x_sym = np.array([sym.Variable("x_{}".format(i)) for i in range(n_x)])
self.u_sym = np.array([sym.Variable("u_{}".format(i)) for i in range(n_u)])
x = self.x_sym
u = self.u_sym
l = cost_stage(x, u)
self.l_x = sym.Jacobian([l], x).ravel()
self.l_u = sym.Jacobian([l], u).ravel()
self.l_xx = sym.Jacobian(self.l_x, x)
self.l_ux = sym.Jacobian(self.l_u, x)
self.l_uu = sym.Jacobian(self.l_u, u)
l_final = cost_final(x)
self.l_final_x = sym.Jacobian([l_final], x).ravel()
self.l_final_xx = sym.Jacobian(self.l_final_x, x)
f = discrete_dynamics(x, u)
self.f_x = sym.Jacobian(f, x)
self.f_u = sym.Jacobian(f, u)
def _polyhedrize_constraints_symbolic(self, zeta):
"""
Given vector of symbolic constraint set
zeta(q,v_o,u_torques,u_m,lambdas) <= 0
Compute the polyhedral set H(x,u,lambda) <= h
"""
try:
H = np.hstack(
(sym.Jacobian(zeta, self.q), sym.Jacobian(zeta, self.v_o),
sym.Jacobian(zeta,
self.u_torques), sym.Jacobian(zeta, self.u_m),
sym.Jacobian(zeta, self.u_lambda)))
except:
H = np.hstack((
sym.Jacobian(zeta, self.q),
sym.Jacobian(zeta, self.v_o),
sym.Jacobian(zeta, self.u_m), # There is no torque input
sym.Jacobian(zeta, self.u_lambda)))
h = np.dot(H, np.hstack((self.x, self.u, self.u_lambda))) - zeta
return (H, h)
def _time_derivative(self, xi):
"""
Time derivative of the vector of symbolic expression xi(q)
"""
return np.dot(sym.Jacobian(xi, self.q_o), self.v_o) + np.dot(
sym.Jacobian(xi, self.q_m), self.u_m)
def _linearize_dynamics_symbolic(self):
# First compute A matrix
A_11 = np.eye(self.q.shape[0])
A_12 = np.vstack((-self.h * np.eye(self.q_o.shape[0]),
np.zeros((self.q_m.shape[0], self.v_o.shape[0]))))
alpha_1=sym.Jacobian(self.tau_c,self.q)-sym.Jacobian(self.tau_g,self.q)-\
sum([sym.Jacobian(self.B[:,i]*self.u_torques[i],self.q) for i in range(self.B.shape[1])])-\
sum([sym.Jacobian(self.J[:,i]*self.u_lambda[i],self.q) for i in range(self.J.shape[1])])
A_21 = self.h * np.dot(self.M_inv, alpha_1)
alpha_2 = sym.Jacobian(self.tau_c, self.v_o) - sum([
sym.Jacobian(self.B[:, i] * self.u_torques[i], self.v_o)
for i in range(self.B.shape[1])
])
A_22 = np.eye(self.v_o.shape[0]) - self.h * np.dot(self.M_inv, alpha_2)
self.dynamics.A_inv = np.vstack((np.hstack(
(A_11, A_12)), np.hstack((A_21, A_22))))
self.dynamics.B_u_1 = np.vstack((np.zeros(
(self.q.shape[0], self.B.shape[1])), self.h * self.B))
self.dynamics.B_u_2=np.vstack((np.zeros((self.q_o.shape[0],self.q_m.shape[0])),\
self.h*np.eye(self.q_m.shape[0]),\
np.zeros((self.v_o.shape[0],self.q_m.shape[0]))))
self.dynamics.B_u = np.hstack(
(self.dynamics.B_u_1, self.dynamics.B_u_2))
self.dynamics.B_lambda = np.vstack((np.zeros(
(self.q.shape[0], self.J.shape[1])), self.h * self.J))
alpha_3=-self.tau_c+np.dot(sym.Jacobian(self.tau_c,self.q),self.q)+\
np.dot(sym.Jacobian(self.tau_c,self.v_o),self.v_o)+\
self.tau_g-np.dot(sym.Jacobian(self.tau_g,self.q),self.q)+\
np.dot(self.B,self.u_torques)-\
sum([np.dot(sym.Jacobian(self.B[:,i],self.q),self.q)*self.u_torques[i] for i in range(self.B.shape[1])])-\
sum([np.dot(sym.Jacobian(self.B[:,i],self.v_o),self.v_o)*self.u_torques[i] for i in range(self.B.shape[1])])-\
sum([np.dot(sym.Jacobian(self.J[:,i],self.q),self.q)*self.u_lambda[i] for i in range(self.J.shape[1])])
self.dynamics.c = np.hstack(
(np.zeros(self.q.shape[0]), self.h * alpha_3))
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
Created on Wed Mar 6 19:09:04 2019
@author: sadra
"""
import numpy as np
import pydrake.symbolic as sym
from PWA_lib.Manipulation.contact_point_pydrake import contact_point_symbolic
from PWA_lib.Manipulation.system_symbolic_pydrake import system_symbolic
mysystem = system_symbolic("my system")
q = np.array([sym.Variable("q%i" % i) for i in range(3)])
J = np.array([q[0]**2 * sym.sin(q[2]), q[1]**3 * sym.cos(q[2])])
z = sym.Jacobian(J, q)
E = {q[i]: i + 1 for i in range(3)}
z_v = sym.Evaluate(z, E)
