def final(self, x):
env = {self.x_sym[i]: x[i] for i in range(x.shape[0])}
l_final_x = sym.Evaluate(self.l_final_x, env).ravel()
l_final_xx = sym.Evaluate(self.l_final_xx, env)
return l_final_x, l_final_xx
def _evaluate_dynamical_matrices(self, Eta):
D = self.dynamics
A_n = np.linalg.inv(sym.Evaluate(D.A_inv, Eta))
B_u_n = np.dot(A_n, sym.Evaluate(D.B_u, Eta))
B_lambda_n = np.dot(A_n, sym.Evaluate(D.B_lambda, Eta))
c_n = np.dot(A_n, sym.Evaluate(D.c, Eta))
return dynamics(A_n, B_u_n, B_lambda_n, c_n)
def test_matrix_evaluate_with_env(self):
m = np.array([[x + y, x * y]])
env = {x: 3.0, y: 4.0}
expected = np.array([[m[0, 0].Evaluate(env), m[0, 1].Evaluate(env)]])
evaluated1 = sym.Evaluate(m, env)
evaluated2 = sym.Evaluate(m=m, env=env)
self.assertTrue(np.array_equal(evaluated1, expected))
self.assertTrue(np.array_equal(evaluated2, expected))
def test_matrix_evaluate_with_random_generator(self):
u = sym.Variable("uni", sym.Variable.Type.RANDOM_UNIFORM)
m = np.array([[x + u, x - u]])
env = {x: 3.0}
g = pydrake.common.RandomGenerator()
evaluated1 = sym.Evaluate(m, env, g)
evaluated2 = sym.Evaluate(m=m, env=env, generator=g)
self.assertEqual(evaluated1[0, 0] + evaluated1[0, 1], 2 * env[x])
self.assertEqual(evaluated2[0, 0] + evaluated2[0, 1], 2 * env[x])
def test_evaluate_polynomial_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²
env = {x: 1.0, y: 2.0}
result = sym.Evaluate([p1, p2], env)
numpy_compare.assert_equal(result, [[3.0], [6.0]])
def Evaluate_polyhedral_matrices(self,dynamical_matrices,Eta,epsilon_max,epsilon_min):
"""
H,h are dictionary of different volumes 2D
"""
H_n,h_n={},{}
N=self.sys.x.shape[0]+self.sys.u.shape[0]+self.sys.u_lambda.shape[0]
for contact_mode in self.Sigma:
H=sym.Evaluate(self.H[contact_mode],Eta)
h=sym.Evaluate(self.h[contact_mode],Eta)
H_n[contact_mode],h_n[contact_mode]=self.time_stepping_geometrical_constraint(H,h,dynamical_matrices)
H_n[contact_mode]=np.vstack((H_n[contact_mode],self.H_cone["forces",contact_mode]))
h_n[contact_mode]=np.vstack((h_n[contact_mode],self.h_cone["forces",contact_mode]))
# Box Constraints
if len(epsilon_max)!=1:
H_n[contact_mode]=np.vstack((H_n[contact_mode],np.eye(N),-np.eye(N)))
h_n[contact_mode]=np.vstack((h_n[contact_mode],epsilon_max,-epsilon_min))
return H_n,h_n
def stage(self, x, u):
env = {self.x_sym[i]: x[i] for i in range(x.shape[0])}
env.update({self.u_sym[i]: u[i] for i in range(u.shape[0])})
l_x = sym.Evaluate(self.l_x, env).ravel()
l_u = sym.Evaluate(self.l_u, env).ravel()
l_xx = sym.Evaluate(self.l_xx, env)
l_ux = sym.Evaluate(self.l_ux, env)
l_uu = sym.Evaluate(self.l_uu, env)
f_x = sym.Evaluate(self.f_x, env)
f_u = sym.Evaluate(self.f_u, env)
return l_x, l_u, l_xx, l_ux, l_uu, f_x, f_u
def test_dynamics(self):
"""Test state space dynamics"""
f = self.notebook_locals['pendulum_with_motor_dynamics']
# Don't use local p since students might have different params.
p = {"m": 1.0, "l": 0.5, "g": 9.81, "N": 160, "I": 3.46e-4}
f_target = np.array([[7.79918792e-01, 7.66080504e+00],
[9.77989512e-01, 9.10363857e+00],
[7.20511334e-02, 4.45713832e+00],
[6.79229996e-01, 1.38617385e+01],
[6.59363469e-02, 4.86183460e+00],
[2.13385354e-01, 7.51773706e+00],
[2.48992276e-02, 1.01181812e+01],
[2.30302879e-01, 9.19761678e+00],
[1.33169446e-01, 8.77027126e+00],
[6.69013241e-01, 7.85009758e+00],
[4.90765889e-01, 6.43240569e+00],
[3.65890386e-01, 1.44728761e+01],
[3.13994677e-01, 9.68534802e+00],
[4.52842933e-01, 6.05424583e+00],
[3.70351083e-01, 7.73613524e+00],
[4.12991829e-01, 1.55689682e+01],
[7.41118873e-01, 7.32350209e+00],
[6.34379869e-01, 8.96351019e+00],
[1.42688056e-03, 1.40375517e+00],
[5.24345597e-01, 1.18791650e+01]])
np.random.seed(7)
n_rands = 20
f_eval = []
for i in range(n_rands):
x = np.random.rand(2)
u = np.random.rand(1)
f_eval.append(ps.Evaluate(f(x, u, p)))
f_eval = np.array(f_eval).squeeze()
self.assertLessEqual(
np.linalg.norm(f_target - np.stack(f_eval)), 1e-6,
'The pendulum with motor dynamics are not corret.')
def test_matrix_evaluate_without_env(self):
m = np.array([[3, 4]])
evaluated1 = sym.Evaluate(m)
evaluated2 = sym.Evaluate(m=m)
self.assertTrue(np.array_equal(evaluated1, m))
self.assertTrue(np.array_equal(evaluated2, m))
def do_rollouts(self, num_samples):
g = pydrake.common.RandomGenerator()
v = [
sym.Evaluate(self.prices, generator=g) for i in range(num_samples)
]
self.v = np.stack(v).reshape(num_samples, 12)
def invariance_time(sys,T,H_X=None,H_U=None,reset_map=[]):
"""
The idea is to find invariance
"""
prog=MP.MathematicalProgram()
Theta={}
theta={}
u_tilde,U_tilde={},{}
# Initial
# Main variables
for t in range(T):
theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t)
u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t)
Theta[0]=theta[0]
# Aggragates
for t in range(T):
U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)])
for t in range(T-1):
Theta[t+1]=triangular_stack(Theta[t],theta[t+1])
X,U={},{}
for t in range(1,T+1):
Ptheta=np.dot(sys.P["u",t],Theta[t-1])
a_x=sys.P["x",t]+np.dot(Ptheta,sys.Xi["x",t-1])
a_w=sys.P["w",t] + np.dot(Ptheta,sys.Xi["w",t-1])
a_v=np.dot(Ptheta,sys.Xi["v",t-1])
Gw=spa.block_diag(*[sys.W[tau].G for tau in range(0,t)])
Gv=spa.block_diag(*[sys.V[tau].G for tau in range(0,t)])
Wbar=np.vstack([sys.W[tau].x for tau in range(0,t)])
Vbar=np.vstack([sys.V[tau].x for tau in range(0,t)])
x_bar=np.dot(sys.P["u",t],U_tilde[t-1]) + np.dot(a_x,sys.X0.x) \
+ np.dot(a_w,Wbar) + np.dot(a_v,Vbar)
G=np.hstack(( np.dot(a_x,sys.X0.G), np.dot(a_w,Gw), np.dot(a_v,Gv) ))
X[t]=zonotope(x_bar,G)
if H_X!=None:
subset(prog,X[t],H_X)
if H_U!=None:
for t in range(T):
u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x)
Gu=np.dot(theta[t],sys.Xi[t].G)
U[t]=zonotope(u_bar,Gu)
subset(prog,U[t],H_U)
print("subset for U done")
# Terminal Constraint
if len(reset_map)!=0:
print("A reset map is found. Are you sure?")
X[T].x=np.dot(reset_map,X[T].x)
X[T].G=np.dot(reset_map,X[T].G)
subset(prog,X[T],sys.X0)
# Optimization
J=0
for t in range(T):
print(t,"u cost")
J+=sum(u_tilde[t].flatten()**2)
# J+=sum(theta[t].flatten()**2)
for t in range(1,T):
print(t,"x cost")
J+=sum(X[t].x.flatten()**2)
# J+=sum(X[t].G.flatten()**2)
prog.AddQuadraticCost(J)
print("Now solving the Quadratic Program")
start=time.time()
result=gurobi_solver.Solve(prog,None,None)
print("time to solve",time.time()-start)
if result.is_success():
print("Synthesis Success!","\n"*5)
theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)}
u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)}
X_n={t:zonotope(sym.Evaluate(result.GetSolution(X[t].x)),\
sym.Evaluate(result.GetSolution(X[t].G))) for t in range(1,T+1)}
if H_U!=None:
U_n={t:zonotope(sym.Evaluate(result.GetSolution(U[t].x)),\
sym.Evaluate(result.GetSolution(U[t].G))) for t in range(T)}
else:
U_n={}
return u_tilde_n,theta_n,X_n,U_n
else:
print("Synthesis Failed!")
def get_jacobians(self, x, u):
env = {self.x_sym[i]: x[i] for i in range(x.shape[0])}
env.update({self.u_sym[i]: u[i] for i in range(u.shape[0])})
f_x = sym.Evaluate(self.f_x, env)
f_u = sym.Evaluate(self.f_u, env)
return f_x, f_u
#!/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)
