Exemple #1
0
    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])
Exemple #5
0
    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
Exemple #7
0
    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))
Exemple #10
0
 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)
Exemple #11
0
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!")
Exemple #12
0
 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
Exemple #13
0
#!/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)