simple_pendulum.controllers.tvlqr.roa

Submodules

simple_pendulum.controllers.tvlqr.roa.plot

simple_pendulum.controllers.tvlqr.roa.plot.TVrhoVerification(pendulum, controller, funnel_path, traj_path, nSimulations, ver_idx)

Function to verify the time-variant RoA estimation. This implementation permitts also to choose which knot has to be tested. Furthermore the a 3d funnel plot has been implemented.

Parameters:

pendulum: simple_pendulum.model.pendulum_plant: configured pendulum plant object
controller: simple_pendulum.controllers.tvlqr.tvlqr: configured tvlqr controller object
x0_t: np.array: pre-computed nominal trajectory
time: np.array: time array related to the nominal trajectory
nSimulations: int: number of simulations for the verification
ver_idx: int: knot point to be verified

simple_pendulum.controllers.tvlqr.roa.plot.funnel2DComparison(csv_pathFunnelSos, csv_pathFunnelProb, traj_path)

simple_pendulum.controllers.tvlqr.roa.plot.get_ellipse_params(rho, M): Returns ellipse params (excl center point)

simple_pendulum.controllers.tvlqr.roa.plot.get_ellipse_patch(px, py, rho, M, alpha_val=1, linec='red', facec='none', linest='solid'): return an ellipse patch

simple_pendulum.controllers.tvlqr.roa.plot.plotFirstLastEllipses(funnel_path, traj_path)

simple_pendulum.controllers.tvlqr.roa.plot.plotFunnel(funnel_path, traj_path, ax=None): Function to draw a continue 2d funnel plot. This implementation makes use of the convex hull concept as done in the MATLAB code of the Robot Locomotion Group (https://groups.csail.mit.edu/locomotion/software.html). :param rho: array that contains the estimated rho value for all the knot points :type rho: np.array :param S: array of matrices that define ellipses in all the knot points, from tvlqr controller. :type S: np.array :param x0: pre-computed nominal trajectory :type x0: np.array :param time: time array related to the nominal trajectory :type time: np.array

simple_pendulum.controllers.tvlqr.roa.plot.plotFunnel3d(csv_path, traj_path, ax): Function to draw a discrete 3d funnel plot. Basically we are plotting a 3d ellipse patch in each knot point. :param rho: array that contains the estimated rho value for all the knot points :type rho: np.array :param S: array of matrices that define ellipses in all the knot points, from tvlqr controller. :type S: np.array :param x0: pre-computed nominal trajectory :type x0: np.array :param time: time array related to the nominal trajectory :type time: np.array :param ax: axes of the plot where we want to add the 3d funnel plot, useful in the verification function. :type ax: matplotlib.axes

simple_pendulum.controllers.tvlqr.roa.plot.plotRhoEvolution(funnel_path, traj_path)

simple_pendulum.controllers.tvlqr.roa.plot.plot_ellipse(px, py, rho, M, save_to=None, show=True)

simple_pendulum.controllers.tvlqr.roa.plot.rhoComparison(csv_pathFunnelSos, csv_pathFunnelProb)

simple_pendulum.controllers.tvlqr.roa.probabilistic

simple_pendulum.controllers.tvlqr.roa.probabilistic.TVprobRhoComputation(pendulum, controller, x0_t, time, N, nSimulations, rhof)

Function to perform the time-variant RoA estimation. This implementation has been inpired by “Feedback-Motion-Planning with Simulation-Based LQR-Trees” by Philipp Reist, Pascal V. Preiswerk and Russ Tedrake (https://groups.csail.mit.edu/robotics-center/public_papers/Reist15.pdf).

Parameters:

pendulum: simple_pendulum.model.pendulum_plant: configured pendulum plant object
controller: simple_pendulum.controllers.tvlqr.tvlqr: configured tvlqr controller object
x0_t: np.array: pre-computed nominal trajectory
time: np.array: time array related to the nominal trajectory
N: int: number of considered knot points
nSimulations: int: max number of simulations for each ellipse estimation
rhof: float: final rho value, from the time-invariant RoA estimation

Returns:

rhonp.array: array that contains the estimated rho value for all the knot points
S: np.array: array that contains the S matrix in each knot point

simple_pendulum.controllers.tvlqr.roa.sos

simple_pendulum.controllers.tvlqr.roa.sos.TVsosRhoComputation(pendulum, controller, time, N, rhof)

Bilinear alternationturn used for the SOS funnel estimation.

Parameters:

pendulum: simple_pendulum.model.pendulum_plant: configured pendulum plant object
controller: simple_pendulum.controllers.tvlqr.tvlqr: configured tvlqr controller object
time: np.array: time array related to the nominal trajectory
N: int: number of considered knot points
rhof: float: final rho value, from the time-invariant RoA estimation

Returns:

rho_tnp.array: array that contains the estimated rho value for all the knot points
S: np.array: array that contains the S matrix in each knot point

simple_pendulum.controllers.tvlqr.roa.utils

simple_pendulum.controllers.tvlqr.roa.utils.TVmultSearch(pendulum, controller, knot, time, rho_t)

Multiplier step of the bilinear alternationturn used in the SOS funnel estimation.

Parameters:

pendulum: simple_pendulum.model.pendulum_plant: configured pendulum plant object
controller: simple_pendulum.controllers.tvlqr.tvlqr: configured tvlqr controller object
knot: int: number of considered knot point
time: np.array: time array related to the nominal trajectory
rho_t: np.array: array that contains the evolving estimation of the rho values for each knot point

Returns:

failboolean: gives info about the correctness of the optimization problem
h_map: Dict[pydrake.symbolic.Monomial, pydrake.symbolic.Expression]: map of the coefficients of the multiplier obtained from the multiplier step
eps: float: optimal cost of the optimization problem that can be useful in the V step

simple_pendulum.controllers.tvlqr.roa.utils.TVrhoSearch(pendulum, controller, knot, time, h_map, rho_t)

V step of the bilinear alternationturn used in the SOS funnel estimation.

Parameters:

pendulum: simple_pendulum.model.pendulum_plant: configured pendulum plant object
controller: simple_pendulum.controllers.tvlqr.tvlqr: configured tvlqr controller object
knot: int: number of considered knot point
time: np.array: time array related to the nominal trajectory
h_map: Dict[pydrake.symbolic.Monomial, pydrake.symbolic.Expression]: map of the coefficients of the multiplier obtained from the multiplier step
rho_t: np.array: array that contains the evolving estimation of the rho values for each knot point

Returns:

failboolean: gives info about the correctness of the optimization problem
rho_opt: float: optimized rho value for this knot point

simple_pendulum.controllers.tvlqr.roa.utils.direct_sphere(d, r_i=0, r_o=1)

Direct Sampling from the d Ball based on Krauth, Werner. Statistical Mechanics: Algorithms and Computations. Oxford Master Series in Physics 13. Oxford: Oxford University Press, 2006. page 42

Parameters:

dint: dimension of the ball
r_iint, optional: inner radius, by default 0
r_oint, optional: outer radius, by default 1

Returns:

np.array: random vector directly sampled from the solid d Ball

simple_pendulum.controllers.tvlqr.roa.utils.getEllipseContour(S, rho, xg)

Returns a certain number(nSamples) of sampled states from the contour of a given ellipse.

Parameters:

Snp.array: Matrix S that define one ellipse
rhonp.array: rho value that define one ellipse
xgnp.array: center of the ellipse

Returns:

cnp.array: random vector of states from the contour of the ellipse

simple_pendulum.controllers.tvlqr.roa.utils.projectedEllipseFromCostToGo(s0Idx, s1Idx, rho, M): Returns ellipses in the plane defined by the states matching the indices s0Idx and s1Idx for funnel plotting.

simple_pendulum.controllers.tvlqr.roa.utils.quad_form(M, x): Helper function to compute quadratic forms such as x^TMx

simple_pendulum.controllers.tvlqr.roa.utils.sample_from_ellipsoid(M, rho, r_i=0, r_o=1)

sample directly from the ellipsoid defined by xT M x.

Parameters:

Mnp.array: Matrix M such that xT M x leq rho defines the hyperellipsoid to sample from
rhofloat: rho such that xT M x leq rho defines the hyperellipsoid to sample from
r_iint, optional: inner radius, by default 0
r_oint, optional: outer radius, by default 1

Returns:

np.array: random vector from within the hyperellipsoid