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 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 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)
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:
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]))