def test_math_overloads_shadowing(self): """Tests that we end up with the more capable pydrake.math functions in the all module, not the more limited pydrake.symbolic functions. """ from pydrake.all import AutoDiffXd, Expression, Variable, sin self.assertIsInstance(sin(1), float) self.assertIsInstance(sin(AutoDiffXd(1, [1, 0])), AutoDiffXd) self.assertIsInstance(sin(Expression(Variable('e'))), Expression) self.assertIsInstance(sin(Variable('x')), Expression)
def make_noisy_robot(): t = Variable("t") # time # state of the robot (in cartesian coordinates) z1 = Variable("z1") # ball angle (theta) z2 = Variable("z2") # body angle z3 = Variable("z3") # ball angular velocity z4 = Variable("z4") # body angular velocity cartesian_state = [z1, z2, z3, z4] # control input of the robot u1 = Variable("u1") # wheel torque ctrl_input = [u1] # nonlinear dynamics, the whole state is measured (output = state) mb = 1 # mass of the ball mB = 1 # mass of the body rb = 1 # radius of the ball l = 1 # height of the center of gravity Ib = 1 # inertia of the ball IB = 1 # inertia of the body g = 1.5 # gravitational acceleration theta = z1 theta_dot = z3 phi = z1 - z2 phi_dot = z3 - z4 gamma1 = Ib + IB + mb*rb**2 + mB*rb**2 + mB*l**2 gamma2 = mB*l**2 + IB m1 = gamma1 + 2*mB*rb*l*cos(theta + phi) m2 = gamma2 + mB*rb*l*cos(theta + phi) m3 = gamma2 + mB*rb*l*cos(theta + phi) m4 = gamma2 Minv = np.array([[m4, -m2],[-m3, m1]])/(m1*m4 - m2*m3) C = np.array([[-mB*rb*l*sin(theta + phi)*(theta_dot + phi_dot)**2], [0]]) G = np.array([[-mB*g*l*sin(theta+phi)], [-mB*g*l*sin(theta+phi)]]) B = np.array([[0],[u1]]) D = np.array([[1.5*theta_dot], [phi_dot]]) qddot = (Minv*(B-C-G-D)).T[0] theta_ddot, phi_ddot = qddot z5 = theta_ddot z6 = theta_ddot - phi_ddot dynamics = np.array([z3, z4, z5, z6]) # dynamics = [u1*cos(z3), u1*sin(z3), u2] robot = SymbolicVectorSystem( state=cartesian_state, input=ctrl_input, output=cartesian_state, dynamics=dynamics, ) return robot
def pendulum_dynamics(x, u, p): q = x[0] qdot = x[1] tau = u[0] return [ qdot, ((-p["m"] * p["g"] * p["l"] * sin(q) + tau) / (p["m"] * p["l"]**2)) ]
def pendulum_with_motor_dynamics(x, u, p): q = x[0] qdot = x[1] tau = u[0] arm_intertia = (-p["m"] * p["g"] * p["l"] * sin(q) + tau) / (p["m"] * p["l"]**2) # Note: should tau actually be tau_motor? Which one do we get as input? q_dd_motor = (p["g"] / p["N"] + tau) / (p["I"] + arm_intertia / p["N"]**2) q_dd = q_dd_motor / p["N"] return [qdot, q_dd]
def create_symbolic_dynamics(): # state of the robot (in cartesian coordinates) x1 = Variable("x1") x2 = Variable("x2") x3 = Variable("x3") cartesian_state = [x1, x2, x3] u1 = Variable("u1") # angular velocity input = [u1] dynamics = [cos(x3), sin(x3), u1] uav = SymbolicVectorSystem( state=cartesian_state, input=input, output=cartesian_state, dynamics=dynamics, ) return uav
The vehicle will be then represented as a Drake `SymbolicVectorSystem`. """ # state of the robot (in cartesian coordinates) z1 = Variable("z1") # horizontal position z2 = Variable("z2") # vertical position z3 = Variable("z3") # angular position cartesian_state = [z1, z2, z3] # control input of the robot u1 = Variable("u1") # linear velocity u2 = Variable("u2") # angular velocity input = [u1, u2] # nonlinear dynamics, the whole state is measured (output = state) dynamics = [u1*cos(z3), u1*sin(z3), u2] robot = SymbolicVectorSystem( state=cartesian_state, input=input, output=cartesian_state, dynamics=dynamics, ) """## Write the Control Law It is time to write the controller. To do that we construct a `VectorSystem` with a method called `DoCalcVectorOutput`. In the latter, we write all the steps needed to compute the instantaneous control action from the vehicle state. """ # you will write the control law in it in the next cell # by defining the function called "lyapunov_controller"