From 6c3744ce2bf2dbbe32d91488502cf2d39863823f Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 4 Nov 2019 15:19:40 -0500 Subject: [PATCH 1/8] Handle multiple engines in JSBSim --- .../motion/JSBSimControl/JSBSimControl.h | 7 +- .../motion/JSBSimControl/JSBSimControl.xml | 8 +- missions/joystick-jsbsim.xml | 79 +++++++++---------- .../motion/JSBSimControl/JSBSimControl.cpp | 43 +++++----- 4 files changed, 70 insertions(+), 67 deletions(-) diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h index 946bbb4686..6bdc4522c7 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h @@ -85,6 +85,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 +116,9 @@ 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; #endif }; } // namespace motion diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml index d1314f8d0d..0d5c09731d 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml @@ -1,7 +1,7 @@ - JSBSimControl_plugin + JSBSimControl_plugin 10 0.5 @@ -19,5 +19,9 @@ localhost 5600 UDP - + + false + false + false + diff --git a/missions/joystick-jsbsim.xml b/missions/joystick-jsbsim.xml index f562eda911..0efed68d58 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 - + + ~/.scrimmage/logs + 35.721025 - -120.767925 + -120.767925 300 true 10 - + SimpleCollision SimpleCollisionMetrics - + 1714060370 - + - 1 + 1 77 77 255 1 1 @@ -49,37 +49,30 @@ 20 20 10 - + -1000 0 200 - 0 + 0 - - + JoystickAutonomy - DirectController - - - - - JSBSimControl + "[ 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 rascal_no_autopilot.xml - zephyr-blue - + zephyr-blue + diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 7d348e69a0..86d5f93df6 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -90,9 +90,9 @@ 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_); // Setup variable index for controllers throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::In); @@ -204,6 +204,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"); @@ -253,11 +254,10 @@ bool JSBSimControl::init(std::map &info, } 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) @@ -267,13 +267,13 @@ 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(); - - - /////////////////////////////////////////////////////////////////////////// // Save state parent_->projection()->Forward(latitude_node_->getDoubleValue(), @@ -346,23 +346,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; From 03c9fff13a40d34c6978c8323ee869478131fe44 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 4 Nov 2019 17:02:06 -0500 Subject: [PATCH 2/8] call FGOutput Print() in JSBSimControl --- src/plugins/motion/JSBSimControl/JSBSimControl.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 86d5f93df6..08e0e2c55c 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -274,6 +274,10 @@ bool JSBSimControl::step(double time, double dt) { exec_->Setdt(dt); exec_->Run(); + if (fg_out_enable_) { + output_fg_->Print(); + } + /////////////////////////////////////////////////////////////////////////// // Save state parent_->projection()->Forward(latitude_node_->getDoubleValue(), From 6e4d138e5bf0343d4d865cbd7f89acd2b5132d7b Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 4 Nov 2019 17:55:33 -0500 Subject: [PATCH 3/8] Update README for building FlightGearMultipler plugin --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9922cacd17..23dd051ecd 100644 --- a/README.md +++ b/README.md @@ -243,7 +243,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 From 5cd07223019aa0201d9953f257c3e1a532db229a Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Thu, 7 Nov 2019 18:33:58 -0500 Subject: [PATCH 4/8] Initializing JSBSimControl --- .../motion/JSBSimControl/JSBSimControl.h | 14 ++ .../motion/JSBSimControl/JSBSimControl.cpp | 203 ++++++++++++------ 2 files changed, 151 insertions(+), 66 deletions(-) diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h index 6bdc4522c7..dc509eb76e 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h @@ -67,6 +67,10 @@ 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); protected: #if ENABLE_JSBSIM == 1 @@ -119,6 +123,16 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase { 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/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 08e0e2c55c..f8008614e8 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -94,6 +94,9 @@ bool JSBSimControl::init(std::map &info, 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); elevator_idx_ = vars_.declare(VariableIO::Type::elevator, VariableIO::Direction::In); @@ -122,71 +125,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 @@ -212,7 +151,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"); @@ -223,6 +161,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(), @@ -250,6 +204,9 @@ bool JSBSimControl::init(std::map &info, -az_pilot_node_->getDoubleValue()); linear_accel_body_ = state_->quat().rotate(a_FLU); + //exec_->PrintPropertyCatalog(); + //exec_->PrintSimulationConfiguration(); + return true; } @@ -393,5 +350,119 @@ 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); +} } // namespace motion } // namespace scrimmage From 7eb578927f26a068dcec466aaf71d29d5b3bcc89 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Wed, 27 Nov 2019 15:09:27 -0500 Subject: [PATCH 5/8] set overrides from std::map as well --- include/scrimmage/parse/MissionParse.h | 1 + src/parse/MissionParse.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index f0a029fa9d..c5567676c6 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/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 072dacf964..9ece25af04 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); From a367fd71d451a2a2b9a3529cc297cebc31fc2df7 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Fri, 17 Jan 2020 15:40:45 -0500 Subject: [PATCH 6/8] Testing JSBSimControl with f16 model --- missions/joystick-jsbsim.xml | 11 ++-- .../motion/JSBSimControl/JSBSimControl.cpp | 52 +++++++++---------- 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/missions/joystick-jsbsim.xml b/missions/joystick-jsbsim.xml index 0efed68d58..03d70f988e 100644 --- a/missions/joystick-jsbsim.xml +++ b/missions/joystick-jsbsim.xml @@ -27,8 +27,8 @@ ~/.scrimmage/logs - 35.721025 - -120.767925 + 33.784887 + -84.374431 300 true 10 @@ -55,6 +55,8 @@ 200 0 + 240 + JSBSimControl + + f16_scrimmage.xml zephyr-blue diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 8fb2ba04af..d177216059 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -149,7 +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]"); + // 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"); @@ -169,20 +169,20 @@ bool JSBSimControl::init(std::map &info, 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); - //} + // 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(), @@ -210,8 +210,8 @@ bool JSBSimControl::init(std::map &info, -az_pilot_node_->getDoubleValue()); linear_accel_body_ = state_->quat().rotate(a_FLU); - //exec_->PrintPropertyCatalog(); - //exec_->PrintSimulationConfiguration(); + // exec_->PrintPropertyCatalog(); + // exec_->PrintSimulationConfiguration(); return true; } @@ -230,9 +230,9 @@ 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_); - } + // if (ap_throttle_1_cmd_node_ != nullptr) { + // ap_throttle_1_cmd_node_->setDoubleValue(throttle_); + // } exec_->Setdt(dt); exec_->Run(); @@ -358,12 +358,12 @@ bool JSBSimControl::step(double time, double dt) { } void JSBSimControl::teleport(StatePtr &state) { - //set_jsbsim_state(*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_)); + // exec_->LoadScript(SGPath(jsbsim_script_path_)); JSBSim::FGInitialCondition *ic = exec_->GetIC(); @@ -389,7 +389,7 @@ void JSBSimControl::set_jsbsim_initial_state(const scrimmage::State& state) { 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/aileron-pos-norm")->setDoubleValue(0.0); mgr->GetNode("fcs/elevator-pos-norm")->setDoubleValue(0.0); mgr->GetNode("fcs/rudder-pos-norm")->setDoubleValue(0.0); @@ -411,7 +411,7 @@ void JSBSimControl::set_jsbsim_initial_state(const scrimmage::State& state) { 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); + // ap_throttle_1_cmd_node_->setDoubleValue(0); if (not init_values_cached_) { @@ -424,7 +424,7 @@ void JSBSimControl::set_jsbsim_initial_state(const scrimmage::State& state) { } mgr->GetNode("propulsion/engine/thrust-lbs")->setDoubleValue(init_thrust_); - mgr->GetNode("propulsion/engine[1]/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_); @@ -432,7 +432,7 @@ void JSBSimControl::set_jsbsim_initial_state(const scrimmage::State& state) { mgr->GetNode("velocities/h-dot-fps")->setDoubleValue(0); - //exec_->RunIC(); + // exec_->RunIC(); exec_->ResetToInitialConditions(0); exec_->Setdt(jsbsim_dt_); From 4d3dd73e497dc87b8b068b4cfb62a03651ee5e47 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Wed, 22 Jan 2020 17:06:37 -0500 Subject: [PATCH 7/8] Recursively find mission files in python --- .../scrimmage/bindings/src/py_openai_env.cpp | 2 +- python/scrimmage/utils.py | 18 +++++++++++++++--- 2 files changed, 16 insertions(+), 4 deletions(-) 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 a3787867fd..8ac84f32a1 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): From 9acde6fe16d33aef8d0a2cbc754197dc31ae56dd Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Fri, 24 Jan 2020 18:40:19 -0500 Subject: [PATCH 8/8] Move sensor after motion model creation --- .../motion/JSBSimControl/JSBSimControl.h | 2 + src/entity/Entity.cpp | 149 +++++++++--------- .../motion/JSBSimControl/JSBSimControl.cpp | 5 + 3 files changed, 82 insertions(+), 74 deletions(-) diff --git a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h index dc509eb76e..22559fdc05 100644 --- a/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h +++ b/include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h @@ -72,6 +72,8 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase { 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 FGFDMExecPtr exec_; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 9ebfdbc69f..f4a2b68c4e 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 //////////////////////////////////////////////////////////// @@ -459,6 +385,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/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index d177216059..53975648f7 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -470,5 +470,10 @@ void JSBSimControl::set_jsbsim_state(const scrimmage::State& state) { q_node_->setDoubleValue(0); r_node_->setDoubleValue(0); } + +std::shared_ptr JSBSimControl::jsbsim_exec() { + return exec_; +} + } // namespace motion } // namespace scrimmage