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
