def test(self): x = sym.Variable("x") y = sym.Variable("y") e = [2 * x + 3 * y, 4 * x + 2 * y, 3 * x] M = sym.DecomposeLinearExpressions(e, [x, y]) np.testing.assert_array_equal(M, np.array([[2, 3], [4., 2.], [3., 0.]]))
def testSimplify(self): x = sym.Variable("x") y = sym.Variable("y") self.assertEqual(str(0 * (x + y)), "0") self.assertEqual(str(x + y - x - y), "0") self.assertEqual(str(x / x - 1), "0") self.assertEqual(str(x / x), "1")
def _build_state_space(self): """ Build the system's state and control space""" # self.qdot_o=np.array([sym.Variable("qdot%d"%i) for i in range(len(self.q_o))]) self.v_o = np.array([ sym.Variable( str(a.__repr__())[10:str(a.__repr__()).index(',') - 1] + "_dot") for a in self.q_o ]) self.u_m = np.array([ sym.Variable("u_" + str(a.__repr__())[10:str(a.__repr__()).index(',') - 1]) for a in self.q_m ]) # self.u_m=np.array([sym.Variable("u_m%d"%i) for i in range(len(self.q_m))]) if self.d == 2: self.u_lambda=np.array([[sym.Variable("lambda_%s_n"%c.name),sym.Variable("lambda_%s_t"%c.name)] \ for c in self.list_of_contact_points]).reshape(2*len(self.list_of_contact_points)) else: raise NotImplementedError self.q = np.hstack((self.q_o, self.q_m)) self.x = np.hstack((self.q_o, self.q_m, self.v_o)) # self.u=np.hstack((self.u_torques,self.u_m,self.u_lambda)) self.u = np.hstack((self.u_torques, self.u_m)) # self.tau_c self.tau_c = np.dot(self.C, self.v_o) # The Jacobian self.J = np.hstack(([c.J for c in self.list_of_contact_points]))
def __init__(self, m=1, m_l=0, l=1, g=1, b=0, initial_state=np.array([0, 0]), input_limits=None): ''' Single link pendulum with mass at the end of a rod :param m: mass at the end of the pendulum. I = m*l**2 :param m_l: mass of the rod. I_l = m_l*l**2/12 :param l: length of the rod. :param g: gravity constant. :param b: damping. tau_damp = -b*theta_dot ''' self.m = m self.m_l = m_l self.l = l self.g = g self.b = b #symbolic variables self.x = np.array([sym.Variable('x_' + str(i)) for i in range(2)]) self.u = np.array([sym.Variable('u_' + str(i)) for i in range(1)]) self.env = {self.x[0]: initial_state[0], self.x[1]: initial_state[1]} self.I = self.m * self.l**2 + self.m_l * self.l**2 / 12 self.t = -(self.m * self.g * self.l * sym.sin(self.x[0]) + self.m_l * self.g * self.l / 2 * sym.sin(self.x[0])) self.f = np.asarray([ self.x[1], 1 / self.I * (self.t + self.u[0] - self.b * self.x[1]) ]) #symbolic system DTContinuousSystem.__init__(self, self.f, self.x, self.u, self.env, input_limits)
def testFormula(self): x = sym.Variable("x") y = sym.Variable("y") self.assertEqual(str(x < y), "(x < y)") self.assertEqual(str(x <= y), "(x <= y)") self.assertEqual(str(x > y), "(x > y)") self.assertEqual(str(x >= y), "(x >= y)") self.assertEqual(str(x == y), "(x = y)")
def test(self): x = sym.Variable("x") y = sym.Variable("y") e = x + sym.sin(y) + x * y variables, map_var_to_index = sym.ExtractVariablesFromExpression(e) self.assertEqual(variables.shape, (2, )) self.assertNotEqual(variables[0].get_id(), variables[1].get_id()) self.assertEqual(len(map_var_to_index), 2) for i in range(2): self.assertEqual(map_var_to_index[variables[i].get_id()], i)
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(self): x = sym.Variable("x") y = sym.Variable("y") e = 2 * x + 3 * y + 4 variables, map_var_to_index = sym.ExtractVariablesFromExpression(e) coeffs, constant_term = sym.DecomposeAffineExpression( e, map_var_to_index) self.assertEqual(constant_term, 4) self.assertEqual(coeffs.shape, (2, )) self.assertEqual(coeffs[map_var_to_index[x.get_id()]], 2) self.assertEqual(coeffs[map_var_to_index[y.get_id()]], 3)
def get_plant_and_pos(): """"Set up plant and position systems.""" # Inputs kappa_F = sym.Variable("kappa_F") kappa_R = sym.Variable("kappa_R") delta = sym.Variable("delta") # Outputs r = sym.Variable("r") r_dot = sym.Variable("r_dot") theta = sym.Variable("theta") theta_dot = sym.Variable("theta_dot") phi = sym.Variable("phi") phi_dot = sym.Variable("phi_dot") # v = r_dot r_hat + r theta_dot theta_hat v_r_hat = r_dot v_theta_hat = r * theta_dot beta = sym.atan2(v_r_hat, v_theta_hat) # Slip angles alpha_F = phi - delta - beta alpha_R = phi - beta F_xf = sym.cos(delta) * S_FL * kappa_F - sym.sin(delta) * S_FC * alpha_F F_xr = S_RL * kappa_R F_yf = sym.sin(delta) * S_FL * kappa_F + sym.cos(delta) * S_FC * alpha_F F_yr = S_RC * alpha_R F_x = F_xf + F_xr F_y = F_yr + F_yf plant_state = np.array([r, r_dot, theta_dot, phi, phi_dot]) theta_ddot = (F_x*sym.cos(phi) - F_y*sym.sin(phi)) / \ (m*r) - 2*r_dot * theta_dot/r plant_dynamics = np.array([ r_dot, (F_x * sym.sin(phi) + F_y * sym.cos(phi)) / m + r * theta_dot**2, theta_ddot, phi_dot, theta_ddot - (l_F * F_yf - l_R * F_yr) / Iz ]) plant_input = [kappa_F, kappa_R, delta] plant_vector_system = SymbolicVectorSystem(state=plant_state, input=plant_input, dynamics=plant_dynamics, output=plant_state) position_state = [theta] position_dynamics = [theta_dot] position_system = SymbolicVectorSystem(state=position_state, input=plant_state, dynamics=position_dynamics, output=position_state) return plant_vector_system, position_system
def test_add_indeterminates_and_decision_variables(self): prog = mp.MathematicalProgram() x0 = sym.Variable("x0") x1 = sym.Variable("x1") a0 = sym.Variable("a0") a1 = sym.Variable("a1") prog.AddIndeterminates(np.array([x0, x1])) prog.AddDecisionVariables(np.array([a0, a1])) numpy_compare.assert_equal(prog.decision_variables()[0], a0) numpy_compare.assert_equal(prog.decision_variables()[1], a1) numpy_compare.assert_equal(prog.indeterminates()[0], x0) numpy_compare.assert_equal(prog.indeterminate(1), x1)
def test(self): x = sym.Variable("x") a = sym.Variable("a") b = sym.Variable("b") f = [a + x, a*a*x*x] [W, alpha, w0] = sym.DecomposeLumpedParameters(f, [a, b]) numpy_compare.assert_equal(W, [[sym.Expression(1), sym.Expression(0)], [sym.Expression(0), x*x]]) numpy_compare.assert_equal(alpha, [sym.Expression(a), a*a]) numpy_compare.assert_equal(w0, [sym.Expression(x), sym.Expression(0)])
def testOperators(self): x = sym.Variable("x") self.assertEqual(str(x), "x") y = sym.Variable("y") self.assertEqual(str(y), "y") self.assertEqual(str(x + y), "(x + y)") self.assertEqual(str(x - y), "(x - y)") self.assertEqual(str(x * y), "(x * y)") self.assertEqual(str(x / y), "(x / y)") self.assertEqual(str((x + y) * x), "(x * (x + y))")
def test_evaluate_with_random_generator(self): g = pydrake.common.RandomGenerator() uni = sym.Variable("uni", sym.Variable.Type.RANDOM_UNIFORM) gau = sym.Variable("gau", sym.Variable.Type.RANDOM_GAUSSIAN) exp = sym.Variable("exp", sym.Variable.Type.RANDOM_EXPONENTIAL) # Checks if we can evaluate an expression with a random number # generator. (uni + gau + exp).Evaluate(g) (uni + gau + exp).Evaluate(generator=g) env = {x: 3.0, y: 4.0} # Checks if we can evaluate an expression with an environment and a # random number generator. (x + y + uni + gau + exp).Evaluate(env, g) (x + y + uni + gau + exp).Evaluate(env=env, generator=g)
def test_eval_binding(self): qp = TestQP() prog = qp.prog x = qp.x x_expected = np.array([1., 1.]) costs = qp.costs cost_values_expected = [2., 1.] constraints = qp.constraints constraint_values_expected = [1., 1., 2., 3.] result = mp.Solve(prog) self.assertTrue(np.allclose(result.GetSolution(x), x_expected)) enum = zip(constraints, constraint_values_expected) for (constraint, value_expected) in enum: value = result.EvalBinding(constraint) self.assertTrue(np.allclose(value, value_expected)) enum = zip(costs, cost_values_expected) for (cost, value_expected) in enum: value = result.EvalBinding(cost) self.assertTrue(np.allclose(value, value_expected)) self.assertIsInstance(result.EvalBinding(costs[0]), np.ndarray) # Bindings for `Eval`. x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x")) T_y_list = (float, AutoDiffXd, sym.Expression) evaluator = costs[0].evaluator() for x_i, T_y_i in zip(x_list, T_y_list): y_i = evaluator.Eval(x=[x_i, x_i]) self.assertIsInstance(y_i[0], T_y_i)
def random_uniform(name, low, high): if name not in uniform_variable_store.keys(): var = sym.Variable(name, sym.Variable.Type.RANDOM_UNIFORM) uniform_variable_store[name] = var uniform_variable_to_range[var] = (low, high) else: var = uniform_variable_store[name] return var * (high - low) + low
def testLogical(self): x = sym.Variable("x") self.assertEqual(str(sym.logical_not(x == 0)), "!((x = 0))") # Test single-operand logical statements self.assertEqual(str(sym.logical_and(x >= 1)), "(x >= 1)") self.assertEqual(str(sym.logical_or(x >= 1)), "(x >= 1)") # Test binary operand logical statements self.assertEqual(str(sym.logical_and(x >= 1, x <= 2)), "((x >= 1) and (x <= 2))") self.assertEqual(str(sym.logical_or(x <= 1, x >= 2)), "((x >= 2) or (x <= 1))") # Test multiple operand logical statements y = sym.Variable("y") self.assertEqual(str(sym.logical_and(x >= 1, x <= 2, y == 2)), "((y = 2) and (x >= 1) and (x <= 2))") self.assertEqual(str(sym.logical_or(x >= 1, x <= 2, y == 2)), "((y = 2) or (x >= 1) or (x <= 2))")
def find_feasible_latents(self, observed): # Build an optimization to estimate the hidden variables try: prog = MathematicalProgram() # Add in all the appropriate variables with their bounds all_vars = self.prices[0].GetVariables() for price in self.prices[1:]: all_vars += price.GetVariables() mp_vars = prog.NewContinuousVariables(len(all_vars)) subs_dict = {} for v, mv in zip(all_vars, mp_vars): subs_dict[v] = mv lb = [] ub = [] prog.AddBoundingBoxConstraint(0., 1., mp_vars) prices_mp = [ self.prices[k].Substitute(subs_dict) for k in range(12) ] # Add the observation constraint for k, val in enumerate(observed[1:]): if val != 0: prog.AddConstraint(prices_mp[k] >= val - 2.) prog.AddConstraint(prices_mp[k] <= val + 2) # Find lower bounds prog.AddCost(sum(prices_mp)) solver = SnoptSolver() result = solver.Solve(prog) if result.is_success(): lb = [result.GetSolution(x).Evaluate() for x in prices_mp] lb_vars = result.GetSolution(mp_vars) # Find upper bound too prog.AddCost(-2. * sum(prices_mp)) result = solver.Solve(prog) if result.is_success(): ub_vars = result.GetSolution(mp_vars) ub = [result.GetSolution(x).Evaluate() for x in prices_mp] self.price_ub = ub self.price_lb = lb subs_dict = {} for k, v in enumerate(all_vars): if lb_vars[k] == ub_vars[k]: subs_dict[v] = lb_vars[k] else: new_var = sym.Variable( "feasible_%d" % k, sym.Variable.Type.RANDOM_UNIFORM) subs_dict[v] = new_var * (ub_vars[k] - lb_vars[k]) + lb_vars[k] self.prices = [ self.prices[k].Substitute(subs_dict) for k in range(12) ] return except RuntimeError as e: print("Runtime error: ", e) self.rollout_probability = 0.
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_with_toy_system(): x = np.array([sym.Variable('x')]) u = np.array([sym.Variable('u')]) parameters = np.array([sym.Variable('param_'+str(i)) for i in range(2)]) f = np.atleast_2d([sym.pow(x[0],3)+sym.sin(u[0])]) print('f:', f) dyn = ContinuousDynamics(f,x,u) print('A:', dyn.A) print('B:', dyn.B) print('c:', dyn.c) env = {x[0]:3,u[0]:4} print('Nonlinear x_dot:', dyn.evaluate_xdot(env)) print('linearized x_dot:', dyn.evaluate_xdot(env, linearize=True)) sys = DTContinuousSystem(f, x, u, initial_env={x[0]:4, u[0]:0}) print('Nonlinear forward step:', sys.forward_step(u=np.array([2]), step_size=1, modify_system=False)) print('Linear forward step:', sys.forward_step(u=np.array([2]), linearlize=True, step_size = 1, modify_system=False)) print(sys.get_reachable_polytopes([0.5]))
def test(self): x = sym.Variable("x") y = sym.Variable("y") e = x * x + 2 * y * y + 4 * x * y + 3 * x + 2 * y + 4 poly = sym.Polynomial(e, [x, y]) variables, map_var_to_index = sym.ExtractVariablesFromExpression(e) Q, b, c = sym.DecomposeQuadraticPolynomial(poly, map_var_to_index) self.assertEqual(c, 4) x_idx = map_var_to_index[x.get_id()] y_idx = map_var_to_index[y.get_id()] self.assertEqual(Q[x_idx, x_idx], 2) self.assertEqual(Q[x_idx, y_idx], 4) self.assertEqual(Q[y_idx, x_idx], 4) self.assertEqual(Q[y_idx, y_idx], 4) self.assertEqual(Q.shape, (2, 2)) self.assertEqual(b[x_idx], 3) self.assertEqual(b[y_idx], 2) self.assertEqual(b.shape, (2, ))
def test(self): x = sym.Variable("x") y = sym.Variable("y") e = [2 * x + 3 * y + 4, 4 * x + 2 * y, 3 * x + 5] M, v = sym.DecomposeAffineExpressions(e, [x, y]) np.testing.assert_allclose(M, np.array([[2, 3], [4., 2.], [3., 0.]])) np.testing.assert_allclose(v, np.array([4., 0., 5.])) A, b, variables = sym.DecomposeAffineExpressions(e) self.assertEqual(variables.shape, (2, )) np.testing.assert_allclose(b, np.array([4., 0., 5.])) if variables[0].get_id() == x.get_id(): np.testing.assert_allclose( A, np.array([[2., 3.], [4., 2.], [3., 0.]])) self.assertEqual(variables[1].get_id(), y.get_id()) else: np.testing.assert_allclose( A, np.array([[3., 2.], [2., 4.], [0., 3.]])) self.assertEqual(variables[0].get_id(), y.get_id()) self.assertEqual(variables[1].get_id(), x.get_id())
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 test_eval_binding(self): qp = TestQP() prog = qp.prog x = qp.x x_expected = np.array([1., 1.]) costs = qp.costs cost_values_expected = [2., 1.] constraints = qp.constraints constraint_values_expected = [1., 1., 2., 3.] with catch_drake_warnings(action='ignore'): prog.Solve() self.assertTrue(np.allclose(prog.GetSolution(x), x_expected)) enum = zip(constraints, constraint_values_expected) for (constraint, value_expected) in enum: value = prog.EvalBindingAtSolution(constraint) self.assertTrue(np.allclose(value, value_expected)) enum = zip(costs, cost_values_expected) for (cost, value_expected) in enum: value = prog.EvalBindingAtSolution(cost) self.assertTrue(np.allclose(value, value_expected)) # Existence check for non-autodiff versions. self.assertIsInstance( prog.EvalBinding(costs[0], x_expected), np.ndarray) self.assertIsInstance( prog.EvalBindings(prog.GetAllConstraints(), x_expected), np.ndarray) # Existence check for autodiff versions. self.assertIsInstance( jacobian(partial(prog.EvalBinding, costs[0]), x_expected), np.ndarray) self.assertIsInstance( jacobian(partial(prog.EvalBindings, prog.GetAllConstraints()), x_expected), np.ndarray) # Bindings for `Eval`. x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x")) T_y_list = (float, AutoDiffXd, sym.Expression) evaluator = costs[0].evaluator() for x_i, T_y_i in zip(x_list, T_y_list): y_i = evaluator.Eval(x=[x_i, x_i]) self.assertIsInstance(y_i[0], T_y_i)
def __init__(self, name, d): self.name = name + " (%dD)" % d self.d = d # Dimensions self.list_of_contact_points = [] # list of contact points # Variables self.q_o = np.empty(0, dtype="object") # Position vector of the Object(s) self.q_m = np.empty( 0, dtype="object" ) # Position vector of the Manipulator(s): those controlled by velocity commands self.u_torques = np.zeros(0) # Vector of external torque/forces # Functions self.M = np.empty(0, dtype="object") # Mass matrix self.C = np.empty(0, dtype="object") # C matrix self.tau_g = np.empty(0, dtype="object") # tau_g vector self.B = np.empty(0, dtype="object") # B matrix # discrete_time step self.h = sym.Variable(name="h") # Contacts: # self.u_lambda=np.hstack(*[contact_point.u_lambda for contact_point in self.contact_points]) # Dynamics self.dynamics = dynamics(None, None, None, None)
# -*- coding: utf-8 -*- import unittest import numpy as np import pydrake.symbolic as sym # Define global variables to make the tests less verbose. x = sym.Variable("x") y = sym.Variable("y") z = sym.Variable("z") class TestSymbolicMonomial(unittest.TestCase): def test_constructor_variable(self): m = sym.Monomial(x) # m = x¹ self.assertEqual(m.degree(x), 1) self.assertEqual(m.total_degree(), 1) def test_constructor_variable_int(self): m = sym.Monomial(x, 2) # m = x² self.assertEqual(m.degree(x), 2) self.assertEqual(m.total_degree(), 2) def test_constructor_map(self): powers_in = {x: 2, y: 3, z: 4} m = sym.Monomial(powers_in) powers_out = m.get_powers() self.assertEqual(powers_out[x], 2) self.assertEqual(powers_out[y], 3) self.assertEqual(powers_out[z], 4)
from copy import copy import unittest import numpy as np import six import pydrake.symbolic as sym from pydrake.test.algebra_test_util import ScalarAlgebra, VectorizedAlgebra from pydrake.util.containers import EqualToDict # TODO(eric.cousineau): Replace usages of `sym` math functions with the # overloads from `pydrake.math`. # Define global variables to make the tests less verbose. x = sym.Variable("x") y = sym.Variable("y") z = sym.Variable("z") w = sym.Variable("w") a = sym.Variable("a") b = sym.Variable("b") c = sym.Variable("c") e_x = sym.Expression(x) e_y = sym.Expression(y) TYPES = [ sym.Variable, sym.Expression, sym.Polynomial, sym.Monomial, ]
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)) R = 1 # The radius of the carrot
def test_get_type(self): i = sym.Variable('i', sym.Variable.Type.INTEGER) self.assertEqual(i.get_type(), sym.Variable.Type.INTEGER) g = sym.Variable('g', sym.Variable.Type.RANDOM_GAUSSIAN) self.assertEqual(g.get_type(), sym.Variable.Type.RANDOM_GAUSSIAN)
def __init__(self, M1=1., I1=1., M2=10., I2=10., r1=0.5, r2=0.4, KL=1e3, KL2=1e5, BL2=125, KG=1e4, BG=75, k0=1., \ g=9.8, ground_height_function=lambda x: 0, initial_state=np.asarray([0.,0.,0.,1.5,1.0,0.,0.,0.,0.,0.])): ''' 2D hopper with actuated piston at the end of the leg. The model of the hopper follows the one described in "Hopping in Legged Systems" (Raibert, 1984) ''' self.M1 = M1 self.I1 = I1 self.M2 = M2 self.I2 = I2 self.r1 = r1 self.r2 = r2 self.KL = KL self.KL2 = KL2 self.BL2 = BL2 self.k0 = k0 self.KG = KG self.BG = BG self.g = g self.ground_height_function = ground_height_function # state machine for touchdown detection self.xTD = sym.Variable('xTD') self.was_in_contact = False # Symbolic variables # State variables are s = [theta1, theta2, x0, y0, w] # self.x = [s, sdot] # Inputs are self.u = [tau, chi] self.x = np.array([sym.Variable('x_' + str(i)) for i in range(10)]) self.u = np.array([sym.Variable('u_' + str(i)) for i in range(2)]) # Initial state self.initial_env = {} for i, state in enumerate(initial_state): self.initial_env[self.x[i]] = state self.initial_env[self.xTD] = 0 # print(self.initial_env) # Dynamic modes FK_extended = self.KL * (self.k0 - self.x[4] + self.u[1]) FK_retracted = self.KL2 * (self.k0 - self.x[4] + self.u[1]) - self.BL2 * self.x[9] FX_contact = -self.KG * ( self.x[2] - self.xTD) - self.BG * self.x[7] #FIXME: handling xTD FX_flight = 0. FY_contact = -self.KG * (self.x[3] - self.ground_height_function( self.x[2])) - self.BG * (self.x[8]) FY_flight = 0. W = self.x[4] - self.r1 # EOM is obtained from Raibert 1984 paper, Appendix I eqn 40~44 # The EOM is in the following form # a1*theta1_ddot + a2*theta2_ddot + a3*x0_ddot + a4*w_ddot = A # b1*theta1_ddot + b2*theta2_ddot + b3*y0_ddot + b4*w_ddot = B # c1*theta1_ddot + c2*x0_ddot = C # d1*theta1_ddot + d2*y0_ddot = D # e1*theta1_ddot + e2*theta2_ddot = E a1 = sym.cos(self.x[0]) * (self.M2 * W * self.x[4] + self.I1) a2 = self.M2 * self.r2 * W * sym.cos(self.x[1]) a3 = self.M2 * W a4 = self.M2 * W * sym.sin(self.x[0]) b1 = -sym.sin(self.x[0]) * (self.M2 * W * self.x[4] + self.I1) b2 = -self.M2 * self.r2 * W * sym.sin(self.x[1]) b3 = self.M2 * W b4 = self.M2 * W * sym.cos(self.x[1]) c1 = sym.cos(self.x[0]) * (self.M1 * self.r1 * W - self.I1) c2 = self.M1 * W d1 = -sym.sin(self.x[0]) * (self.M1 * self.r1 * W - self.I1) d2 = self.M1 * W e1 = -sym.cos(self.x[1] - self.x[0]) * self.I1 * self.r2 e2 = self.I2 * self.x[4] # free flight with extended leg flight_extended_conditions = np.asarray([ self.k0 - self.x[4] + self.u[1] > 0, self.x[3] - self.ground_height_function(self.x[2]) > 0 ]) A_flight_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \ self.r1 * FX_flight * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \ FK_extended * W * sym.sin(self.x[0]) B_flight_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \ self.r1 * FX_flight * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \ FK_extended * W * sym.cos(self.x[0]) C_flight_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_extended * sym.sin(self.x[0]) + FX_flight) - \ sym.cos(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0]) D_flight_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_extended * sym.cos(self.x[0]) + FY_flight - self.M1 * self.g) - \ sym.sin(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0]) E_flight_extended = W * (FK_extended * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \ (self.r1 * FY_flight * sym.sin(self.x[0]) - self.r1 * FX_flight * sym.cos(self.x[0]) - self.u[0]) theta1_ddot_flight_extended = ((a4*b2/b4-a2)*E_flight_extended/e2-a3*C_flight_extended/c2+a4*b3*D_flight_extended/(b4*d2)+\ A_flight_extended-a4*B_flight_extended/b4)/\ (a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2)) theta2_ddot_flight_extended = (E_flight_extended - e1 * theta1_ddot_flight_extended) / e2 x_ddot_flight_extended = (C_flight_extended - c1 * theta1_ddot_flight_extended) / c2 y_ddot_flight_extended = (D_flight_extended - d1 * theta1_ddot_flight_extended) / d2 w_ddot_flight_extended = (A_flight_extended+B_flight_extended-(a1+b1)*theta1_ddot_flight_extended-(a2+b2)*theta2_ddot_flight_extended-\ a3*x_ddot_flight_extended-b3*y_ddot_flight_extended)/(a4+b4) flight_extended_ddots = np.asarray([ theta1_ddot_flight_extended, theta2_ddot_flight_extended, x_ddot_flight_extended, y_ddot_flight_extended, w_ddot_flight_extended ]) flight_extended_dynamics = np.hstack( (self.x[5:], flight_extended_ddots)) # free flight with retracted leg flight_retracted_conditions = np.asarray([ self.k0 - self.x[4] + self.u[1] <= 0, self.x[3] - self.ground_height_function(self.x[2]) > 0 ]) A_flight_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \ self.r1 * FX_flight * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \ FK_retracted * W * sym.sin(self.x[0]) B_flight_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \ self.r1 * FX_flight * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \ FK_retracted * W * sym.cos(self.x[0]) C_flight_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_retracted * sym.sin(self.x[0]) + FX_flight) - \ sym.cos(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0]) D_flight_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_retracted * sym.cos(self.x[0]) + FY_flight - self.M1 * self.g) - \ sym.sin(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0]) E_flight_retracted = W * (FK_retracted * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \ (self.r1 * FY_flight * sym.sin(self.x[0]) - self.r1 * FX_flight * sym.cos(self.x[0]) - self.u[0]) theta1_ddot_flight_retracted = ((a4*b2/b4-a2)*E_flight_retracted/e2-a3*C_flight_retracted/c2+a4*b3*D_flight_retracted/(b4*d2)+\ A_flight_retracted-a4*B_flight_retracted/b4)/\ (a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2)) theta2_ddot_flight_retracted = (E_flight_retracted - e1 * theta1_ddot_flight_retracted) / e2 x_ddot_flight_retracted = (C_flight_retracted - c1 * theta1_ddot_flight_retracted) / c2 y_ddot_flight_retracted = (D_flight_retracted - d1 * theta1_ddot_flight_retracted) / d2 w_ddot_flight_retracted = (A_flight_retracted+B_flight_retracted-(a1+b1)*theta1_ddot_flight_retracted-(a2+b2)*theta2_ddot_flight_retracted-\ a3*x_ddot_flight_retracted-b3*y_ddot_flight_retracted)/(a4+b4) flight_retracted_ddots = np.asarray([ theta1_ddot_flight_retracted, theta2_ddot_flight_retracted, x_ddot_flight_retracted, y_ddot_flight_retracted, w_ddot_flight_retracted ]) flight_retracted_dynamics = np.hstack( (self.x[5:], flight_retracted_ddots)) # contact with extended leg contact_extended_conditions = np.asarray([ self.k0 - self.x[4] + self.u[1] > 0, self.x[3] - self.ground_height_function(self.x[2]) <= 0 ]) A_contact_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \ self.r1 * FX_contact * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \ FK_extended * W * sym.sin(self.x[0]) B_contact_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \ self.r1 * FX_contact * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \ FK_extended * W * sym.cos(self.x[0]) C_contact_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_extended * sym.sin(self.x[0]) + FX_contact) - \ sym.cos(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0]) D_contact_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_extended * sym.cos(self.x[0]) + FY_contact - self.M1 * self.g) - \ sym.sin(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0]) E_contact_extended = W * (FK_extended * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \ (self.r1 * FY_contact * sym.sin(self.x[0]) - self.r1 * FX_contact * sym.cos(self.x[0]) - self.u[0]) theta1_ddot_contact_extended = ((a4*b2/b4-a2)*E_contact_extended/e2-a3*C_contact_extended/c2+a4*b3*D_contact_extended/(b4*d2)+\ A_contact_extended-a4*B_contact_extended/b4)/\ (a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2)) theta2_ddot_contact_extended = (E_contact_extended - e1 * theta1_ddot_contact_extended) / e2 x_ddot_contact_extended = (C_contact_extended - c1 * theta1_ddot_contact_extended) / c2 y_ddot_contact_extended = (D_contact_extended - d1 * theta1_ddot_contact_extended) / d2 w_ddot_contact_extended = (A_contact_extended+B_contact_extended-(a1+b1)*theta1_ddot_contact_extended-(a2+b2)*theta2_ddot_contact_extended-\ a3*x_ddot_contact_extended-b3*y_ddot_contact_extended)/(a4+b4) contact_extended_ddots = np.asarray([ theta1_ddot_contact_extended, theta2_ddot_contact_extended, x_ddot_contact_extended, y_ddot_contact_extended, w_ddot_contact_extended ]) contact_extended_dynamics = np.hstack( (self.x[5:], contact_extended_ddots)) # contact with retracted leg contact_retracted_conditions = np.asarray([ self.k0 - self.x[4] + self.u[1] <= 0, self.x[3] - self.ground_height_function(self.x[2]) <= 0 ]) A_contact_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \ self.r1 * FX_contact * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \ FK_retracted * W * sym.sin(self.x[0]) B_contact_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \ self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \ self.r1 * FX_contact * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \ FK_retracted * W * sym.cos(self.x[0]) C_contact_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_retracted * sym.sin(self.x[0]) + FX_contact) - \ sym.cos(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0]) D_contact_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_retracted * sym.cos(self.x[0]) + FY_contact - self.M1 * self.g) - \ sym.sin(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0]) E_contact_retracted = W * (FK_retracted * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \ (self.r1 * FY_contact * sym.sin(self.x[0]) - self.r1 * FX_contact * sym.cos(self.x[0]) - self.u[0]) theta1_ddot_contact_retracted = ((a4*b2/b4-a2)*E_contact_retracted/e2-a3*C_contact_retracted/c2+a4*b3*D_contact_retracted/(b4*d2)+\ A_contact_retracted-a4*B_contact_retracted/b4)/\ (a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2)) theta2_ddot_contact_retracted = ( E_contact_retracted - e1 * theta1_ddot_contact_retracted) / e2 x_ddot_contact_retracted = (C_contact_retracted - c1 * theta1_ddot_contact_retracted) / c2 y_ddot_contact_retracted = (D_contact_retracted - d1 * theta1_ddot_contact_retracted) / d2 w_ddot_contact_retracted = (A_contact_retracted+B_contact_retracted-(a1+b1)*theta1_ddot_contact_retracted-(a2+b2)*theta2_ddot_contact_retracted-\ a3*x_ddot_contact_retracted-b3*y_ddot_contact_retracted)/(a4+b4) contact_retracted_ddots = np.asarray([ theta1_ddot_contact_retracted, theta2_ddot_contact_retracted, x_ddot_contact_retracted, y_ddot_contact_retracted, w_ddot_contact_retracted ]) contact_retracted_dynamics = np.hstack( (self.x[5:], contact_retracted_ddots)) self.f_list = np.asarray([ flight_extended_dynamics, flight_retracted_dynamics, contact_extended_dynamics, contact_retracted_dynamics ]) self.f_type_list = np.asarray( ['continuous', 'continuous', 'continuous', 'continuous']) self.c_list = np.asarray([ flight_extended_conditions, flight_retracted_conditions, contact_extended_conditions, contact_retracted_conditions ]) DTHybridSystem.__init__(self, self.f_list, self.f_type_list, self.x, self.u, self.c_list, \ self.initial_env)
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 I=M/3.0*a**2