Receding horizon ergodic trajectory optimization. More...

#include <ergodic_control.hpp>

Public Member Functions
void	addStateMemory (const vec &x)
	Add the robot's state to memory. More...

void	configTarget (const GridMap &grid)
	Compose target distribution grid and spatial coefficients. More...

vec	control (const GridMap &grid, const vec &x)
	Update the control signal. More...

	ErgodicControl (const ModelT &model, const Collision &collision, double dt, double horizon, double resolution, double exploration_weight, unsigned int num_basis, unsigned int buffer_size, unsigned int batch_size, const mat &Rinv, const vec &umin, const vec &umax)
	Constructor. More...

mat	optTraj () const
	Return optimized trajectory. More...

nav_msgs::Path	path (const std::string &map_frame_id) const
	Return optimized trajectory. More...

void	setTarget (const Target &target)
	Set the target distribution. More...

double	timeStep () const
	return time step More...

Private Member Functions
mat	gradBarrier (const mat &xt)
	Gradient of the barrier function. More...

mat	gradErgodicMetric (const vec &ck, const mat &xt)
	Compose the gradient of the ergodic metric. More...

void	updateControl (const mat &xt, const mat &rhot)
	Update the control signal. More...

Private Attributes
Basis	basis_

ReplayBuffer	buffer_

Collision	collision_

double	dt_

double	expl_weight_

double	horizon_

vec	map_pos_

ModelT	model_

vec	phik_

vec	pose_

double	resolution_

CoStateFunc	rhodot_

vec	rhoT_

mat	Rinv_

RungeKutta	rk4_

unsigned int	steps_

Target	target_

vec	umax_

vec	umin_

mat	ut_

Detailed Description

template<class ModelT>
class ergodic_exploration::ErgodicControl< ModelT >

Receding horizon ergodic trajectory optimization.

Definition at line 73 of file ergodic_control.hpp.

Constructor & Destructor Documentation

◆ ErgodicControl()

template<class ModelT >

ergodic_exploration::ErgodicControl< ModelT >::ErgodicControl	(	const ModelT &	model,
		const Collision &	collision,
		double	dt,
		double	horizon,
		double	resolution,
		double	exploration_weight,
		unsigned int	num_basis,
		unsigned int	buffer_size,
		unsigned int	batch_size,
		const mat &	Rinv,
		const vec &	umin,
		const vec &	umax
	)

Constructor.

Parameters

model	- robot's dynamic model
collision	- collision detector
dt	- time step in integration
horizon	- control horizon
resolution	- discretization of target grid
num_basis	- number of cosine basis functions per dimension
buffer_size	- number of past states in memory
batch_size	- number of states sampled from memory
Rinv	- inverse of a positive definite matrix that penalizes controls
umin	- body twist lower limits
umax	- body twist upper limits

Definition at line 188 of file ergodic_control.hpp.

Member Function Documentation

◆ addStateMemory()

template<class ModelT >

void ergodic_exploration::ErgodicControl< ModelT >::addStateMemory ( const vec & x )

Add the robot's state to memory.

Parameters

x	- current state [x, y, theta]

Definition at line 345 of file ergodic_control.hpp.

◆ configTarget()

template<class ModelT >

void ergodic_exploration::ErgodicControl< ModelT >::configTarget ( const GridMap & grid )

Compose target distribution grid and spatial coefficients.

Parameters

grid	- grid map

Updates the spatial coefficients if the map is larger than before

Definition at line 363 of file ergodic_control.hpp.

◆ control()

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::control	(	const GridMap &	grid,
		const vec &	x
	)

Update the control signal.

Parameters

grid	- grid map
x	- current state [x, y, theta]

Returns: first twist in the updated control signal [vx, vy, w]

Definition at line 225 of file ergodic_control.hpp.

◆ gradBarrier()

template<class ModelT >

mat ergodic_exploration::ErgodicControl< ModelT >::gradBarrier ( const mat & xt )

private

Gradient of the barrier function.

Parameters

xt	- forward simulated trajectory in fourier domain

Returns: barrier funcion gradients

Updates a matrix conatining the barrier function derivatve for each state in xt. The barrier function tries to keep the robot's predicted trajectory within the fourier domain.

Definition at line 454 of file ergodic_control.hpp.

◆ gradErgodicMetric()

template<class ModelT >

mat ergodic_exploration::ErgodicControl< ModelT >::gradErgodicMetric	(	const vec &	ck,
		const mat &	xt
	)

private

Compose the gradient of the ergodic metric.

Parameters

ck	- trajectory fourier coefficients
xt	- forward simulated trajectory in fourier domain

Returns: ergodic measure gradients

Updates a matrix containing ergodic metric gradient for each state in xt

Definition at line 419 of file ergodic_control.hpp.

◆ optTraj()

template<class ModelT >

mat ergodic_exploration::ErgodicControl< ModelT >::optTraj

Return optimized trajectory.

Definition at line 314 of file ergodic_control.hpp.

◆ path()

template<class ModelT >

nav_msgs::Path ergodic_exploration::ErgodicControl< ModelT >::path ( const std::string & map_frame_id ) const

Return optimized trajectory.

Definition at line 320 of file ergodic_control.hpp.

◆ setTarget()

template<class ModelT >

void ergodic_exploration::ErgodicControl< ModelT >::setTarget ( const Target & target )

Set the target distribution.

Parameters

target - target distribution

Definition at line 357 of file ergodic_control.hpp.

◆ timeStep()

template<class ModelT >

double ergodic_exploration::ErgodicControl< ModelT >::timeStep

return time step

Definition at line 351 of file ergodic_control.hpp.

◆ updateControl()

template<class ModelT >

void ergodic_exploration::ErgodicControl< ModelT >::updateControl	(	const mat &	xt,
		const mat &	rhot
	)

private

Update the control signal.

Parameters

xt	- forward simulated trajectory in fourier domain
rhot	- co-state variable solution

rhot is assumed to already be sorted from [t0 tf] with t0 at index 0

Definition at line 439 of file ergodic_control.hpp.

Member Data Documentation

◆ basis_

template<class ModelT >

Basis ergodic_exploration::ErgodicControl< ModelT >::basis_

private

Definition at line 180 of file ergodic_control.hpp.

◆ buffer_

template<class ModelT >

ReplayBuffer ergodic_exploration::ErgodicControl< ModelT >::buffer_

private

Definition at line 181 of file ergodic_control.hpp.

◆ collision_

template<class ModelT >

Collision ergodic_exploration::ErgodicControl< ModelT >::collision_

private

Definition at line 166 of file ergodic_control.hpp.

◆ dt_

template<class ModelT >

double ergodic_exploration::ErgodicControl< ModelT >::dt_

private

Definition at line 167 of file ergodic_control.hpp.

◆ expl_weight_

template<class ModelT >

double ergodic_exploration::ErgodicControl< ModelT >::expl_weight_

private

Definition at line 170 of file ergodic_control.hpp.

◆ horizon_

template<class ModelT >

double ergodic_exploration::ErgodicControl< ModelT >::horizon_

private

Definition at line 168 of file ergodic_control.hpp.

◆ map_pos_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::map_pos_

private

Definition at line 176 of file ergodic_control.hpp.

◆ model_

template<class ModelT >

ModelT ergodic_exploration::ErgodicControl< ModelT >::model_

private

Definition at line 165 of file ergodic_control.hpp.

◆ phik_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::phik_

private

Definition at line 174 of file ergodic_control.hpp.

◆ pose_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::pose_

private

Definition at line 179 of file ergodic_control.hpp.

◆ resolution_

template<class ModelT >

double ergodic_exploration::ErgodicControl< ModelT >::resolution_

private

Definition at line 169 of file ergodic_control.hpp.

◆ rhodot_

template<class ModelT >

CoStateFunc ergodic_exploration::ErgodicControl< ModelT >::rhodot_

private

Definition at line 183 of file ergodic_control.hpp.

◆ rhoT_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::rhoT_

private

Definition at line 175 of file ergodic_control.hpp.

◆ Rinv_

template<class ModelT >

mat ergodic_exploration::ErgodicControl< ModelT >::Rinv_

private

Definition at line 172 of file ergodic_control.hpp.

◆ rk4_

template<class ModelT >

RungeKutta ergodic_exploration::ErgodicControl< ModelT >::rk4_

private

Definition at line 182 of file ergodic_control.hpp.

◆ steps_

template<class ModelT >

unsigned int ergodic_exploration::ErgodicControl< ModelT >::steps_

private

Definition at line 171 of file ergodic_control.hpp.

◆ target_

template<class ModelT >

Target ergodic_exploration::ErgodicControl< ModelT >::target_

private

Definition at line 184 of file ergodic_control.hpp.

◆ umax_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::umax_

private

Definition at line 178 of file ergodic_control.hpp.

◆ umin_

template<class ModelT >

vec ergodic_exploration::ErgodicControl< ModelT >::umin_

private

Definition at line 177 of file ergodic_control.hpp.

◆ ut_

template<class ModelT >

mat ergodic_exploration::ErgodicControl< ModelT >::ut_

private

Definition at line 173 of file ergodic_control.hpp.

The documentation for this class was generated from the following file:

ergodic_control.hpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

template<class ModelT> class ergodic_exploration::ErgodicControl< ModelT >

Constructor & Destructor Documentation

◆ ErgodicControl()

Member Function Documentation

◆ addStateMemory()

◆ configTarget()

◆ control()

◆ gradBarrier()

◆ gradErgodicMetric()

◆ optTraj()

◆ path()

◆ setTarget()

◆ timeStep()

◆ updateControl()

Member Data Documentation

◆ basis_

◆ buffer_

◆ collision_

◆ dt_

◆ expl_weight_

◆ horizon_

◆ map_pos_

◆ model_

◆ phik_

◆ pose_

◆ resolution_

◆ rhodot_

◆ rhoT_

◆ Rinv_

◆ rk4_

◆ steps_

◆ target_

◆ umax_

◆ umin_

◆ ut_

template<class ModelT>
class ergodic_exploration::ErgodicControl< ModelT >