diff --git a/README.md b/README.md index 20a59108f9..28ea0d4e86 100644 --- a/README.md +++ b/README.md @@ -248,7 +248,7 @@ The FGMS plugin interacts with SCRIMMAGE to receive the state variables of each entity. To build FGMS, you have to provide SCRIMMAGE's CMake project the path to the FGMS root source: - $ cmake .. -DFGMS_SOURCE_TREE_BASE=/path/to/fgms + $ cmake .. -DFGMS_ROOT_SEARCH=/path/to/fgms ## Running SCRIMMAGE inside of Docker diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 9f0f5b658f..35782fc2e7 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -83,6 +83,7 @@ class MissionParse { public: bool create_log_dir(); void set_overrides(const std::string &overrides); + void set_overrides(const std::map& overrides); bool parse(const std::string &filename); bool write(const std::string &filename); diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h index 946bbb4686..22559fdc05 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h @@ -67,6 +67,12 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase { bool init(std::map &info, std::map ¶ms) override; bool step(double time, double dt) override; + void teleport(StatePtr &state) override; + + void set_jsbsim_initial_state(const scrimmage::State& state); + void set_jsbsim_state(const scrimmage::State& state); + + std::shared_ptr jsbsim_exec(); protected: #if ENABLE_JSBSIM == 1 @@ -85,6 +91,7 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase { JSBSim::FGPropertyNode *ap_elevator_cmd_node_ = nullptr; JSBSim::FGPropertyNode *ap_rudder_cmd_node_ = nullptr; JSBSim::FGPropertyNode *ap_throttle_cmd_node_ = nullptr; + JSBSim::FGPropertyNode *ap_throttle_1_cmd_node_ = nullptr; JSBSim::FGPropertyNode *vel_north_node_ = nullptr; JSBSim::FGPropertyNode *vel_east_node_ = nullptr; @@ -115,9 +122,19 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase { double delta_aileron_ = 0; double delta_rudder_ = 0; - double draw_vel_ = 0; - double draw_ang_vel_ = 0; - double draw_acc_ = 0; + bool draw_vel_ = false; + bool draw_ang_vel_ = false; + bool draw_acc_ = false; + + double jsbsim_dt_ = 0.0083333; + std::string jsbsim_script_path_ = ""; + + bool init_values_cached_ = false; + double init_thrust_ = 0; + double init_fuel_rate_gph_ = 0; + double init_fuel_used_lbs_ = 0; + double init_total_fuel_lbs_ = 0; + #endif }; } // namespace motion diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml index 01fb37b2b7..fe865839df 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml @@ -22,4 +22,8 @@ 5600 UDP + false + false + false + diff --git a/missions/joystick-jsbsim.xml b/missions/joystick-jsbsim.xml index f562eda911..03d70f988e 100644 --- a/missions/joystick-jsbsim.xml +++ b/missions/joystick-jsbsim.xml @@ -1,47 +1,47 @@ - - - - + 50051 localhost - - none - + + none + 10 1000 - + mcmillan 191 191 191 10 - + false all false - - ~/.scrimmage/logs - - 35.721025 - -120.767925 + + ~/.scrimmage/logs + + 33.784887 + -84.374431 300 true 10 - + SimpleCollision SimpleCollisionMetrics - + 1714060370 - + - 1 + 1 77 77 255 1 1 @@ -49,37 +49,33 @@ 20 20 10 - + -1000 0 200 - 0 + 0 - - + 240 + + JoystickAutonomy - DirectController - - - - - JSBSimControl - rascal_no_autopilot.xml - zephyr-blue - + "[ throttle 2 -32767 +32767 -1 +1 -1 ] + [ elevator 1 -32767 +32767 -1 +1 -1 ] + [ aileron 0 -32767 +32767 -1 +1 +1 ] + [ rudder 3 -32767 +32767 -1 +1 -1 ]" + >JoystickAutonomy + + + DirectController + + + JSBSimControl + + f16_scrimmage.xml + zephyr-blue + diff --git a/python/scrimmage/bindings/src/py_openai_env.cpp b/python/scrimmage/bindings/src/py_openai_env.cpp index df6bf97e78..e6f9e3b9c7 100644 --- a/python/scrimmage/bindings/src/py_openai_env.cpp +++ b/python/scrimmage/bindings/src/py_openai_env.cpp @@ -256,7 +256,7 @@ void ScrimmageOpenAIEnv::reset_scrimmage(bool enable_gui) { simcontrol_->mp()->set_network_gui(true); simcontrol_->mp()->set_enable_gui(false); } else { - simcontrol_->mp()->set_time_warp(0); + // simcontrol_->mp()->set_time_warp(0); } // users may have forgotten to turn off nonlearning_mode. // If this code is being called then it should never be nonlearning_mode diff --git a/python/scrimmage/utils.py b/python/scrimmage/utils.py index 0aa509a835..0847db33d3 100644 --- a/python/scrimmage/utils.py +++ b/python/scrimmage/utils.py @@ -49,9 +49,21 @@ def find_mission(fname): Raises a StopIteration exception when it cannot find the file. """ - files = (os.path.join(path, fname) - for path in os.environ["SCRIMMAGE_MISSION_PATH"].split(':')) - return next((f for f in files if os.path.exists(f))) + + if "SCRIMMAGE_MISSION_PATH" not in os.environ: + print('WARNING: SCRIMMAGE_MISSION_PATH is empty.') + print('Please source ~/.scrimmage/setup.sh') + + dirs = [ path for path in os.environ["SCRIMMAGE_MISSION_PATH"].split(':') ] + + # Recursively search over directories for mission file + for dir in dirs: + for dirpath, dirnames, files in os.walk(dir): + for name in files: + if name == fname: + return os.path.join(dirpath, name) + + return None def tornado(ax, data, labels, base=None, bar_width=0.8, whitespace_buffer=0.1): diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index afee8f2c76..7f3d2876ff 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -148,80 +148,6 @@ bool Entity::init(AttributeMap &overrides, mp_->entity_params()[id] = info; mp_->ent_id_to_block_id()[id] = ent_desc_id; - //////////////////////////////////////////////////////////// - // sensor - //////////////////////////////////////////////////////////// - // The MissionParser appends the order number to the sensor (e.g., sensor0, - // sensor1, etc.) - int sensor_ct = 0; - std::string sensor_order_name = std::string("sensor") + - std::to_string(sensor_ct); - - while (info.count(sensor_order_name) > 0) { - ConfigParse config_parse; - std::string sensor_name = info[sensor_order_name]; - PluginStatus status = - plugin_manager->make_plugin("scrimmage::Sensor", - sensor_name, *file_search, - config_parse, - overrides[sensor_order_name], - plugin_tags); - if (status.status == PluginStatus::cast_failed) { - std::cout << "Failed to open sensor plugin: " << sensor_name - << std::endl; - return false; - } else if (status.status == PluginStatus::parse_failed) { - return false; - } else if (status.status == PluginStatus::loaded) { - SensorPtr sensor = status.plugin; - - // Get sensor's offset from entity origin - std::vector tf_xyz = {0.0, 0.0, 0.0}; - auto it_xyz = overrides[sensor_order_name].find("xyz"); - if (it_xyz != overrides[sensor_order_name].end()) { - str2container(it_xyz->second, " ", tf_xyz, 3); - } - sensor->transform()->pos() << tf_xyz[0], tf_xyz[1], tf_xyz[2]; - - // Get sensor's orientation relative to entity's coordinate frame - std::vector tf_rpy = {0.0, 0.0, 0.0}; - auto it_rpy = overrides[sensor_order_name].find("rpy"); - if (it_rpy != overrides[sensor_order_name].end()) { - str2container(it_rpy->second, " ", tf_rpy, 3); - } - sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), - Angles::deg2rad(tf_rpy[1]), - Angles::deg2rad(tf_rpy[2])); - - sensor->set_parent(parent); - sensor->set_pubsub(pubsub); - sensor->set_time(time); - sensor->set_id_to_team_map(id_to_team_map); - sensor->set_id_to_ent_map(id_to_ent_map); - sensor->set_param_server(param_server); - param_override_func(config_parse.params()); - - // get loop rate from plugin's params - auto it_loop_rate = config_parse.params().find("loop_rate"); - if (it_loop_rate != config_parse.params().end()) { - const double loop_rate = std::stod(it_loop_rate->second); - sensor->set_loop_rate(loop_rate); - } - - std::string given_name = sensor_name + std::to_string(sensor_ct); - sensor->set_name(given_name); - - if (debug_level > 1) { - cout << "--------------------------------" << endl; - cout << "Sensor plugin params: " << given_name << endl; - cout << config_parse; - } - sensor->init(config_parse.params()); - sensors_[given_name] = sensor; - } - sensor_order_name = std::string("sensor") + std::to_string(++sensor_ct); - } - //////////////////////////////////////////////////////////// // motion model //////////////////////////////////////////////////////////// @@ -460,6 +386,81 @@ bool Entity::init(AttributeMap &overrides, controllers_.front()->set_desired_state(autonomies_.front()->desired_state()); } } + + //////////////////////////////////////////////////////////// + // sensor + //////////////////////////////////////////////////////////// + // The MissionParser appends the order number to the sensor (e.g., sensor0, + // sensor1, etc.) + int sensor_ct = 0; + std::string sensor_order_name = std::string("sensor") + + std::to_string(sensor_ct); + + while (info.count(sensor_order_name) > 0) { + ConfigParse config_parse; + std::string sensor_name = info[sensor_order_name]; + PluginStatus status = + plugin_manager->make_plugin("scrimmage::Sensor", + sensor_name, *file_search, + config_parse, + overrides[sensor_order_name], + plugin_tags); + if (status.status == PluginStatus::cast_failed) { + std::cout << "Failed to open sensor plugin: " << sensor_name + << std::endl; + return false; + } else if (status.status == PluginStatus::parse_failed) { + return false; + } else if (status.status == PluginStatus::loaded) { + SensorPtr sensor = status.plugin; + + // Get sensor's offset from entity origin + std::vector tf_xyz = {0.0, 0.0, 0.0}; + auto it_xyz = overrides[sensor_order_name].find("xyz"); + if (it_xyz != overrides[sensor_order_name].end()) { + str2container(it_xyz->second, " ", tf_xyz, 3); + } + sensor->transform()->pos() << tf_xyz[0], tf_xyz[1], tf_xyz[2]; + + // Get sensor's orientation relative to entity's coordinate frame + std::vector tf_rpy = {0.0, 0.0, 0.0}; + auto it_rpy = overrides[sensor_order_name].find("rpy"); + if (it_rpy != overrides[sensor_order_name].end()) { + str2container(it_rpy->second, " ", tf_rpy, 3); + } + sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), + Angles::deg2rad(tf_rpy[1]), + Angles::deg2rad(tf_rpy[2])); + + sensor->set_parent(parent); + sensor->set_pubsub(pubsub); + sensor->set_time(time); + sensor->set_id_to_team_map(id_to_team_map); + sensor->set_id_to_ent_map(id_to_ent_map); + sensor->set_param_server(param_server); + param_override_func(config_parse.params()); + + // get loop rate from plugin's params + auto it_loop_rate = config_parse.params().find("loop_rate"); + if (it_loop_rate != config_parse.params().end()) { + const double loop_rate = std::stod(it_loop_rate->second); + sensor->set_loop_rate(loop_rate); + } + + std::string given_name = sensor_name + std::to_string(sensor_ct); + sensor->set_name(given_name); + + if (debug_level > 1) { + cout << "--------------------------------" << endl; + cout << "Sensor plugin params: " << given_name << endl; + cout << config_parse; + } + sensor->init(config_parse.params()); + sensors_[given_name] = sensor; + } + sensor_order_name = std::string("sensor") + std::to_string(++sensor_ct); + } + return true; } diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 8de608ddf0..56453ff146 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -80,6 +80,10 @@ void MissionParse::set_overrides(const std::string &overrides) { } } +void MissionParse::set_overrides(const std::map& overrides) { + overrides_map_.insert(overrides.begin(), overrides.end()); +} + bool MissionParse::parse(const std::string &filename) { mission_filename_ = expand_user(filename); diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 54a3a20c74..53975648f7 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -90,9 +90,12 @@ std::tuple JSBSimControl::version() { bool JSBSimControl::init(std::map &info, std::map ¶ms) { - draw_vel_ = sc::get("drawVel", params, 1.0); - draw_ang_vel_ = sc::get("drawAngVel", params, 10.0); - draw_acc_ = sc::get("drawAcc", params, 1.0); + draw_vel_ = sc::get("draw_vel", params, draw_vel_); + draw_ang_vel_ = sc::get("draw_ang_vel", params, draw_ang_vel_); + draw_acc_ = sc::get("draw_acc", params, draw_acc_); + + jsbsim_dt_ = std::stod(info["dt"])/std::stod(info["motion_multiplier"]); + jsbsim_script_path_ = "scripts/" + info["script_name"]; // Setup variable index for controllers throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::In); @@ -128,71 +131,7 @@ bool JSBSimControl::init(std::map &info, exec_->SetEnginePath(SGPath("engine")); exec_->SetSystemsPath(SGPath("systems")); - exec_->LoadScript(SGPath("scripts/"+info["script_name"])); - - JSBSim::FGInitialCondition *ic = exec_->GetIC(); - - Quaternion q_ned_enu(M_PI, 0.0, M_PI/2.0); - Quaternion q_flu_frd(M_PI, 0.0, 0.0); - Quaternion q_frd_enu(q_ned_enu * state_->quat() * q_flu_frd); - ic->SetPsiRadIC(q_frd_enu.yaw()); - ic->SetThetaRadIC(q_frd_enu.pitch()); - ic->SetPhiRadIC(q_frd_enu.roll()); - - ic->SetVEastFpsIC(state_->vel()[0] * meters2feet); - ic->SetVNorthFpsIC(state_->vel()[1] * meters2feet); - ic->SetVDownFpsIC(-state_->vel()[2] * meters2feet); - - ic->SetTerrainElevationFtIC(parent_->projection()->HeightOrigin() * meters2feet); - - Eigen::Vector3d lla; - parent_->projection()->Reverse(state_->pos()[0], state_->pos()[1], state_->pos()[2], lla[0], lla[1], lla[2]); - ic->SetLatitudeDegIC(lla[0]); - ic->SetLongitudeDegIC(lla[1]); - ic->SetAltitudeASLFtIC(lla[2] * meters2feet); - -#if 0 - cout << "--------------------------------------------------------" << endl; - cout << " State information in JSBSImControl" << endl; - cout << "--------------------------------------------------------" << endl; - int prec = 5; - cout << std::setprecision(prec) << "state_->quat(): " << state_->quat() << endl; - cout << std::setprecision(prec) << "lla[0]: " << lla[0] << endl; - cout << std::setprecision(prec) << "lla[1]: " << lla[1] << endl; - cout << std::setprecision(prec) << "lla[2]: " << lla[2] << endl; - cout << std::setprecision(prec) << "lla[0]: " << lla[0] << endl; - cout << std::setprecision(prec) << "lla[1]: " << lla[1] << endl; - cout << std::setprecision(prec) << "lla[2]: " << lla[2] << endl; - - cout << std::setprecision(prec) << "GetVEastFpsIC: " << ic->GetVEastFpsIC() << endl; - cout << std::setprecision(prec) << "GetVNorthFpsIC: " << ic->GetVNorthFpsIC() << endl; - cout << std::setprecision(prec) << "GetVDownFpsIC: " << ic->GetVDownFpsIC() << endl; - cout << std::setprecision(prec) << "GetPsiRadIC: " << ic->GetPsiRadIC() << endl; - cout << std::setprecision(prec) << "GetThetaRadIC: " << ic->GetThetaRadIC() << endl; - cout << std::setprecision(prec) << "GetPhiRadIC: " << ic->GetPhiRadIC() << endl; - cout << std::setprecision(prec) << "GetLatitudeDegIC: " << ic->GetLatitudeDegIC() << endl; - cout << std::setprecision(prec) << "GetLongitudeDegIC: " << ic->GetLongitudeDegIC() << endl; - cout << std::setprecision(prec) << "GetAltitudeASLFtIC: " << ic->GetAltitudeASLFtIC() << endl; -#endif - - if (info.count("latitude") > 0) { - ic->SetLatitudeDegIC(std::stod(info["latitude"])); - } - if (info.count("longitude") > 0) { - ic->SetLongitudeDegIC(std::stod(info["longitude"])); - } - if (info.count("heading") > 0) { - angles_to_jsbsim_.set_angle(std::stod(info["heading"])); - ic->SetPsiDegIC(angles_to_jsbsim_.angle()); - } - if (info.count("altitude") > 0) { - double alt_asl_meters = std::stod(info["altitude"]); - ic->SetAltitudeASLFtIC(alt_asl_meters * meters2feet); - } - - exec_->RunIC(); - exec_->Setdt(std::stod(info["dt"])/std::stod(info["motion_multiplier"])); - exec_->Run(); + exec_->LoadScript(SGPath(jsbsim_script_path_)); // Get references to each of the nodes that hold properties that we // care about @@ -210,6 +149,7 @@ bool JSBSimControl::init(std::map &info, ap_elevator_cmd_node_ = mgr->GetNode("fcs/elevator-cmd-norm"); ap_rudder_cmd_node_ = mgr->GetNode("fcs/rudder-cmd-norm"); ap_throttle_cmd_node_ = mgr->GetNode("fcs/throttle-cmd-norm"); + // ap_throttle_1_cmd_node_ = mgr->GetNode("fcs/throttle-cmd-norm[1]"); vel_north_node_ = mgr->GetNode("velocities/v-north-fps"); vel_east_node_ = mgr->GetNode("velocities/v-east-fps"); @@ -217,7 +157,6 @@ bool JSBSimControl::init(std::map &info, u_vel_node_ = mgr->GetNode("velocities/u-fps"); - // angular velocity in ECEF frame p_node_ = mgr->GetNode("velocities/p-rad_sec"); q_node_ = mgr->GetNode("velocities/q-rad_sec"); @@ -228,6 +167,22 @@ bool JSBSimControl::init(std::map &info, ay_pilot_node_ = mgr->GetNode("accelerations/a-pilot-y-ft_sec2"); az_pilot_node_ = mgr->GetNode("accelerations/a-pilot-z-ft_sec2"); + set_jsbsim_initial_state(*state_); + + // if (info.count("latitude") > 0) { + // ic->SetLatitudeDegIC(std::stod(info["latitude"])); + // } + // if (info.count("longitude") > 0) { + // ic->SetLongitudeDegIC(std::stod(info["longitude"])); + // } + // if (info.count("heading") > 0) { + // angles_to_jsbsim_.set_angle(std::stod(info["heading"])); + // ic->SetPsiDegIC(angles_to_jsbsim_.angle()); + // } + // if (info.count("altitude") > 0) { + // double alt_asl_meters = std::stod(info["altitude"]); + // ic->SetAltitudeASLFtIC(alt_asl_meters * meters2feet); + // } // Save state parent_->projection()->Forward(latitude_node_->getDoubleValue(), @@ -255,15 +210,17 @@ bool JSBSimControl::init(std::map &info, -az_pilot_node_->getDoubleValue()); linear_accel_body_ = state_->quat().rotate(a_FLU); + // exec_->PrintPropertyCatalog(); + // exec_->PrintSimulationConfiguration(); + return true; } bool JSBSimControl::step(double time, double dt) { - - throttle_ = ba::clamp(vars_.input(throttle_idx_), -1.0, 1.0); - delta_elevator_ = ba::clamp(vars_.input(elevator_idx_), -1.0, 1.0); - delta_aileron_ = ba::clamp(vars_.input(aileron_idx_), -1.0, 1.0); - delta_rudder_ = ba::clamp(vars_.input(rudder_idx_), -1.0, 1.0); + throttle_ = vars_.input(throttle_idx_); + delta_elevator_ = vars_.input(elevator_idx_); + delta_aileron_ = vars_.input(aileron_idx_); + delta_rudder_ = vars_.input(rudder_idx_); // TODO: for some reason, jsb sim does not like it when there is an immediate thottle input if (time < .05) @@ -273,12 +230,16 @@ bool JSBSimControl::step(double time, double dt) { ap_elevator_cmd_node_->setDoubleValue(delta_elevator_); ap_rudder_cmd_node_->setDoubleValue(delta_rudder_); ap_throttle_cmd_node_->setDoubleValue(throttle_); + // if (ap_throttle_1_cmd_node_ != nullptr) { + // ap_throttle_1_cmd_node_->setDoubleValue(throttle_); + // } exec_->Setdt(dt); exec_->Run(); - - + if (fg_out_enable_) { + output_fg_->Print(); + } /////////////////////////////////////////////////////////////////////////// // Save state @@ -352,23 +313,28 @@ bool JSBSimControl::step(double time, double dt) { cout << "--------------------------------------------------------" << endl; int prec = 5; // std::cout << "processing time, ms: " << ((double)time_diff.total_microseconds())/1000 << std::endl; - cout << std::setprecision(prec) << "dt: " << dt << endl; - cout << std::setprecision(prec) << "time: " << time << endl; - cout << std::setprecision(prec) << "Altitude AGL: " << altitudeAGL_node_->getDoubleValue() * feet2meters << endl; + // cout << std::setprecision(prec) << "dt: " << dt << endl; + // cout << std::setprecision(prec) << "time: " << time << endl; + // cout << std::setprecision(prec) << "Altitude AGL: " << altitudeAGL_node_->getDoubleValue() * feet2meters << endl; // cout << std::setprecision(prec) << "WOW[0]: " << mgr->GetNode("gear/unit/WOW")->getDoubleValue() << endl; // cout << std::setprecision(prec) << "WOW[1]: " << mgr->GetNode("gear/unit[1]/WOW")->getDoubleValue() << endl; // cout << std::setprecision(prec) << "WOW[2]: " << mgr->GetNode("gear/unit[2]/WOW")->getDoubleValue() << endl; + cout << "Speed: " << state_->vel().norm() << endl; cout << std::setprecision(prec) << "xAccel: " << linear_accel_body_(0) << endl; cout << std::setprecision(prec) << "yAccel: " << linear_accel_body_(1) << endl; cout << std::setprecision(prec) << "zAccel: " << linear_accel_body_(2) << endl; - cout << std::setprecision(prec) << "aileron cmd: " << delta_aileron_ << endl; - cout << std::setprecision(prec) << "elevator cmd: " << delta_elevator_ << endl; - cout << std::setprecision(prec) << "rudder cmd: " << delta_rudder_ << endl; - cout << std::setprecision(prec) << "throttle cmd: " << throttle_ << endl; + cout << std::setprecision(prec) << "desired aileron cmd: " << delta_aileron_ << endl; + cout << std::setprecision(prec) << "desired elevator cmd: " << delta_elevator_ << endl; + cout << std::setprecision(prec) << "desired rudder cmd: " << delta_rudder_ << endl; + cout << std::setprecision(prec) << "desired throttle cmd: " << throttle_ << endl; cout << std::setprecision(prec) << "aileron jsb: " << mgr->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue() << endl; cout << std::setprecision(prec) << "elevator jsb: " << mgr->GetNode("fcs/elevator-pos-rad")->getDoubleValue() << endl; cout << std::setprecision(prec) << "rudder jsb: " << mgr->GetNode("fcs/rudder-pos-rad")->getDoubleValue() << endl; - cout << std::setprecision(prec) << "throttle jsb: " << mgr->GetNode("fcs/throttle-cmd-norm")->getDoubleValue() << endl; + cout << std::setprecision(prec) << "fcs/throttle-cmd-norm: " << mgr->GetNode("fcs/throttle-cmd-norm")->getDoubleValue() << endl; + cout << std::setprecision(prec) << "fcs/aileron-cmd-norm: " << mgr->GetNode("fcs/aileron-cmd-norm")->getDoubleValue() << endl; + cout << std::setprecision(prec) << "fcs/elevator-cmd-norm: " << mgr->GetNode("fcs/elevator-cmd-norm")->getDoubleValue() << endl; + cout << std::setprecision(prec) << "fcs/rudder-cmd-norm: " << mgr->GetNode("fcs/rudder-cmd-norm")->getDoubleValue() << endl; + cout << std::setprecision(prec) << "thrust (N): " << mgr->GetNode("propulsion/engine/thrust-lbs")->getDoubleValue()*4.44 << endl; cout << std::setprecision(prec) << "alpha: " << mgr->GetNode("aero/alpha-rad")->getDoubleValue() << endl; cout << std::setprecision(prec) << "drag: " << mgr->GetNode("forces/fwx-aero-lbs")->getDoubleValue() << endl; @@ -390,5 +356,124 @@ bool JSBSimControl::step(double time, double dt) { return true; } + +void JSBSimControl::teleport(StatePtr &state) { + // set_jsbsim_state(*state); + set_jsbsim_initial_state(*state); +} + +void JSBSimControl::set_jsbsim_initial_state(const scrimmage::State& state) { + // exec_->LoadScript(SGPath(jsbsim_script_path_)); + + JSBSim::FGInitialCondition *ic = exec_->GetIC(); + + Quaternion q_ned_enu(M_PI, 0.0, M_PI/2.0); + Quaternion q_flu_frd(M_PI, 0.0, 0.0); + Quaternion q_frd_enu(q_ned_enu * state_->quat() * q_flu_frd); + ic->SetPsiRadIC(q_frd_enu.yaw()); + ic->SetThetaRadIC(q_frd_enu.pitch()); + ic->SetPhiRadIC(q_frd_enu.roll()); + + ic->SetVEastFpsIC(state_->vel()[0] * meters2feet); + ic->SetVNorthFpsIC(state_->vel()[1] * meters2feet); + ic->SetVDownFpsIC(-state_->vel()[2] * meters2feet); + + ic->SetTerrainElevationFtIC(parent_->projection()->HeightOrigin() * meters2feet); + + Eigen::Vector3d lla; + parent_->projection()->Reverse(state_->pos()[0], state_->pos()[1], state_->pos()[2], lla[0], lla[1], lla[2]); + ic->SetLatitudeDegIC(lla[0]); + ic->SetLongitudeDegIC(lla[1]); + ic->SetAltitudeASLFtIC(lla[2] * meters2feet); + + JSBSim::FGPropertyManager* mgr = exec_->GetPropertyManager(); + mgr->GetNode("fcs/left-aileron-pos-rad")->setDoubleValue(0.0); + mgr->GetNode("fcs/right-aileron-pos-rad")->setDoubleValue(0.0); + // mgr->GetNode("fcs/aileron-pos-norm")->setDoubleValue(0.0); + + mgr->GetNode("fcs/elevator-pos-norm")->setDoubleValue(0.0); + mgr->GetNode("fcs/rudder-pos-norm")->setDoubleValue(0.0); + mgr->GetNode("fcs/throttle-pos-norm")->setDoubleValue(0.0); + + mgr->GetNode("accelerations/pdot-rad_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/qdot-rad_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/rdot-rad_sec2")->setDoubleValue(0.0); + + mgr->GetNode("accelerations/udot-ft_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/vdot-ft_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/wdot-ft_sec2")->setDoubleValue(0.0); + + mgr->GetNode("accelerations/a-pilot-x-ft_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/a-pilot-y-ft_sec2")->setDoubleValue(0.0); + mgr->GetNode("accelerations/a-pilot-z-ft_sec2")->setDoubleValue(0.0); + + ap_aileron_cmd_node_->setDoubleValue(0); + ap_elevator_cmd_node_->setDoubleValue(0); + ap_rudder_cmd_node_->setDoubleValue(0); + ap_throttle_cmd_node_->setDoubleValue(0); + // ap_throttle_1_cmd_node_->setDoubleValue(0); + + + if (not init_values_cached_) { + init_values_cached_ = true; + init_thrust_ = mgr->GetNode("propulsion/engine/thrust-lbs")->getDoubleValue(); + + init_fuel_rate_gph_ = mgr->GetNode("propulsion/engine/fuel-flow-rate-gph")->getDoubleValue(); + init_fuel_used_lbs_ = mgr->GetNode("propulsion/engine/fuel-used-lbs")->getDoubleValue(); + init_total_fuel_lbs_ = mgr->GetNode("propulsion/total-fuel-lbs")->getDoubleValue(); + + } + mgr->GetNode("propulsion/engine/thrust-lbs")->setDoubleValue(init_thrust_); + // mgr->GetNode("propulsion/engine[1]/thrust-lbs")->setDoubleValue(init_thrust_); + + mgr->GetNode("propulsion/engine/fuel-flow-rate-gph")->setDoubleValue(init_fuel_rate_gph_); + mgr->GetNode("propulsion/engine/fuel-used-lbs")->setDoubleValue(init_fuel_used_lbs_); + mgr->GetNode("propulsion/total-fuel-lbs")->setDoubleValue(init_total_fuel_lbs_); + + mgr->GetNode("velocities/h-dot-fps")->setDoubleValue(0); + + // exec_->RunIC(); + exec_->ResetToInitialConditions(0); + + exec_->Setdt(jsbsim_dt_); + exec_->Run(); + + mgr->GetNode("aero/alpha-deg")->setDoubleValue(0); // OK? + mgr->GetNode("aero/beta-deg")->setDoubleValue(0); // OK? + +} + +void JSBSimControl::set_jsbsim_state(const scrimmage::State& state) { + // JSBSim lat/lon/alt from scrimmage state + double lat, lon, alt; + parent_->projection()->Reverse(state.pos()(0), state.pos()(1), state.pos()(2), + lat, lon, alt); + latitude_node_->setDoubleValue(lat); + longitude_node_->setDoubleValue(lon); + altitude_node_->setDoubleValue(alt / feet2meters); + + // Set JSBSim orientation from scrimmage state + roll_node_->setDoubleValue(state.quat().roll()); + pitch_node_->setDoubleValue(-state.quat().pitch()); + + angles_to_jsbsim_.set_angle(ang::rad2deg(state.quat().yaw())); + yaw_node_->setDoubleValue(ang::deg2rad(angles_to_jsbsim_.angle())); + + // set JSBSim linear velocity (NED) from scrimmage state (ENU) + vel_north_node_->setDoubleValue(state.vel()(1) / feet2meters); // scrimmage y-axis + vel_east_node_->setDoubleValue(state.vel()(0) / feet2meters); // scrimmage x-axis + vel_down_node_->setDoubleValue(-state.vel()(2) / feet2meters); // scrimmage -z-axis + + // set JSBSim angular velocity (?) from scrimmage state (ENU) + // TODO: What frame is JSBSim using? zeros for now + p_node_->setDoubleValue(0); + q_node_->setDoubleValue(0); + r_node_->setDoubleValue(0); +} + +std::shared_ptr JSBSimControl::jsbsim_exec() { + return exec_; +} + } // namespace motion } // namespace scrimmage