Class mppi::MppiController_Impl

class MppiController_Impl : public controls::mppi::MppiController

Public Functions

MppiController_Impl(std::mutex &mutex, LoggerFunc logger)

Construct a new MppiController_Impl object.

Parameters:

• mutex – Mutex to prevent multiple simultaneous Thrust calls (generate_action and display)

• logger – Logger function to log messages

Private Functions

void generate_brownians()

Populates m_action_trajectories with samples taken from a zero-centered Brownian distrubtion. Computed for each sample trajectory in parallel (num_samples wide).

void generate_log_probability_density()

Generates log probability densities from the Brownian samples. Reads from m_action_trajectories and writes to m_log_prob_densities. Computed over each “control action” in parallel (num_samples * num_timesteps wide).

void populate_cost()

Calculates costs to go, storing them in m_cost_to_gos. Heavy lifting function. To calculate cost, reads from global state and spline information, and simulates state into the future using the dynamics model. Side effect of modifying m_action_trajectories by adding m_last_action_trajectory. Side effect of making m_log_prob_densities “to-go”. Reads/writes to every device vector except m_action_weight_tuples. Computed over each sample trajectory in parallel (num_samples wide).

rollout states and populate cost

void generate_action_weight_tuples()

Populates m_action_weight_tuples in timestep-major order with actions and their associated weights. All samples at time 0, all samples at time 1, etc. This is transposed from m_cost_to_gos (which is sample-major order) so that the reduction has good locality. Reads from the corresponding indexes in m_action_trajectories, m_cost_to_gos and m_log_prob_densities, bundles them, then places the tuple in the new position in m_action_weight_tuples. Computed over each control action/cost in parallel (num_samples * num_timesteps wide)

thrust::device_vector<DeviceAction> reduce_actions()

For every timestep, reduces the samples together using a weighted average. Reads from m_action_weight_tuples.

Returns:

Optimal control action trajectory based on state and spline information.

Private Members

thrust::device_vector<DeviceAction> m_last_action_trajectory

num_timesteps x action_dims device tensor. Best-guess action trajectory to which perturbations are added. The only device tensor that is retained between each call to generate_action.

thrust::device_vector<float> m_action_trajectories

num_samples x num_timesteps x actions_dims (num_action_trajectories) device tensor. Used to store brownians, perturbed actions, and action trajectories at different points in the algorithm.

thrust::device_vector<float> m_cost_to_gos

num_samples x num_timesteps device tensor. Stores costs to go.

thrust::device_vector<float> m_log_prob_densities

num_samples x num_timesteps device tensor. Stores the log probability densities (calculated after brownian generations), then made “to-go”. Used for importance sampling.

thrust::device_vector<ActionWeightTuple> m_action_weight_tuples

num_samples x num_timesteps device tensor. Used to store the control actions and their corresponding weights (based on cost and log probability density).

DeviceAction m_last_action

last action generated by the controller. Used for ML-like momentum and display purposes.

curandGenerator_t m_rng

Pseudo random generator used to generate brownian perturbations.

LoggerFunc m_logger

Logger for debugging. Can be attached to ROS or echo to std::cerr.

std::mutex &m_mutex

Mutex to prevent multiple simultaneous Thrust calls (generate_action and display)