asif_func.hpp File Reference

Functions for active Set Invariance (ASI) filtering on Lie groups. More...

#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/concepts/lie_group.hpp>
#include <smooth/diff.hpp>
#include <algorithm>
#include <limits>
#include <utility>
#include "common.hpp"
#include "qp.hpp"

Include dependency graph for asif_func.hpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	smooth::feedback::ASIFProblem< X, U >
	Active set invariance problem definition. More...
struct	smooth::feedback::ASIFtoQPParams
	Parameters for asif_to_qp. More...

Functions
template<LieGroup X, Manifold U> requires (Dof<X> > 0 && Dof<U> > 0)
void	smooth::feedback::asif_to_qp_allocate (QuadraticProgram<-1, -1, double > &qp, std::size_t K, std::size_t nu_ineq, std::size_t nh)
	Allocate QP matrices (part 1 of asif_to_qp()) More...
template<LieGroup X, Manifold U, diff::Type DT = diff::Type::Default> requires (Dof<X> > 0 && Dof<U> > 0)
void	smooth::feedback::asif_to_qp_update (QuadraticProgram<-1, -1, double > &qp, const ASIFProblem< X, U > &pbm, const ASIFtoQPParams &prm, auto &&f, auto &&h, auto &&bu)
	Fill QP matrices (part 2 of asif_to_qp()) More...
template<LieGroup X, Manifold U, diff::Type DT = diff::Type::Default>
QuadraticProgram<-1, -1, double >	smooth::feedback::asif_to_qp (const ASIFProblem< X, U > &pbm, const ASIFtoQPParams &prm, auto &&f, auto &&h, auto &&bu)
	Convert an ASIFProblem to a QuadraticProgram. More...

Detailed Description

Functions for active Set Invariance (ASI) filtering on Lie groups.

Definition in file asif_func.hpp.

Function Documentation

asif_to_qp()

template<LieGroup X, Manifold U, diff::Type DT = diff::Type::Default>

QuadraticProgram<-1, -1, double > smooth::feedback::asif_to_qp	(	const ASIFProblem< X, U > &	pbm,
		const ASIFtoQPParams &	prm,
		auto &&	f,
		auto &&	h,
		auto &&	bu
	)

Convert an ASIFProblem to a QuadraticProgram.

The objective is to impose constraints on the current input \( u \) of a system \( \mathrm{d}^r x_t = f(x, u) \) s.t.

\[ \frac{\mathrm{d}}{\mathrm{d}t} h(\phi(t; x_0, bu(\cdot))) \geq \alpha h(\phi(t; x_0, bu(\cdot))) \]

which enforces forward invariance of the set \( \{ x : h(t, x) \geq 0 \} \) along the backup trajectory \( bu \). The constraint is enforced at \(K\) look-ahead time steps \( t_k = k \tau\) for \(k = 0, \ldots, K \).

This function encodes the problem as a QuadraticProgram that solves

\[ \begin{cases} \min_{\mu} & \left\| \mu \right\|^2 \\ \text{s.t.} & \text{constraint above holds for } u = u_{des} + \mu \end{cases} \]

A solution \( \mu^* \) to the QuadraticProgram corresponds to an input \( u_{des} \oplus \mu^* \) applied to the system.

Template Parameters

X	state LieGroup type \(\mathbb{X}\)
U	input Manifold type \(\mathbb{X}\)

Parameters

pbm	problem definition
prm	algorithm parameters
f	system model \(f : \mathbb{R} \times \mathbb{X} \times \mathbb{U} \rightarrow \mathbb{R}^{\dim \mathfrak g}\) s.t. \( \mathrm{d}^r x_t = f(t, x, u) \)
h	safe set \(h : \mathbb{R} \times \mathbb{X} \rightarrow \mathbb{R}^{n_h}\) s.t. \( S(t) = \{ h(t, x) \geq 0 \} \) denotes the safe set at time \( t \)
bu	backup controller \(ub : \mathbb{R} \times \mathbb{X} \rightarrow \mathbb{U} \)

Note

The algorithm relies on automatic differentiation. The following supplied functions must be differentiable (i.e. be templated on the scalar type if an automatic differentiation method is selected):

• f differentiable w.r.t. x and u
• h differentiable w.r.t. t and x
• bu differentiable w.r.t. x

Returns

QuadraticProgram modeling the ASIF filtering problem

Definition at line 246 of file asif_func.hpp.

asif_to_qp_allocate()

template<LieGroup X, Manifold U>
requires (Dof<X> > 0 && Dof<U> > 0)

void smooth::feedback::asif_to_qp_allocate	(	QuadraticProgram<-1, -1, double > &	qp,
		std::size_t	K,
		std::size_t	nu_ineq,
		std::size_t	nh
	)

Allocate QP matrices (part 1 of asif_to_qp())

Parameters

[out]	qp	allocated QP with zero matrices
[in]	K	number of constraint instances
[in]	nu_ineq	number in inequalities in input constraint
[in]	nh	number of barrier constraints

Definition at line 80 of file asif_func.hpp.

asif_to_qp_update()

template<LieGroup X, Manifold U, diff::Type DT = diff::Type::Default>
requires (Dof<X> > 0 && Dof<U> > 0)

void smooth::feedback::asif_to_qp_update	(	QuadraticProgram<-1, -1, double > &	qp,
		const ASIFProblem< X, U > &	pbm,
		const ASIFtoQPParams &	prm,
		auto &&	f,
		auto &&	h,
		auto &&	bu
	)

Fill QP matrices (part 2 of asif_to_qp())

Note that the (dense) QP matrices must be pre-allocated and filled with zeros.

Definition at line 106 of file asif_func.hpp.