ode_step efficiencies

include/scrimmage/entity/Entity.h

@@ -138,6 +138,14 @@ class Entity : public std::enable_shared_from_this<Entity> {
     MotionModelPtr &motion();
     std::vector<ControllerPtr> &controllers();
 
+    ///////////////////////////////////////////////////////////////////////////////////////
+    // Tracking the number of times the ode step function has been called for the given entity
+    ///////////////////////////////////////////////////////////////////////////////////////
+    int stepsCalled;
+    void incrementSteps();
+    int get_stepsCalled();
+    ///////////////////////////////////////////////////////////////////////////////////////
+
     void set_id(ID &id);
     ID &id();
 

include/scrimmage/motion/MotionModel.h

@@ -56,6 +56,14 @@ class MotionModel : public EntityPlugin {
                       std::map<std::string, std::string> &params);
 
     virtual bool step(double time, double dt);
+    virtual bool step(double time, double dt, int iteration);
+    virtual bool step(double time, double dt, vector_t odevect);
+    virtual bool offset_step(double time, double dt);
+
+    //Getters and setters in the case the motion model flag requires
+    //odestep only occuring once
+    virtual std::vector<double> getOdeStepVal();
+
     virtual bool posthumous(double t);
     virtual StatePtr &state();
     virtual void set_state(StatePtr &state);
@@ -77,6 +85,9 @@ class MotionModel : public EntityPlugin {
     StatePtr state_;
     vector_t x_;
 
+    //Adding a vector for the Dubin's case
+    vector_t odeVal_;
+
     Eigen::Vector3d ext_force_;
     Eigen::Vector3d ext_moment_;
     double mass_;

include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h

@@ -49,7 +49,22 @@ class DubinsAirplane3D : public scrimmage::MotionModel {
  public:
     bool init(std::map<std::string, std::string> &info,
               std::map<std::string, std::string> &params) override;
-    bool step(double t, double dt) override;
+    bool step(double t, double dt) override; // Original step function
+
+   ///////////////////////////////////////////////////////////////////////////////////////
+   // Natalie's step functions
+   ///////////////////////////////////////////////////////////////////////////////////////
+
+    bool offset_step(double time, double dt) override; // Used by entity #1 to update ode_step offset vector
+    bool step(double t, double dt, vector_t odevect) override; // Step function used by entities that use entity #1's ode_step function offset
+    bool step(double t, double dt, int iteration) override; // Test that can be deleted
+
+   ///////////////////////////////////////////////////////////////////////////////////////
+
+    //Getters and setters in the case the motion model flag requires
+    //odestep only occuring once
+    std::vector<double> getOdeStepVal();
+
     void model(const vector_t &x , vector_t &dxdt , double t) override;
 
  protected:

include/scrimmage/simcontrol/SimControl.h

@@ -326,6 +326,17 @@ class SimControl {
     std::shared_ptr<std::unordered_map<int, int>> id_to_team_map_;
     std::shared_ptr<std::unordered_map<int, EntityPtr>> id_to_ent_map_;
 
+    ///////////////////////////////////////////////////////////////////////////////////////
+    // map that has ent_desc_id and entity id
+    ///////////////////////////////////////////////////////////////////////////////////////
+    std::map<int, int> ent_desc_to_id_map;
+    bool stepIterDone = false;
+
+    std::map<int, std::vector<double>> ent_desc_to_ode_vector;
+
+    std::map<int, int> ent_id_to_ode_step;
+    ///////////////////////////////////////////////////////////////////////////////////////
+
     InterfacePtr incoming_interface_;
     InterfacePtr outgoing_interface_;
 

src/entity/Entity.cpp

@@ -122,6 +122,8 @@ bool Entity::init(AttributeMap &overrides,
     }
     state_truth_ = state_;
 
+    //std::cout << "Generating the initial state of entity: " << id;
+
     double x = get("x", info, 0.0);
     double y = get("y", info, 0.0);
     double z = get("z", info, 0.0);
@@ -278,6 +280,9 @@ bool Entity::init(AttributeMap &overrides,
         motion_model_->set_name("BLANK");
     }
 
+    // Tracking ode steps called, intialize to 0
+    stepsCalled = 0;
+
     ////////////////////////////////////////////////////////////
     // controller
     ////////////////////////////////////////////////////////////
@@ -465,6 +470,20 @@ bool Entity::init(AttributeMap &overrides,
     return true;
 }
 
+///////////////////////////////////////////////////////////////////////////////////////
+// Tracking ode step calls
+///////////////////////////////////////////////////////////////////////////////////////
+
+void Entity::incrementSteps(){
+    stepsCalled++;
+}
+
+int Entity::get_stepsCalled(){
+    return stepsCalled;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+
 bool Entity::parse_visual(std::map<std::string, std::string> &info,
                           MissionParsePtr mp,
                           std::map<std::string, std::string> &overrides) {

src/parse/MissionParse.cpp

@@ -369,6 +369,7 @@ bool MissionParse::parse(const std::string &filename) {
 
             if (node_name == "autonomy") {
                 node_name += std::to_string(orders[nm]["autonomy"]++);
+                cout << node_name << endl;
             } else if (node_name == "controller") {
                 node_name += std::to_string(orders[nm]["controller"]++);
             } else if (node_name == "sensor") {

src/plugin_manager/MotionModel.cpp

@@ -48,6 +48,18 @@ bool MotionModel::init(std::map<std::string, std::string> &info, std::map<std::s
 
 bool MotionModel::step(double time, double dt) { return true; }
 
+///////////////////////////////////////////////////////////////////////////////////////
+// Natalie's step functions
+///////////////////////////////////////////////////////////////////////////////////////
+
+bool MotionModel::offset_step(double time, double dt){ return true; }
+bool MotionModel::step(double time, double dt, vector_t odevect) { return true; }
+bool MotionModel::step(double time, double dt, int iteration) { return true; } // Can be deleted, not being used
+
+///////////////////////////////////////////////////////////////////////////////////////
+
+std::vector<double> MotionModel::getOdeStepVal() { return odeVal_;}
+
 bool MotionModel::posthumous(double t) { return true; }
 
 StatePtr &MotionModel::state() {return state_;}
@@ -60,6 +72,18 @@ void MotionModel::ode_step(double dt) {
     auto sys = std::bind(&MotionModel::model, this, pl::_1, pl::_2, pl::_3);
     boost::numeric::odeint::runge_kutta4<std::vector<double>> stepper;
     stepper.do_step(sys, x_, 0, dt);
+
+    // ABM Stepper
+    // This did not result in a change of wall time...
+    // boost::numeric::odeint::adams_bashforth< 5, std::vector<double>> abm;
+    // double t = 0;
+    // abm.initialize(sys , x_ , t , dt );
+    // abm.do_step( sys , x_ , t , dt );
+
+    //Adjustable stepper
+    //boost::numeric::odeint::controlled_runge_kutta
+
+
 }
 
 void MotionModel::model(const MotionModel::vector_t &x, MotionModel::vector_t &dxdt, double t) {}