import pydrake.symbolic as sym
from pypolycontain.lib.zonotope import zonotope
from PWA_lib.Manipulation.contact_point_pydrake import contact_point_symbolic_2D
from PWA_lib.Manipulation.system_symbolic_pydrake import system_symbolic
from PWA_lib.Manipulation.system_numeric import system_hard_contact_PWA_numeric as system_numeric
from PWA_lib.Manipulation.system_numeric import environment,merge_timed_vectors_glue,merge_timed_vectors_skip,\
trajectory_to_list_of_linear_cells,trajectory_to_list_of_linear_cells_full_linearization,\
PWA_cells_from_state,hybrid_reachable_sets_from_state,environment_from_state
from PWA_lib.trajectory.poly_trajectory import point_trajectory_tishcom,\
polytopic_trajectory_given_modes,point_trajectory_given_modes_and_central_traj,\
point_trajectory_tishcom_time_varying
mysystem=system_symbolic("Picking Up a Box", 2)
x,y,theta=sym.Variable("x"),sym.Variable("y"),sym.Variable("theta")
x_1,y_1,x_2,y_2=sym.Variable("x_1"),sym.Variable("y_1"),sym.Variable("x_2"),sym.Variable("y_2")
mysystem.q_o=np.array([x,y,theta])
mysystem.q_m=np.array([x_1,y_1,x_2,y_2])
a=1 # The half of the length of the vertical side
b=1 # The half the length of the horizonatl side
# Dynamics:
mysystem.C=np.zeros((3,3))
g=9.8
mysystem.tau_g=np.array([0,-g,0])
mysystem.B=np.zeros((3,0))
# Mass Matrix
M=1
import pydrake.symbolic as sym
from pypolycontain.lib.zonotope import zonotope
from PWA_lib.Manipulation.contact_point_pydrake import contact_point_symbolic_2D
from PWA_lib.Manipulation.system_symbolic_pydrake import system_symbolic
from PWA_lib.Manipulation.system_numeric import system_hard_contact_PWA_numeric as system_numeric
from PWA_lib.Manipulation.system_numeric import environment,merge_timed_vectors_glue,merge_timed_vectors_skip,\
trajectory_to_list_of_linear_cells,trajectory_to_list_of_linear_cells_full_linearization,\
PWA_cells_from_state,hybrid_reachable_sets_from_state,environment_from_state
from PWA_lib.trajectory.poly_trajectory import point_trajectory_tishcom,\
polytopic_trajectory_given_modes,point_trajectory_given_modes_and_central_traj,\
point_trajectory_tishcom_time_varying
mysystem = system_symbolic("Balacning a bar", 2)
x, y, theta = sym.Variable("x"), sym.Variable("y"), sym.Variable("theta")
x_1, y_1, x_2, y_2 = sym.Variable("x_1"), sym.Variable("y_1"), sym.Variable(
"x_2"), sym.Variable("y_2")
mysystem.q_o = np.array([x, y, theta])
mysystem.q_m = np.array([x_1, y_1, x_2, y_2])
# Dynamics:
mysystem.C = np.zeros((3, 3))
g = 9.8
mysystem.tau_g = np.array([0, -g, 0])
mysystem.B = np.zeros((3, 0))
M = 1
I = M / 3.0
mysystem.M = blk(*[M, M, I])
import pydrake.symbolic as sym
from pypolycontain.lib.zonotope import zonotope
from PWA_lib.Manipulation.contact_point_pydrake import contact_point_symbolic_2D
from PWA_lib.Manipulation.system_symbolic_pydrake import system_symbolic
from PWA_lib.Manipulation.system_numeric import system_hard_contact_PWA_numeric as system_numeric
from PWA_lib.Manipulation.system_numeric import environment,merge_timed_vectors_glue,merge_timed_vectors_skip,\
trajectory_to_list_of_linear_cells,trajectory_to_list_of_linear_cells_full_linearization,\
PWA_cells_from_state,hybrid_reachable_sets_from_state,environment_from_state
from PWA_lib.trajectory.poly_trajectory import point_trajectory_tishcom,\
polytopic_trajectory_given_modes,point_trajectory_given_modes_and_central_traj,\
point_trajectory_tishcom_time_varying
mysystem = system_symbolic("Carrot", 2)
x, y, theta = sym.Variable("x"), sym.Variable("y"), sym.Variable("theta")
x_m, y_m, theta_m, s_m = sym.Variable("x_m"), sym.Variable(
"y_m"), sym.Variable("theta_m"), sym.Variable("s_m")
mysystem.q_o = np.array([x, y, theta])
mysystem.q_m = np.array([x_m, y_m, theta_m, s_m])
def Rotate(t):
return np.array([[sym.cos(t), -sym.sin(t)], [sym.sin(t), sym.cos(t)]])
def ReLU(cond, K):
return sym.log(1 + sym.exp(K * cond))
#!/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)