double_pendulum.controller.ilqr
Submodules
double_pendulum.controller.ilqr.ilqr_mpc_cpp
- class double_pendulum.controller.ilqr.ilqr_mpc_cpp.ILQRMPCCPPController(mass=[0.608, 0.63], length=[0.3, 0.2], com=[0.275, 0.166], damping=[0.081, 0.0], coulomb_fric=[0.093, 0.186], gravity=9.81, inertia=[0.05472, 0.02522], torque_limit=[0.0, 6.0], model_pars=None)
Bases:
AbstractControllerILQR Controller
iLQR MPC controller This controller uses the python bindings of the Cpp ilqr optimizer.
- Parameters:
- massarray_like, optional
shape=(2,), dtype=float, default=[0.608, 0.630] 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.275, 0.166] center of mass lengths of the double pendulum links [r1, r2], units=[m]
- dampingarray_like, optional
shape=(2,), dtype=float, default=[0.081, 0.0] 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.093, 0.186] 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, 6.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 (Default value=None)
- compute_init_traj(N=1000, dt=0.005, max_iter=100, regu_init=100, max_regu=10000.0, min_regu=0.01, break_cost_redu=1e-06, sCu=[0.005, 0.005], sCp=[0.0, 0.0], sCv=[0.0, 0.0], sCen=0.0, fCp=[1000.0, 1000.0], fCv=[10.0, 10.0], fCen=0.0, integrator='runge_kutta')
Compute an initial trajectory.
- Parameters:
- Nint
number of timesteps for horizon (Default value = 1000)
- dtfloat
timestep for horizon (Default value = 0.005)
- max_iterint
maximum of optimization iterations per step (Default value = 1)
- regu_initfloat
inital regularization (Default value = 1.)
- max_regufloat
maximum regularization (Default value = 10000.)
- min_regufloat
minimum regularization (Default value = 0.01)
- break_cost_redufloat
- Stop the optimization at this cost reduction.
(Default value = 1e-6)
- sCulist
shape=(2,), dtype=float stage cost weights for control input (Default value = [0.005, 0.005])
- sCplist
shape=(2,), dtype=float stage cost weights for position error (Default value = [0.,0.])
- sCvlist
shape=(2,), dtype=float stage cost weights for velocity error (Default value = [0., 0.])
- sCenfloat
stage cost weight for energy error (Default value = 0.)
- fCplist
shape=(2,), dtype=float final cost weights for position error
(Default value = [1000., 1000.])
- fCvlist
shape=(2,), dtype=float final cost weights for velocity error
(Default value = [10., 10.])
- fCenfloat
final cost weight for energy error (Default value = 0.)
- integratorstring
string determining the integration method “euler” : Euler integrator “runge_kutta” : Runge Kutta integrator
(Default value = “runge_kutta”)
- 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]
- get_forecast()
Get the MPC forecast trajectory as planned by the controller. numpy_array
time points, unit=[s] shape=(N,)
- numpy_array
shape=(N, 4) states, units=[rad, rad, rad/s, rad/s] order=[angle1, angle2, velocity1, velocity2]
- numpy_array
shape=(N, 2) actuations/motor torques order=[u1, u2], units=[Nm]
- get_init_trajectory()
Get the initial (reference) trajectory used by the controller.
- Returns:
- numpy_array
time points, unit=[s] shape=(N,)
- numpy_array
shape=(N, 4) states, units=[rad, rad, rad/s, rad/s] order=[angle1, angle2, velocity1, velocity2]
- numpy_array
shape=(N, 2) actuations/motor torques order=[u1, u2], units=[Nm]
- init_()
Initalize the controller.
- load_init_traj(csv_path, num_break=40, poly_degree=3)
Load initial trajectory from csv file.
- Parameters:
- csv_pathstring or path object
path to csv file where the trajectory is stored. csv file should use standarf formatting used in this repo.
- num_breakint
number of break points used for interpolation (Default value = 40)
- poly_degreeint
degree of polynomials used for interpolation (Default value = 3)
- save_(save_dir)
Save controller parameters
- Parameters:
- save_dirstring or path object
directory where the parameters will be saved
- set_cost_parameters(sCu=[0.005, 0.005], sCp=[0.0, 0.0], sCv=[0.0, 0.0], sCen=0.0, fCp=[1000.0, 1000.0], fCv=[10.0, 10.0], fCen=0.0)
Set cost parameters used for optimization.
- Parameters:
- sCulist
shape=(2,), dtype=float stage cost weights for control input (Default value = [0.005, 0.005])
- sCplist
shape=(2,), dtype=float stage cost weights for position error (Default value = [0.,0.])
- sCvlist
shape=(2,), dtype=float stage cost weights for velocity error (Default value = [0., 0.])
- sCenfloat
stage cost weight for energy error (Default value = 0.)
- fCplist
shape=(2,), dtype=float final cost weights for position error
(Default value = [1000., 1000.])
- fCvlist
shape=(2,), dtype=float final cost weights for velocity error
(Default value = [10., 10.])
- fCenfloat
final cost weight for energy error (Default value = 0.)
- set_cost_parameters_(pars=[0.005, 0.0, 0.0, 0.0, 0.0, 1000.0, 1000.0, 10.0, 10.0])
Set cost parameters used for optimization in form of a list. (used for parameter optimization)
- Parameters:
- parslist
list order=[sCu1, sCp1, sCp2, sCv1, sCv2, fCp1, fCp2, fCv1, fCv2] energy costs are set to 0. (Default value = [0.005, 0., 0., 0., 0., 1000., 1000., 10., 10.])
- set_final_cost_parameters(sCu=[0.005, 0.005], sCp=[0.0, 0.0], sCv=[0.0, 0.0], sCen=0.0, fCp=[1000.0, 1000.0], fCv=[10.0, 10.0], fCen=0.0)
Set cost parameters used for optimization for the stabilization of the final state of the trajectory.
- Parameters:
- sCulist
shape=(2,), dtype=float stage cost weights for control input (Default value = [0.005, 0.005])
- sCplist
shape=(2,), dtype=float stage cost weights for position error (Default value = [0.,0.])
- sCvlist
shape=(2,), dtype=float stage cost weights for velocity error (Default value = [0., 0.])
- sCenfloat
stage cost weight for energy error (Default value = 0.)
- fCplist
shape=(2,), dtype=float final cost weights for position error
(Default value = [1000., 1000.])
- fCvlist
shape=(2,), dtype=float final cost weights for velocity error
(Default value = [10., 10.])
- fCenfloat
final cost weight for energy error (Default value = 0.)
- 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(N=1000, dt=0.005, max_iter=1, regu_init=1.0, max_regu=10000.0, min_regu=0.01, break_cost_redu=1e-06, integrator='runge_kutta', trajectory_stabilization=True, shifting=1)
- Parameters:
- Nint
number of timesteps for horizon (Default value = 1000)
- dtfloat
timestep for horizon (Default value = 0.005)
- max_iterint
maximum of optimization iterations per step (Default value = 1)
- regu_initfloat
inital regularization (Default value = 1.)
- max_regufloat
maximum regularization (Default value = 10000.)
- min_regufloat
minimum regularization (Default value = 0.01)
- break_cost_redufloat
- Stop the optimization at this cost reduction.
(Default value = 1e-6)
- integratorstring
string determining the integration method “euler” : Euler integrator “runge_kutta” : Runge Kutta integrator
(Default value = “runge_kutta”)
- trajectory_stabilizationbool
Whether to stabilize a trajectory or optimize freely (Default value = True)
- shiftingint
The trajectory will be shifted this number of timesteps to warm start the optimization at the next timestep (Default value = 1)
- set_start(x=[0.0, 0.0, 0.0, 0.0])
Set start state 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=[0., 0., 0., 0.])
double_pendulum.controller.ilqr.paropt
- class double_pendulum.controller.ilqr.paropt.ilqrmpc_swingup_loss(bounds, loss_weights, start, goal, csv_path)
Bases:
object- init()
- rescale_pars(pars)
- set_model_parameters(mass=[0.608, 0.63], length=[0.3, 0.2], com=[0.275, 0.166], damping=[0.081, 0.0], coulomb_fric=[0.093, 0.186], gravity=9.81, inertia=[0.05472, 0.02522], torque_limit=[0.0, 6.0], model_pars=None)
- set_parameters(N=1000, dt=0.005, max_iter=100, regu_init=100, max_regu=10000.0, min_regu=0.01, break_cost_redu=1e-06, integrator='runge_kutta')
- unscale_pars(pars)