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"
