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_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