def test_functions_with_float(self):
v_x = 1.0
v_y = 1.0
self.assertEqual(sym.abs(v_x), np.abs(v_x))
self.assertEqual(sym.exp(v_x), np.exp(v_x))
self.assertEqual(sym.sqrt(v_x), np.sqrt(v_x))
self.assertEqual(sym.pow(v_x, v_y), v_x**v_y)
self.assertEqual(sym.sin(v_x), np.sin(v_x))
self.assertEqual(sym.cos(v_x), np.cos(v_x))
self.assertEqual(sym.tan(v_x), np.tan(v_x))
self.assertEqual(sym.asin(v_x), np.arcsin(v_x))
self.assertEqual(sym.acos(v_x), np.arccos(v_x))
self.assertEqual(sym.atan(v_x), np.arctan(v_x))
self.assertEqual(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self.assertEqual(sym.sinh(v_x), np.sinh(v_x))
self.assertEqual(sym.cosh(v_x), np.cosh(v_x))
self.assertEqual(sym.tanh(v_x), np.tanh(v_x))
self.assertEqual(sym.min(v_x, v_y), min(v_x, v_y))
self.assertEqual(sym.max(v_x, v_y), max(v_x, v_y))
self.assertEqual(sym.ceil(v_x), np.ceil(v_x))
self.assertEqual(sym.floor(v_x), np.floor(v_x))
self.assertEqual(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y), v_x, v_y),
v_x if v_x > v_y else v_y)
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5 * np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x**v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y), v_x, v_y),
v_x if v_x > v_y else v_y)
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5*np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y),
v_x, v_y),
v_x if v_x > v_y else v_y)
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_functions_with_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_expression(self):
self.assertEqual(str(sym.abs(e_x)), "abs(x)")
self.assertEqual(str(sym.exp(e_x)), "exp(x)")
self.assertEqual(str(sym.sqrt(e_x)), "sqrt(x)")
self.assertEqual(str(sym.pow(e_x, e_y)), "pow(x, y)")
self.assertEqual(str(sym.sin(e_x)), "sin(x)")
self.assertEqual(str(sym.cos(e_x)), "cos(x)")
self.assertEqual(str(sym.tan(e_x)), "tan(x)")
self.assertEqual(str(sym.asin(e_x)), "asin(x)")
self.assertEqual(str(sym.acos(e_x)), "acos(x)")
self.assertEqual(str(sym.atan(e_x)), "atan(x)")
self.assertEqual(str(sym.atan2(e_x, e_y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(e_x)), "sinh(x)")
self.assertEqual(str(sym.cosh(e_x)), "cosh(x)")
self.assertEqual(str(sym.tanh(e_x)), "tanh(x)")
self.assertEqual(str(sym.min(e_x, e_y)), "min(x, y)")
self.assertEqual(str(sym.max(e_x, e_y)), "max(x, y)")
self.assertEqual(str(sym.ceil(e_x)), "ceil(x)")
self.assertEqual(str(sym.floor(e_x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(e_x > e_y, e_x, e_y)),
"(if (x > y) then x else y)")
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
# WARNING: If these math functions have `float` overloads that return
# `float`, then `assertEqual`-like tests are meaningful (current state,
# and before `math` overloads were introduced).
# If these math functions implicitly cast `float` to `Expression`, then
# `assertEqual` tests are meaningless, as it tests `__nonzero__` for
# `Formula`, which will always be True.
self.assertEqual(sym.abs(v_x), 0.5*np.abs(v_x))
self.assertNotEqual(str(sym.abs(v_x)), str(0.5*np.abs(v_x)))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y),
v_x, v_y),
v_x if v_x > v_y else v_y)
# Set state to all epsilons -- otherwise, solver gets divide by zero error
prog.SetInitialGuessForAllVariables(np.full((prog.num_vars(), 1), 1e-5))
R_ = 0.01 # Cost on input "effort".
u = prog.input()
# prog.AddRunningCost(R_ * u[0]**2)
# prog.AddRunningCost(R_ * u[1]**2)
# prog.AddRunningCost(R_ * u[2]**2)
x = prog.state()
# prog.AddRunningCost(0.01*x[1]**2)
r = x[0]
r_dot = x[1]
theta_dot = x[2]
phi = x[4]
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 - x[2] - beta
alpha_R = phi - beta
prog.AddConstraintToAllKnotPoints(alpha_F <= max_alpha)
prog.AddConstraintToAllKnotPoints(alpha_F >= -max_alpha)
prog.AddConstraintToAllKnotPoints(alpha_R <= max_alpha)
prog.AddConstraintToAllKnotPoints(alpha_R >= -max_alpha)
# prog.AddConstraintToAllKnotPoints(u[0] <= max_kappa)
# prog.AddConstraintToAllKnotPoints(max_kappa >= -u[0])
# prog.AddConstraintToAllKnotPoints(u[1] <= max_kappa)
# prog.AddConstraintToAllKnotPoints(max_kappa >= -u[1])
# Start at initial condition
