double_pendulum.controller.lqr

Subpackages

Submodules

double_pendulum.controller.lqr.lqr

double_pendulum.controller.lqr.lqr.dlqr(A, B, Q, R)

Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]

double_pendulum.controller.lqr.lqr.iterative_riccati(plant, Q, R, Qf, dt, x_traj, u_traj)

iteratively solve the dynamic ricatti equation. intended for finite horizon lqr/tvlqr

For more Information see for example:
Parameters:
plantSymbolicDoublePendulum or DoublePendulumPlant object

A plant object containing the kinematics and dynamics of the double pendulum Q : numpy_array

shape=(4,4) Q-matrix describing quadratic state cost

Rnumpy_array

shape=(2,2) R-matrix describing quadratic control cost

Qfnumpy_array

shape=(4,4) Q-matrix describing quadratic state cost for the final point stabilization

dtfloat

timestep, unit=[s]

x_trajnumpy_array

shape=(N, 4) states, units=[rad, rad, rad/s, rad/s] order=[angle1, angle2, velocity1, velocity2]

u_trajnumpy_array

shape=(N, 2) actuations/motor torques order=[u1, u2], units=[Nm]

Returns
——-
numpy_array

Array of feedback matrices (K Matrices) shape=(N, 4, 4)

numpy_array

Array of feedback matrices of the Lagrange multipliers from dynamics constraint shape=(N, 4, 4)

double_pendulum.controller.lqr.lqr.lqr(A, B, Q, R)

Solve the continuous time lqr controller. dx/dt = A x + B u cost = integral x.T*Q*x + u.T*R*u

double_pendulum.controller.lqr.lqr_controller

class double_pendulum.controller.lqr.lqr_controller.LQRController(mass=[0.5, 0.6], length=[0.3, 0.2], com=[0.3, 0.2], damping=[0.1, 0.1], coulomb_fric=[0.0, 0.0], gravity=9.81, inertia=[None, None], torque_limit=[0.0, 1.0], model_pars=None)

Bases: AbstractController

LQRController. Controller which uses LQR to stabilize a (unstable) fixpoint.

Parameters:
massarray_like, optional

shape=(2,), dtype=float, default=[0.5, 0.6] masses of the double pendulum, [m1, m2], units=[kg]

lengtharray_like, optional

shape=(2,), dtype=float, default=[0.3, 0.2] link lengths of the double pendulum, [l1, l2], units=[m]

comarray_like, optional

shape=(2,), dtype=float, default=[0.3, 0.3] center of mass lengths of the double pendulum links [r1, r2], units=[m]

dampingarray_like, optional

shape=(2,), dtype=float, default=[0.1, 0.1] damping coefficients of the double pendulum actuators [b1, b2], units=[kg*m/s]

gravityfloat, optional

default=9.81 gravity acceleration (pointing downwards), units=[m/s²]

coulomb_fricarray_like, optional

shape=(2,), dtype=float, default=[0.0, 0.0] coulomb friction coefficients for the double pendulum actuators [cf1, cf2], units=[Nm]

inertiaarray_like, optional

shape=(2,), dtype=float, default=[None, None] inertia of the double pendulum links [I1, I2], units=[kg*m²] if entry is None defaults to point mass m*l² inertia for the entry

torque_limitarray_like, optional

shape=(2,), dtype=float, default=[0.0, 1.0] torque limit of the motors [tl1, tl2], units=[Nm, Nm]

model_parsmodel_parameters object, optional

object of the model_parameters class, default=None Can be used to set all model parameters above If provided, the model_pars parameters overwrite the other provided parameters

get_control_output_(x, t=None)

The function to compute the control input for the double pendulum’s actuator(s).

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s]

tfloat, optional

time, unit=[s] (Default value=None)

Returns:
array_like

shape=(2,), dtype=float actuation input/motor torque, order=[u1, u2], units=[Nm]

init_()

Initalize the controller.

save_(save_dir)

Save controller parameters

Parameters:
save_dirstring or path object

directory where the parameters will be saved

set_cost_matrices(Q, R)

Set the Q and R matrices directly.

Parameters:
Qnumpy_array

shape=(4,4) Q-matrix describing quadratic state cost

Rnumpy_array

shape=(2,2) R-matrix describing quadratic control cost

set_cost_parameters(p1p1_cost=1.0, p2p2_cost=1.0, v1v1_cost=1.0, v2v2_cost=1.0, p1p2_cost=0.0, v1v2_cost=0.0, p1v1_cost=0.0, p1v2_cost=0.0, p2v1_cost=0.0, p2v2_cost=0.0, u1u1_cost=0.01, u2u2_cost=0.01, u1u2_cost=0.0)

set_cost_parameters. Parameters of Q and R matrices. The parameters are

Q = ((p1p1, p1p2, p1v1, p1v2),

(p1p2, p2p2, p2v1, p2v2), (p1v1, p2v1, v1v1, v1v2), (p1v2, p2v2, v1v2, v2v2))

R = ((u1u1, u1u2),

(u1u2, u2u2))

Parameters:
p1p1_costfloat

p1p1_cost (Default value=1.)

p2p2_costfloat

p2p2_cost (Default value=1.)

v1v1_costfloat

v1v1_cost (Default value=1.)

v2v2_costfloat

v2v2_cost (Default value=0.)

p1p2_costfloat

p1p2_cost (Default value=0.)

v1v2_costfloat

v1v2_cost (Default value=0.)

p1v1_costfloat

p1v1_cost (Default value=0.)

p1v2_costfloat

p1v2_cost (Default value=0.)

p2v1_costfloat

p2v1_cost (Default value=0.)

p2v2_costfloat

p2v2_cost (Default value=0.)

u1u1_costfloat

u1u1_cost (Default value=0.01)

u2u2_costfloat

u2u2_cost (Default value=0.01)

u1u2_costfloat

u1u2_cost (Default value=0.)

set_cost_parameters_(pars=[1.0, 1.0, 1.0, 1.0, 1.0])

Set the diagonal parameters of Q and R matrices with a list.

The parameters are Q = ((pars[0], 0, 0, 0),

(0, pars[1], 0, 0), (0, 0, pars[2], 0), (0, 0, 0, pars[3]))

R = ((pars[4], 0)

(0, pars[4]))

Parameters:
parslist

shape=(5,), dtype=float (Default value=[1., 1., 1., 1., 1.])

set_goal(x=[3.141592653589793, 0.0, 0.0, 0.0])

set_goal. Set goal for the controller.

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s] (Default value=[np.pi, 0., 0., 0.])

set_parameters(failure_value=nan, cost_to_go_cut=15.0)

set_parameters. Set parameters for this controller.

Parameters:
failure_valuefloat

if the cost-to-go exceeds cost_to_go_cut this value is retured as torque (Default value=np.nan)

cost_to_go_cutfloat

if the cost-to-go exceeds this values the controller returns failure_value (Default value=15.)

class double_pendulum.controller.lqr.lqr_controller.LQRController_nonsymbolic(mass=[0.5, 0.6], length=[0.3, 0.2], com=[0.3, 0.2], damping=[0.1, 0.1], coulomb_fric=[0.0, 0.0], gravity=9.81, inertia=[None, None], torque_limit=[0.0, 1.0], model_pars=None)

Bases: AbstractController

LQRController. Controller which uses LQR to stabilize a (unstable) fixpoint. This version of the LQR controller does not use the symbolic plant and thus is compatible with the cma-es optimizer for parameter optimization.

Parameters:
massarray_like, optional

shape=(2,), dtype=float, default=[0.5, 0.6] masses of the double pendulum, [m1, m2], units=[kg]

lengtharray_like, optional

shape=(2,), dtype=float, default=[0.3, 0.2] link lengths of the double pendulum, [l1, l2], units=[m]

comarray_like, optional

shape=(2,), dtype=float, default=[0.3, 0.3] center of mass lengths of the double pendulum links [r1, r2], units=[m]

dampingarray_like, optional

shape=(2,), dtype=float, default=[0.1, 0.1] damping coefficients of the double pendulum actuators [b1, b2], units=[kg*m/s]

gravityfloat, optional

default=9.81 gravity acceleration (pointing downwards), units=[m/s²]

coulomb_fricarray_like, optional

shape=(2,), dtype=float, default=[0.0, 0.0] coulomb friction coefficients for the double pendulum actuators [cf1, cf2], units=[Nm]

inertiaarray_like, optional

shape=(2,), dtype=float, default=[None, None] inertia of the double pendulum links [I1, I2], units=[kg*m²] if entry is None defaults to point mass m*l² inertia for the entry

torque_limitarray_like, optional

shape=(2,), dtype=float, default=[0.0, 1.0] torque limit of the motors [tl1, tl2], units=[Nm, Nm]

model_parsmodel_parameters object, optional

object of the model_parameters class, default=None Can be used to set all model parameters above If provided, the model_pars parameters overwrite the other provided parameters

get_control_output_(x, t=None)

The function to compute the control input for the double pendulum’s actuator(s).

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s]

tfloat, optional

time, unit=[s] (Default value=None)

Returns:
array_like

shape=(2,), dtype=float actuation input/motor torque, order=[u1, u2], units=[Nm]

init_()

Initalize the controller.

save_(save_dir)

Save the energy trajectory to file.

Parameters:
pathstring or path object

directory where the parameters will be saved

set_cost_matrices(Q, R)

Set the Q and R matrices directly.

Parameters:
Qnumpy_array

shape=(4,4) Q-matrix describing quadratic state cost

Rnumpy_array

shape=(2,2) R-matrix describing quadratic control cost

set_cost_parameters(p1p1_cost=1.0, p2p2_cost=1.0, v1v1_cost=1.0, v2v2_cost=1.0, p1p2_cost=0.0, v1v2_cost=0.0, p1v1_cost=0.0, p1v2_cost=0.0, p2v1_cost=0.0, p2v2_cost=0.0, u1u1_cost=0.01, u2u2_cost=0.01, u1u2_cost=0.0)

set_cost_parameters. Parameters of Q and R matrices. The parameters are

Q = ((p1p1, p1p2, p1v1, p1v2),

(p1p2, p2p2, p2v1, p2v2), (p1v1, p2v1, v1v1, v1v2), (p1v2, p2v2, v1v2, v2v2))

R = ((u1u1, u1u2),

(u1u2, u2u2))

Parameters:
p1p1_costfloat

p1p1_cost (Default value=1.)

p2p2_costfloat

p2p2_cost (Default value=1.)

v1v1_costfloat

v1v1_cost (Default value=1.)

v2v2_costfloat

v2v2_cost (Default value=0.)

p1p2_costfloat

p1p2_cost (Default value=0.)

v1v2_costfloat

v1v2_cost (Default value=0.)

p1v1_costfloat

p1v1_cost (Default value=0.)

p1v2_costfloat

p1v2_cost (Default value=0.)

p2v1_costfloat

p2v1_cost (Default value=0.)

p2v2_costfloat

p2v2_cost (Default value=0.)

u1u1_costfloat

u1u1_cost (Default value=0.01)

u2u2_costfloat

u2u2_cost (Default value=0.01)

u1u2_costfloat

u1u2_cost (Default value=0.)

set_cost_parameters_(pars=[1.0, 1.0, 1.0, 1.0, 1.0])

Set the diagonal parameters of Q and R matrices with a list.

The parameters are Q = ((pars[0], 0, 0, 0),

(0, pars[1], 0, 0), (0, 0, pars[2], 0), (0, 0, 0, pars[3]))

R = ((pars[4], 0)

(0, pars[4]))

Parameters:
parslist

shape=(5,), dtype=float (Default value=[1., 1., 1., 1., 1.])

set_goal(x=[3.141592653589793, 0.0, 0.0, 0.0])

set_goal. Set goal for the controller.

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s] (Default value=[np.pi, 0., 0., 0.])

set_parameters(failure_value=nan, cost_to_go_cut=15.0)

set_parameters. Set parameters for this controller.

Parameters:
failure_valuefloat

if the cost-to-go exceeds cost_to_go_cut this value is retured as torque (Default value=np.nan)

cost_to_go_cutfloat

if the cost-to-go exceeds this values the controller returns failure_value (Default value=15.)

double_pendulum.controller.lqr.lqr_controller_drake

class double_pendulum.controller.lqr.lqr_controller_drake.LQRController(urdf_path, model_pars, robot='acrobot', torque_limit=[0.0, 1.0], save_dir='.')

Bases: AbstractController

LQRController. Controller which uses LQR to stabilize a (unstable) fixpoint. Uses drake LQR.

Parameters:
urdf_pathstring or path object

path to urdf file

model_parsmodel_parameters object

object of the model_parameters class

robotstring
robot which is used, Options:

  • acrobot

  • pendubot

torque_limitarray_like, optional

shape=(2,), dtype=float, default=[0.0, 1.0] torque limit of the motors [tl1, tl2], units=[Nm, Nm]

save_dirstring

path to directory where log data can be stored (necessary for temporary generated urdf) (Default value=”.”)

get_control_output_(x, t=None)

The function to compute the control input for the double pendulum’s actuator(s).

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s]

tfloat, optional

time, unit=[s] (Default value=None)

Returns:
array_like

shape=(2,), dtype=float actuation input/motor torque, order=[u1, u2], units=[Nm]

init_()

Initalize the controller.

save_(save_dir)

Save controller parameters

Parameters:
save_dirstring or path object

directory where the parameters will be saved

set_cost_matrices(Q, R)

Set the Q and R matrices directly.

Parameters:
Qnumpy_array

shape=(4,4) Q-matrix describing quadratic state cost

Rnumpy_array

shape=(2,2) R-matrix describing quadratic control cost

set_cost_parameters(p1p1_cost=1.0, p2p2_cost=1.0, v1v1_cost=1.0, v2v2_cost=1.0, p1p2_cost=0.0, v1v2_cost=0.0, p1v1_cost=0.0, p1v2_cost=0.0, p2v1_cost=0.0, p2v2_cost=0.0, u1u1_cost=0.01)

set_cost_parameters. Parameters of Q and R matrices. The parameters are

Q = ((p1p1, p1p2, p1v1, p1v2),

(p1p2, p2p2, p2v1, p2v2), (p1v1, p2v1, v1v1, v1v2), (p1v2, p2v2, v1v2, v2v2))

R = ((u1u1))

Parameters:
p1p1_costfloat

p1p1_cost (Default value=1.)

p2p2_costfloat

p2p2_cost (Default value=1.)

v1v1_costfloat

v1v1_cost (Default value=1.)

v2v2_costfloat

v2v2_cost (Default value=0.)

p1p2_costfloat

p1p2_cost (Default value=0.)

v1v2_costfloat

v1v2_cost (Default value=0.)

p1v1_costfloat

p1v1_cost (Default value=0.)

p1v2_costfloat

p1v2_cost (Default value=0.)

p2v1_costfloat

p2v1_cost (Default value=0.)

p2v2_costfloat

p2v2_cost (Default value=0.)

u1u1_costfloat

u1u1_cost (Default value=0.01)

set_cost_parameters_(pars=[1.0, 1.0, 1.0, 1.0, 1.0])

Set the diagonal parameters of Q and R matrices with a list.

The parameters are Q = ((pars[0], 0, 0, 0),

(0, pars[1], 0, 0), (0, 0, pars[2], 0), (0, 0, 0, pars[3]))

R = ((pars[4]))

Parameters:
parslist

shape=(5,), dtype=float (Default value=[1., 1., 1., 1., 1.])

set_goal(x=[3.141592653589793, 0.0, 0.0, 0.0])

set_goal. Set goal for the controller.

Parameters:
xarray_like, shape=(4,), dtype=float,

state of the double pendulum, order=[angle1, angle2, velocity1, velocity2], units=[rad, rad, rad/s, rad/s] (Default value=[np.pi, 0., 0., 0.])