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)