File functors.cuh

Functors for MPPI control algorithm.

Functors are classes whose instances can be called like functions (by overloading operator()). They can be passed parameters both initially during construction and when called. MPPI uses functors because Thrust requires “functions” that only take in one parameter (e.g. an index). We pass in other parameters that the functions need (e.g. pointers to memory structures) during construction.

namespace controls

namespace mppi

Functions

static __device__ void model (const float world_state[], const float action[], float world_state_out[], float timestep)

Advances curvilinear state one timestep according to action.

Parameters:

• world_state – [in] world state of the vehicle

• action – [in] Action taken

• world_state_out – [out] Returned next state

• timestep – [in] Timestep

static __device__ void corner_creation (const float center_state[], float forward, float right, float corner_state[])

static __device__ float cost (const float world_state[state_dims], const float action[], const float last_taken_action[], float start_progress, float time_since_traj_start, bool first, bool follow_midline_only)

Calculate cost at a particular state.

Parameters:

• world_state – World state of the vehicle

• action – Action considered

• last_taken_action – Last action actually taken by the vehicle

• start_progress – Progress at the start of the trajectory

• time_since_traj_start – Time elapsed since trjaectory start

• first – whether this is the first action in the trajectory

Returns:

Cost at the given state

static __device__ float dot_with_action_matrix (const float *perturb)

static __device__ ActionWeightTuple operator+ (const ActionWeightTuple &action_weight_t0, const ActionWeightTuple &action_weight_t1)

Adds two ActionWeightTuples whilst checking/clamping to reasonable bounds.

struct AbsoluteError

Public Functions

inline __device__ Action operator() (const DeviceAction &x, const DeviceAction &y) const

struct ActionWeightTupleToAction

Extracts the action inside an ActionWeightTuple.

Public Functions

inline __device__ DeviceAction operator() (const ActionWeightTuple &awt) const

struct AddActions

Note: Thrust needs a functor (similar to std::hash), so we can’t use a standalone function Adds two device actions

Public Functions

inline __device__ DeviceAction operator() (const DeviceAction &a1, const DeviceAction &a2) const

template<size_t k>
struct DivBy

Unary function to divide by the template parameter.

Public Functions

inline __host__ __device__ size_t operator() (size_t i) const

template<typename T>
struct Equal

Public Functions

inline __device__ bool operator() (T a, T b)

struct GeometricSquaredError

Public Functions

inline __device__ Action operator() (const DeviceAction &x, const DeviceAction &y) const

struct IndexToActionWeightTuple

Captures pointers to action trajectories and cost to gos Produces a weight for a single action based on its cost to go. Bundles the action and weight into an ActionWeightTuple, then stores it in the output array in a different position so as to “transpose” the matrix.

Param action_weight_tuples:

[out] output (transposed) matrix of action-weight tuples

Param action_trajectories:

[in] perturbed action trajectories

Param cost_to_gos:

[in] cost to go for each action

Param log_prob_densities:

[in] log probability densities for each action

Param idx:

[in] index into action_weight_tuples

Public Functions

inline IndexToActionWeightTuple(ActionWeightTuple *action_weight_tuples, const float *action_trajectories, const float *cost_to_gos, const float *log_prob_densities)

inline __device__ void operator() (const uint32_t idx) const

Parameters:

idx – refers to the index for the timesteps x samples matrix (transposed from normal to make the reduction work)

Returns:

a tuple of the action indexed from the trajectories and the weight of that action (strictly positive) based on its cost to go

Public Members

const float *action_trajectories

const float *cost_to_gos

const float *log_prob_densities

ActionWeightTuple *action_weight_tuples

struct LogProbabilityDensity

Computes and stores log probability density from sampled perturbations for the sake of.

Importance Sampling . Uses the covariance matrix in cuda_globals::perturbs_incr_var_inv.

Param perturbation:

[in] n x m x q perturbation matrix

Param log_probability_densities:

[out] n x m matrix to store result

Param idx:

[in] index from 0 to n x m, used for thrust::for_each

Public Functions

inline explicit LogProbabilityDensity(thrust::device_ptr<float> perturbation, thrust::device_ptr<float> log_probability_densities)

inline __device__ void operator() (size_t idx) const

Public Members

thrust::device_ptr<float> perturbation

thrust::device_ptr<float> log_probability_densities

struct PopulateCost

Computes cost using the model and cost function, triangularizes both D and J.

Param brownians:

[in] brownian perturbation with mean 0

Param sampled_action_trajectories:

[out] same as brownians but doesn’t have to be

Param sampled_state_trajectories:

[in] for publishing states

Param cost_to_gos:

[out] stores the negative cost so far

Param [in/out]:

log_prob_densities stores the negative log_prob_densities so far

Param action_trajectory_base:

[in] current best guess of optimal action trajectory, from previous iteration of mppi.

Param i:

[in] index over timesteps

Public Functions

inline PopulateCost(thrust::device_ptr<float> brownians, thrust::device_ptr<float> sampled_action_trajectories, thrust::device_ptr<float> cost_to_gos, thrust::device_ptr<float> log_prob_densities, const thrust::device_ptr<DeviceAction> &action_trajectory_base, DeviceAction last_taken_action, bool follow_midline_only)

inline __device__ void operator() (uint32_t i) const

Public Members

float *brownians

float *sampled_action_trajectories

float *cost_to_gos

float *log_prob_densities

DeviceAction last_taken_action

bool follow_midline_only

const DeviceAction *action_trajectory_base

struct SquaredError

Public Functions

inline __device__ Action operator() (const DeviceAction &x, const DeviceAction &y) const

struct TransformStdNormal

Modifies a disturbance tensor from standard normal to brownian in-place, keeping within reasonable bounds.

Uses cuda_globals::perturbs_incr_std for variance information.

Param std_normals:

[in] Standard normal disturbance tensor generated by cuRAND, size is num_action_trajectories

Param idx:

[in] index into std_normals, used by thrust::for_each

Public Functions

inline explicit TransformStdNormal(thrust::device_ptr<float> std_normals)

inline __device__ void operator() (size_t idx) const

Public Members

thrust::device_ptr<float> std_normals

Private Members

float m_sqrt_timestep = std::sqrt(controller_period)