diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index caa37a9b90..3d1d58995a 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -138,6 +138,14 @@ class Entity : public std::enable_shared_from_this { MotionModelPtr &motion(); std::vector &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(); diff --git a/include/scrimmage/motion/MotionModel.h b/include/scrimmage/motion/MotionModel.h index 1fdf2ce45d..763d0aa333 100644 --- a/include/scrimmage/motion/MotionModel.h +++ b/include/scrimmage/motion/MotionModel.h @@ -56,6 +56,14 @@ class MotionModel : public EntityPlugin { std::map ¶ms); 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 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_; diff --git a/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h b/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h index 9f2964273f..765aefaa20 100644 --- a/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h +++ b/include/scrimmage/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.h @@ -49,7 +49,22 @@ class DubinsAirplane3D : public scrimmage::MotionModel { public: bool init(std::map &info, std::map ¶ms) 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 getOdeStepVal(); + void model(const vector_t &x , vector_t &dxdt , double t) override; protected: diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index ff7d5d027b..d36bbe3028 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -326,6 +326,17 @@ class SimControl { std::shared_ptr> id_to_team_map_; std::shared_ptr> id_to_ent_map_; + /////////////////////////////////////////////////////////////////////////////////////// + // map that has ent_desc_id and entity id + /////////////////////////////////////////////////////////////////////////////////////// + std::map ent_desc_to_id_map; + bool stepIterDone = false; + + std::map> ent_desc_to_ode_vector; + + std::map ent_id_to_ode_step; + /////////////////////////////////////////////////////////////////////////////////////// + InterfacePtr incoming_interface_; InterfacePtr outgoing_interface_; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 1ae63f00cf..26c65b522e 100644 --- a/src/entity/Entity.cpp +++ b/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 &info, MissionParsePtr mp, std::map &overrides) { diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 7c19f020f4..f61ce7a24e 100644 --- a/src/parse/MissionParse.cpp +++ b/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") { diff --git a/src/plugin_manager/MotionModel.cpp b/src/plugin_manager/MotionModel.cpp index 7710ec9467..e9c5c001b2 100644 --- a/src/plugin_manager/MotionModel.cpp +++ b/src/plugin_manager/MotionModel.cpp @@ -48,6 +48,18 @@ bool MotionModel::init(std::map &info, std::map 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> 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> 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) {} diff --git a/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp b/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp index 8ae2566648..7fd224998c 100644 --- a/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp +++ b/src/plugins/motion/DubinsAirplane3D/DubinsAirplane3D.cpp @@ -38,6 +38,11 @@ #include #include #include +#include // NOLINT + +#include +using std::cout; +using namespace std::chrono; REGISTER_PLUGIN(scrimmage::MotionModel, scrimmage::motion::DubinsAirplane3D, DubinsAirplane3D_plugin) @@ -70,6 +75,7 @@ enum ModelParams { bool DubinsAirplane3D::init(std::map &info, std::map ¶ms) { + write_csv_ = sc::get("write_csv", params, false); // Model limits @@ -80,8 +86,9 @@ bool DubinsAirplane3D::init(std::map &info, desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::In); desired_pitch_idx_ = vars_.declare(VariableIO::Type::desired_pitch, VariableIO::Direction::In); desired_roll_idx_ = vars_.declare(VariableIO::Type::desired_roll, VariableIO::Direction::In); - + x_.resize(MODEL_NUM_ITEMS); + odeVal_.resize(MODEL_NUM_ITEMS); // I had this set to 9 because I thought it was related to the number of doubles in the vector... but that was causing memory chunk control structure fields in the adjacent following chunk are being overwritten due to out-of-bounds access by the code Eigen::Vector3d &pos = state_->pos(); quat_world_ = state_->quat(); quat_world_.normalize(); @@ -89,6 +96,20 @@ bool DubinsAirplane3D::init(std::map &info, quat_world_inverse_ = quat_world_.inverse(); quat_world_inverse_.normalize(); + //Initializing odeVal_ vector + odeVal_[q0] = 0; + odeVal_[q1] = 0; + odeVal_[q2] = 0; + odeVal_[q3] = 0; + + odeVal_[U] = 0; + odeVal_[V] = 0; + odeVal_[W] = 0; + + odeVal_[Xw] = 0; + odeVal_[Yw] = 0; + odeVal_[Zw] = 0; + x_[U] = 0; x_[V] = 0; x_[W] = 0; @@ -132,6 +153,200 @@ bool DubinsAirplane3D::init(std::map &info, return true; } +/////////////////////////////////////////////////////////////////////////////////////// +// Trying one ode step function +/////////////////////////////////////////////////////////////////////////////////////// +bool DubinsAirplane3D::offset_step(double t, double dt) { + // Get inputs and saturate + speed_ = boost::algorithm::clamp(vars_.input(desired_speed_idx_), speed_min_, speed_max_); + pitch_ = vars_.input(desired_pitch_idx_); + roll_ = vars_.input(desired_roll_idx_); + + x_[Uw] = state_->vel()(0); + x_[Vw] = state_->vel()(1); + x_[Ww] = state_->vel()(2); + + x_[Xw] = state_->pos()(0); + x_[Yw] = state_->pos()(1); + x_[Zw] = state_->pos()(2); + + // state_->quat().normalize(); + state_->quat().set(roll_, pitch_, state_->quat().yaw()); + quat_local_ = state_->quat() * quat_world_inverse_; + quat_local_.normalize(); + x_[q0] = quat_local_.w(); + x_[q1] = quat_local_.x(); + x_[q2] = quat_local_.y(); + x_[q3] = quat_local_.z(); + + Eigen::Vector3d force_body = quat_local_.rotate_reverse(ext_force_); + Eigen::Vector3d ext_moment_body = ext_moment_; + ext_force_ = Eigen::Vector3d::Zero(); + ext_moment_ = Eigen::Vector3d::Zero(); + + x_[U] = speed_ + force_body(0) / mass_; + x_[V] = force_body(1) / mass_; + x_[W] = force_body(2) / mass_; + + double turn_rate = 0; + if (std::abs(speed_) >= std::numeric_limits::epsilon()) { + turn_rate = -g_ / speed_ * tan(roll_); + } + + x_[P] = ext_moment_body(0) / mass_; + x_[Q] = ext_moment_body(1) / mass_; + x_[R] = ext_moment_body(2) / mass_ + turn_rate; + + // Setting the odeVals vector to values before ode_step + odeVal_[q0] = x_[q0]; + odeVal_[q1] = x_[q1]; + odeVal_[q2] = x_[q2]; + odeVal_[q3] = x_[q3]; + + odeVal_[U] = x_[U]; + odeVal_[V] = x_[V]; + odeVal_[W] = x_[W]; + + odeVal_[Xw] = x_[Xw]; + odeVal_[Yw] = x_[Yw]; + odeVal_[Zw] = x_[Zw]; + + ode_step(dt); + + // Normalize quaternion + quat_local_.w() = x_[q0]; + quat_local_.x() = x_[q1]; + quat_local_.y() = x_[q2]; + quat_local_.z() = x_[q3]; + quat_local_.normalize(); + + x_[q0] = quat_local_.w(); + x_[q1] = quat_local_.x(); + x_[q2] = quat_local_.y(); + x_[q3] = quat_local_.z(); + + Eigen::Vector3d vel_local(x_[U], x_[V], x_[W]); + + // Convert local coordinates to world coordinates + state_->quat() = quat_local_ * quat_world_; + state_->quat().normalize(); + state_->pos() << x_[Xw], x_[Yw], x_[Zw]; + state_->vel() << state_->quat().toRotationMatrix() * vel_local; + + speed_ = vel_local.norm(); + + // Updating the odeVals vector with the difference that occured from the step function + odeVal_[q0] = x_[q0] - odeVal_[q0]; + odeVal_[q1] = x_[q1] - odeVal_[q1]; + odeVal_[q2] = x_[q2] - odeVal_[q2]; + odeVal_[q3] = x_[q3] - odeVal_[q3]; + + odeVal_[U] = x_[U] - odeVal_[U]; + odeVal_[V] = x_[V] - odeVal_[V]; + odeVal_[W] = x_[W] - odeVal_[W]; + + odeVal_[Xw] = x_[Xw] - odeVal_[Xw]; + odeVal_[Yw] = x_[Yw] - odeVal_[Yw]; + odeVal_[Zw] = x_[Zw] - odeVal_[Zw]; + + if (write_csv_) { + // Log state to CSV + csv_.append(sc::CSV::Pairs{ + {"t", t}, + {"x", x_[Xw]}, + {"y", x_[Yw]}, + {"z", x_[Zw]}, + {"U", x_[U]}, + {"V", x_[V]}, + {"W", x_[W]}, + {"P", x_[P]}, + {"Q", x_[Q]}, + {"R", x_[R]}, + {"roll", state_->quat().roll()}, + {"pitch", state_->quat().pitch()}, + {"yaw", state_->quat().yaw()}, + {"speed", speed_}, + {"Uw", state_->vel()(0)}, + {"Vw", state_->vel()(1)}, + {"Ww", state_->vel()(2)}}); + } + + return true; +} + +bool DubinsAirplane3D::step(double t, double dt, vector_t odevect_) { + // Get inputs and saturate + speed_ = boost::algorithm::clamp(vars_.input(desired_speed_idx_), speed_min_, speed_max_); + pitch_ = vars_.input(desired_pitch_idx_); + roll_ = vars_.input(desired_roll_idx_); + + //Final value that can be fed into the state position vector... maybe we just add to the state pos vector instead of this and then feeding it in... + x_[Xw] = state_->pos()(0) + odevect_[Xw]; + x_[Yw] = state_->pos()(1) + odevect_[Yw]; + x_[Zw] = state_->pos()(2) + odevect_[Zw]; + + //Final value that can be fed into the state position vector + state_->quat().set(roll_, pitch_, state_->quat().yaw()); + quat_local_ = state_->quat() * quat_world_inverse_; + quat_local_.normalize(); + quat_local_.w() += odevect_[q0]; + quat_local_.x() += odevect_[q1]; + quat_local_.y() += odevect_[q2]; + quat_local_.z() += odevect_[q3]; + quat_local_.normalize(); + + Eigen::Vector3d force_body = quat_local_.rotate_reverse(ext_force_); + Eigen::Vector3d ext_moment_body = ext_moment_; + ext_force_ = Eigen::Vector3d::Zero(); + ext_moment_ = Eigen::Vector3d::Zero(); + + //Final value that can be fed into the local velocity calculation + x_[U] = (speed_ + force_body(0) / mass_) + odevect_[U]; + x_[V] = (force_body(1) / mass_) + odevect_[V]; + x_[W] = (force_body(2) / mass_) + odevect_[W]; + + Eigen::Vector3d vel_local(x_[U], x_[V], x_[W]); + + // Convert local coordinates to world coordinates + state_->quat() = quat_local_ * quat_world_; //Need to check if the quat world value is updated by the step function... otherwise need to hold the difference updated in the vector + state_->quat().normalize(); + state_->pos() << x_[Xw], x_[Yw], x_[Zw]; + state_->vel() << state_->quat().toRotationMatrix() * vel_local; + + speed_ = vel_local.norm(); + + if (write_csv_) { + // Log state to CSV + csv_.append(sc::CSV::Pairs{ + {"t", t}, + {"x", x_[Xw]}, + {"y", x_[Yw]}, + {"z", x_[Zw]}, + {"U", x_[U]}, + {"V", x_[V]}, + {"W", x_[W]}, + {"P", x_[P]}, + {"Q", x_[Q]}, + {"R", x_[R]}, + {"roll", state_->quat().roll()}, + {"pitch", state_->quat().pitch()}, + {"yaw", state_->quat().yaw()}, + {"speed", speed_}, + {"Uw", state_->vel()(0)}, + {"Vw", state_->vel()(1)}, + {"Ww", state_->vel()(2)}}); + } + + return true; +} + +std::vector DubinsAirplane3D::getOdeStepVal(){ + return odeVal_; +} + +/////////////////////////////////////////////////////////////////////////////////////// +// Original ode step function +/////////////////////////////////////////////////////////////////////////////////////// bool DubinsAirplane3D::step(double t, double dt) { // Get inputs and saturate speed_ = boost::algorithm::clamp(vars_.input(desired_speed_idx_), speed_min_, speed_max_); @@ -222,6 +437,297 @@ bool DubinsAirplane3D::step(double t, double dt) { return true; } +/////////////////////////////////////////////////////////////////////////////////////// +// Only returning true to see how run-time differs +/////////////////////////////////////////////////////////////////////////////////////// +//bool DubinsAirplane3D::step(double t, double dt) { return true; } + +/////////////////////////////////////////////////////////////////////////////////////// +// Executing simple array index updates to see how run-time differs +/////////////////////////////////////////////////////////////////////////////////////// +// bool DubinsAirplane3D::step(double t, double dt) { + +// x_[Uw] = 1; +// x_[Vw] = 1; +// x_[Ww] = 1; + +// x_[Xw] = 1; +// x_[Yw] = 1; +// x_[Zw] = 1; + +// x_[U] = 1; +// x_[V] = 1; +// x_[W] = 1; + +// x_[P] = 1; +// x_[Q] = 1; +// x_[R] = 1; + +// x_[q0] = 1; +// x_[q1] = 1; +// x_[q2] = 1; +// x_[q3] = 1; + +// return true; +// } + +/////////////////////////////////////////////////////////////////////////////////////// +// Original with timing incorporated +/////////////////////////////////////////////////////////////////////////////////////// +// bool DubinsAirplane3D::step(double t, double dt) { +// auto startTot = high_resolution_clock::now(); + +// //Commenting out lines that do not directly affect the state only decreases the wall time by 0.023 seconds +// auto start = high_resolution_clock::now(); + +// // Get inputs and saturate +// speed_ = boost::algorithm::clamp(vars_.input(desired_speed_idx_), speed_min_, speed_max_); +// pitch_ = vars_.input(desired_pitch_idx_); +// roll_ = vars_.input(desired_roll_idx_); + +// // Nat - These are not used anywhere, but maybe like below Xw they are somewhere +// x_[Uw] = state_->vel()(0); +// x_[Vw] = state_->vel()(1); +// x_[Ww] = state_->vel()(2); +// // Nat - end + +// // Nat - no further calculations are done on Xw, Yw, and Zw. Then, state_->pos is set back +// // to these values... is this necessary? +// x_[Xw] = state_->pos()(0); +// x_[Yw] = state_->pos()(1); +// x_[Zw] = state_->pos()(2); + +// //std::cout << "State position is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; + +// // Nat - end + +// //state_->quat().normalize(); +// state_->quat().set(roll_, pitch_, state_->quat().yaw()); // Does setting the state quaternion update the state pos? Maybe that is why it is updating and I'm not seeing the change in value.. but the state quaternion isnt changing value here... so I'm not sure if that's the reason why +// quat_local_ = state_->quat() * quat_world_inverse_; +// quat_local_.normalize(); +// x_[q0] = quat_local_.w(); +// x_[q1] = quat_local_.x(); +// x_[q2] = quat_local_.y(); +// x_[q3] = quat_local_.z(); + +// Eigen::Vector3d force_body = quat_local_.rotate_reverse(ext_force_); +// Eigen::Vector3d ext_moment_body = ext_moment_; +// ext_force_ = Eigen::Vector3d::Zero(); +// ext_moment_ = Eigen::Vector3d::Zero(); + +// x_[U] = speed_ + force_body(0) / mass_; +// x_[V] = force_body(1) / mass_; +// x_[W] = force_body(2) / mass_; + +// double turn_rate = 0; +// if (std::abs(speed_) >= std::numeric_limits::epsilon()) { +// turn_rate = -g_ / speed_ * tan(roll_); +// } + +// x_[P] = ext_moment_body(0) / mass_; +// x_[Q] = ext_moment_body(1) / mass_; +// x_[R] = ext_moment_body(2) / mass_ + turn_rate; + +// auto stop = high_resolution_clock::now(); + +// auto startode = high_resolution_clock::now(); + +// auto duration = duration_cast(stop - start); +// std::cout << "Execution time before ode_step: " << duration.count() << std::endl; + +// std::cout << "State position before ODE is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; +// //std::cout << "State position value: (" << state_->pos()(0) << ", " << state_->pos()(1) << ", " << state_->pos()(2) << ")" << std::endl; //Cannot use the state variable in the matrix, have to use the local x[] value because that is what gets updated by the ode step function + +// //This ODE step function is what causes the local variables to update +// ode_step(dt); //see if this needs to be called for each entity, or if it can be called once + + +// std::cout << "State position after ODE is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; + +// auto stopode = high_resolution_clock::now(); +// auto startlast = high_resolution_clock::now(); + +// auto durationode = duration_cast(stopode - startode); +// std::cout << "Execution time of ode_step: " << durationode.count() << std::endl; + +// //std::cout << "State position value: (" << state_->pos()(0) << ", " << state_->pos()(1) << ", " << state_->pos()(2) << ")" << std::endl; + +// // Normalize quaternion +// quat_local_.w() = x_[q0]; // Nat - the back and forth of setting these variables seems redundant, why do we do this? +// quat_local_.x() = x_[q1]; +// quat_local_.y() = x_[q2]; +// quat_local_.z() = x_[q3]; +// quat_local_.normalize(); + +// x_[q0] = quat_local_.w(); // Nat - what are these local variables used for? Not seeing anything else in this function... I looked into entity interaction plugins, and am not seeing use of x_[q0] through q3 +// x_[q1] = quat_local_.x(); // the couts are not picking up any changes in values, maybe it is because they are only going straight and not turning? +// x_[q2] = quat_local_.y(); +// x_[q3] = quat_local_.z(); + +// Eigen::Vector3d vel_local(x_[U], x_[V], x_[W]); + +// // Convert local coordinates to world coordinates +// state_->quat() = quat_local_ * quat_world_; +// state_->quat().normalize(); // Must be normalized in order to compute vector rotations and be represented as a valid 3D orientation +// state_->pos() << x_[Xw], x_[Yw], x_[Zw]; + +// state_->vel() << state_->quat().toRotationMatrix() * vel_local; + +// speed_ = vel_local.norm(); + +// if (write_csv_) { +// // Log state to CSV +// csv_.append(sc::CSV::Pairs{ +// {"t", t}, +// {"x", x_[Xw]}, +// {"y", x_[Yw]}, +// {"z", x_[Zw]}, +// {"U", x_[U]}, +// {"V", x_[V]}, +// {"W", x_[W]}, +// {"P", x_[P]}, +// {"Q", x_[Q]}, +// {"R", x_[R]}, +// {"roll", state_->quat().roll()}, +// {"pitch", state_->quat().pitch()}, +// {"yaw", state_->quat().yaw()}, +// {"speed", speed_}, +// {"Uw", state_->vel()(0)}, +// {"Vw", state_->vel()(1)}, +// {"Ww", state_->vel()(2)}}); +// } + +// auto stoplast = high_resolution_clock::now(); +// auto durationlast = duration_cast(stoplast - startlast); +// std::cout << "Execution time after ode_step: " << durationlast.count() << std::endl; + +// auto stopTot = high_resolution_clock::now(); +// auto durationfinal = duration_cast(stopTot - startTot); +// std::cout << "Total execution time: " << durationfinal.count() << std::endl; + +// return true; +// } + +bool DubinsAirplane3D::step(double t, double dt, int iteration) { + + //Commenting out lines that do not directly affect the state only decreases the wall time by 0.023 seconds + + // Get inputs and saturate + if(iteration == 1){ + speed_ = boost::algorithm::clamp(vars_.input(desired_speed_idx_), speed_min_, speed_max_); + pitch_ = vars_.input(desired_pitch_idx_); + roll_ = vars_.input(desired_roll_idx_); + + // Nat - These are not used anywhere, but maybe like below Xw they are somewhere + x_[Uw] = state_->vel()(0); + x_[Vw] = state_->vel()(1); + x_[Ww] = state_->vel()(2); + // Nat - end + + // Nat - no further calculations are done on Xw, Yw, and Zw. Then, state_->pos is set back + // to these values... is this necessary? + x_[Xw] = state_->pos()(0); + x_[Yw] = state_->pos()(1); + x_[Zw] = state_->pos()(2); + + //std::cout << "State position is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; + + // Nat - end + + // state_->quat().normalize(); + state_->quat().set(roll_, pitch_, state_->quat().yaw()); // Does setting the state quaternion update the state pos? Maybe that is why it is updating and I'm not seeing the change in value.. but the state quaternion isnt changing value here... so I'm not sure if that's the reason why + quat_local_ = state_->quat() * quat_world_inverse_; + quat_local_.normalize(); + x_[q0] = quat_local_.w(); + x_[q1] = quat_local_.x(); + x_[q2] = quat_local_.y(); + x_[q3] = quat_local_.z(); + + Eigen::Vector3d force_body = quat_local_.rotate_reverse(ext_force_); + Eigen::Vector3d ext_moment_body = ext_moment_; + ext_force_ = Eigen::Vector3d::Zero(); + ext_moment_ = Eigen::Vector3d::Zero(); + + x_[U] = speed_ + force_body(0) / mass_; + x_[V] = force_body(1) / mass_; + x_[W] = force_body(2) / mass_; + + double turn_rate = 0; + if (std::abs(speed_) >= std::numeric_limits::epsilon()) { + turn_rate = -g_ / speed_ * tan(roll_); + } + + x_[P] = ext_moment_body(0) / mass_; + x_[Q] = ext_moment_body(1) / mass_; + x_[R] = ext_moment_body(2) / mass_ + turn_rate; + + std::cout << "State position before ODE is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; + //std::cout << "State position value: (" << state_->pos()(0) << ", " << state_->pos()(1) << ", " << state_->pos()(2) << ")" << std::endl; //Cannot use the state variable in the matrix, have to use the local x[] value because that is what gets updated by the ode step function + + //Only call the ODE step once and see if it updates for all variables + //I called the ODE only once, using the last entity as reference, and it does not update + // all variables... it must be called by each object + //This ODE step function is what causes the local variables to update + + ode_step(dt); //see if this needs to be called for each entity, or if it can be called once + } + + else { + + std::cout << "In the second iteration" << std::endl; + std::cout << "State position after ODE is: (" << x_[Xw] << ", " << x_[Yw] << ", " << x_[Zw] << ") for id: " << parent()->id().id() << std::endl; + //std::cout << "State position value: (" << state_->pos()(0) << ", " << state_->pos()(1) << ", " << state_->pos()(2) << ")" << std::endl; + + + // Normalize quaternion + quat_local_.w() = x_[q0]; // Nat - the back and forth of setting these variables seems redundant, why do we do this? + quat_local_.x() = x_[q1]; + quat_local_.y() = x_[q2]; + quat_local_.z() = x_[q3]; + quat_local_.normalize(); + + x_[q0] = quat_local_.w(); + x_[q1] = quat_local_.x(); + x_[q2] = quat_local_.y(); + x_[q3] = quat_local_.z(); + + Eigen::Vector3d vel_local(x_[U], x_[V], x_[W]); + + // Convert local coordinates to world coordinates + state_->quat() = quat_local_ * quat_world_; + state_->quat().normalize(); // Must be normalized in order to compute vector rotations and be represented as a valid 3D orientation + state_->pos() << x_[Xw], x_[Yw], x_[Zw]; + + state_->vel() << state_->quat().toRotationMatrix() * vel_local; + + speed_ = vel_local.norm(); + + if (write_csv_) { + // Log state to CSV + csv_.append(sc::CSV::Pairs{ + {"t", t}, + {"x", x_[Xw]}, + {"y", x_[Yw]}, + {"z", x_[Zw]}, + {"U", x_[U]}, + {"V", x_[V]}, + {"W", x_[W]}, + {"P", x_[P]}, + {"Q", x_[Q]}, + {"R", x_[R]}, + {"roll", state_->quat().roll()}, + {"pitch", state_->quat().pitch()}, + {"yaw", state_->quat().yaw()}, + {"speed", speed_}, + {"Uw", state_->vel()(0)}, + {"Vw", state_->vel()(1)}, + {"Ww", state_->vel()(2)}}); + } + } + + return true; +} + void DubinsAirplane3D::model(const vector_t &x , vector_t &dxdt , double t) { dxdt[U] = 0; dxdt[V] = 0; @@ -230,7 +736,6 @@ void DubinsAirplane3D::model(const vector_t &x , vector_t &dxdt , double t) { dxdt[P] = 0; dxdt[Q] = 0; dxdt[R] = 0; - double lambda = 1 - (pow(x[q0], 2) + pow(x[q1], 2) + pow(x[q2], 2) + pow(x[q3], 2)); dxdt[q0] = -0.5 * (x[q1]*x[P] + x[q2]*x[Q] + x[q3]*x[R]) + lambda * x[q0]; dxdt[q1] = +0.5 * (x[q0]*x[P] + x[q2]*x[R] - x[q3]*x[Q]) + lambda * x[q1]; diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index b8cb504e3a..6dd5ba8948 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -76,6 +76,8 @@ #include // NOLINT #include // NOLINT +#include + #if ENABLE_PYTHON_BINDINGS == 1 #include #ifdef __clang__ @@ -107,6 +109,8 @@ using std::cout; using std::endl; using NormDistribution = std::normal_distribution; +using namespace std::chrono; + namespace scrimmage { SimControl::SimControl() : @@ -206,7 +210,7 @@ bool SimControl::init(const std::string& mission_file, return true; } -void SimControl::request_screenshot() { +void SimControl::request_screenshot() { //Natalie for next project prev_paused_ = paused(); pause(true); scrimmage_proto::GUIMsg gui_msg; @@ -563,7 +567,7 @@ bool SimControl::run_single_step(const int& loop_number) { run_callbacks(sim_plugin_); - if (screenshot_task_.update(t_).first) { + if (screenshot_task_.update(t_).first) { //Natlie, investigate when this is called request_screenshot(); } @@ -1514,9 +1518,6 @@ bool SimControl::add_tasks(Task::Type type, double t, double dt) { } entity_pool_mutex_.unlock(); - // tell the threads to run - entity_pool_condition_var_.notify_all(); - // wait for results auto get = [&](auto &future) {return future.get();}; return std::all_of(futures.begin(), futures.end(), get); @@ -1536,7 +1537,6 @@ bool SimControl::run_entities() { } }; - // run autonomies threaded or in a single thread if (entity_thread_types_.count(Task::Type::AUTONOMY)) { success &= add_tasks(Task::Type::AUTONOMY, t_, dt_); } else { @@ -1565,11 +1565,59 @@ bool SimControl::run_entities() { auto step_all = [&](Task::Type type, auto getter) { if (entity_thread_types_.count(type)) { success &= add_tasks(type, temp_t, motion_dt); - } else { + } + else { + int entTracker = 0; //should be able to replace this with the actual entity id number later + for (EntityPtr &ent : ents_) { - auto step = [&](auto p){return p->step(temp_t, motion_dt);}; - success &= exec_step(getter(ent), step); + //Tracks which entity we are on for the if statement + entTracker++; //Not a good method for multiple teams, need to figure something else out. All teams are part of ents_ + + // Tracks which entity is the first entity for a given team + if(!stepIterDone && (ent_desc_to_id_map.count(ent->id().sub_swarm_id())==0)){ + ent_desc_to_id_map.insert({ent->id().sub_swarm_id(), entTracker}); + } + + // Assigns the ODE step value for how many steps from the entity's ode_step function should be called before depending + // on state updates from the team's first entity's ode_step offset vector + if(!stepIterDone){ + if(mp_->entity_attributes()[ent->id().sub_swarm_id()]["motion_model"]["ode_step_count"] == ""){ + ent_id_to_ode_step[ent->id().id()] = 0; + } + else{ + ent_id_to_ode_step[ent->id().id()] = stoi(mp_->entity_attributes()[ent->id().sub_swarm_id()]["motion_model"]["ode_step_count"]); + } + } + + /////////////////////////////////////////////////////////////////////////////////////// + // Method that uses list tracker + /////////////////////////////////////////////////////////////////////////////////////// + //ent_desc_id can be accessed by: ent->id().sub_swarm_id() + auto step = [&](auto p){ + if(mp_->entity_attributes()[ent->id().sub_swarm_id()]["motion_model"]["grouped_model"] == "true"){ + if((ent_desc_to_id_map[ent->id().sub_swarm_id()] != entTracker) && (ent->get_stepsCalled() >= ent_id_to_ode_step[ent->id().id()])){ // If more than 10 steps have been taken by the entity, then utilize entity ID #1's step function offset + return p->step(temp_t, motion_dt, ent_desc_to_ode_vector[ent->id().sub_swarm_id()]); + } + else if((ent_desc_to_id_map[ent->id().sub_swarm_id()] != entTracker) && (ent->get_stepsCalled() < ent_id_to_ode_step[ent->id().id()])){ // Enitity utilizes its own step function for the first 10 steps + ent->incrementSteps(); + return p->step(temp_t, motion_dt); //original step function + } + else{ //If entity is #1, run the ODE step function + //could the get_stepscalled check be applied here also? Wondering if adding this in the longterm would add + // to the run-time processing due to condition it has to check everytime + bool stepReturn = p->offset_step(temp_t, motion_dt); //odestepvals update function just for entity #1 + ent_desc_to_ode_vector[ent->id().sub_swarm_id()] = p->getOdeStepVal(); + return stepReturn; + } + } + else{ + return p->step(temp_t, motion_dt); //original step function + } + }; + + success &= exec_step(getter(ent), step); } + stepIterDone = true; } }; step_all(Task::Type::MOTION, [&](auto ent){return ent->motion();});