def test_simulation(self):
# Basic constant-torque acrobot simulation.
acrobot = AcrobotPlant()
# Create the simulator.
simulator = Simulator(acrobot)
context = simulator.get_mutable_context()
# Set an input torque.
input = AcrobotInput()
input.set_tau(1.)
acrobot.GetInputPort("elbow_torque").FixValue(context, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta1(1.)
state.set_theta1dot(0.)
state.set_theta2(0.)
state.set_theta2dot(0.)
self.assertTrue(acrobot.DynamicsBiasTerm(context).shape == (2, ))
self.assertTrue(acrobot.MassMatrix(context).shape == (2, 2))
initial_total_energy = acrobot.EvalPotentialEnergy(context) + \
acrobot.EvalKineticEnergy(context)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertLessEqual(
acrobot.EvalPotentialEnergy(context) +
acrobot.EvalKineticEnergy(context), initial_total_energy)
def calculate_lqr(self):
upright_plant = AcrobotPlant()
upright_plant_context = upright_plant.CreateDefaultContext()
upright_plant_context.get_mutable_continuous_state() \
.SetFromVector(np.array([np.pi, 0, 0, 0]))
upright_plant.GetInputPort("elbow_torque") \
.FixValue(upright_plant_context, 0)
lqr = LinearQuadraticRegulator(upright_plant, upright_plant_context,
self.Q, self.R)
return lqr.D()
def simulate(*, initial_state, controller_params, t_final, tape_period):
"""Simulates an Acrobot + Spong controller from the given initial state and
parameters until the given final time. Returns the state sampled at the
given tape_period.
"""
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
controller = builder.AddSystem(AcrobotSpongController())
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_logger = LogOutput(plant.get_output_port(0), builder)
state_logger.set_publish_period(tape_period)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(plant, context)
controller_context = diagram.GetMutableSubsystemContext(
controller, context)
plant_context.SetContinuousState(initial_state)
controller_context.get_mutable_numeric_parameter(0).SetFromVector(
controller_params)
simulator.AdvanceTo(t_final)
x_tape = state_logger.data()
return x_tape
def test_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
scene_graph = builder.AddSystem(SceneGraph())
geom = AcrobotGeometry.AddToBuilder(
builder=builder, acrobot_state_port=plant.get_output_port(0),
acrobot_params=AcrobotParams(), scene_graph=scene_graph)
builder.Build()
self.assertIsInstance(geom, AcrobotGeometry)
def make_real_dircol_mp():
global tree
global plant
global context
global dircol
# expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque
tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf",
FloatingBaseType.kFixed)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant, context, num_time_samples=21,
minimum_timestep=0.05, maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (math.pi, 0., 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
R = 10 # Cost on input "effort".
u = dircol.input()
dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
print(id(dircol))
return dircol
def __init__(self, k1, k2, k3, lqr_angle_range=1.0):
super().__init__()
self.k1 = k1
self.k2 = k2
self.k3 = k3
self.acrobot_plant = AcrobotPlant()
self.acrobot_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context.get_mutable_continuous_state()\
.SetFromVector(np.array([np.pi, 0, 0, 0]))
self.lqr_angle_range = lqr_angle_range
self.DeclareInputPort('estimated state', PortDataType.kVectorValued, 4)
self.DeclareVectorOutputPort('control', BasicVector(1),
self.calculate_control)
self.Q = np.diag((10., 10., 1., 1.))
self.R = [10]
self.K = self.calculate_lqr()
self.LQR_on = False
def BalancingLQR():
# Design an LQR controller for stabilizing the Acrobot around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original AcrobotState coordinates).
acrobot = AcrobotPlant()
context = acrobot.CreateDefaultContext()
input = AcrobotInput()
input.set_tau(0.)
context.FixInputPort(0, input)
context.get_mutable_continuous_state_vector()\
.SetFromVector(UprightState().CopyToVector())
Q = np.diag((10., 10., 1., 1.))
R = [1]
return LinearQuadraticRegulator(acrobot, context, Q, R)
def simulate_acrobot():
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * math.pi)
wrapangles.set_interval(1, -math.pi, math.pi)
wrapto = builder.AddSystem(wrapangles)
builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0))
controller = builder.AddSystem(BalancingLQR())
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
parser = argparse.ArgumentParser()
parser.add_argument("-N",
"--trials",
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
args = parser.parse_args()
print(AcrobotPlant)
for i in range(args.trials):
context.set_time(0.)
state.SetFromVector(UprightState().CopyToVector() +
0.05 * np.random.randn(4, ))
simulator.StepTo(args.duration)
import math
import numpy as np
import matplotlib.pyplot as plt
from pydrake.all import (DirectCollocation, FloatingBaseType,
PiecewisePolynomial, RigidBodyTree, RigidBodyPlant,
Solve)
from pydrake.examples.acrobot import AcrobotPlant
from underactuated import (FindResource, PlanarRigidBodyVisualizer)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.05,
maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
# More elegant version is blocked on drake #8315:
from pydrake.all import (DiagramBuilder, PlanarSceneGraphVisualizer, Simulator,
SceneGraph, VectorSystem)
from pydrake.examples.acrobot import (AcrobotGeometry, AcrobotInput,
AcrobotPlant, AcrobotState)
from underactuated import SliderSystem
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
scene_graph = builder.AddSystem(SceneGraph())
AcrobotGeometry.AddToBuilder(builder, acrobot.get_output_port(0), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Torque", -5., 5.))
builder.Connect(torque_system.get_output_port(0), acrobot.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
class EnergyShapingControl(LeafSystem):
def __init__(self, k1, k2, k3, lqr_angle_range=1.0):
super().__init__()
self.k1 = k1
self.k2 = k2
self.k3 = k3
self.acrobot_plant = AcrobotPlant()
self.acrobot_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context.get_mutable_continuous_state()\
.SetFromVector(np.array([np.pi, 0, 0, 0]))
self.lqr_angle_range = lqr_angle_range
self.DeclareInputPort('estimated state', PortDataType.kVectorValued, 4)
self.DeclareVectorOutputPort('control', BasicVector(1),
self.calculate_control)
self.Q = np.diag((10., 10., 1., 1.))
self.R = [10]
self.K = self.calculate_lqr()
self.LQR_on = False
def calculate_lqr(self):
upright_plant = AcrobotPlant()
upright_plant_context = upright_plant.CreateDefaultContext()
upright_plant_context.get_mutable_continuous_state() \
.SetFromVector(np.array([np.pi, 0, 0, 0]))
upright_plant.GetInputPort("elbow_torque") \
.FixValue(upright_plant_context, 0)
lqr = LinearQuadraticRegulator(upright_plant, upright_plant_context,
self.Q, self.R)
return lqr.D()
def calculate_control(self, context, control_vec):
input = self.get_input_port(0).Eval(context)
theta1 = input[0]
theta2 = input[1]
theta1_dot = input[2]
theta2_dot = input[3]
# Current acrobot context based on input.
self.acrobot_context \
.get_mutable_continuous_state() \
.SetFromVector(input)
desired_state = np.array([np.pi, 0, 0, 0])
cost = (desired_state - input).T @ self.Q @ (desired_state - input)
should_use_lqr = cost < 1
if not self.LQR_on:
self.LQR_on = should_use_lqr
if should_use_lqr:
print("LQR on {0}".format(context.get_time()))
if self.LQR_on:
u = -self.K @ (desired_state - input)
control_vec.SetFromVector(u)
else:
# Bias term of the dynamics.
bias = self.acrobot_plant.DynamicsBiasTerm(self.acrobot_context)
# Mass matrix
M = self.acrobot_plant.MassMatrix(self.acrobot_context)
m11 = M[0, 0]
m12 = M[0, 1]
m21 = M[1, 0]
m22 = M[1, 1]
m22_bar = m22 - m21 * m12 / m11
bias_bar = bias[1] - m21 / m11 * bias[0]
# Desired energy, upright configuration.
Ed = self.acrobot_plant.EvalPotentialEnergy(self.upright_context) \
+ self.acrobot_plant.EvalKineticEnergy(self.upright_context)
# Current energy.
Ec = self.acrobot_plant.EvalPotentialEnergy(self.acrobot_context) \
+ self.acrobot_plant.EvalKineticEnergy(self.acrobot_context)
E_error = Ec - Ed
u_bar = E_error * theta1_dot
u = -self.k1 * theta2 - self.k2 * theta2_dot + self.k3 * u_bar
tau = m22_bar * u + bias_bar
control_vec.SetFromVector(np.array([tau]))
# import torch import tensorflow from pydrake.examples.acrobot import AcrobotPlant import sys print(sys.executable) acrobot = AcrobotPlant()
def make_real_dircol_mp(expmt="cartpole", seed=1776):
global tree
global plant
global context
global dircol
# TODO: use the seed in some meaningful way:
# https://github.com/RobotLocomotion/drake/blob/master/systems/stochastic_systems.h
assert expmt in ("cartpole", "acrobot")
# expmt = "cartpole" # State: (x, theta, x_dot, theta_dot) Input: x force
# expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque
if expmt == "cartpole":
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
else:
tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf",
FloatingBaseType.kFixed)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
if expmt == "cartpole":
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.1,
maximum_timestep=0.4)
else:
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.05,
maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
if expmt == "acrobot":
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
# More elegant version is blocked on drake #8315:
# dircol.AddLinearConstraint(dircol.initial_state() == initial_state)
if expmt == "cartpole":
final_state = (0., math.pi, 0., 0.)
else:
final_state = (math.pi, 0., 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state)
# R = 10 # Cost on input "effort".
# u = dircol.input()
# dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
return dircol, tree