Jsbsim control

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
 

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<std::string, std::string>& overrides);
     bool parse(const std::string &filename);
     bool write(const std::string &filename);
 

include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.h

@@ -67,6 +67,12 @@ class JSBSimControl : public scrimmage::motion::RigidBody6DOFBase {
      bool init(std::map<std::string, std::string> &info,
                        std::map<std::string, std::string> &params) 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::FGFDMExec> 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

include/scrimmage/plugins/motion/JSBSimControl/JSBSimControl.xml

@@ -22,4 +22,8 @@
   <flightgear_port>5600</flightgear_port>
   <flightgear_protocol>UDP</flightgear_protocol> <!-- UDP or TCP -->
 
+  <draw_vel>false</draw_vel>
+  <draw_ang_vel>false</draw_ang_vel>
+  <draw_acc>false</draw_acc>
+
 </params>

missions/joystick-jsbsim.xml

@@ -1,85 +1,81 @@
 <?xml version="1.0"?>
 <?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
-<runscript xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"           
+<runscript xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
     name="SASC Capture the Flag">
-  
-  <run start="0.0" end="100" dt="0.0083333" 
-       time_warp="1" 
+
+  <run start="0.0" end="100" dt="0.0083333"
+       time_warp="1"
        enable_gui="true"
        network_gui="false"
        start_paused="true"/>
-  
+
   <stream_port>50051</stream_port>
   <stream_ip>localhost</stream_ip>
-  
-  <end_condition>none</end_condition> <!-- time, one_team, none-->  
-  
+
+  <end_condition>none</end_condition> <!-- time, one_team, none-->
+
   <grid_spacing>10</grid_spacing>
   <grid_size>1000</grid_size>
-  
+
   <terrain>mcmillan</terrain>
   <background_color>191 191 191</background_color> <!-- Red Green Blue -->
   <gui_update_period>10</gui_update_period> <!-- milliseconds -->
-  
+
   <plot_tracks>false</plot_tracks>
   <output_type>all</output_type>
   <show_plugins>false</show_plugins>
-  
-  <log_dir>~/.scrimmage/logs</log_dir>  
-      
-  <latitude_origin>35.721025</latitude_origin>
-  <longitude_origin>-120.767925</longitude_origin>      
+
+  <log_dir>~/.scrimmage/logs</log_dir>
+
+  <latitude_origin>33.784887</latitude_origin>
+  <longitude_origin>-84.374431</longitude_origin>
   <altitude_origin>300</altitude_origin>
   <show_origin>true</show_origin>
   <origin_length>10</origin_length>
-  
+
   <entity_interaction>SimpleCollision</entity_interaction>
   <metrics>SimpleCollisionMetrics</metrics>
-  
+
   <!-- uncomment "seed" and use integer for deterministic results -->
   <seed>1714060370</seed>
-  
+
   <!-- ========================== TEAM 1 ========================= -->
   <entity>
-    <team_id>1</team_id> 
+    <team_id>1</team_id>
     <color>77 77 255</color>
     <count>1</count>
     <health>1</health>
 
     <variance_x>20</variance_x>
     <variance_y>20</variance_y>
     <variance_z>10</variance_z>
-    
+
     <x>-1000</x>
     <y>0</y>
     <z>200</z>
-    <heading>0</heading>            
+    <heading>0</heading>
 
-    <!--
-        You can use the DirectController with an autonomy plugin to forward
-        control inputs directly from the autonomy plugin.
-    -->
-
+    <vx>240</vx>
+
+    <!-- JoystickAutonomy reads the joystick inputs from an autonomy plugin -->
     <autonomy print_raw_joystick_values="false" axis_map=
-    "[ throttle 1 -32767 +32767 -1 +1 -1 ]
-    [ elevator  3 -32767 +32767 -1 +1 -1 ]
-    [ aileron   2 -32767 +32767 -1 +1 +1 ]
-    [ rudder    0 -32767 +32767 -1 +1 -1 ]">JoystickAutonomy</autonomy>
-    <controller>DirectController</controller>    
-
-    <!--
-        If you use the JoystickController, the control inputs from the autonomy
-        plugin will be ignored.
-    -->
-    <!--
-        <autonomy>Straight</autonomy>    
-        <controller>JoystickController</controller>
-    -->
-
-    <motion_model parachute_reef="1.0">JSBSimControl</motion_model>
-    <script_name>rascal_no_autopilot.xml</script_name>
-    <visual_model>zephyr-blue</visual_model>    
-
+    "[ 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</autonomy>
+
+    <!-- DirectController passes the control inputs directly to the control
+         outputs without modification -->
+    <controller>DirectController</controller>
+
+    <!-- The JSBSimControl motion model directlyl consumes throttle, elevator,
+         aileron, and rudder -->
+    <motion_model flightgear_output_enable="true">JSBSimControl</motion_model>
+    <!-- <script_name>rascal_no_autopilot.xml</script_name> -->
+    <script_name>f16_scrimmage.xml</script_name>
+    <visual_model>zephyr-blue</visual_model>
+
   </entity>
 
 </runscript>

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

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):

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<Sensor> status =
-            plugin_manager->make_plugin<Sensor>("scrimmage::Sensor",
-                                                sensor_name, *file_search,
-                                                config_parse,
-                                                overrides[sensor_order_name],
-                                                plugin_tags);
-        if (status.status == PluginStatus<Sensor>::cast_failed) {
-            std::cout << "Failed to open sensor plugin: " << sensor_name
-                      << std::endl;
-            return false;
-        } else if (status.status == PluginStatus<Sensor>::parse_failed) {
-            return false;
-        } else if (status.status == PluginStatus<Sensor>::loaded) {
-            SensorPtr sensor = status.plugin;
-
-            // Get sensor's offset from entity origin
-            std::vector<double> 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<double> 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<Sensor> status =
+            plugin_manager->make_plugin<Sensor>("scrimmage::Sensor",
+                                                sensor_name, *file_search,
+                                                config_parse,
+                                                overrides[sensor_order_name],
+                                                plugin_tags);
+        if (status.status == PluginStatus<Sensor>::cast_failed) {
+            std::cout << "Failed to open sensor plugin: " << sensor_name
+                      << std::endl;
+            return false;
+        } else if (status.status == PluginStatus<Sensor>::parse_failed) {
+            return false;
+        } else if (status.status == PluginStatus<Sensor>::loaded) {
+            SensorPtr sensor = status.plugin;
+
+            // Get sensor's offset from entity origin
+            std::vector<double> 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<double> 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;
 }