def __init__(self, feedback_rule):
VectorSystem.__init__(
self,
3,
1 # Three inputs (theta, dtheta, base position).
) # One output (torque at shoulder).
self.feedback_rule = feedback_rule
def __init__(self, ballandbeam):
'''
Controls a ball and beam system described
in ball_and_beam.sdf.
:param ballandbeam: A pydrake RigidBodyTree() loaded
from ball_and_beam.sdf.
'''
VectorSystem.__init__(
self,
4, # 4 inputs: r, theta, and their derivatives
1) # 1 output: The torque of the beam
self.ballandbeam = ballandbeam
# Default parameters for the ball and beam -- should match
# ball_and_beam.sdf, where applicable.
# Probably don't need all of these.
self.g = 9.8
self.m = 0.1 # kg
self.R = 0.5 # m
self.J_b = self.m * self.R**2 #2.25*10**(-5) # kg m^2
self.theta_bar = 0.5 # rad, < pi/2
self.M = 0.2 # kg
self.L = 3 # m
self.J = self.L**2 #0.36 # kg m^
self.beta = 0.2 # Nm/s
self.b = self.beta / (self.m + (self.J_b / (self.R**3)))
self.d = self.m / (self.m + (self.J_b / (self.R**3)))
self.n = (self.m * self.g) / (self.m + (self.J_b / (self.R**3)))
self.k_p = 250
self.k1_bar = 1 / self.n
self.k_d = 50
self.i = complex(0, 1)
def __init__(self, plant, y_traj, duration):
n_in = 12
n_out = 4
VectorSystem.__init__(self, 12, 4)
self.plant = plant
# Find x_traj, u_traj @ traj_freq
t_traj = np.linspace(0, duration,
np.ceil(duration * QuadrotorController.traj_freq))
x_traj, u_traj = np.zeros((len(t_traj), n_in)), np.zeros(
(len(t_traj), n_out))
for idx, t in enumerate(t_traj):
x_traj[idx, :], u_traj[idx, :] = self.get_x_and_u(plant, y_traj, t)
# Create spline for x_traj, u_traj
x_spline, u_spline = CubicSpline(t_traj,
x_traj), CubicSpline(t_traj, u_traj)
# Give the spline to TVLQR
K_traj = self.tvlqr(plant, x_spline, u_spline, t_traj)
K_spline = CubicSpline(t_traj, np.reshape(K_traj,
(K_traj.shape[0], -1)))
# Save the TVLQR; use it in DoCalcVectorOutput
# TVLQR gives K(t), x0(t), u0(t)
# Want to return u^*=u0(T)-K(T)(x(t)-x0(T)) where T is that which is closest to t
self.x_spline = x_spline
self.u_spline = u_spline
self.K_spline = K_spline
def __init__(self,
mb=1.,
lb=0.2,
m1=2.,
l1=0.2,
m2=2.,
l2=0.2,
g=10.,
input_max=10.):
VectorSystem.__init__(
self,
2, # Two input (thrust of each rotor).
10
) # Ten outputs (xb, yb, thetab, theta1, theta2) and its derivatives
self._DeclareContinuousState(
10
) # Ten states (xb, yb, thetab, theta1, theta2) and its derivatives.
self.mb = float(mb)
self.lb = float(lb)
self.m1 = float(m1)
self.l1 = float(l1)
self.m2 = float(m2)
self.l2 = float(l2)
self.g = float(g)
self.input_max = float(input_max)
# Go ahead and calculate rotational inertias.
# Treat the drone as a rectangle.
self.Ib = 1. / 3. * self.mb * self.lb**2
# Treat the first link as a line.
self.I1 = 1. / 3. * self.m1 * self.l1**2
# Treat the second link as a line.
self.I2 = 1. / 3. * self.m2 * self.l2**2
def __init__(self,
m1=1.,
l1=1.,
m2=2.,
l2=2.,
r=1.0,
g=10.,
input_max=10.):
VectorSystem.__init__(
self,
1, # One input (torque at reaction wheel).
4) # Four outputs (theta, phi, dtheta, dphi)
self._DeclareContinuousState(
4) # Four states (theta, phi, dtheta, dphi).
self.m1 = float(m1)
self.l1 = float(l1)
self.m2 = float(m2)
self.l2 = float(l2)
self.r = float(r)
self.g = float(g)
self.input_max = float(input_max)
# Go ahead and calculate rotational inertias.
# Treat the first link as a point mass.
self.I1 = self.m1 * self.l1**2
# Treat the second link as a disk.
self.I2 = 0.5 * self.m2 * self.r**2
def __init__(self, hopper, desired_lateral_velocity=0.0, print_period=0.0):
# hopper = a rigid body tree representing the 1D hopper
# desired_lateral_velocity = How fast should the controller
# aim to run sideways?
# print_period = print to console to indicate sim progress
# if nonzero
VectorSystem.__init__(
self,
10, # 10 inputs: x, z, theta, alpha, l, and their derivatives
2) # 2 outputs: Torque on thigh, and force on the leg extension
# link. (The passive spring for on the leg is calculated as
# part of this output.)
self.hopper = hopper
self.desired_lateral_velocity = desired_lateral_velocity
self.print_period = print_period
self.last_print_time = -print_period
# Remember what the index of the foot is
# self.foot_body_index = hopper.GetFrameByName("foot").get_body_index()
self.foot_body_frame = hopper.GetFrameByName("foot")
self.world_frame = hopper.world_frame()
# The context for the hopper
self.plant_context = self.hopper.CreateDefaultContext()
# Default parameters for the hopper -- should match
# raibert_hopper_1d.sdf, where applicable.
# You're welcome to use these, but you probably don't need them.
self.hopper_leg_length = 1.0
self.m_b = 1.0
self.m_f = 0.1
self.l_max = 0.5
# This is an arbitrary choice of spring constant for the leg.
self.K_l = 100
def __init__(self, pusher_slider):
# 17 inputs state of the pusher slider
nx = pusher_slider.num_positions() + pusher_slider.num_velocities()
# 2 outputs force on the pusher
nu = pusher_slider.num_actuators()
# instantiate controller
VectorSystem.__init__(self, nx, nu)
self.pusher_slider = pusher_slider
def __init__(self, plant, print_period=0.0):
VectorSystem.__init__(self,
plant.num_positions() + plant.num_velocities(),
plant.num_actuators())
print("n_inputs", plant.num_positions() + plant.num_velocities())
print('n_actuators', plant.num_actuators())
# 6 inputs (from the 6 outputs of the plant)
# 3 outputs (for the three actuators on the plant)
self.plant = plant
self.print_period = print_period
self.last_print_time = -print_period
def __init__(self, m=3, l=1, g=10, b=2, C=2, w=2):
VectorSystem.__init__(
self, 1, 3,
direct_feedthrough=False) # Two inputs (torque at shoulder).
# 3) # Three outputs (theta, dtheta, base positions)
self.DeclareContinuousState(2) # Two state variables (theta, dtheta).
self.m = float(m)
self.l = float(l)
self.g = float(g)
self.b = float(b)
self.C = float(C)
self.w = float(w)
def __init__(self, rigid_body_tree, B, v_des):
# 4 inputs (double pend state), 2 torque outputs.
self.tree = rigid_body_tree
VectorSystem.__init__(
self,
self.tree.get_num_positions() + self.tree.get_num_velocities(),
self.tree.get_num_actuators())
self.B = B
self.v_des = v_des
self.num_arms = (self.tree.get_num_velocities() - 3) // 2
def __init__(self, ballandbeam):
'''
Controls a ball and beam system described
in ball_and_beam.sdf.
:param ballandbeam: A pydrake RigidBodyTree() loaded
from ball_and_beam.sdf.
'''
VectorSystem.__init__(
self,
4, # 4 inputs: r, theta, and their derivatives
1) # 1 output: The torque of the beam
self.ballandbeam = ballandbeam
def __init__(self,
hopper,
actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
# hopper = a rigid body tree representing the 1D hopper
# desired_lateral_velocity = How fast should the controller
# aim to run sideways?
# print_period = print to console to indicate sim progress
# if nonzero
VectorSystem.__init__(
self,
10, # 10 inputs: x, z, theta, alpha, l, and their derivatives
2) # 2 outputs: Torque on thigh, and force on the leg extension
# link. (The passive spring for on the leg is calculated as
# part of this output.)
self.hopper = hopper
self.desired_lateral_velocity = desired_lateral_velocity
self.desired_height = desired_height
self.actuators_off = actuators_off
self.print_period = print_period
self.printed_lifted_off = False
self.last_print_time = -print_period
self.current_desired_touchdown_beta = None
self.total_number_of_hops = 0.0
# Remember what the index of the foot is
self.foot_frame = hopper.GetFrameByName("foot")
self.body_frame = hopper.GetFrameByName("body")
self.world_frame = hopper.world_frame()
self.l_rest = 1.0
# The context for the hopper
self.plant_context = self.hopper.CreateDefaultContext()
# Default parameters for the hopper -- should match
# raibert_hopper_1d.sdf, where applicable.
# You're welcome to use these, but you probably don't need them.
self.hopper_leg_length = 2.0
self.body_size_height = 0.5
# Based on the potential energy from plant.CalcPotentialEnergy(plant_context)
self.total_mass = 3.095499
# Based on the potential energy from plant.CalcPotentialEnergy(plant_context)
self.m_f = 0.1 # If I increase this mass, it decreases the energy loss... this doesn't make any sense
self.m_b = self.total_mass - self.m_f
self.l_max = 0.5
self.gravity = 9.81
# This is an arbitrary choice of spring constant for the leg.
self.K_l = 100.0
self.desired_alpha_array = []
def __init__(self, m=1., Ixx=1., Iyy=2., Izz=3., g=10., input_max=10.):
VectorSystem.__init__(
self,
4, # One input (torque at reaction wheel).
15) # Four outputs (theta, phi, dtheta, dphi)
self._DeclareContinuousState(
15) # Four states (theta, phi, dtheta, dphi).
self.m = float(m)
self.Ixx = float(Ixx)
self.Iyy = float(Iyy)
self.Izz = float(Izz)
self.g = float(g)
self.input_max = float(input_max)
def __init__(self, rbt, time_step, A, B): #, rigid_body_tree, gravity):
# 4 inputs (double pend state), 2 torque outputs.
self.num_positions = rbt.get_num_positions()
self.num_velocities = rbt.get_num_velocities()
self.num_cmds = rbt.get_num_actuators() - 3
VectorSystem.__init__(self, self.num_cmds,
self.num_positions + self.num_velocities)
VectorSystem._DeclarePeriodicDiscreteUpdate(self, time_step)
VectorSystem._DeclareDiscreteState(
self, self.num_positions + self.num_velocities)
self.tree = rbt
#self.g = gravity
self.time_step = time_step
self.A = A
self.B = B
def __init__(self, lyapunov_controller, params, final_state): # 3 inputs (UAV state) # 1 outut (UAV inputs) VectorSystem.__init__(self, 3, 1) self.lyapunov_controller = lyapunov_controller if not isinstance(params, np.float64): self.params = params # a, u_max, eps self.r = 1/self.params[1] else: self.params = params self.r = 0 self.fx1, self.fx2, self.fx3 = final_state
def __init__(self, m = 1., Ixx = 1.,
Iyy = 2., Izz = 3., g = 10., rw_l = 1, bw=1, bw_bound =0.3,
input_max = 10.):
VectorSystem.__init__(self,
4, # One input (torque at reaction wheel).
13) # Four outputs (theta, phi, dtheta, dphi)
self._DeclareContinuousState(13) # Four states (theta, phi, dtheta, dphi).
self.m = float(m)
self.Ixx = float(Ixx)
self.Iyy = float(Iyy)
self.Izz = float(Izz)
self.g = float(g)
self.rw_l = float(rw_l)
self.bw = float(bw)*1.0
self.bw_bound = float(bw_bound)*1.0
self.input_max = float(input_max)
def __init__(self, S, N, Q, R, P, X_N):
"""
Arguments
----------
S : PieceWiseAffineSystem
PWA system to be controlled.
N : int
Horizon of the optimal control problem.
Q : numpy.ndarray
Quadratic cost for the state.
R : numpy.ndarray
Quadratic cost for the input.
P : numpy.ndarray
Quadratic cost for the terminal state.
X_N : Polyhedron
Terminal set.
"""
# bouild drake controller
VectorSystem.__init__(self, S.nx, S.nu)
self.controller = HybridModelPredictiveController(S, N, Q, R, Q, X_N)
def __init__(self, ramone,
desired_goal = 0.0,
print_period = 0.0):
'''
Controls a planar RAMone described
in ramone.urdf.
:param ramone: A pydrake RigidBodyTree() loaded
from ramone.urdf.
:param desired_goal: The goal for the model to reach
:param print_period: If nonzero, this controller will print to
the python console every print_period (simulated) seconds
to indicate simulation progress.
'''
VectorSystem.__init__(self,
12, # 12 inputs: x, z, alpha_l, beta_l, alpha_r, beta_r, and their derivatives
4) # Torque on hip, and on knee for the two legs left and right
self.ramone = ramone
self.desired_goal = desired_goal
self.print_period = print_period
self.last_print_time = -print_period
# The index of the left foot and the right foot
self.left_foot_index = ramone.FindBody("left_foot").get_body_index()
self.right_foot_index = ramone.FindBody("right_foot").get_body_index()
# Default parameters for the ramone
# self.left_foot_x = 0
# self.right_foot_x = 0
# self.com_x = 0
# self.X = np.zeros(12)
self.left_contact = False
self.right_contact = False
# Current phase
self.isLeft = True
self.err = 0
self.up = True
self.down = False
def __init__(self, hopper, desired_lateral_velocity=0.0, print_period=0.0):
'''
Controls a planar hopper described
in raibert_hopper_2d.sdf.
:param hopper: A pydrake RigidBodyTree() loaded
from raibert_hopper_2d.sdf.
:param desired_lateral_velocity: How fast should the controller
aim to run sideways?
:param print_period: If nonzero, this controller will print to
the python console every print_period (simulated) seconds
to indicate simulation progress.
'''
VectorSystem.__init__(
self,
10, # 10 inputs: x, z, theta, alpha, l, and their derivatives
2) # 2 outputs: Torque on thigh, and force on the leg extension
# link. (The passive spring for on the leg is calculated as
# part of this output.)
self.hopper = hopper
self.desired_lateral_velocity = desired_lateral_velocity
self.print_period = print_period
self.last_print_time = -print_period
# Remember what the index of the foot is
self.foot_body_index = hopper.FindBody("foot").get_body_index()
# Default parameters for the hopper -- should match
# raibert_hopper_1d.sdf, where applicable.
# You're welcome to use these, but you probably don't need them.
self.hopper_leg_length = 1.0
self.m_b = 1.0
self.m_f = 0.1
self.l_max = 0.5
# This is an arbitrary choice of spring constant for the leg.
self.K_l = 100
def __init__(self, rigid_body_tree, gravity):
# 4 inputs (double pend state), 2 torque outputs.
VectorSystem.__init__(self, 4, 2)
self.tree = rigid_body_tree
self.g = gravity
def __init__(self, multibody_plant, gravity):
# 4 inputs (double pend state), 2 torque outputs.
VectorSystem.__init__(self, 4, 2)
self.plant = multibody_plant
self.g = gravity
def __init__(self):
VectorSystem.__init__(
self,
15, # Four inputs: full state inertial wheel pendulum..
13) # One output (torque for reaction wheel).
def __init__(self):
VectorSystem.__init__(self, 2, 2)
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
def __init__(self, vibrating_pendulum, pendulum_torque):
# 2 inputs: pendulum state
# 1 output: torque on pendulum
VectorSystem.__init__(self, 2, 1)
self.vibrating_pendulum = vibrating_pendulum
self.pendulum_torque = pendulum_torque
def __init__(self, feedback_rule):
VectorSystem.__init__(
self,
4, # Four inputs: full state inertial wheel pendulum..
1) # One output (torque for reaction wheel).
self.feedback_rule = feedback_rule
def __init__(self):
VectorSystem.__init__(self, 4, 4)
def __init__(self, ax, title, min, max):
# 0 inputs, 1 output.
VectorSystem.__init__(self, 0, 1)
self.value = 0
self.slider = Slider(ax, title, min, max, valinit=self.value)
self.slider.on_changed(self.update)
def __init__(self, ax):
# 0 inputs, 1 output.
VectorSystem.__init__(self, 0, 1)
self.value = 0
self.slider = Slider(ax, 'Torque', -5, 5, valinit=self.value)
self.slider.on_changed(self.update)
def __init__(self, vibrating_pendulum):
# 5 inputs: state of base + pendulum, pendulum torque
# 2 outputs: force on base + torque on pendulum
VectorSystem.__init__(self,5, 2)
self.vibrating_pendulum = vibrating_pendulum
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
def __init__(self, ax, title, min, max):
# 0 inputs, 1 output.
VectorSystem.__init__(self, 0, 1)
self.value = 0
self.slider = Slider(ax, title, min, max, valinit=self.value)
self.slider.on_changed(self.update)
def __init__(self):
VectorSystem.__init__(self, settings['state_dim'], settings['state_dim'])
def __init__(self, multibody_plant, gravity):
# 4 inputs (double pend state), 2 torque outputs.
VectorSystem.__init__(self, 4, 2)
self.plant = multibody_plant
self.g = gravity