ERROR: Nan in state inside plant

[XinChen-stars]

Hi, thank you for sharing the code！

Description

I tried following the MPC Usage: Listing 2 & Listing 3
the cartpole_plant_example running result:

However，when trying to run the quadrotor example it returned the following error:

quadrotor plant code :

#pragma once
#include <mppi/core/base_plant.hpp>
#include <mppi/dynamics/quadrotor/quadrotor_dynamics.cuh>
template <class CONTROLLER_T>
class SimpleQuadrotorPlant : public BasePlant<CONTROLLER_T>
{
public:
 using control_array = typename QuadrotorDynamics::control_array;
 using state_array = typename QuadrotorDynamics::state_array;
 using output_array = typename QuadrotorDynamics::output_array;

 SimpleQuadrotorPlant(std::shared_ptr<CONTROLLER_T> controller, int hz, int optimization_stride)
   : BasePlant<CONTROLLER_T>(controller, hz, optimization_stride)
 {
   system_dynamics_ = std::make_shared<QuadrotorDynamics>();
 }

 void pubControl(const control_array& u)
 {
   state_array state_derivative;
   output_array dynamics_output;
   state_array prev_state = current_state_;
   float t = this->state_time_;
   float dt = this->controller_->getDt();
   system_dynamics_->step(prev_state, current_state_, state_derivative, u, dynamics_output, t, dt);
   current_time_ += dt;
 }

 void pubNominalState(const state_array& s)
 {
 }

 void pubFreeEnergyStatistics(MPPIFreeEnergyStatistics& fe_stats)
 {
 }

 int checkStatus()
 {
   return 0;
 }

 double getCurrentTime()
 {
   return current_time_;
 }

 double getPoseTime()
 {
   return this->state_time_;
 }

 double getAvgLoopTime() const
 {
   return this->avg_loop_time_ms_;
 }

 double getLastOptimizationTime() const
 {
   return this->optimization_duration_;
 }

 state_array current_state_ = state_array::Zero();

protected:
 std::shared_ptr<QuadrotorDynamics> system_dynamics_;
 double current_time_ = 0.0;
};

quadrotor example code :

#include <mppi/instantiations/quadrotor_mppi/quadrotor_mppi.cuh>
#include <quadrotor_plant.hpp>

const int NUM_TIMESTEPS = 100;
const int NUM_ROLLOUTS = 1024;
const int DYN_BLOCK_X = 32;
using DYN_T = QuadrotorDynamics;
const int DYN_BLOCK_Y = DYN_T::STATE_DIM;
using COST_T = QuadrotorQuadraticCost;
using FB_T = DDPFeedback<DYN_T, NUM_TIMESTEPS>;
using SAMPLING_T = mppi::sampling_distributions::GaussianDistribution<DYN_T::DYN_PARAMS_T>;
using CONTROLLER_T = VanillaMPPIController<DYN_T, COST_T, FB_T, NUM_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T>;
using CONTROLLER_PARAMS_T = CONTROLLER_T::TEMPLATED_PARAMS;

using PLANT_T = SimpleQuadrotorPlant<CONTROLLER_T>;

int main(int argc, char** argv)
{
 float dt = 0.02;
 DYN_T dynamics;                     // set up dynamics
 COST_T cost;                        // set up cost
 FB_T fb_controller(&dynamics, dt);  // set up feedback controller
 // set up sampling distribution
 SAMPLING_T sampler;
 auto sampler_params = sampler.getParams();
 std::fill(sampler_params.std_dev, sampler_params.std_dev + DYN_T::CONTROL_DIM, 10.0);
 sampler.setParams(sampler_params);

 // set up MPPI Controller
 CONTROLLER_PARAMS_T controller_params;
 controller_params.dt_ = dt;
 controller_params.lambda_ = 1.0;
 controller_params.dynamics_rollout_dim_ = dim3(DYN_BLOCK_X, DYN_BLOCK_Y, 1);
 controller_params.cost_rollout_dim_ = dim3(96, 1, 1);
 std::shared_ptr<CONTROLLER_T> controller =
     std::make_shared<CONTROLLER_T>(&dynamics, &cost, &fb_controller, &sampler, controller_params);

 // Create plant
 PLANT_T plant(controller, (1.0 / dt), 1);

 std::atomic<bool> alive(true);
 for (int t = 0; t < 10000; t++)
 {
   plant.updateState(plant.current_state_, (t + 1) * dt);
   plant.runControlIteration(&alive);
 }

 std::cout << "Avg Optimization time: " << plant.getAvgOptimizationTime() << " ms" << std::endl;
 std::cout << "Last Optimization time: " << plant.getLastOptimizationTime() << " ms" << std::endl;
 std::cout << "Avg Loop time: " << plant.getAvgLoopTime() << " ms" << std::endl;
 std::cout << "Avg Optimization Hz: " << 1.0 / (plant.getAvgOptimizationTime() * 1e-3) << " Hz" << std::endl;

 auto control_sequence = controller->getControlSeq();
 std::cout << "State: \n" << plant.current_state_.transpose() << std::endl;
 std::cout << "Control Sequence:\n" << control_sequence << std::endl;
 return 0;
}

Is there anything I am overlooking on coding an quadrotor example?

[JasonGibson274]

Hello,

Yes the quadrotor dynamics have quaternions in them. By initializing it all to zero it will throw nans when doing update state here https://github.com/ACDSLab/MPPI-Generic/blob/main/include/mppi/dynamics/quadrotor/quadrotor_dynamics.cu#L182C24-L182C77.

Just use the method getZeroState https://github.com/ACDSLab/MPPI-Generic/blob/main/include/mppi/dynamics/quadrotor/quadrotor_dynamics.cu#L212 to get a valid initial state.

[JasonGibson274]

I will make a note to move the examples to using the getZeroState method to prevent this issue in the future.

[XinChen-stars]

Thank you for your reply！
I solved this problem by using the method getZeroState:

state_array current_state_ = system_dynamics_->getZeroState();