double_pendulum.controller.tvlqr
Submodules
double_pendulum.controller.tvlqr.tvlqr_controller
- class double_pendulum.controller.tvlqr.tvlqr_controller.TVLQRController(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, csv_path='', num_break=40)
Bases:
AbstractController
Controller to stabilize a trajectory with TVLQR
- 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.2] 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 (Default value=None)
- csv_pathstring or path object
path to csv file where the trajectory is stored. csv file should use standarf formatting used in this repo. If T, X, or U are provided they are preferred. (Default value=””)
- num_breakint
number of break points used for interpolation (Default value = 40) (Default value=100)
- get_control_output_(x, t)
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_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.
- save_(save_dir)
Save the energy trajectory to file.
- Parameters:
- pathstring or path object
directory where the parameters will be saved
- set_cost_parameters(Q=array([[4., 0., 0., 0.], [0., 4., 0., 0.], [0., 0., 0.1, 0.], [0., 0., 0., 0.1]]), R=array([[2.]]), Qf=array([[4., 0., 0., 0.], [0., 4., 0., 0.], [0., 0., 0.1, 0.], [0., 0., 0., 0.1]]))
Set the cost matrices Q, R and Qf. (Qf for the final stabilization)
- Parameters:
- Qnumpy_array
shape=(4,4) Q-matrix describing quadratic state cost (Default value=np.diag([4., 4., 0.1, 0.1]))
- Rnumpy_array
shape=(2,2) R-matrix describing quadratic control cost (Default value=2*np.eye(1))
- Qfnumpy_array
shape=(4,4) Q-matrix describing quadratic state cost for the final point stabilization (Default value=np.diag([4., 4., 0.1, 0.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.])
double_pendulum.controller.tvlqr.tvlqr_controller_drake
- class double_pendulum.controller.tvlqr.tvlqr_controller_drake.TVLQRController(csv_path, urdf_path, model_pars, torque_limit=[0.0, 3.0], robot='acrobot', save_dir='.')
Bases:
AbstractController
Controller to stabilize a trajectory with TVLQR Uses the TVLQR controller from drake.
- 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. If T, X, or U are provided they are preferred. (Default value=””)
- urdf_pathstring or path object
path to urdf file
- model_parsmodel_parameters object
object of the model_parameters class
- torque_limitarray_like, optional
shape=(2,), dtype=float, default=[0.0, 3.0] torque limit of the motors [tl1, tl2], units=[Nm, Nm]
- robotstring
- robot which is used, Options:
“acrobot”
“pendubot”
- save_dirstring
path to directory where log data can be stored (necessary for temporary generated urdf) (Default value=”.”)
- get_control_output_(x, t)
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_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.
- save_(save_dir)
Save the energy trajectory to file.
- Parameters:
- pathstring or path object
directory where the parameters will be saved
- set_cost_parameters(Q=array([[4., 0., 0., 0.], [0., 4., 0., 0.], [0., 0., 0.1, 0.], [0., 0., 0., 0.1]]), R=array([[2.]]), Qf=array([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.]]))
Set the cost matrices Q, R and Qf. (Qf for the final stabilization)
- Parameters:
- Qnumpy_array
shape=(4,4) Q-matrix describing quadratic state cost (Default value=np.diag([4., 4., 0.1, 0.1]))
- Rnumpy_array
shape=(2,2) R-matrix describing quadratic control cost (Default value=2*np.eye(1))
- Qfnumpy_array
shape=(4,4) Q-matrix describing quadratic state cost for the final point stabilization (Default value=np.zeros((4, 4)))