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