diff --git a/.gitignore b/.gitignore index d3a929b5..f32fe2a4 100644 --- a/.gitignore +++ b/.gitignore @@ -376,3 +376,24 @@ env/ # Simulink cache **/slprj *.slxc + +# Jsbsim repository files +core_sim/jsbsim/ + +# temporary for Jonathan +physics/test/CMakeLists.txt +core_sim/test/CMakeLists.txt +unreal/Blocks/Plugins/InternalDevUseContent/ +unreal/Blocks/Plugins/ProjectAirSim/SDK/ +unreal/Blocks/Samples/ + +# Compilation with Visual Studio +# TODO: Make this cleaner. This is a temporary fix. +CMakeFiles/ +CMakeCache.txt +CTestTestfile.cmake +*.vcxproj +*.filters +*.cmake +DartConfiguration.tcl +ProjectAirSimLibs.sln \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 6f6be387..5a081a39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,6 +45,7 @@ set(SIMLIBS_TEST_DIR ${CMAKE_BINARY_DIR}/unit_tests) set(UE_PLUGIN_SIMLIBS_DIR ${CMAKE_SOURCE_DIR}/unreal/Blocks/Plugins/ProjectAirSim/SimLibs) set(UNITY_WRAPPER_DLL_DIR ${CMAKE_SOURCE_DIR}/unity/BlocksUnity/Assets/Plugins) set(MATLAB_PHYSICS_DIR ${CMAKE_SOURCE_DIR}/physics/matlab_sfunc) +set(JSBSIM_CORESIM_DIR ${CMAKE_SOURCE_DIR}/core_sim/jsbsim) set(MATLAB_CONTROL_DIR ${CMAKE_SOURCE_DIR}/vehicle_apis/multirotor_api/matlab_sfunc) # set(UE_PLUGIN_CESIUM_NATIVE_DIR ${CMAKE_SOURCE_DIR}/unreal/Blocks/Plugins/ProjectAirSim/SimLibs) # set(UE_CESIUM_PLUGIN_DIR ${CMAKE_SOURCE_DIR}/unreal/Blocks/Plugins/CesiumForUnreal/) @@ -72,6 +73,87 @@ elseif(UNIX) # set(CESIUM_LINUX_TOOLCHAIN ${UE_CESIUM_PLUGIN_DIR}/extern/unreal-linux-toolchain.cmake) endif() +# Set up dependency: jsbsim +message("Setting up [jsbsim] dependency as an external project...") +# CMake external projects don't adopt the parent's CMAKE_MSVC_RUNTIME_LIBRARY setting, +# so to force /MD non-debug CRT to match UE, build Debug config as Relwithdebinfo and +# build Release as Release. +set(JSBSIM_BUILD_TYPE $,Relwithdebinfo,Release>) +set(JSBSIM_SRC_DIR ${CMAKE_BINARY_DIR}/_deps/jsbsim/src/jsbsim-repo) +set(JSBSIM_SHARED_LIB_DIR ${CMAKE_BINARY_DIR}/_deps/jsbsim-install/bin) +set(JSBSIM_STATIC_LIB_DIR ${CMAKE_BINARY_DIR}/_deps/jsbsim-install/lib) +set(JSBSIM_INCLUDE_DIR ${CMAKE_BINARY_DIR}/_deps/jsbsim-install/include/JSBSim ${CMAKE_BINARY_DIR}/_deps/jsbsim-install/include/JSBSim/math) + +# On Linux, build JSBSim as static library; on Windows, build as shared library +if(UNIX) + set(JSBSIM_BUILD_SHARED_LIBS OFF) + set(JSBSIM_BYPRODUCTS ${JSBSIM_STATIC_LIB_DIR}/libjsbsim.a) +else() + set(JSBSIM_BUILD_SHARED_LIBS ON) + set(JSBSIM_BYPRODUCTS ${JSBSIM_SHARED_LIB_DIR}/JSBSim.dll ${JSBSIM_STATIC_LIB_DIR}/JSBSim.lib) +endif() +ExternalProject_Add(jsbsim-repo + GIT_REPOSITORY https://github.com/JSBSim-Team/jsbsim + GIT_TAG "v1.1.12" + GIT_CONFIG "advice.detachedHead=false" + PREFIX ${CMAKE_BINARY_DIR}/_deps/jsbsim + BUILD_COMMAND ${CMAKE_COMMAND} --build --config ${JSBSIM_BUILD_TYPE} + INSTALL_COMMAND ${CMAKE_COMMAND} --build --config ${JSBSIM_BUILD_TYPE} --target install + UPDATE_COMMAND "" # disable update step + CMAKE_ARGS + # -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} + -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} + -DCMAKE_POSITION_INDEPENDENT_CODE=True + -DCMAKE_BUILD_TYPE:STRING=${JSBSIM_BUILD_TYPE} + -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_BINARY_DIR}/_deps/jsbsim-install + -DBUILD_SHARED_LIBS=${JSBSIM_BUILD_SHARED_LIBS} + -DBUILD_PYTHON_MODULE=OFF + -DBUILD_DOCS=OFF + TEST_COMMAND "" # disable test step + BUILD_BYPRODUCTS ${JSBSIM_BYPRODUCTS} + BUILD_ALWAYS 1 +) +ExternalProject_Add_Step(jsbsim-repo post-install + COMMAND ${CMAKE_COMMAND} -E echo "Copying [jsbsim] library to ${JSBSIM_CORESIM_DIR}" + COMMAND ${CMAKE_COMMAND} -E copy_directory "${JSBSIM_INCLUDE_DIR}" "${JSBSIM_CORESIM_DIR}/include" + # Copy JSBSim models + COMMAND ${CMAKE_COMMAND} -E copy_directory "${JSBSIM_SRC_DIR}/aircraft" "${JSBSIM_CORESIM_DIR}/models/aircraft" + COMMAND ${CMAKE_COMMAND} -E copy_directory "${JSBSIM_SRC_DIR}/engine" "${JSBSIM_CORESIM_DIR}/models/engine" + COMMAND ${CMAKE_COMMAND} -E copy_directory "${JSBSIM_SRC_DIR}/systems" "${JSBSIM_CORESIM_DIR}/models/systems" + # TODO: take out if we are not going to use JSBSim scripts + COMMAND ${CMAKE_COMMAND} -E copy_directory "${JSBSIM_SRC_DIR}/scripts" "${JSBSIM_CORESIM_DIR}/models/scripts" + DEPENDEES install +) +# Platform-specific post-install commands for JSBSim library files +if(WIN32) + ExternalProject_Add_Step(jsbsim-repo post-install-libs + COMMAND ${CMAKE_COMMAND} -E copy "${JSBSIM_SHARED_LIB_DIR}/JSBSim.dll" "${JSBSIM_CORESIM_DIR}/lib/$,Release,Debug>/JSBSim.dll" + COMMAND ${CMAKE_COMMAND} -E copy "${JSBSIM_STATIC_LIB_DIR}/JSBSim.lib" "${JSBSIM_CORESIM_DIR}/lib/$,Release,Debug>/JSBSim.lib" + #COMMAND ${CMAKE_COMMAND} -E copy "${JSBSIM_LIB_DIR}/JSBSim.pdb" "${JSBSIM_PHYSICS_DIR}/lib/$,Release,Debug>/JSBSim.pdb" + DEPENDEES post-install + ) +else() + ExternalProject_Add_Step(jsbsim-repo post-install-libs + COMMAND ${CMAKE_COMMAND} -E copy "${JSBSIM_STATIC_LIB_DIR}/libjsbsim.a" "${JSBSIM_CORESIM_DIR}/lib/$,Release,Debug>/libjsbsim.a" + DEPENDEES post-install + ) +endif() +# add jsbsim library +if(WIN32) + add_library(jsbsim SHARED IMPORTED) + set_target_properties(jsbsim PROPERTIES + IMPORTED_LOCATION ${JSBSIM_SHARED_LIB_DIR}/JSBSim.dll + IMPORTED_IMPLIB ${JSBSIM_STATIC_LIB_DIR}/JSBSim.lib + ) +else() + # On Linux, use static library (no IMPORTED_IMPLIB needed for static libraries) + add_library(jsbsim STATIC IMPORTED) + set_target_properties(jsbsim PROPERTIES + IMPORTED_LOCATION ${JSBSIM_STATIC_LIB_DIR}/libjsbsim.a + ) +endif() + + # Set up dependency: nlohmann JSON # Directly download single include file json.hpp diff --git a/client/python/example_user_scripts/hello_fixed_wing.py b/client/python/example_user_scripts/hello_fixed_wing.py new file mode 100644 index 00000000..39a3b2c8 --- /dev/null +++ b/client/python/example_user_scripts/hello_fixed_wing.py @@ -0,0 +1,151 @@ +""" +Copyright (C) Microsoft Corporation. All rights reserved. + +Demonstrates flying a quadrotor drone with camera sensors. +""" + +import asyncio +import math + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log +from projectairsim.image_utils import ImageDisplay + + +# Async main function to wrap async drone commands +async def main(): + # Create a Project AirSim client + client = ProjectAirSimClient() + + # Initialize an ImageDisplay object to display camera sub-windows + image_display = ImageDisplay() + + try: + # Connect to simulation environment + client.connect() + + # Create a World object to interact with the sim world and load a scene + world = World(client, "scene_basic_fixed_wing.jsonc", delay_after_load_sec=2) + + # Create a Drone object to interact with a drone in the loaded sim world + drone = Drone(client, world, "x8") +# + ## ------------------------------------------------------------------------------ +# + ## Subscribe to chase camera sensor as a client-side pop-up window + #chase_cam_window = "ChaseCam" + #image_display.add_chase_cam(chase_cam_window) + #client.subscribe( + # drone.sensors["DownCamera"]["scene_camera"], + # lambda _, chase: image_display.receive(chase, chase_cam_window), + #) +# + ## Subscribe to the downward-facing camera sensor's RGB and Depth images + #rgb_name = "RGB-Image" + #image_display.add_image(rgb_name, subwin_idx=0) + #client.subscribe( + # drone.sensors["DownCamera"]["scene_camera"], + # lambda _, rgb: image_display.receive(rgb, rgb_name), + #) +# + #depth_name = "Depth-Image" + #image_display.add_image(depth_name, subwin_idx=2) + #client.subscribe( + # drone.sensors["DownCamera"]["depth_camera"], + # lambda _, depth: image_display.receive(depth, depth_name), + #) +# + #image_display.start() +# + ## ------------------------------------------------------------------------------ +# + ## Set the drone to be ready to fly + ## JSBSim robot currently does not support control the drone at runtime + #drone.enable_api_control() + ##set brakes to 1 + #drone.set_brakes(1) + #drone.arm() +# + ## ------------------------------------------------------------------------------ +# + ## set takeoff z to 120 meters + #drone.set_take_off_z(-120) +# + ## ------------------------------------------------------------------------------ +# + ## Sleep for two seconds to + #await asyncio.sleep(2) + # + ## release brakes + #drone.set_brakes(0) +# + #projectairsim_log().info("takeoff_async: starting") + #takeoff_task = ( + # await drone.takeoff_async(timeout_sec=1200) + #) # schedule an async task to start the command +# + #await takeoff_task + #projectairsim_log().info("takeoff_async: completed") +# + #projectairsim_log().info("Waiting to stabilize altitude... (10 seconds)") + #await asyncio.sleep(10) +# + ## ------------------------------------------------------------------------------ + # + ## Command the drone to move to position 1000,1000,-200 + #move_up_task = await drone.move_to_position_async( + # north=1000, east=1000, down=-200, velocity=33.0, lookahead=100, timeout_sec=60 + #) + #projectairsim_log().info("Move to position 1000,1000,-200 invoked") +# + #await move_up_task + #projectairsim_log().info("Move to position completed") +# + ## ------------------------------------------------------------------------------ +# + ## Command vehicle to fly at a specific heading and speed + #projectairsim_log().info("Heading 90 invoked") + #heading_45_task = await drone.move_by_heading_async( + # heading=math.radians(90.0), speed=20.0, duration=10 + #) + #await heading_45_task + #projectairsim_log().info("Heading 90 complete.") +# + ## ------------------------------------------------------------------------------ +# + ## Command the drone to move to position 0,0,-100 + #move_up_task = await drone.move_to_position_async( + # north=0, east=0, down=-100, velocity=33.0, lookahead=100, timeout_sec=60 + #) + #projectairsim_log().info("Move to position 0,0,-100 invoked") +# + #await move_up_task + #projectairsim_log().info("Move to position completed") + ## ------------------------------------------------------------------------------ +# + #projectairsim_log().info("land_async: starting") + #land_task = await drone.land_async() + #await land_task + #projectairsim_log().info("land_async: completed") + ## set brakes to 50% + #drone.set_brakes(0.5) + + # ------------------------------------------------------------------------------ + + # Shut down the drone + drone.disarm() + drone.disable_api_control() + + # logs exception on the console + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + # Always disconnect from the simulation environment to allow next connection + client.disconnect() + + image_display.stop() + + +if __name__ == "__main__": + asyncio.run(main()) # Runner for async main function \ No newline at end of file diff --git a/client/python/example_user_scripts/hello_helicopter.py b/client/python/example_user_scripts/hello_helicopter.py new file mode 100644 index 00000000..fbadb248 --- /dev/null +++ b/client/python/example_user_scripts/hello_helicopter.py @@ -0,0 +1,133 @@ +""" +Copyright (C) Microsoft Corporation. All rights reserved. + +Demonstrates flying a quadrotor drone with camera sensors. +""" + +import asyncio + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log +from projectairsim.image_utils import ImageDisplay + + +# Async main function to wrap async drone commands +async def main(): + # Create a Project AirSim client + client = ProjectAirSimClient() + + # Initialize an ImageDisplay object to display camera sub-windows + image_display = ImageDisplay() + + try: + # Connect to simulation environment + client.connect() + + # Create a World object to interact with the sim world and load a scene + world = World(client, "scene_basic_helicopter.jsonc", delay_after_load_sec=2) + + # Create a Drone object to interact with a drone in the loaded sim world + drone = Drone(client, world, "Drone1") + + # ------------------------------------------------------------------------------ + + # Subscribe to chase camera sensor as a client-side pop-up window + chase_cam_window = "ChaseCam" + image_display.add_chase_cam(chase_cam_window) + client.subscribe( + drone.sensors["Chase"]["scene_camera"], + lambda _, chase: image_display.receive(chase, chase_cam_window), + ) + + # Subscribe to the downward-facing camera sensor's RGB and Depth images + rgb_name = "RGB-Image" + image_display.add_image(rgb_name, subwin_idx=0) + client.subscribe( + drone.sensors["DownCamera"]["scene_camera"], + lambda _, rgb: image_display.receive(rgb, rgb_name), + ) + + depth_name = "Depth-Image" + image_display.add_image(depth_name, subwin_idx=2) + client.subscribe( + drone.sensors["DownCamera"]["depth_camera"], + lambda _, depth: image_display.receive(depth, depth_name), + ) + + image_display.start() + + # ------------------------------------------------------------------------------ + + # Set the drone to be ready to fly + # JSBSim robot currently does not support control the drone at runtime + # drone.enable_api_control() + # drone.arm() + + # # ------------------------------------------------------------------------------ + + # projectairsim_log().info("takeoff_async: starting") + # takeoff_task = ( + # await drone.takeoff_async() + # ) # schedule an async task to start the command + + # # Example 1: Wait on the result of async operation using 'await' keyword + # await takeoff_task + # projectairsim_log().info("takeoff_async: completed") + + # # ------------------------------------------------------------------------------ + + # # Command the drone to move up in NED coordinate system at 1 m/s for 4 seconds + # move_up_task = await drone.move_by_velocity_async( + # v_north=0.0, v_east=0.0, v_down=-1.0, duration=4.0 + # ) + # projectairsim_log().info("Move-Up invoked") + + # await move_up_task + # projectairsim_log().info("Move-Up completed") + + # # ------------------------------------------------------------------------------ + + # # Command the Drone to move down in NED coordinate system at 1 m/s for 4 seconds + # move_down_task = await drone.move_by_velocity_async( + # v_north=0.0, v_east=0.0, v_down=1.0, duration=4.0 + # ) # schedule an async task to start the command + # projectairsim_log().info("Move-Down invoked") + + # # Example 2: Wait for move_down_task to complete before continuing + # while not move_down_task.done(): + # await asyncio.sleep(0.005) + # projectairsim_log().info("Move-Down completed") + + # # ------------------------------------------------------------------------------ + + # projectairsim_log().info("land_async: starting") + # land_task = await drone.land_async() + # await land_task + # projectairsim_log().info("land_async: completed") + + # # # ------------------------------------------------------------------------------ + + + + # ------------------------------------------------------------------------------ + # sleep for 5 minutes to contiue seeing the camera images + await asyncio.sleep(300) + + + # Shut down the drone + #drone.disarm() + #drone.disable_api_control() + + # logs exception on the console + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + # Always disconnect from the simulation environment to allow next connection + client.disconnect() + + image_display.stop() + + +if __name__ == "__main__": + asyncio.run(main()) # Runner for async main function \ No newline at end of file diff --git a/client/python/example_user_scripts/jsbsim_env_actor.py b/client/python/example_user_scripts/jsbsim_env_actor.py index 11a5b4ba..075b0699 100644 --- a/client/python/example_user_scripts/jsbsim_env_actor.py +++ b/client/python/example_user_scripts/jsbsim_env_actor.py @@ -16,7 +16,6 @@ """ from projectairsim import ProjectAirSimClient, World, EnvActor from projectairsim.utils import projectairsim_log -import asyncio async def env_actor_motion_plan(env_actor: EnvActor, world: World): diff --git a/client/python/example_user_scripts/sim_config/robot_f22_jsbsim.jsonc b/client/python/example_user_scripts/sim_config/robot_f22_jsbsim.jsonc new file mode 100644 index 00000000..20f9ffdb --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_f22_jsbsim.jsonc @@ -0,0 +1,283 @@ +{ + "physics-type": "jsbsim-physics", + "jsbsim-script": "scripts/x8_test.xml", + "jsbsim-model": "737", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "enabled": false, + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Rover/OffroadCar/SM_Offroad_Body" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 1, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "7 0.0 0.0", + "rpy": "0 -1.5707963267948966 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/InternalDevUseContent/Cessna/CessnaProp" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "1 0 0" + } + ], + "controller": { + "id": "JSBSim_Controller", + "airframe-setup": "fixed-wing", + "type": "jsbsim-api" + //"jsbsim-api-settings": { + //} + }, + "actuators": [ + { + "name": "fcs/throttle-cmd-norm", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "1.0 0.0 0.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-50.0 0.0 0", + "rpy": "0 0 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.02, + "capture-settings": [ + { + "image-type": 0, // scene camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, // depth planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 2, // depth perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 3, // segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 4, // depth vis + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "max-depth-meters": 100 + }, + { + "image-type": 5, // disparity normalized + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 6, // surface normals + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy": "0 -1.5708 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": false, + "parent-link": "Frame" + } + ] +} \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/robot_fixed_wing.jsonc b/client/python/example_user_scripts/sim_config/robot_fixed_wing.jsonc new file mode 100644 index 00000000..b1be9a09 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_fixed_wing.jsonc @@ -0,0 +1,283 @@ +{ + "physics-type": "jsbsim-physics", + "jsbsim-script": "scripts/c3104.xml", + "jsbsim-model": "c310", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "enabled": false, + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Rover/OffroadCar/SM_Offroad_Body" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 1, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "7 0.0 0.0", + "rpy": "0 -1.5707963267948966 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/InternalDevUseContent/Cessna/CessnaProp" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "1 0 0" + } + ], + "controller": { + "id": "JSBSim_Controller", + "airframe-setup": "fixed-wing", + "type": "jsbsim-api" + //"jsbsim-api-settings": { + //} + }, + "actuators": [ + { + "name": "fcs/throttle-cmd-norm", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "1.0 0.0 0.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-50.0 0.0 0", + "rpy": "0 0 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.02, + "capture-settings": [ + { + "image-type": 0, // scene camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, // depth planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 2, // depth perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 3, // segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 4, // depth vis + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "max-depth-meters": 100 + }, + { + "image-type": 5, // disparity normalized + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 6, // surface normals + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy": "0 -1.5708 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": false, + "parent-link": "Frame" + } + ] +} \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/robot_helicopter.jsonc b/client/python/example_user_scripts/sim_config/robot_helicopter.jsonc new file mode 100644 index 00000000..e296e85f --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_helicopter.jsonc @@ -0,0 +1,475 @@ +{ + "physics-type": "jsbsim-physics", + "jsbsim-script": "scripts/ah1s_flight_test.xml", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/helicopter/Helicopter_Corps" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 1, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.0 0.0 0.0", + "rpy": "0 1.5707963267948966 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "" + } + } + }, + { + "name": "Prop_FR", + "inertial": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "" + } + } + }, + { + "name": "Prop_RL", + "inertial": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "" + } + } + }, + { + "name": "Prop_RR", + "inertial": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_FR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FR", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RR", + "axis": "0 0 1" + } + ], + "controller": { + "id": "Simple_Flight_Controller", + "airframe-setup": "quadrotor-x", + "type": "simple-flight-api", + "simple-flight-api-settings": { + "actuator-order": [ + { + "id": "Prop_FR_actuator" + }, + { + "id": "Prop_RL_actuator" + }, + { + "id": "Prop_FL_actuator" + }, + { + "id": "Prop_RR_actuator" + } + ] + } + }, + "actuators": [ + { + "name": "Prop_FL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_FR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FR", + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RL", + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RR", + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-10.0 0.0 -1.0", + "rpy": "0 -0.2 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.02, + "capture-settings": [ + { + "image-type": 0, // scene camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, // depth planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 2, // depth perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 3, // segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 4, // depth vis + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "max-depth-meters": 100 + }, + { + "image-type": 5, // disparity normalized + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 6, // surface normals + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy": "0 -1.5708 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": false, + "parent-link": "Frame" + } + ] +} \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/robot_quadtiltrotor_fastphysics.jsonc b/client/python/example_user_scripts/sim_config/robot_quadtiltrotor_fastphysics.jsonc index f8016b5e..f1f4b77a 100644 --- a/client/python/example_user_scripts/sim_config/robot_quadtiltrotor_fastphysics.jsonc +++ b/client/python/example_user_scripts/sim_config/robot_quadtiltrotor_fastphysics.jsonc @@ -1,57 +1,25 @@ { + "$schema":"../../projectairsim/src/projectairsim/schema/robot_config_schema.jsonc", "physics-type": "fast-physics", "links": [ { "name": "Frame", "inertial": { - "mass": 10.0, - "inertia": { - "type": "geometry", - "geometry": { - "box": { - "size": "3.2 3.2 2.68" - } - } - }, - "aerodynamics": { - "drag-coefficient": 0.04, - "type": "geometry", - "geometry": { - "box": { - "size": "3.2 3.2 2.68" - } - } - } - }, - "collision": { - "restitution": 0.1, - "friction": 0.5 - }, - "visual": { - "geometry": { - "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Fuselage" - } - } - }, - { - "name": "Shroud_FL", - "inertial": { - "mass": 0.0, + "mass": 1.0, "inertia": { "type": "geometry", "geometry": { "box": { - "size": "2.10 2.10 0.32" + "size": "0.180 0.110 0.040" } } }, "aerodynamics": { - "drag-coefficient": 0.0, + "drag-coefficient": 0.325, "type": "geometry", "geometry": { "box": { - "size": "2.10 2.10 0.32" + "size": "0.180 0.110 0.040" } } } @@ -61,13 +29,9 @@ "friction": 0.5 }, "visual": { - "origin": { - "xyz": "2.00 -2.59 -0.6543", - "rpy-deg": "0 0 -30" - }, "geometry": { "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Shroud" + "name": "/Drone/Quadrotor1" } } }, @@ -75,69 +39,32 @@ "name": "Prop_FL", "inertial": { "origin": { - "xyz": "2.00 -2.59 -0.6543", + "xyz": "0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, - "mass": 0.0, + "mass": 0.055, "inertia": { "type": "point-mass" }, "aerodynamics": { - "drag-coefficient": 0.0, + "drag-coefficient": 0.325, "type": "geometry", "geometry": { "cylinder": { - "radius": 0.79, - "length": 0.29 + "radius": 0.1143, + "length": 0.01 } } } }, "visual": { "origin": { - "xyz": "2.00 -2.59 -0.6543", + "xyz": "0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, "geometry": { "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Rotor" - } - } - }, - { - "name": "Shroud_FR", - "inertial": { - "mass": 1.0, - "inertia": { - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - }, - "aerodynamics": { - "drag-coefficient": 0.0, - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - } - }, - "collision": { - "restitution": 0.1, - "friction": 0.5 - }, - "visual": { - "origin": { - "xyz": "2.00 2.59 -0.6543", - "rpy-deg": "0 0 30" - }, - "geometry": { - "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Shroud" + "name": "/Drone/PropellerRed" } } }, @@ -145,69 +72,32 @@ "name": "Prop_FR", "inertial": { "origin": { - "xyz": "2.00 2.59 -0.6543", + "xyz": "0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, - "mass": 0.0, + "mass": 0.055, "inertia": { "type": "point-mass" }, "aerodynamics": { - "drag-coefficient": 0.0, + "drag-coefficient": 0.325, "type": "geometry", "geometry": { "cylinder": { - "radius": 0.79, - "length": 0.29 + "radius": 0.1143, + "length": 0.01 } } } }, "visual": { "origin": { - "xyz": "2.00 2.59 -0.6543", + "xyz": "0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, "geometry": { "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Rotor" - } - } - }, - { - "name": "Shroud_RL", - "inertial": { - "mass": 0.0, - "inertia": { - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - }, - "aerodynamics": { - "drag-coefficient": 0.0, - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - } - }, - "collision": { - "restitution": 0.1, - "friction": 0.5 - }, - "visual": { - "origin": { - "xyz": "-1.5 -2.6225 -2.395", - "rpy-deg": "0 0 -30" - }, - "geometry": { - "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Shroud" + "name": "/Drone/PropellerRed" } } }, @@ -215,69 +105,32 @@ "name": "Prop_RL", "inertial": { "origin": { - "xyz": "-1.5 -2.6225 -2.395", + "xyz": "-0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, - "mass": 0.0, + "mass": 0.055, "inertia": { "type": "point-mass" }, "aerodynamics": { - "drag-coefficient": 0.0, + "drag-coefficient": 0.325, "type": "geometry", "geometry": { "cylinder": { - "radius": 0.79, - "length": 0.29 + "radius": 0.1143, + "length": 0.01 } } } }, "visual": { "origin": { - "xyz": "-1.5 -2.6225 -2.395", + "xyz": "-0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, "geometry": { "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Rotor" - } - } - }, - { - "name": "Shroud_RR", - "inertial": { - "mass": 0.0, - "inertia": { - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - }, - "aerodynamics": { - "drag-coefficient": 0.0, - "type": "geometry", - "geometry": { - "box": { - "size": "2.10 2.10 0.32" - } - } - } - }, - "collision": { - "restitution": 0.1, - "friction": 0.5 - }, - "visual": { - "origin": { - "xyz": "-1.5 2.6225 -2.395", - "rpy-deg": "0 0 30" - }, - "geometry": { - "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Shroud" + "name": "/Drone/PropellerWhite" } } }, @@ -285,182 +138,37 @@ "name": "Prop_RR", "inertial": { "origin": { - "xyz": "-1.5 2.6225 -2.395", + "xyz": "-0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, - "mass": 0.0, + "mass": 0.055, "inertia": { "type": "point-mass" }, "aerodynamics": { - "drag-coefficient": 0.0, + "drag-coefficient": 0.325, "type": "geometry", "geometry": { "cylinder": { - "radius": 0.79, - "length": 0.29 + "radius": 0.1143, + "length": 0.01 } } } }, "visual": { "origin": { - "xyz": "-1.5 2.6225 -2.395", + "xyz": "-0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, "geometry": { "type": "unreal_mesh", - "name": "/Drone/AirTaxi/AirTaxi_Rotor" - } - } - }, - { - "name": "Wing_L", - "inertial": { - "origin": { - "xyz": "0.0 -1.0 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" - }, - "aerodynamics": { - "type": "lift-drag", - "lift-drag": { - "alpha-0": 0.06, - "alpha-stall": 0.64, - "c-lift-alpha": 0.5, - "c-lift-alpha-stall": -0.8, - "c-drag-alpha": 0.6, - "c-drag-alpha-stall": -0.9, - "c-moment-alpha": 0.0, - "c-moment-alpha-stall": 0.0, - "area": 5.0, - "control-surface-cl-per-rad": -0.5, - "control-surface-cd-per-rad": 0.0, - "control-surface-cm-per-rad": -0.1, - "center-pressure-xyz": "0 0 0", - "forward-xyz": "1.0 0 0", - "upward-xyz": "0 0 -1.0" - } - } - } - }, - { - "name": "Wing_L_Aileron", - "inertial": { - "origin": { - "xyz": "0.0 -0.5 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" - } - } - }, - { - "name": "Wing_R", - "inertial": { - "origin": { - "xyz": "0.0 1.0 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" - }, - "aerodynamics": { - "type": "lift-drag", - "lift-drag": { - "alpha-0": 0.06, - "alpha-stall": 0.64, - "c-lift-alpha": 0.5, - "c-lift-alpha-stall": -0.8, - "c-drag-alpha": 0.6, - "c-drag-alpha-stall": -0.9, - "c-moment-alpha": 0.0, - "c-moment-alpha-stall": 0.0, - "area": 5.0, - "control-surface-cl-per-rad": -0.5, - "control-surface-cd-per-rad": 0.0, - "control-surface-cm-per-rad": -0.1, - "center-pressure-xyz": "0 0 0", - "forward-xyz": "1.0 0 0", - "upward-xyz": "0 0 -1.0" - } - } - } - }, - { - "name": "Wing_R_Aileron", - "inertial": { - "origin": { - "xyz": "0.0 0.5 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" - } - } - }, - { - "name": "Wing_Tail", - "inertial": { - "origin": { - "xyz": "0.0 0.0 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" - }, - "aerodynamics": { - "type": "lift-drag", - "lift-drag": { - "alpha-0": 0.0, - "alpha-stall": 0.3, - "c-lift-alpha": 0, - "c-lift-alpha-stall": 0, - "c-drag-alpha": 0.0, - "c-drag-alpha-stall": 0.0, - "c-moment-alpha": 0.0, - "c-moment-alpha-stall": 0.0, - "area": 0.5, - "control-surface-cl-per-rad": 0.0, - "control-surface-cd-per-rad": 0.0, - "control-surface-cm-per-rad": 2.0, // pure elevator pitch control - "center-pressure-xyz": "0 0 0", - "forward-xyz": "1.0 0 0", - "upward-xyz": "0 0 -1.0" - } - } - } - }, - { - "name": "Elevator", - "inertial": { - "origin": { - "xyz": "-6.0 0.0 0.0", - "rpy-deg": "0 0 0" - }, - "mass": 0.0, - "inertia": { - "type": "point-mass" + "name": "/Drone/PropellerWhite" } } } ], "joints": [ - { - "id": "Frame_Shroud_FL", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Shroud_FL", - "axis": "0 0 1" - }, { "id": "Frame_Prop_FL", "type": "fixed", @@ -468,13 +176,6 @@ "child-link": "Prop_FL", "axis": "0 0 1" }, - { - "id": "Frame_Shroud_FR", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Shroud_FR", - "axis": "0 0 1" - }, { "id": "Frame_Prop_FR", "type": "fixed", @@ -482,13 +183,6 @@ "child-link": "Prop_FR", "axis": "0 0 1" }, - { - "id": "Frame_Shroud_RL", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Shroud_RL", - "axis": "0 0 1" - }, { "id": "Frame_Prop_RL", "type": "fixed", @@ -496,113 +190,19 @@ "child-link": "Prop_RL", "axis": "0 0 1" }, - { - "id": "Frame_Shroud_RR", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Shroud_RR", - "axis": "0 0 1" - }, { "id": "Frame_Prop_RR", "type": "fixed", "parent-link": "Frame", "child-link": "Prop_RR", "axis": "0 0 1" - }, - { - "id": "Frame_Wing_L", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Wing_L", - "axis": "0 1 0" - }, - { - "id": "Wing_L_Wing_L_Aileron", - "type": "fixed", - "parent-link": "Wing_L", - "child-link": "Wing_L_Aileron", - "axis": "0 1 0" - }, - { - "id": "Frame_Wing_R", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Wing_R", - "axis": "0 1 0" - }, - { - "id": "Wing_R_Wing_R_Aileron", - "type": "fixed", - "parent-link": "Wing_R", - "child-link": "Wing_R_Aileron", - "axis": "0 1 0" - }, - { - "id": "Frame_Wing_Tail", - "type": "fixed", - "parent-link": "Frame", - "child-link": "Wing_Tail", - "axis": "0 1 0" - }, - { - "id": "Wing_Tail_Elevator", - "type": "fixed", - "parent-link": "Wing_Tail", - "child-link": "Elevator", - "axis": "0 1 0" } ], "controller": { "id": "Simple_Flight_Controller", - "airframe-setup": "vtol-quad-tiltrotor", + "airframe-setup": "quadrotor-x", "type": "simple-flight-api", "simple-flight-api-settings": { - "parameters": { - // Multirotor AngleRate PID - // max_xxx_rate > 5 would introduce wobble/oscillations - "MC_ROLLRATE_MAX": 0.3, // rad/s - "MC_PITCHRATE_MAX": 0.3, // rad/s - "MC_YAWRATE_MAX": 0.3, // rad/s - // p_xxx_rate params are sensitive to gyro noise. Values higher than 0.5 would require noise filtration - "MC_YAWRATE_P": 1.2, - "MC_YAWRATE_D": 0.8, - - // Multirotor AngleLevel PID - // max_pitch/roll_angle > (PI / 5.5) would produce vertical thrust that is - // not enough to keep vehicle in air at extremities of controls - "MC_ROLL_MAX": 0.262, // rad, PI / 12 = 0.262 - "MC_PITCH_MAX": 0.262, // rad - "MC_YAW_P": 1.2, - "MC_YAW_D": 0.8, - - // Multirotor Position PID - "MPC_XY_P": 0.05, - "MPC_XY_I": 0.01, - "MPC_XY_D": 0.05, - - // Multirotor Velocity PID - "MPC_MIN_THR": 0.001, // larger vehicles are stable even with a lower min, max = 1 - - //////////////////////////////////////////////////////////////////////////// - // Fixed-wing vehicle parameters - "FW_AIRSPD_STALL": 2, // m/s - - // Fixed-wing AngleRate PID - // max_xxx_rate > 5 would introduce wobble/oscillations - "FW_Y_RMAX": 1.0, // rad/s - - // Fixed-wing AngleLevel PID - // max_pitch/roll_angle > (PI / 5.5) would produce vertical thrust that is - // not enough to keep vehicle in air at extremities of controls - "FW_PITCH_MAX": 0.262, // rad, PI / 12 = 0.262 - "FW_YAW_P": 0.4, - - // Fixed-wing Velocity PID - "FW_Z_VEL_P": 0.07, - "FW_Z_VEL_I": 0.02, - "FW_Z_VEL_D": 0.05 - }, "actuator-order": [ { "id": "Prop_FR_actuator" @@ -615,27 +215,6 @@ }, { "id": "Prop_RR_actuator" - }, - { - "id": "Prop_FR_tilt_actuator" - }, - { - "id": "Prop_RL_tilt_actuator" - }, - { - "id": "Prop_FL_tilt_actuator" - }, - { - "id": "Prop_RR_tilt_actuator" - }, - { - "id": "Wing_L_Aileron_actuator" - }, - { - "id": "Wing_R_Aileron_actuator" - }, - { - "id": "Elevator_actuator" } ] } @@ -648,7 +227,7 @@ "parent-link": "Frame", "child-link": "Prop_FL", "origin": { - "xyz": "3.2 -3.2 -0.01", + "xyz": "0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, "rotor-settings": { @@ -657,7 +236,7 @@ "coeff-of-thrust": 0.109919, "coeff-of-torque": 0.040164, "max-rpm": 6396.667, - "propeller-diameter": 0.6, + "propeller-diameter": 0.2286, "smoothing-tc": 0.005 } }, @@ -668,7 +247,7 @@ "parent-link": "Frame", "child-link": "Prop_FR", "origin": { - "xyz": "3.2 3.2 -0.01", + "xyz": "0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, "rotor-settings": { @@ -677,7 +256,7 @@ "coeff-of-thrust": 0.109919, "coeff-of-torque": 0.040164, "max-rpm": 6396.667, - "propeller-diameter": 0.6, + "propeller-diameter": 0.2286, "smoothing-tc": 0.005 } }, @@ -688,7 +267,7 @@ "parent-link": "Frame", "child-link": "Prop_RL", "origin": { - "xyz": "-3.2 -3.2 -0.01", + "xyz": "-0.253 -0.253 -0.01", "rpy-deg": "0 0 0" }, "rotor-settings": { @@ -697,7 +276,7 @@ "coeff-of-thrust": 0.109919, "coeff-of-torque": 0.040164, "max-rpm": 6396.667, - "propeller-diameter": 0.6, + "propeller-diameter": 0.2286, "smoothing-tc": 0.005 } }, @@ -708,7 +287,7 @@ "parent-link": "Frame", "child-link": "Prop_RR", "origin": { - "xyz": "-3.2 3.2 -0.01", + "xyz": "-0.253 0.253 -0.01", "rpy-deg": "0 0 0" }, "rotor-settings": { @@ -717,96 +296,7 @@ "coeff-of-thrust": 0.109919, "coeff-of-torque": 0.040164, "max-rpm": 6396.667, - "propeller-diameter": 0.6, - "smoothing-tc": 0.005 - } - }, - { - "name": "Prop_FL_tilt_actuator", - "type": "tilt", - "enabled": true, - "parent-link": "Frame", - "child-link": "Shroud_FL", - "tilt-settings": { - "target": "Prop_FL_actuator", - "angle-min": 0.0, - "angle-max": 1.57, - "axis": "0.0 -1.0 0.0", - "smoothing-tc": 0.5 - } - }, - { - "name": "Prop_RL_tilt_actuator", - "type": "tilt", - "enabled": true, - "parent-link": "Frame", - "child-link": "Shroud_RL", - "tilt-settings": { - "target": "Prop_RL_actuator", - "angle-min": 0.0, - "angle-max": 1.57, - "axis": "0.0 -1.0 0.0", - "smoothing-tc": 0.5 - } - }, - { - "name": "Prop_FR_tilt_actuator", - "type": "tilt", - "enabled": true, - "parent-link": "Frame", - "child-link": "Shroud_FR", - "tilt-settings": { - "target": "Prop_FR_actuator", - "angle-min": 0.0, - "angle-max": 1.57, - "axis": "0.0 -1.0 0.0", - "smoothing-tc": 0.5 - } - }, - { - "name": "Prop_RR_tilt_actuator", - "type": "tilt", - "enabled": true, - "parent-link": "Frame", - "child-link": "Shroud_RR", - "tilt-settings": { - "target": "Prop_RR_actuator", - "angle-min": 0.0, - "angle-max": 1.57, - "axis": "0.0 -1.0 0.0", - "smoothing-tc": 0.5 - } - }, - { - "name": "Wing_L_Aileron_actuator", - "type": "lift-drag-control-surface", - "enabled": true, - "parent-link": "Wing_L", - "child-link": "Wing_L_Aileron", - "lift-drag-control-surface-settings": { - "rotation-rate": 0.524, // radians per 1.0 actuation amount - "smoothing-tc": 0.005 - } - }, - { - "name": "Wing_R_Aileron_actuator", - "type": "lift-drag-control-surface", - "enabled": true, - "parent-link": "Wing_R", - "child-link": "Wing_R_Aileron", - "lift-drag-control-surface-settings": { - "rotation-rate": 0.524, // radians per 1.0 actuation amount - "smoothing-tc": 0.005 - } - }, - { - "name": "Elevator_actuator", - "type": "lift-drag-control-surface", - "enabled": true, - "parent-link": "Wing_Tail", - "child-link": "Elevator", - "lift-drag-control-surface-settings": { - "rotation-rate": 0.524, // radians per 1.0 actuation amount + "propeller-diameter": 0.2286, "smoothing-tc": 0.005 } } @@ -824,7 +314,7 @@ "width": 1280, "height": 720, "fov-degrees": 90, - "capture-enabled": false, + "capture-enabled": true, "streaming-enabled": true, "pixels-as-float": false, "compress": false, @@ -837,7 +327,7 @@ "lock-yaw": false }, "origin": { - "xyz": "-10.0 0.0 -3.0", + "xyz": "-10.0 0.0 -1.0", "rpy-deg": "0 -11.46 0" } }, @@ -849,7 +339,7 @@ "capture-interval": 0.02, "capture-settings": [ { - "image-type": 0, //rgb_camera + "image-type": 0, // scene camera "width": 400, "height": 225, "fov-degrees": 90, @@ -860,17 +350,48 @@ "target-gamma": 2.5 }, { - "image-type": 1, //depth_planar + "image-type": 1, // depth planar "width": 400, "height": 225, "fov-degrees": 90, - "capture-enabled": false, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 2, // depth perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 3, // segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, "streaming-enabled": false, "pixels-as-float": false, "compress": false }, { - "image-type": 2, //depth_perspective + "image-type": 4, // depth vis + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "max-depth-meters": 300 + }, + { + "image-type": 5, // disparity normalized "width": 400, "height": 225, "fov-degrees": 90, @@ -880,11 +401,11 @@ "compress": false }, { - "image-type": 3, //segmentation + "image-type": 6, // surface normals "width": 400, "height": 225, "fov-degrees": 90, - "capture-enabled": false, + "capture-enabled": true, "streaming-enabled": false, "pixels-as-float": false, "compress": false @@ -910,8 +431,8 @@ } ], "origin": { - "xyz": "0.0 0.0 0.5", - "rpy-deg": "0.0 -90 0.0" + "xyz": "0.0 0.0 -1.0", + "rpy-deg": "0.0 -11.46 0.0" } }, { @@ -920,23 +441,123 @@ "enabled": true, "parent-link": "Frame", "accelerometer": { - "velocity-random-walk": 0.0123, + "velocity-random-walk": 2.3e-3, "tau": 800, - "bias-stability": 2e-5, + "bias-stability": 3.53e-4, "turn-on-bias": "0 0 0" }, "gyroscope": { - "angle-random-walk": 0.0123, + "angle-random-walk": 1.027e-4, "tau": 500, - "bias-stability": 1e-6, + "bias-stability": 2.23e-5, "turn-on-bias": "0 0 0" } }, + { + "id": "lidar1", + "type": "lidar", + "enabled": true, + "parent-link": "Frame", + "number-of-channels": 16, + "range": 100, + "points-per-second": 100000, + "horizontal-rotation-frequency": 10, + "horizontal-fov-start-deg": 0.0, + "horizontal-fov-end-deg": 360.0, + "vertical-fov-upper-deg": 0.0, + "vertical-fov-lower-deg": -90.0, + "draw-debug-points": false, + "origin": { + "xyz": "0 0 0.2", + "rpy-deg": "0 0 0" + } + }, + { + "id": "DistanceSensor", + "type": "distance-sensor", + "enabled": true, + "parent-link": "Frame", + "max-distance": 50.0, + "min-distance": 0.5, + "draw-debug-points": true, + "origin": { + "xyz": "0 0 0.2", + "rpy-deg": "0 -90 0" + } + }, { "id": "GPS", "type": "gps", - "enabled": false, - "parent-link": "Frame" + "enabled": true, + "parent-link": "Frame", + "eph-time-constant": 0.9, + "epv-time-constant": 0.9, + "eph-initial": 100, + "epv-initial": 100, + "eph-final": 0.1, + "epv-final": 0.1, + "eph-min_3d": 3.0, + "eph-min_2d": 4.0 + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": true, + "parent-link": "Frame", + "qnh": 1013.250, + "pressure-factor-sigma": 0.001825, + "pressure-factor-tau": 3600, + "uncorrelated-noise-sigma": 2.7, + "update-latency": 0, + "update-frequency": 50, + "startup-delay": 0 + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": true, + "parent-link": "Frame", + "scale-factor": 1, + "noise-sigma": "0.005 0.005 0.005", // 5 mgauss as per LSM303D spec-sheet + "noise-bias": "0.0 0.0 0.0" // no offset as per specsheet (zero gauss level) + }, + { + "id": "Airspeed", + "type": "airspeed", + "enabled": true, + "parent-link": "Frame", + "pressure-factor-sigma": 0.001825, + "pressure-factor-tau": 3600, + "uncorrelated-noise-sigma": 1.052, + "forward-xyz": "1.0 0.0 0.0" + }, + { + "id": "Battery", + "type": "battery", + "enabled": true, + "parent-link": "Frame", + + // Total battery capacity. + // Typically, on start capacity should be lower than total battery capacity. + // Use joules as unit for capacity. + // 1 joule = 3600 * Amp hour + // 1 joule = 3.6 * milli Amp hour + "total-battery-capacity": 36000, + "battery-capacity-on-start": 30000, + + // Battery has two simulated mode: Rotor power discharge or simple discharge model + + // Rotor power discharge mode depletes power proportional to torque*angular_velocity + // Simple discharge mode, depletes at rate specified. + + // Config for rotor power discharge mode. + // You can configure the constant of proportionality using config option. + "battery-mode": "rotor-power-discharge-mode", + "rotor-power-coefficient": 1 + + // Config for simple discharge model. The drain rate can be adjusted at runtime using client api calls. + // "battery-mode":"simple-discharge-mode", + // "battery-drain-rate-on-start": 1 } ] } \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/robot_x8_skywalker_jsbsim.jsonc b/client/python/example_user_scripts/sim_config/robot_x8_skywalker_jsbsim.jsonc new file mode 100644 index 00000000..94bfee8e --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_x8_skywalker_jsbsim.jsonc @@ -0,0 +1,478 @@ +{ + "physics-type": "jsbsim-physics", + "jsbsim-script": "scripts/x8_test.xml", + "jsbsim-model": "c310", + "visual-scheink": "JSBSimX8", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "enabled": false, + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/Quadrotor1" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_FR", + "inertial": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_RL", + "inertial": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + }, + { + "name": "Prop_RR", + "inertial": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_FR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FR", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RR", + "axis": "0 0 1" + } + ], + "controller": { + "id": "Simple_Flight_Controller", + "airframe-setup": "quadrotor-x", + "type": "simple-flight-api", + "simple-flight-api-settings": { + "actuator-order": [ + { + "id": "Prop_FR_actuator" + }, + { + "id": "Prop_RL_actuator" + }, + { + "id": "Prop_FL_actuator" + }, + { + "id": "Prop_RR_actuator" + } + ] + } + }, + "actuators": [ + { + "name": "Prop_FL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_FR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FR", + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RL", + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RR", + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-10.0 0.0 -1.0", + "rpy-deg": "0 -11.46 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.02, + "capture-settings": [ + { + "image-type": 0, // scene camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, // depth planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 2, // depth perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": true, + "compress": false + }, + { + "image-type": 3, // segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 4, // depth vis + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "max-depth-meters": 100 + }, + { + "image-type": 5, // disparity normalized + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 6, // surface normals + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy-deg": "0 -90 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": false, + "parent-link": "Frame" + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": false, + "parent-link": "Frame" + } + ] +} \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/scene_basic_fixed_wing.jsonc b/client/python/example_user_scripts/sim_config/scene_basic_fixed_wing.jsonc new file mode 100644 index 00000000..afbd63a0 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/scene_basic_fixed_wing.jsonc @@ -0,0 +1,247 @@ +{ + "id": "SceneBasicFixedWing", + "actors": [ + { + "type": "robot", + "name": "x8", + "origin": { + "xyz": "3 -2 -0.95", + "rpy": "0 0 0" + }, + "robot-config": "robot_x8_skywalker_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22", + "origin": { + "xyz": "-833 200 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22_2", + "origin": { + "xyz": "-833 100 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22_3", + "origin": { + "xyz": "-833 -2 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22_4", + "origin": { + "xyz": "-833 -100 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22_5", + "origin": { + "xyz": "-833 -200 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "f22_6", + "origin": { + "xyz": "-833 -300 -5", + "rpy": "0 0 0" + }, + "robot-config": "robot_f22_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "Drone", + "origin": { + "xyz": "1110 -2 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_1", + "origin": { + "xyz": "1110 -5 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_2", + "origin": { + "xyz": "1105 -2 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_3", + "origin": { + "xyz": "1105 -5 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_4", + "origin": { + "xyz": "1100 -2 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_5", + "origin": { + "xyz": "1100 -5 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_6", + "origin": { + "xyz": "1095 -10 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_7", + "origin": { + "xyz": "1095 -13 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_8", + "origin": { + "xyz": "1090 -10 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_9", + "origin": { + "xyz": "1090 -13 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_10", + "origin": { + "xyz": "1085 -10 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "Drone_11", + "origin": { + "xyz": "1085 -13 -178", + "rpy": "0 0 0" + }, + "robot-config": "robot_quadrotor_fastphysics.jsonc" + }, + { + "type": "robot", + "name": "737", + "origin": { + "xyz": "500 2000 -30", + "rpy": "0 0 -90" + }, + "robot-config": "robot_737_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "737_2", + "origin": { + "xyz": "2100 -4500 -30", + "rpy": "0 0 90" + }, + "robot-config": "robot_737_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "Helicopter", + "origin": { + "xyz": "800 -5 -2", + "rpy": "0 0 180" + }, + "robot-config": "robot_ah1s_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "Cessna", + "origin": { + "xyz": "700 5 -2", + "rpy": "0 0 150" + }, + "robot-config": "robot_c1723_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "Cessna_2", + "origin": { + "xyz": "700 70 -2", + "rpy": "0 0 150" + }, + "robot-config": "robot_c1723_jsbsim.jsonc" + }, + { + "type": "robot", + "name": "Rocket", + "origin": { + "xyz": "900 -5 150", + "rpy": "89.99 90 90" + }, + "robot-config": "robot_j246_jsbsim.jsonc" + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 29.4296741, + "longitude": -95.16384722, + "altitude": 7.5 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} \ No newline at end of file diff --git a/client/python/example_user_scripts/sim_config/scene_basic_helicopter.jsonc b/client/python/example_user_scripts/sim_config/scene_basic_helicopter.jsonc new file mode 100644 index 00000000..29201c32 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/scene_basic_helicopter.jsonc @@ -0,0 +1,33 @@ +{ + "id": "SceneBasicHelicopter", + "actors": [ + { + "type": "robot", + "name": "Drone1", + //currently not used by jsbsim physics: + "origin": { + "xyz": "0.0 0.0 -12", + //yaw=pi, pitch=0, roll=0 + "rpy": "0 0 3.1416" + }, + "robot-config": "robot_helicopter.jsonc" + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 29.4296741, + "longitude": 34.95416, + "altitude": 686 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} \ No newline at end of file diff --git a/client/python/projectairsim/src/projectairsim/drone.py b/client/python/projectairsim/src/projectairsim/drone.py index dca82e2f..cf320262 100644 --- a/client/python/projectairsim/src/projectairsim/drone.py +++ b/client/python/projectairsim/src/projectairsim/drone.py @@ -228,6 +228,39 @@ def disarm(self) -> bool: disarmed = self.client.request(disarm_req) return disarmed + #get_take_off_z + def get_take_off_z(self) -> float: + """Get take off z + + Returns: + float: Take off z + """ + get_take_off_z_req: Dict = { + "method": f"{self.parent_topic}/GetTakeOffZ", + "params": {}, + "version": 1.0, + } + take_off_z = self.client.request(get_take_off_z_req) + return take_off_z + + #set_take_off_z + def set_take_off_z(self, take_off_z: float) -> bool: + """Set take off z + + Args: + take_off_z (float): Take off z + + Returns: + bool: True if take off z is set + """ + set_take_off_z_req: Dict = { + "method": f"{self.parent_topic}/SetTakeOffZ", + "params": {"_take_off_z": take_off_z}, + "version": 1.0, + } + take_off_z_set = self.client.request(set_take_off_z_req) + return take_off_z_set + def can_arm(self) -> bool: """Checks if the vehicle can be armed @@ -1447,6 +1480,64 @@ async def set_vtol_mode_async( taskcr = await self.client.request_async(req, callback) return taskcr + #set_brakes + def set_brakes(self, brakes_value: float) -> bool: + """Set brakes + + Args: + brakes_value (float): Brakes value + + Returns: + bool: True if brakes are set + """ + set_brakes_req: Dict = { + "method": f"{self.parent_topic}/SetBrakes", + "params": {"_brakes_value": brakes_value}, + "version": 1.0, + } + brakes_set = self.client.request(set_brakes_req) + return brakes_set + + # get_jsbsim_property + def get_jsbsim_property(self, property_name: str) -> float: + """Get JSBSim property + + Args: + property_name (str): Property name + + Returns: + float: Property value + """ + get_jsbsim_property_req: Dict = { + "method": f"{self.parent_topic}/GetJSBSimProperty", + "params": {"_property_name": property_name}, + "version": 1.0, + } + property_value = self.client.request(get_jsbsim_property_req) + return property_value + + # set_jsbsim_property + def set_jsbsim_property(self, property_name: str, property_value: float) -> bool: + """Set JSBSim property + + Args: + property_name (str): Property name + property_value (float): Property value + + Returns: + bool: True if property is set + """ + set_jsbsim_property_req: Dict = { + "method": f"{self.parent_topic}/SetJSBSimProperty", + "params": { + "_property_name": property_name, + "_value": property_value, + }, + "version": 1.0, + } + property_set = self.client.request(set_jsbsim_property_req) + return property_set + def set_control_signals(self, control_signal_map: Dict) -> bool: """set_control_signals for Manual Controller type diff --git a/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc b/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc index d73d844e..78939022 100644 --- a/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc +++ b/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc @@ -7,7 +7,7 @@ "physics-type": { "description": "Defines the physics type for the robot", "type": "string", - "enum": [ "fast-physics", "non-physics", "matlab-physics" ] + "enum": [ "fast-physics", "non-physics", "matlab-physics", "jsbsim-physics"] }, "links": { "description": "Define the links for the robot", @@ -64,11 +64,11 @@ "id": { "type": "string" }, "airframe-setup": { "type": "string", - "enum": [ "quadrotor-x", "hexarotor-x", "vtol-quad-x-tailsitter", "vtol-quad-tiltrotor", "ackermann"] + "enum": [ "quadrotor-x", "hexarotor-x", "vtol-quad-x-tailsitter", "vtol-quad-tiltrotor", "ackermann", "fixed-wing"] }, "type": { "type": "string", - "enum": [ "simple-flight-api", "simple-drive-api","px4-api", "ardupilot-api", "manual-controller-api", "matlab-controller-api"] + "enum": [ "simple-flight-api", "simple-drive-api","px4-api", "ardupilot-api", "manual-controller-api", "matlab-controller-api", "jsbsim-api" ] }, "simple-flight-api-settings": { "type": "object", @@ -84,6 +84,11 @@ "actuator-order": { "$ref": "#/definitions/actuator-order" } } }, + // TODO "jsbsim-api-settings": { + // "type": "object", + // "properties": { + // } + // }, "px4-settings": { "$ref": "#/definitions/px4-settings" }, "manual-controller-api-settings": { "type": "object", diff --git a/core_sim/include/core_sim/actor/robot.hpp b/core_sim/include/core_sim/actor/robot.hpp index fae59460..8115cc72 100644 --- a/core_sim/include/core_sim/actor/robot.hpp +++ b/core_sim/include/core_sim/actor/robot.hpp @@ -23,6 +23,7 @@ #include "core_sim/service_method.hpp" #include "core_sim/transforms/transform.hpp" #include "core_sim/transforms/transform_tree.hpp" +#include "FGFDMExec.h" namespace microsoft { namespace projectairsim { @@ -59,6 +60,8 @@ class Robot : public Actor { const PhysicsType& GetPhysicsType() const; void SetPhysicsType(const PhysicsType& phys_type); + //used only by JSBSim physics and controller + std::shared_ptr GetJSBSimModel() const; const std::string& GetPhysicsConnectionSettings() const; void SetPhysicsConnectionSettings(const std::string& phys_conn_settings); const std::string& GetControlConnectionSettings() const; @@ -143,12 +146,14 @@ class Robot : public Actor { Robot(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager); + const StateManager& state_manager, + const std::string& working_simulation_path); Robot(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager, HomeGeoPoint home_geo_point); + const StateManager& state_manager, HomeGeoPoint home_geo_point, + const std::string& working_simulation_path); void Load(ConfigJson config_json) override; diff --git a/core_sim/include/core_sim/environment.hpp b/core_sim/include/core_sim/environment.hpp index c6ea9c58..91a4a80a 100644 --- a/core_sim/include/core_sim/environment.hpp +++ b/core_sim/include/core_sim/environment.hpp @@ -20,7 +20,7 @@ struct Environment { inline static Vector3 wind_velocity = Vector3::Zero(); Vector3 position_ned = Vector3::Zero(); - HomeGeoPoint home_geo_point; + HomeGeoPoint home_geo_point; //These are the world origin coordinates (0,0,0), not the home position of the drone EnvInfo env_info; // Current environmental condition that can vary (e.g., by // current location, time of day, etc.) diff --git a/core_sim/include/core_sim/math_utils.hpp b/core_sim/include/core_sim/math_utils.hpp index f6a576aa..e9560b8a 100644 --- a/core_sim/include/core_sim/math_utils.hpp +++ b/core_sim/include/core_sim/math_utils.hpp @@ -123,6 +123,10 @@ class MathUtils { static constexpr E ToEnum(typename std::underlying_type::type u) { return static_cast(u); } + + static constexpr float feets_to_meters = 0.3048f; + static constexpr float meters_to_feets = 1.0f / feets_to_meters; + }; } // namespace projectairsim diff --git a/core_sim/include/core_sim/physics_common_types.hpp b/core_sim/include/core_sim/physics_common_types.hpp index 29e804b4..6688dc62 100644 --- a/core_sim/include/core_sim/physics_common_types.hpp +++ b/core_sim/include/core_sim/physics_common_types.hpp @@ -16,7 +16,8 @@ enum class PhysicsType { kNonPhysics = 0, kFastPhysics = 1, kUnrealPhysics = 2, - kMatlabPhysics = 3 + kMatlabPhysics = 3, + kJSBSimPhysics = 4 }; struct Pose { diff --git a/core_sim/include/core_sim/scene.hpp b/core_sim/include/core_sim/scene.hpp index 3e5a4115..f2fb8fca 100644 --- a/core_sim/include/core_sim/scene.hpp +++ b/core_sim/include/core_sim/scene.hpp @@ -128,7 +128,8 @@ class Scene { Scene(const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager); + const StateManager& state_manager, + const std::string& working_simulation_path_); void LoadWithJSON(ConfigJson config_json); diff --git a/core_sim/include/core_sim/simulator.hpp b/core_sim/include/core_sim/simulator.hpp index ce120579..9498e519 100644 --- a/core_sim/include/core_sim/simulator.hpp +++ b/core_sim/include/core_sim/simulator.hpp @@ -22,7 +22,7 @@ class Simulator { Simulator(); - Simulator(LoggerCallback logger_callback, LogLevel level = LogLevel::kError); + Simulator(LoggerCallback logger_callback, LogLevel level = LogLevel::kError, const std::string& working_simulation_path=""); // Simulator controls diff --git a/core_sim/src/CMakeLists.txt b/core_sim/src/CMakeLists.txt index 709457e1..615395bc 100644 --- a/core_sim/src/CMakeLists.txt +++ b/core_sim/src/CMakeLists.txt @@ -105,11 +105,14 @@ set_target_properties(${TARGET_NAME} PROPERTIES ) if(WIN32) - add_dependencies(${TARGET_NAME} nng-external) + add_dependencies(${TARGET_NAME} nng-external jsbsim-repo) else() - add_dependencies(${TARGET_NAME} nng-external openssl-external) + add_dependencies(${TARGET_NAME} nng-external jsbsim-repo openssl-external) endif() +# Set up JSBSim for linking +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_STATIC_LIB_DIR}) + # Set up nng and openssl for linking target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR}) target_compile_definitions(${TARGET_NAME} PRIVATE NNG_STATIC_LIB=ON) @@ -124,6 +127,7 @@ target_include_directories( PUBLIC $ $ + ${JSBSIM_INCLUDE_DIR} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ${JSON_INCLUDE_DIR} @@ -145,6 +149,7 @@ if(WIN32) Crypt32 # Client authorization Ncrypt # Client authorization lvmon + jsbsim ) else() target_link_libraries( @@ -152,6 +157,7 @@ else() PRIVATE nng crypto # Client authorization + jsbsim ) endif() @@ -160,7 +166,10 @@ add_custom_command(TARGET ${TARGET_NAME} COMMAND ${CMAKE_COMMAND} -E echo "Packaging [${TARGET_NAME}] build outputs to ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>" COMMAND ${CMAKE_COMMAND} -E copy_directory $ ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug> COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_SOURCE_DIR}/../include ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/include + COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_SOURCE_DIR}/../jsbsim ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/jsbsim COMMAND ${CMAKE_COMMAND} -E remove_directory ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/CMakeFiles COMMAND ${CMAKE_COMMAND} -E remove -f ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/cmake_install.cmake COMMAND ${CMAKE_COMMAND} -E remove -f ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/CTestTestfile.cmake ++ COMMAND ${CMAKE_COMMAND} -E copy_directory ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/jsbsim ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/jsbsim + ) diff --git a/core_sim/src/actor/robot.cpp b/core_sim/src/actor/robot.cpp index ce73da9f..54ec8de1 100644 --- a/core_sim/src/actor/robot.cpp +++ b/core_sim/src/actor/robot.cpp @@ -60,7 +60,8 @@ class Robot::Impl : public ActorImpl { Impl(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager); + const StateManager& state_manager, + const std::string& working_simulation_path); void Load(ConfigJson config_json); @@ -131,6 +132,8 @@ class Robot::Impl : public ActorImpl { const PhysicsType& GetPhysicsType() const; void SetPhysicsType(const PhysicsType& phys_type); + //used only by JSBSim physics and controller + std::shared_ptr GetJSBSimModel() const; const std::string& GetPhysicsConnectionSettings() const; void SetPhysicsConnectionSettings(const std::string& phys_conn_settings); const std::string& GetControlConnectionSettings() const; @@ -166,6 +169,9 @@ class Robot::Impl : public ActorImpl { private: friend class Robot::Loader; + const std::string& GetJSBSimScript() const; + void InitializeJSBSimModel(); + Robot::Loader loader_; std::vector links_; std::vector joints_; @@ -179,6 +185,9 @@ class Robot::Impl : public ActorImpl { TimeNano kinematics_updated_timestamp_; PhysicsType physics_type_; + std::string jsbsim_script_; + std::string jsbsim_model_; + std::shared_ptr model_; std::string physics_connection_settings_; std::string control_connection_settings_; bool start_landed_; @@ -213,6 +222,8 @@ class Robot::Impl : public ActorImpl { indirectrefframe_; // Current pose reference frame TransformTree::StaticRefFrame staticrefframe_home_; // Home pose reference frame + + const std::string working_simulation_path_; }; // ----------------------------------------------------------------------------- @@ -224,19 +235,21 @@ Robot::Robot(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager) + const StateManager& state_manager, + const std::string& working_simulation_path) : Actor(std::shared_ptr( new Robot::Impl(id, origin, logger, topic_manager, parent_topic_path, - service_manager, state_manager))) {} + service_manager, state_manager, working_simulation_path))) {} Robot::Robot(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager, HomeGeoPoint home_geo_point) + const StateManager& state_manager, HomeGeoPoint home_geo_point, + const std::string& working_simulation_path) : Actor(std::shared_ptr( new Robot::Impl(id, origin, logger, topic_manager, parent_topic_path, - service_manager, state_manager))) { + service_manager, state_manager, working_simulation_path))) { static_cast(pimpl_.get())->SetHomeGeoPoint(home_geo_point); } @@ -369,6 +382,10 @@ void Robot::SetPhysicsType(const PhysicsType& phys_type) { static_cast(pimpl_.get())->SetPhysicsType(phys_type); } +std::shared_ptr Robot::GetJSBSimModel() const { + return static_cast(pimpl_.get())->GetJSBSimModel(); +} + const std::string& Robot::GetPhysicsConnectionSettings() const { return static_cast(pimpl_.get()) ->GetPhysicsConnectionSettings(); @@ -478,7 +495,8 @@ Robot::Impl::Impl(const std::string& id, const Transform& origin, const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager) + const StateManager& state_manager, + const std::string& working_simulation_path) : ActorImpl(ActorType::kRobot, id, origin, Constant::Component::robot, logger, topic_manager, parent_topic_path, service_manager, state_manager), @@ -488,7 +506,8 @@ Robot::Impl::Impl(const std::string& id, const Transform& origin, indirectrefframe_(std::string("R ") + id, &kinematics_.pose), staticrefframe_home_(std::string("R ") + id + "_home", kinematics_.pose), last_actuated_rotation_simtime_(0), - start_landed_(false) { + start_landed_(false), + working_simulation_path_(working_simulation_path) { SetTopicPath(); CreateTopics(); RegisterServiceMethods(); @@ -513,6 +532,11 @@ void Robot::Impl::Load(ConfigJson config_json) { //! Initialize Sensors InitializeSensors(GetKinematics(), GetEnvironment()); + // check if using jsbsim physics to initialize jsbsim + if(physics_type_ == PhysicsType::kJSBSimPhysics) { + InitializeJSBSimModel(); + } + // Create tilt actuator target list { std::vector vecpactuatorTilt; @@ -719,6 +743,23 @@ void Robot::Impl::RegisterServiceMethods() { service_manager_.RegisterMethod(get_camera_ray, get_camera_ray_handler); } +void Robot::Impl::InitializeJSBSimModel(){ + model_ = std::make_shared(); + std::string jsbsim_root_path = + working_simulation_path_ + "/SimLibs/core_sim/jsbsim/models/"; + model_->SetRootDir(SGPath(jsbsim_root_path)); + model_->SetAircraftPath(SGPath("aircraft")); + model_->SetEnginePath(SGPath("engine")); + model_->SetSystemsPath(SGPath("systems")); + if(jsbsim_script_!="") { + if (!model_->LoadScript(SGPath(jsbsim_script_))) { + throw std::runtime_error("Failed to load JSBSim script"); + } + } else if (!model_->LoadModel(jsbsim_model_)) { + throw std::runtime_error("Failed to load JSBSim model"); + } +} + bool Robot::Impl::SetExternalForce(const std::vector& ext_force) { ext_force_ = Vector3(ext_force[0], ext_force[1], ext_force[2]); return true; @@ -961,6 +1002,10 @@ void Robot::Impl::SetPhysicsType(const PhysicsType& phys_type) { physics_type_ = phys_type; } +const std::string& Robot::Impl::GetJSBSimScript() const { return jsbsim_script_; } + +std::shared_ptr Robot::Impl::GetJSBSimModel() const { return model_; } + const std::string& Robot::Impl::GetPhysicsConnectionSettings() const { return physics_connection_settings_; } @@ -1402,6 +1447,16 @@ void Robot::Loader::LoadPhysicsType(const json& json) { impl_.physics_type_ = PhysicsType::kMatlabPhysics; } else if (physics_type == Constant::Config::unreal_physics) { impl_.physics_type_ = PhysicsType::kUnrealPhysics; + } else if (physics_type == Constant::Config::jsbsim_physics) { + impl_.physics_type_ = PhysicsType::kJSBSimPhysics; + impl_.jsbsim_script_ = + JsonUtils::GetString( + json, Constant::Config::jsbsim_script); + if(impl_.jsbsim_script_ == "") { + impl_.jsbsim_model_ = + JsonUtils::GetString( + json, Constant::Config::jsbsim_model); + } } else { impl_.physics_type_ = PhysicsType::kNonPhysics; impl_.logger_.LogWarning( @@ -1436,4 +1491,4 @@ void Robot::Loader::LoadController(const json& json) { } } // namespace projectairsim -} // namespace microsoft +} // namespace microsoft \ No newline at end of file diff --git a/core_sim/src/actuators/actuator_impl.hpp b/core_sim/src/actuators/actuator_impl.hpp index 91923807..53e8d364 100644 --- a/core_sim/src/actuators/actuator_impl.hpp +++ b/core_sim/src/actuators/actuator_impl.hpp @@ -157,7 +157,7 @@ class ActuatorImpl : public ComponentWithTopicsAndServiceMethods { const std::string& actor_id) { actor_logger.LogVerbose(actor_name, "[%s] Loading 'actuator.name'.", actor_id.c_str()); - auto id = JsonUtils::GetIdentifier(json, Constant::Config::name); + auto id = JsonUtils::GetString(json, Constant::Config::name); //TODO: This allowed an identifier with "/" in it for JSBsim actuators. Review if necessary or if we should use any property. actor_logger.LogVerbose(actor_name, "[%s][%s] 'actuator.name' loaded.", actor_id.c_str(), id.c_str()); diff --git a/core_sim/src/constant.hpp b/core_sim/src/constant.hpp index 24887a93..8f9b088c 100644 --- a/core_sim/src/constant.hpp +++ b/core_sim/src/constant.hpp @@ -384,6 +384,9 @@ class Constant { static constexpr const char* control_connection = "control-connection"; static constexpr const char* start_landed = "start-landed"; static constexpr const char* unreal_physics = "unreal-physics"; + static constexpr const char* jsbsim_physics = "jsbsim-physics"; + static constexpr const char* jsbsim_script = "jsbsim-script"; + static constexpr const char* jsbsim_model = "jsbsim-model"; static constexpr const char* restitution = "restitution"; static constexpr const char* friction = "friction"; static constexpr const char* body_box_xyz = "body-box-xyz"; @@ -429,6 +432,9 @@ class Constant { static constexpr const char* simple_flight_api = "simple-flight-api"; static constexpr const char* simple_flight_api_settings = "simple-flight-api-settings"; + static constexpr const char* jsbsim_api = "jsbsim-api"; + static constexpr const char* jsbsim_api_settings = + "jsbsim-api-settings"; static constexpr const char* simple_drive_api = "simple-drive-api"; static constexpr const char* simple_drive_api_settings = "simple-drive-api-settings"; diff --git a/core_sim/src/scene.cpp b/core_sim/src/scene.cpp index 37deb038..4abc7330 100644 --- a/core_sim/src/scene.cpp +++ b/core_sim/src/scene.cpp @@ -76,7 +76,8 @@ class Scene::Impl : public ComponentWithTopicsAndServiceMethods { Impl(const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager); + const StateManager& state_manager, + const std::string& working_simulation_path); void LoadSceneWithJSON(ConfigJson config_json); @@ -217,6 +218,7 @@ class Scene::Impl : public ComponentWithTopicsAndServiceMethods { int tiles_lod_min_; TransformTree transform_tree_; std::vector> trajectories_; + const std::string& working_simulation_path_; std::shared_ptr viewport_camera_; std::shared_ptr geodetic_converter_; }; @@ -229,9 +231,10 @@ Scene::Scene() : pimpl_(std::shared_ptr(nullptr)) {} Scene::Scene(const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager) + const StateManager& state_manager, + const std::string& working_simulation_path) : pimpl_(std::make_shared(logger, topic_manager, parent_topic_path, - service_manager, state_manager)) {} + service_manager, state_manager, working_simulation_path)) {} void Scene::LoadWithJSON(ConfigJson config_json) { pimpl_->LoadSceneWithJSON(config_json); @@ -405,10 +408,12 @@ void Scene::Stop() { Scene::Impl::Impl(const Logger& logger, const TopicManager& topic_manager, const std::string& parent_topic_path, const ServiceManager& service_manager, - const StateManager& state_manager) + const StateManager& state_manager, + const std::string& working_simulation_path) : ComponentWithTopicsAndServiceMethods(Constant::Component::scene, logger, topic_manager, parent_topic_path, service_manager, state_manager), + working_simulation_path_(working_simulation_path), loader_(*this), geodetic_converter_(std::make_shared()) {} @@ -941,7 +946,7 @@ bool Scene::Impl::SceneTick() { // After kinematics have been updated, update each actor's environment // to match the new state, and then update the sensor state to match the // new environments. - for (auto& actor : actors_) { + for (auto& actor : actors_) { if (actor->GetType() == ActorType::kRobot) { auto& robot = static_cast(*actor); @@ -1371,7 +1376,8 @@ std::unique_ptr Scene::Loader::LoadActorWithJSON(const json& json) { auto robot = new Robot(id, origin, impl_.logger_, impl_.topic_manager_, impl_.topic_path_ + "/robots", impl_.service_manager_, - impl_.state_manager_, impl_.home_geo_point_); + impl_.state_manager_, impl_.home_geo_point_, + impl_.working_simulation_path_); robot->Load(robot_config); robot->SetPhysicsConnectionSettings(physics_connection_json.dump()); robot->SetControlConnectionSettings(control_connection_json.dump()); diff --git a/core_sim/src/simulator.cpp b/core_sim/src/simulator.cpp index 8387d0b8..cdce53ca 100644 --- a/core_sim/src/simulator.cpp +++ b/core_sim/src/simulator.cpp @@ -49,7 +49,8 @@ class Simulator::Loader { class Simulator::Impl : public ComponentWithTopicsAndServiceMethods { public: explicit Impl(LoggerCallback logger_callback, - LogLevel level = LogLevel::kError); + LogLevel level = LogLevel::kError, + const std::string& working_simulation_path=""); void LoadSimulator(const std::string& sim_json, std::string client_auth_public_key); @@ -90,6 +91,7 @@ class Simulator::Impl : public ComponentWithTopicsAndServiceMethods { std::string scene_config_; static constexpr TimeNano kSceneTickPeriod = 3000000; // 3 ms = 333 Hz + const std::string working_simulation_path_; }; // ----------------------------------------------------------------------------- @@ -98,8 +100,9 @@ class Simulator::Impl : public ComponentWithTopicsAndServiceMethods { Simulator::Simulator() : pimpl_(std::make_shared(default_logger_callback)) {} -Simulator::Simulator(LoggerCallback logger_callback, LogLevel level) - : pimpl_(std::make_shared(logger_callback, level)) {} +Simulator::Simulator(LoggerCallback logger_callback, LogLevel level, + const std::string& working_simulation_path) + : pimpl_(std::make_shared(logger_callback, level, working_simulation_path)) {} void Simulator::LoadSimulator(const std::string& sim_json, std::string client_auth_public_key) { @@ -147,10 +150,12 @@ void Simulator::RegisterServiceMethod(const ServiceMethod& method, // ----------------------------------------------------------------------------- // class Simulator::Impl -Simulator::Impl::Impl(LoggerCallback logger_callback, LogLevel level) +Simulator::Impl::Impl(LoggerCallback logger_callback, LogLevel level, + const std::string& working_simulation_path) : ComponentWithTopicsAndServiceMethods(Constant::Component::simulator, Logger(logger_callback, level)), - loader_(*this) { + loader_(*this), + working_simulation_path_(working_simulation_path) { client_authorization_.SetLogger(GetLogger()); } @@ -341,7 +346,7 @@ void Simulator::Loader::LoadSceneWithJSON(const std::string& json_data) { impl_.sim_scene_ = Scene(impl_.logger_, impl_.topic_manager_, impl_.topic_path_, - impl_.service_manager_, impl_.state_manager_); + impl_.service_manager_, impl_.state_manager_, impl_.working_simulation_path_); impl_.state_manager_.Initialize(&impl_.sim_scene_); diff --git a/core_sim/test/CMakeLists.txt b/core_sim/test/CMakeLists.txt index 18517b61..ea33eb07 100644 --- a/core_sim/test/CMakeLists.txt +++ b/core_sim/test/CMakeLists.txt @@ -106,6 +106,7 @@ if(WIN32) Crypt32 # Client authorization Ncrypt # Client authorization lvmon + jsbsim ) else() target_link_libraries( @@ -115,19 +116,25 @@ else() core_sim nng crypto # Client authorization + jsbsim ) endif() target_include_directories(${TARGET_NAME} PRIVATE "${ONNXRUNTIME_ROOTDIR}/include") target_link_directories(${TARGET_NAME} PRIVATE "${ONNX_LIB_DIR}") -target_link_libraries(${TARGET_NAME} PRIVATE onnxruntime) +target_link_libraries(${TARGET_NAME} PRIVATE onnxruntime jsbsim) +add_custom_command( + TARGET ${TARGET_NAME} + PRE_LINK + COMMAND ${CMAKE_COMMAND} -E copy_directory ${UE_PLUGIN_SIMLIBS_DIR}/shared_libs/ ${SIMLIBS_TEST_DIR} +) add_custom_command( TARGET ${TARGET_NAME} PRE_LINK - COMMAND ${CMAKE_COMMAND} -E copy_directory ${UE_PLUGIN_SIMLIBS_DIR}/shared_libs/ ${SIMLIBS_TEST_DIR} + COMMAND ${CMAKE_COMMAND} -E copy_directory ${JSBSIM_CORESIM_DIR}/lib/Debug/ ${SIMLIBS_TEST_DIR} ) # Include CMake's GoogleTest module to use gtest_discover_tests() helper diff --git a/core_sim/test/gtest_actuator.cpp b/core_sim/test/gtest_actuator.cpp index 7805cd02..26cbcfb5 100644 --- a/core_sim/test/gtest_actuator.cpp +++ b/core_sim/test/gtest_actuator.cpp @@ -22,7 +22,7 @@ class Scene { // : public ::testing::Test { Logger logger(logger_callback); Transform origin = {{0, 0, 0}, {1, 0, 0, 0}}; return Robot("TestRobotID", origin, logger, TopicManager(logger), "", - ServiceManager(logger), StateManager(logger)); + ServiceManager(logger), StateManager(logger),""); } static void LoadRobot(Robot& robot, ConfigJson config_json) { diff --git a/core_sim/test/gtest_robot.cpp b/core_sim/test/gtest_robot.cpp index a569dc11..b1e2f63f 100644 --- a/core_sim/test/gtest_robot.cpp +++ b/core_sim/test/gtest_robot.cpp @@ -26,7 +26,7 @@ class Scene { const std::string& message) {}; Logger logger(callback); return Robot(id, origin, logger, TopicManager(logger), "", - ServiceManager(logger), StateManager(logger)); + ServiceManager(logger), StateManager(logger), ""); } static void LoadRobot(Robot& robot, ConfigJson config_json) { diff --git a/core_sim/test/gtest_scene.cpp b/core_sim/test/gtest_scene.cpp index d18ee299..b8946f95 100644 --- a/core_sim/test/gtest_scene.cpp +++ b/core_sim/test/gtest_scene.cpp @@ -25,7 +25,7 @@ class Simulator { const std::string& message) {}; Logger logger(callback); return Scene(logger, TopicManager(logger), "", ServiceManager(logger), - StateManager(logger)); + StateManager(logger),""); } static void LoadScene(Scene& scene, ConfigJson config_json) { diff --git a/core_sim/test/gtest_sensor.cpp b/core_sim/test/gtest_sensor.cpp index c14ba8f2..8596a1c2 100644 --- a/core_sim/test/gtest_sensor.cpp +++ b/core_sim/test/gtest_sensor.cpp @@ -24,7 +24,7 @@ class Scene { // : public ::testing::Test { Logger logger(logger_callback); Transform origin = {{0, 0, 0}, {1, 0, 0, 0}}; return Robot("TestRobotID", origin, logger, TopicManager(logger), "", - ServiceManager(logger), StateManager(logger)); + ServiceManager(logger), StateManager(logger),""); } static void LoadRobot(Robot& robot, ConfigJson config_json) { diff --git a/core_sim/test/gtest_simulator.cpp b/core_sim/test/gtest_simulator.cpp index 599fa819..dabc5ad2 100644 --- a/core_sim/test/gtest_simulator.cpp +++ b/core_sim/test/gtest_simulator.cpp @@ -101,4 +101,4 @@ TEST(Simulator, GetID) { sim_config = "{ \"id\": \"abc\" "; EXPECT_THROW(simulator.LoadSimulator(sim_config), std::exception); EXPECT_TRUE(simulator.GetID().empty()); -} +} \ No newline at end of file diff --git a/physics/c3104.xml b/physics/c3104.xml new file mode 100644 index 00000000..7769530e --- /dev/null +++ b/physics/c3104.xml @@ -0,0 +1,123 @@ + + + + + For testing autopilot capability + + + + + + + Start engine and set initial heading and waypoints, turn on heading-hold mode. + + simulation/sim-time-sec ge 0.25 + + + + + + + + + + + + + + + + + + + + + + + + + + position/h-agl-ft ge 40 + + + + + + Start takeoff event + + + airsim/takeoff eq 1.0 + airsim/land eq 0.0 + + + + + + + + + + + + + + + airsim/takeoff eq 1.0 + airsim/land eq 0.0 + velocities/vg-fps ge 150 + + + + + + + airsim/takeoff eq 1.0 + velocities/v-down-fps lt -50 + position/h-sl-ft gt 36.57 + + + + + + + + + position/h-sl-ft gt 300 + attitude/theta-deg lt 10 + + + + + + + + Start landing event + + + airsim/land eq 1.0 + airsim/takeoff eq 0.0 + + + + + + + + + + + + + + airsim/land eq 1.0 + position/h-sl-ft lt 45.0 + + + + + + + + diff --git a/physics/include/jsbsim_physics.hpp b/physics/include/jsbsim_physics.hpp new file mode 100644 index 00000000..85ff7e2f --- /dev/null +++ b/physics/include/jsbsim_physics.hpp @@ -0,0 +1,105 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#ifndef PHYSICS_INCLUDE_JSBSIM_PHYSICS_HPP_ +#define PHYSICS_INCLUDE_JSBSIM_PHYSICS_HPP_ + +#include + +#include "base_physics.hpp" +#include "core_sim/actor/robot.hpp" +#include "core_sim/environment.hpp" +#include "core_sim/math_utils.hpp" +#include "core_sim/physics_common_types.hpp" + + + +namespace microsoft { +namespace projectairsim { + +// ----------------------------------------------------------------------------- +// class JSBSimPhysicsBody + +class JSBSimPhysicsBody : public BasePhysicsBody { + public: + JSBSimPhysicsBody() {} + explicit JSBSimPhysicsBody(const Robot& robot); + ~JSBSimPhysicsBody() override {} + + void InitializeJSBSimPhysicsBody(const Robot& robot); + + // Aggregate all externally applied wrenches on body CG into wrench_ + void CalculateExternalWrench() override; + + void ReadRobotData(); + void WriteRobotData(const Kinematics& kinematics, + TimeNano external_time_stamp = -1); + + bool IsStillGrounded(); + bool IsLandingCollision(); + bool NeedsCollisionResponse(const Kinematics& next_kin); + + protected: + friend class JSBSimPhysicsModel; + + Robot sim_robot_; + + bool is_grounded_; + Vector3 env_gravity_; + float env_air_density_; + Vector3 env_wind_velocity_; + std::vector drag_faces_; + std::vector> lift_drag_links_; + std::unordered_map lift_drag_control_angles_; + std::vector external_wrench_points_; + + CollisionInfo collision_info_; + + static constexpr float kGroundCollisionAxisTol = 0.01f; + static constexpr float kCollisionOffset = 0.001f; // land with 1 mm air gap + + private: + void InitializeJSBSim(); + Kinematics GetKinematicsFromModel(); + Vector3 GetModelAngularAcceleration(); + Vector3 GetModelLinearAcceleration(); + Vector3 GetModelAngularVelocity(); + Vector3 GetModelLinearVelocity(); + Quaternion GetModelOrientation(); + Vector3 GetModelPosition(); + Vector3 GetModelCoordinates(); + void SetJSBSimAltitudeGrounded(float delta_altitude); + + Vector3d home_geopoint_ned_; + Vector3 home_geopoint_; + std::shared_ptr model_; +}; + +// ----------------------------------------------------------------------------- +// class JSBSimPhysicsModel + +class JSBSimPhysicsModel { + public: + JSBSimPhysicsModel() {} + ~JSBSimPhysicsModel() {} + + void SetWrenchesOnPhysicsBody(std::shared_ptr body); + + void StepPhysicsBody(TimeNano dt_nanos, + std::shared_ptr body); + + Kinematics CalcNextKinematicsNoCollision( + TimeSec dt_sec, std::shared_ptr& fp_body); + + Kinematics CalcNextKinematicsWithCollision( + TimeSec dt_sec, std::shared_ptr& fp_body); + + protected: + static constexpr float kDragMinVelocity = 0.1f; + private: + TimeSec residual_time_; +}; + +} // namespace projectairsim +} // namespace microsoft + +#endif // PHYSICS_INCLUDE_JSBSIM_PHYSICS_HPP_ \ No newline at end of file diff --git a/physics/include/physics_world.hpp b/physics/include/physics_world.hpp index 6560b9f8..476d6238 100644 --- a/physics/include/physics_world.hpp +++ b/physics/include/physics_world.hpp @@ -13,6 +13,7 @@ #include "fast_physics.hpp" #include "matlab_physics.hpp" #include "unreal_physics.hpp" +#include "jsbsim_physics.hpp" namespace microsoft { namespace projectairsim { @@ -22,7 +23,7 @@ namespace projectairsim { // is decided that could work for all physics types, this could be changed to // use standard base-class inheritance instead of variants. using ModelVariant = - std::variant; + std::variant; class PhysicsWorld { public: diff --git a/physics/src/CMakeLists.txt b/physics/src/CMakeLists.txt index a09dd61f..54044e4e 100644 --- a/physics/src/CMakeLists.txt +++ b/physics/src/CMakeLists.txt @@ -21,6 +21,7 @@ add_library( unreal_physics.cpp physics_world.cpp matlab_physics.cpp + jsbsim_physics.cpp ) set_target_properties(${TARGET_NAME} PROPERTIES @@ -29,7 +30,10 @@ set_target_properties(${TARGET_NAME} PROPERTIES COMPILE_PDB_OUTPUT_DIR ${CMAKE_BINARY_DIR} ) -add_dependencies(${TARGET_NAME} core_sim nng-external) +add_dependencies(${TARGET_NAME} core_sim jsbsim-repo nng-external) + +# Set up JSBSim for linking +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_STATIC_LIB_DIR}) # Set up nng for linking target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR}) @@ -46,6 +50,7 @@ target_include_directories( ${JSON_INCLUDE_DIR} ${NNG_INCLUDE_DIR} ${MSGPACK_INCLUDE_DIR} + ${JSBSIM_INCLUDE_DIR} ) if(WIN32) @@ -58,6 +63,7 @@ if(WIN32) mswsock # req by nng on Win advapi32 # req by nng on Win lvmon + jsbsim ) else() target_link_libraries( @@ -65,6 +71,7 @@ else() PRIVATE core_sim nng + jsbsim ) endif() diff --git a/physics/src/jsbsim_physics.cpp b/physics/src/jsbsim_physics.cpp new file mode 100644 index 00000000..7a8c8b18 --- /dev/null +++ b/physics/src/jsbsim_physics.cpp @@ -0,0 +1,458 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#include "jsbsim_physics.hpp" + +#include +#include + +#include "core_sim/actuators/actuator.hpp" +#include "core_sim/actuators/lift_drag_control_surface.hpp" +#include "core_sim/actuators/rotor.hpp" +#include "core_sim/earth_utils.hpp" +#include "core_sim/math_utils.hpp" +#include "core_sim/physics_common_types.hpp" +#include "core_sim/physics_common_utils.hpp" +#include "core_sim/transforms/transform_utils.hpp" +#include "core_sim/geodetic_converter.hpp" +#include "initialization/FGInitialCondition.h" +#include "initialization/FGTrim.h" +#include "FGFDMExec.h" // JSBSim + + + +namespace microsoft { +namespace projectairsim { + +// ----------------------------------------------------------------------------- +// class JSBSimPhysicsBody + +JSBSimPhysicsBody::JSBSimPhysicsBody(const Robot& robot) + : sim_robot_(robot) { + SetName(robot.GetID()); + SetPhysicsType(PhysicsType::kJSBSimPhysics); + InitializeJSBSimPhysicsBody(robot); +} + +void JSBSimPhysicsBody::InitializeJSBSimPhysicsBody( + const Robot& robot) { + const auto& links = sim_robot_.GetLinks(); + const auto& root_link = links.at(0); // TODO allow config to choose root link + + //external_wrench_points_.clear(); + SetMass(root_link.GetInertial().GetMass()); // TODO get mass from JSBSim + friction_ = root_link.GetCollision().GetFriction(); + restitution_ = root_link.GetCollision().GetRestitution(); + is_grounded_ = sim_robot_.GetStartLanded(); + + model_ = sim_robot_.GetJSBSimModel(); + model_->SetPropertyValue("simulation/dt", 0.008333333333); //TODO: use configuration instead of harcoded value + + InitializeJSBSim(); + + // get difference betweeen JSBSim's initial geopoint position and home geopoint position + const auto& world_geopoint = sim_robot_.GetEnvironment().home_geo_point.geo_point; + home_geopoint_ = GetModelCoordinates(); + //Get distance between home geopoint and JSBSim's initial geopoint + GeodeticConverter converter(world_geopoint.latitude, world_geopoint.longitude, + world_geopoint.altitude); + converter.geodetic2Ned(home_geopoint_[0], home_geopoint_[1], home_geopoint_[2], &home_geopoint_ned_[0], &home_geopoint_ned_[1], &home_geopoint_ned_[2]); + // unreal terrain is flat. Use initial altitude + home_geopoint_ned_[2] = -(home_geopoint_[2]-world_geopoint.altitude); +} + +void JSBSimPhysicsBody::InitializeJSBSim(){ + //updates jsbsim initial conditions with current state + auto& init = *model_->GetIC(); + //SGPath init_file("reset00.xml"); + //init.Load(init_file); + init.SetLatitudeDegIC(sim_robot_.GetEnvironment().env_info.geo_point.latitude); + init.SetLongitudeDegIC(sim_robot_.GetEnvironment().env_info.geo_point.longitude); + auto alt_ft = + sim_robot_.GetEnvironment().env_info.geo_point.altitude * MathUtils::meters_to_feets; // TODO: method to convert from m/s to fps + init.SetAltitudeASLFtIC(alt_ft); + // Set terrain elevation at initial position + init.SetTerrainElevationFtIC(alt_ft); + auto orientation = TransformUtils::ToRPY(sim_robot_.GetKinematics().pose.orientation); + init.SetPsiDegIC(MathUtils::rad2Deg(orientation.z())); + init.SetThetaDegIC(MathUtils::rad2Deg(orientation.y())); + init.SetPhiDegIC(MathUtils::rad2Deg(orientation.x())); + auto velocity = sim_robot_.GetKinematics().twist.linear; + init.SetVNorthFpsIC(velocity.x() * MathUtils::meters_to_feets); // TODO: method to convert from m/s to fps + init.SetVEastFpsIC(velocity.y() * MathUtils::meters_to_feets); + init.SetVDownFpsIC(velocity.z() * MathUtils::meters_to_feets); + auto angular_velocity = sim_robot_.GetKinematics().twist.angular; + init.SetPRadpsIC(angular_velocity.x()); + init.SetQRadpsIC(angular_velocity.y()); + init.SetRRadpsIC(angular_velocity.z()); + auto wind_velocity = sim_robot_.GetEnvironment().wind_velocity; + init.SetWindNEDFpsIC(wind_velocity.x() * MathUtils::meters_to_feets, wind_velocity.y() * MathUtils::meters_to_feets, + wind_velocity.z() * MathUtils::meters_to_feets); + + // run initial conditions + if(model_->RunIC() == false){ + throw std::runtime_error("JSBSim failed to initialize"); + } + + /*JSBSim::FGTrim trim_(model_.get(), JSBSim::tGround); + if (!trim_.DoTrim()) + { + trim_.Report(); + trim_.TrimStats(); + + throw std::runtime_error("TRIM FAILED"); + }*/ +} + +void JSBSimPhysicsBody::SetJSBSimAltitudeGrounded(float delta_altitude){ + //model_->SetPropertyValue("position/h-sl-meters", -position[2]); + auto terrain_elevation_val = model_->GetPropertyValue("position/terrain-elevation-asl-ft") + delta_altitude * MathUtils::meters_to_feets; + model_->SetPropertyValue("position/terrain-elevation-asl-ft", terrain_elevation_val); + auto altitude = model_->GetPropertyValue("position/h-sl-meters"); + auto new_altitude = altitude + delta_altitude * MathUtils::meters_to_feets; + model_->SetPropertyValue("position/h-sl-meters", new_altitude); +} + +// TODO Rename this to "SetActuationOutputs"? +void JSBSimPhysicsBody::CalculateExternalWrench() { + // No need to do anything here, all wrenches are calculated in the step by JSBSim +} + +void JSBSimPhysicsBody::ReadRobotData() { + kinematics_ = sim_robot_.GetKinematics(); + collision_info_ = sim_robot_.GetCollisionInfo(); + const auto& cur_env = sim_robot_.GetEnvironment(); + const auto& cur_env_info = cur_env.env_info; + env_gravity_ = cur_env_info.gravity; + env_air_density_ = cur_env_info.air_density; + env_wind_velocity_ = cur_env.wind_velocity; +} + +void JSBSimPhysicsBody::WriteRobotData(const Kinematics& kinematics, + TimeNano external_time_stamp) { + sim_robot_.UpdateKinematics(kinematics, external_time_stamp); +} + +bool JSBSimPhysicsBody::IsStillGrounded() { + if (is_grounded_ && + kinematics_.twist.linear.z() >= 0) { + is_grounded_ = true; + } else { + is_grounded_ = false; + } + + return is_grounded_; +} + +bool JSBSimPhysicsBody::IsLandingCollision() { + if (collision_info_.has_collided == false) { + return false; + } + + // Check if collision normal is primarily vertically upward (assumes NED) + const Vector3 normal_body = PhysicsUtils::TransformVectorToBodyFrame( + collision_info_.normal, kinematics_.pose.orientation); + + const bool is_ground_normal = MathUtils::IsApproximatelyEqual( + normal_body.z(), -1.0f, kGroundCollisionAxisTol); + + if (is_ground_normal) { + is_grounded_ = true; + return true; + } else { + return is_grounded_; + } +} + +bool JSBSimPhysicsBody::NeedsCollisionResponse(const Kinematics& next_kin) { + const bool is_moving_into_collision = + (collision_info_.normal.dot(next_kin.twist.linear) < 0.0f); + + if (collision_info_.has_collided && is_moving_into_collision) { + return true; + } else { + return false; + } +} + +// ----------------------------------------------------------------------------- +// class JSBSimPhysicsModel + +void JSBSimPhysicsModel::SetWrenchesOnPhysicsBody( + std::shared_ptr body) { +} + +void JSBSimPhysicsModel::StepPhysicsBody(TimeNano dt_nanos, + std::shared_ptr body) { + // Dynamic cast to a JSBSimPhysicsBody + std::shared_ptr fp_body = + std::dynamic_pointer_cast(body); + + if (fp_body != nullptr) { + auto dt_sec = SimClock::Get()->NanosToSec(dt_nanos); + Kinematics next_kin; + // To simulate collision response start uncommenting next lines and delete the two previous ones + + // Step 1 - Update physics body's data from robot's latest data + fp_body->ReadRobotData(); + + + + //Step 2 - Calculate kinematics with collision response if needed + if (fp_body->NeedsCollisionResponse(fp_body->kinematics_)) { + if (fp_body->IsLandingCollision()) { + // Calculate the landed position to stick at by offsetting back + // along the collision normal with an additional kCollisionOffset margin + // to prevent getting stuck in a collided state. + // Calculate the landed position to stick at by offsetting back + // along the collision normal with an additional kCollisionOffset margin + // to prevent getting stuck in a collided state. + const Vector3 delta_position = + fp_body->collision_info_.normal * + (fp_body->collision_info_.penetration_depth + + JSBSimPhysicsBody::kCollisionOffset); + auto landed_position = fp_body->collision_info_.position + delta_position; + //next_kin = CalcNextKinematicsGrounded( + // landed_position, fp_body->kinematics_.pose.orientation); + // TO-DO: set ground forces to model + next_kin = CalcNextKinematicsNoCollision(dt_sec, fp_body); + // set jsbsim to new position + fp_body->SetJSBSimAltitudeGrounded(-delta_position[2]); + next_kin.pose.position = landed_position; + fp_body->WriteRobotData(next_kin); + } else { + next_kin = CalcNextKinematicsWithCollision(dt_sec, fp_body); + fp_body->WriteRobotData(next_kin); + // Update environment to get updated geopoint + fp_body->sim_robot_.UpdateEnvironment(); + fp_body->InitializeJSBSim(); + } + } else { + next_kin = CalcNextKinematicsNoCollision(dt_sec, fp_body); + fp_body->WriteRobotData(next_kin); + } + } +} + +//Kinematics JSBSimPhysicsModel::CalcNextKinematicsGrounded( +// const Vector3& position, const Quaternion& orientation) { +// Kinematics kin_grounded; +// // Zero out accels/velocities +// kin_grounded.accels.linear = Vector3::Zero(); +// kin_grounded.accels.angular = Vector3::Zero(); +// kin_grounded.twist.linear = Vector3::Zero(); +// kin_grounded.twist.angular = Vector3::Zero(); +// +// // Pass-through position +// kin_grounded.pose.position = position; +// +// // Zero out roll/pitch, pass-through yaw +// auto rpy = TransformUtils::ToRPY(orientation); +// kin_grounded.pose.orientation = TransformUtils::ToQuaternion(0., 0., rpy[2]); +// +// return kin_grounded; +//} + + +Vector3 JSBSimPhysicsBody::GetModelCoordinates(){ //TODO: use GeoPoint instead of Vector3 +//Get JSBSim geopoint + auto lat_deg = model_->GetPropertyValue("position/lat-gc-deg"); + auto lon_deg = model_->GetPropertyValue("position/long-gc-deg"); + auto alt_mt = model_->GetPropertyValue("position/h-sl-meters"); + return Vector3(lat_deg, lon_deg, alt_mt); +} + +Vector3 JSBSimPhysicsBody::GetModelPosition() { + //get latitude displacement + auto delta_lat = model_->GetPropertyValue("position/lat-gc-deg") - + home_geopoint_[0]; + //get latitude in meters + double n = std::copysign( + model_->GetPropertyValue("position/distance-from-start-lat-mt"), + delta_lat); + // get longitud displacement + auto delta_lon = model_->GetPropertyValue("position/long-gc-deg") - + home_geopoint_[1]; + //get longitud in meters + double e = std::copysign( + model_->GetPropertyValue("position/distance-from-start-lon-mt"), + delta_lon); + double d = + -(model_->GetPropertyValue("position/h-sl-meters") - home_geopoint_[2]); + return Vector3(n, e, d); +} + +Quaternion JSBSimPhysicsBody::GetModelOrientation() { + double roll = model_->GetPropertyValue("attitude/roll-rad"); + double pitch = model_->GetPropertyValue("attitude/pitch-rad"); + double yaw = MathUtils::deg2Rad(model_->GetPropertyValue("attitude/psi-deg")); + return TransformUtils::ToQuaternion(roll, pitch, yaw); +} + +Vector3 JSBSimPhysicsBody::GetModelLinearVelocity() { + double n = model_->GetPropertyValue("velocities/v-north-fps") * MathUtils::feets_to_meters; //TODO: replace for a FeetsToMeters method + double e = model_->GetPropertyValue("velocities/v-east-fps") * MathUtils::feets_to_meters; + double d = model_->GetPropertyValue("velocities/v-down-fps") * MathUtils::feets_to_meters; + return Vector3(n, e, d); +} + +//Body Frame +Vector3 JSBSimPhysicsBody::GetModelAngularVelocity() { + double p = model_->GetPropertyValue("velocities/p-rad_sec"); + double q = model_->GetPropertyValue("velocities/q-rad_sec"); + double r = model_->GetPropertyValue("velocities/r-rad_sec"); + return Vector3(p, q, r); +} + +Vector3 JSBSimPhysicsBody::GetModelLinearAcceleration() { + double ui = model_->GetPropertyValue("accelerations/uidot-ft_sec2") * MathUtils::feets_to_meters; + double vi = model_->GetPropertyValue("accelerations/vidot-ft_sec2") * MathUtils::feets_to_meters; + double wi = model_->GetPropertyValue("accelerations/widot-ft_sec2") * MathUtils::feets_to_meters; + return Vector3(ui, vi, wi); +} + +//Body Frame +Vector3 JSBSimPhysicsBody::GetModelAngularAcceleration() { + double pdot = model_->GetPropertyValue("accelerations/pdot-rad_sec2"); + double qdot = model_->GetPropertyValue("accelerations/qdot-rad_sec2"); + double rdot = model_->GetPropertyValue("accelerations/rdot-rad_sec2"); + return Vector3(pdot, qdot, rdot); +} + +Kinematics JSBSimPhysicsBody::GetKinematicsFromModel() { + Kinematics kinematics; + kinematics.pose.position = GetModelPosition(); + //add initial x,y position to the model's relative position + kinematics.pose.position[0] += home_geopoint_ned_[0]; + kinematics.pose.position[1] += home_geopoint_ned_[1]; + kinematics.pose.position[2] += home_geopoint_ned_[2]; + kinematics.pose.orientation = GetModelOrientation(); + kinematics.twist.linear = GetModelLinearVelocity(); + kinematics.twist.angular = GetModelAngularVelocity(); + kinematics.accels.linear = GetModelLinearAcceleration(); + kinematics.accels.angular = GetModelAngularAcceleration(); + return kinematics; +} + +Kinematics JSBSimPhysicsModel::CalcNextKinematicsNoCollision( + TimeSec dt_sec, std::shared_ptr& fp_body) { + + + residual_time_ += dt_sec; + // get jsbsim dt + double jsbsim_dt = fp_body->model_->GetPropertyValue("simulation/dt"); + + // Run JSBSim for the residual time + while (residual_time_ >= jsbsim_dt) { + // Run JSBSim for one time step + fp_body->model_->Run(); + residual_time_ -= jsbsim_dt; + } + + return fp_body->GetKinematicsFromModel(); +} + +// TO-DO: this is a copy-paste from fast physics. Extract as a common collision reaction +Kinematics JSBSimPhysicsModel::CalcNextKinematicsWithCollision( + TimeSec dt_sec, std::shared_ptr& fp_body) { + const auto& collision_info = fp_body->collision_info_; + const auto& cur_kin = fp_body->kinematics_; + const auto& restitution = fp_body->restitution_; + const auto& friction = fp_body->friction_; + const auto& mass_inv = fp_body->mass_inv_; + const auto& inertia_inv = fp_body->inertia_inv_; + + Kinematics kin_with_collision; // main output of this function + + const Vector3 normal_body = PhysicsUtils::TransformVectorToBodyFrame( + collision_info.normal, cur_kin.pose.orientation); + + const Vector3 ave_vel_lin = + cur_kin.twist.linear + cur_kin.accels.linear * dt_sec; + + const Vector3 ave_vel_ang = + cur_kin.twist.angular + cur_kin.accels.angular * dt_sec; + + // Step 1 - Calculate restitution impulse (normal to impact) + + // Velocity at contact point + const Vector3 ave_vel_lin_body = PhysicsUtils::TransformVectorToBodyFrame( + ave_vel_lin, cur_kin.pose.orientation); + + // Contact point vector + const Vector3 r_contact = + collision_info.impact_point - collision_info.position; + + const Vector3 contact_vel_body = + ave_vel_lin_body + ave_vel_ang.cross(r_contact); + + // GafferOnGames - Collision response with columb friction + // http://gafferongames.com/virtual-go/collision-response-and-coulomb-friction/ + // Assuming collision is with static fixed body, + // impulse magnitude = j = -(1 + R)V.N / (1/m + (I'(r X N) X r).N) + // Physics Part 3, Collision Response, Chris Hecker, eq 4(a) + // http://chrishecker.com/images/e/e7/Gdmphys3.pdf + // V(t+1) = V(t) + j*N / m + // TODO(edufford) This formula doesn't produce realistic bounces, improve it + const float restitution_impulse_denom = + mass_inv + (inertia_inv * r_contact.cross(normal_body)) + .cross(r_contact) + .dot(normal_body); + + const float restitution_impulse = -contact_vel_body.dot(normal_body) * + (1.0f + restitution) / + restitution_impulse_denom; + + // Step 2 - Add restitution impulse to next twist velocities + + kin_with_collision.twist.linear = + ave_vel_lin + collision_info.normal * restitution_impulse * mass_inv; + + // TODO(edufford) Shouldn't this formula should account for inertia? + kin_with_collision.twist.angular = + ave_vel_ang + r_contact.cross(normal_body) * restitution_impulse; + + // Step 3 - Calculate friction impulse (tangent to contact) + + const Vector3 contact_tang_body = + contact_vel_body - normal_body * normal_body.dot(contact_vel_body); + + const Vector3 contact_tang_unit_body = contact_tang_body.normalized(); + + const float friction_mag_denom = + mass_inv + (inertia_inv * r_contact.cross(contact_tang_unit_body)) + .cross(r_contact) + .dot(contact_tang_unit_body); + + const float friction_mag = + -contact_tang_body.norm() * friction / friction_mag_denom; + + // Step 4 - Add friction impulse to next twist velocities + + const Vector3 contact_tang_unit = PhysicsUtils::TransformVectorToWorldFrame( + contact_tang_unit_body, cur_kin.pose.orientation); + + kin_with_collision.twist.linear += contact_tang_unit * friction_mag; + + kin_with_collision.twist.angular += + r_contact.cross(contact_tang_unit_body) * friction_mag * mass_inv; + + // Step 5 - Zero out next accels during collision response to prevent + // cancelling out impulse response + + kin_with_collision.accels.linear = Vector3::Zero(); + kin_with_collision.accels.angular = Vector3::Zero(); + + // Step 6 - Calculate next position/orientation from collision position + + kin_with_collision.pose.position = + collision_info.position + + (collision_info.normal * collision_info.penetration_depth) + + (kin_with_collision.twist.linear * dt_sec); + + kin_with_collision.pose.orientation = cur_kin.pose.orientation; + + return kin_with_collision; +} + +} // namespace projectairsim +} // namespace microsoft \ No newline at end of file diff --git a/physics/src/physics_world.cpp b/physics/src/physics_world.cpp index a2e7f5a4..59347e06 100644 --- a/physics/src/physics_world.cpp +++ b/physics/src/physics_world.cpp @@ -7,6 +7,7 @@ #include "core_sim/scene.hpp" #include "fast_physics.hpp" #include "unreal_physics.hpp" +#include "jsbsim_physics.hpp" namespace microsoft { namespace projectairsim { @@ -53,6 +54,19 @@ void PhysicsWorld::AddRobot(const Robot& robot) { physics_models_.emplace(PhysicsType::kUnrealPhysics, std::in_place_type); } + } else if (robot.GetPhysicsType() == PhysicsType::kJSBSimPhysics) { + // Construct physics body passing params from robot JSON physics params + auto jsb_physics_body = + std::make_shared(robot); + + // Add physics body pointer to physics world + AddPhysicsBody(jsb_physics_body); + + // Add FastPhysics model to the world's models if not already added + if (physics_models_.count(PhysicsType::kJSBSimPhysics) == 0) { + physics_models_.emplace(PhysicsType::kJSBSimPhysics, + std::in_place_type); + } } } @@ -107,6 +121,9 @@ void PhysicsWorld::Start() { case PhysicsType::kUnrealPhysics: { break; } + case PhysicsType::kJSBSimPhysics: { + break; + } default: { break; } @@ -135,6 +152,12 @@ void PhysicsWorld::SetWrenchesOnPhysicsBodies() { up_model.SetWrenchesOnPhysicsBody(body); break; } + case PhysicsType::kJSBSimPhysics: { + auto& jsb_model = std::get( + physics_models_.at(PhysicsType::kJSBSimPhysics)); + jsb_model.SetWrenchesOnPhysicsBody(body); + break; + } default: { break; } @@ -161,6 +184,12 @@ void PhysicsWorld::StepPhysicsWorld(TimeNano dt_nanos) { // No-op, Unreal steps physics body directly break; } + case PhysicsType::kJSBSimPhysics: { + auto& jsb_model = std::get( + physics_models_.at(PhysicsType::kJSBSimPhysics)); + jsb_model.StepPhysicsBody(dt_nanos, body); + break; + } default: { break; } diff --git a/physics/test/CMakeLists.txt b/physics/test/CMakeLists.txt index 2bc6a95f..aa3d11da 100644 --- a/physics/test/CMakeLists.txt +++ b/physics/test/CMakeLists.txt @@ -30,9 +30,9 @@ set_target_properties( # Add dependency to core_sim_gtests since it copies the runtime DLLs into ${SIMLIBS_TEST_DIR} if(WIN32) - add_dependencies(${TARGET_NAME} physics core_sim core_sim_gtests nng-external gtest_main) + add_dependencies(${TARGET_NAME} physics core_sim core_sim_gtests nng-external jsbsim-repo gtest_main) else() - add_dependencies(${TARGET_NAME} physics core_sim core_sim_gtests nng-external openssl-external gtest_main) + add_dependencies(${TARGET_NAME} physics core_sim core_sim_gtests nng-external jsbsim-repo openssl-external gtest_main) endif() # Set up nng and openssl for linking @@ -40,6 +40,9 @@ target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR} ${OPENSSL_LIB_DIR} target_compile_definitions(${TARGET_NAME} PRIVATE NNG_STATIC_LIB=ON OPENSSL_STATIC_LIB=ON) target_link_directories(${TARGET_NAME} PRIVATE ${ONNX_LIB_DIR}) +# Set up JSBSim for linking +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_LIB_DIR}) + target_include_directories( ${TARGET_NAME} PRIVATE @@ -51,6 +54,7 @@ target_include_directories( ${NNG_INCLUDE_DIR} ${MSGPACK_INCLUDE_DIR} ${OPENSSL_INCLUDE_DIR} + ${JSBSIM_INCLUDE_DIR} ) if(WIN32) @@ -61,6 +65,7 @@ target_link_libraries( physics nng core_sim + jsbsim lvmon ) else() @@ -72,6 +77,7 @@ target_link_libraries( nng crypto # Client authorization core_sim + jsbsim lvmon ) endif() @@ -80,6 +86,7 @@ add_custom_command(TARGET ${TARGET_NAME} POST_BUILD COMMAND ${CMAKE_COMMAND} -E echo "Copying [${TARGET_NAME}] test data to ${SIMLIBS_TEST_DIR}/test_data" COMMAND ${CMAKE_COMMAND} -E copy_directory ${TARGET_SOURCE_DIR}/test/test_data ${SIMLIBS_TEST_DIR}/test_data + COMMAND ${CMAKE_COMMAND} -E copy_directory ${JSBSIM_CORESIM_DIR}/lib/Debug/ ${SIMLIBS_TEST_DIR} ) # Include CMake's GoogleTest module to use gtest_discover_tests() helper diff --git a/samples/standalone_sim/CMakeLists.txt b/samples/standalone_sim/CMakeLists.txt index d3673bc8..6a8fa868 100644 --- a/samples/standalone_sim/CMakeLists.txt +++ b/samples/standalone_sim/CMakeLists.txt @@ -25,6 +25,7 @@ if(WIN32) simserver core_sim nng-external + jsbsim-repo physics multirotor_api rendering_scene @@ -35,6 +36,7 @@ else() simserver core_sim nng-external + jsbsim-repo openssl-external physics multirotor_api @@ -46,6 +48,10 @@ endif() target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR} ${OPENSSL_LIB_DIR}) target_compile_definitions(${TARGET_NAME} PRIVATE NNG_STATIC_LIB=ON OPENSSL_STATIC_LIB=ON) target_link_directories(${TARGET_NAME} PRIVATE ${ONNX_LIB_DIR}) + +# Set up JSBSim for linking +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_STATIC_LIB_DIR}) + target_include_directories( ${TARGET_NAME} PUBLIC @@ -67,6 +73,7 @@ if(WIN32) simserver core_sim physics + jsbsim mavlinkcom multirotor_api rendering_scene @@ -83,6 +90,7 @@ else() simserver core_sim physics + jsbsim mavlinkcom multirotor_api rendering_scene diff --git a/simserver/include/simserver.hpp b/simserver/include/simserver.hpp index bcc93094..b81eee01 100644 --- a/simserver/include/simserver.hpp +++ b/simserver/include/simserver.hpp @@ -25,7 +25,7 @@ class TileManager; class SimServer { public: - SimServer(Simulator::LoggerCallback logger_callback, LogLevel level); + SimServer(Simulator::LoggerCallback logger_callback, LogLevel level, const std::string& working_simulation_path=""); ~SimServer(); diff --git a/simserver/src/CMakeLists.txt b/simserver/src/CMakeLists.txt index 9a81d7b4..0a22ab48 100644 --- a/simserver/src/CMakeLists.txt +++ b/simserver/src/CMakeLists.txt @@ -34,12 +34,14 @@ add_dependencies( multirotor_api rover_api rendering_scene + jsbsim-repo ) # Set up nng for linking target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR}) target_compile_definitions(${TARGET_NAME} PRIVATE NNG_STATIC_LIB=ON) target_link_directories(${TARGET_NAME} PRIVATE ${ONNX_LIB_DIR}) +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_STATIC_LIB_DIR}) target_include_directories( ${TARGET_NAME} @@ -52,6 +54,7 @@ target_include_directories( ${JSON_INCLUDE_DIR} ${NNG_INCLUDE_DIR} ${MSGPACK_INCLUDE_DIR} + ${JSBSIM_INCLUDE_DIR} ) if(WIN32) @@ -69,6 +72,7 @@ if(WIN32) mswsock # req by nng on Win advapi32 # req by nng on Win lvmon + jsbsim ) else() target_link_libraries( @@ -81,6 +85,7 @@ else() rover_api rendering_scene nng + jsbsim ) endif() diff --git a/simserver/src/simserver.cpp b/simserver/src/simserver.cpp index 6058c8f3..ce0cff14 100644 --- a/simserver/src/simserver.cpp +++ b/simserver/src/simserver.cpp @@ -10,6 +10,7 @@ #include "core_sim/simulator.hpp" #include "manual_controller_api.hpp" #include "matlab_controller_api.hpp" +#include "jsbsim_api.hpp" #include "mavlink_api.hpp" #include "physics_world.hpp" #include "tile_manager.hpp" @@ -17,8 +18,8 @@ namespace microsoft { namespace projectairsim { -SimServer::SimServer(Simulator::LoggerCallback logger_callback, LogLevel level) - : simulator_(std::make_shared(logger_callback, level)), +SimServer::SimServer(Simulator::LoggerCallback logger_callback, LogLevel level, const std::string& working_simulation_path) + : simulator_(std::make_shared(logger_callback, level, working_simulation_path)), physics_world_(std::make_shared()), tile_manager_(nullptr), load_external_scene_callback_(nullptr), @@ -290,6 +291,10 @@ void SimServer::LoadControllers(Scene& scene) { auto simple_flight_api = new SimpleFlightApi(sim_robot, ptransformtree); sim_robot.SetController( std::unique_ptr(simple_flight_api)); + } else if (controller_type == "jsbsim-api") { + auto jsbsim_api = new JSBSimApi(sim_robot, ptransformtree); + sim_robot.SetController( + std::unique_ptr(jsbsim_api)); } else if (controller_type == "px4-api") { auto px4_api = new MavLinkApi(sim_robot, ptransformtree); sim_robot.SetController(std::unique_ptr(px4_api)); diff --git a/unity/sim_unity_wrapper/src/CMakeLists.txt b/unity/sim_unity_wrapper/src/CMakeLists.txt index 06fb6641..4078ffda 100644 --- a/unity/sim_unity_wrapper/src/CMakeLists.txt +++ b/unity/sim_unity_wrapper/src/CMakeLists.txt @@ -39,6 +39,8 @@ target_link_directories(${TARGET_NAME} PRIVATE ${NNG_LIB_DIR} ${OPENSSL_LIB_DIR} target_compile_definitions(${TARGET_NAME} PRIVATE NNG_STATIC_LIB=ON) target_link_directories(${TARGET_NAME} PRIVATE ${ONNX_LIB_DIR}) +# Set up JSBSim for linking +target_link_directories(${TARGET_NAME} PRIVATE ${JSBSIM_PHYSICS_DIR}/lib) target_include_directories( ${TARGET_NAME} diff --git a/unreal/Blocks/Blocks.uproject b/unreal/Blocks/Blocks.uproject index ba3e1b24..e3d70f56 100644 --- a/unreal/Blocks/Blocks.uproject +++ b/unreal/Blocks/Blocks.uproject @@ -1,6 +1,6 @@ { "FileVersion": 3, - "EngineAssociation": "5.1", + "EngineAssociation": "5.2", "Category": "", "Description": "", "Modules": [ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealSimLoader.cpp b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealSimLoader.cpp index 0ad674da..3c13ff12 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealSimLoader.cpp +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealSimLoader.cpp @@ -24,9 +24,14 @@ namespace projectairsim = microsoft::projectairsim; DEFINE_LOG_CATEGORY(SimPlugin); -AUnrealSimLoader::AUnrealSimLoader(const FString& SimLogDir) - : SimServer(std::make_shared( - UnrealLogger::LogSim, projectairsim::LogLevel::kVerbose)) { +AUnrealSimLoader::AUnrealSimLoader(const FString& SimLogDir) { + + auto PluginDir = IPluginManager::Get().FindPlugin(Constant::SimPluginName)->GetBaseDir(); + + SimServer = std::make_shared( + UnrealLogger::LogSim, projectairsim::LogLevel::kVerbose, + TCHAR_TO_UTF8(*PluginDir)); + // Open log file output stream to start capturing clog FString SimLogPath = FPaths::Combine(SimLogDir, TEXT("projectairsim_server.log")); diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs index 4db4fca4..c5225634 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs @@ -8,6 +8,7 @@ public class ProjectAirSim : ModuleRules { public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) { + bEnableUndefinedIdentifierWarnings = false; CppStandard = CppStandardVersion.Cpp17; PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; PrivatePCHHeaderFile = "Public/ProjectAirSim.h"; @@ -48,6 +49,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/core_sim/include", PluginDirectory + "/SimLibs/simserver/include", PluginDirectory + "/SimLibs/physics/include", + PluginDirectory + "/SimLibs/core_sim/jsbsim/include", PluginDirectory + "/SimLibs/rendering_scene/include", PluginDirectory + "/SimLibs/multirotor_api/include", PluginDirectory + "/SimLibs/rover_api/include", @@ -72,6 +74,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/core_sim/include", PluginDirectory + "/SimLibs/simserver/include", PluginDirectory + "/SimLibs/physics/include", + PluginDirectory + "/SimLibs/core_sim/jsbsim/include", PluginDirectory + "/SimLibs/multirotor_api/include", PluginDirectory + "/SimLibs/rover_api/include", PluginDirectory + "/SimLibs/rendering_scene/include", @@ -140,6 +143,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/core_sim/" + buildType + "/core_sim.lib", PluginDirectory + "/SimLibs/simserver/" + buildType + "/simserver.lib", PluginDirectory + "/SimLibs/physics/" + buildType + "/physics.lib", + PluginDirectory + "/SimLibs/core_sim/jsbsim/lib/" + buildType + "/jsbsim.lib", PluginDirectory + "/SimLibs/multirotor_api/" + buildType + "/multirotor_api.lib", PluginDirectory + "/SimLibs/rover_api/" + buildType + "/rover_api.lib", PluginDirectory + "/SimLibs/rendering_scene/" + buildType + "/rendering_scene.lib", @@ -171,6 +175,9 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) var fileName = Path.GetFileName(file); RuntimeDependencies.Add("$(BinaryOutputDir)/" + fileName, PluginDirectory + "/SimLibs/shared_libs/" + fileName); } + + // JSBSim dll + RuntimeDependencies.Add("$(BinaryOutputDir)/" + "JSBSim.dll", PluginDirectory + "/SimLibs/core_sim/jsbsim/lib/" + buildType + "/" + "JSBSim.dll"); } else { @@ -178,6 +185,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/core_sim/" + buildType + "/libcore_sim.a", PluginDirectory + "/SimLibs/simserver/" + buildType + "/libsimserver.a", PluginDirectory + "/SimLibs/physics/" + buildType + "/libphysics.a", + PluginDirectory + "/SimLibs/core_sim/jsbsim/lib/" + buildType + "/libjsbsim.a", PluginDirectory + "/SimLibs/multirotor_api/" + buildType + "/libmultirotor_api.a", PluginDirectory + "/SimLibs/rover_api/" + buildType + "/librover_api.a", PluginDirectory + "/SimLibs/rendering_scene/" + buildType + "/librendering_scene.a", @@ -198,6 +206,9 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) RuntimeDependencies.Add("$(BinaryOutputDir)/" + fileName, PluginDirectory + "/SimLibs/shared_libs/" + fileName); } + // JSBSim so + RuntimeDependencies.Add("$(BinaryOutputDir)/" + "libjsbsim.so", PluginDirectory + "/SimLibs/core_sim/jsbsim/lib/" + buildType + "/" + "JSBSim.so"); + PublicAdditionalLibraries.AddRange(liststrLibraries); PublicSystemLibraries.AddRange( new string[] { diff --git a/unreal/Blocks/Source/Blocks/Blocks.Build.cs b/unreal/Blocks/Source/Blocks/Blocks.Build.cs index 3be4071d..fe924c86 100644 --- a/unreal/Blocks/Source/Blocks/Blocks.Build.cs +++ b/unreal/Blocks/Source/Blocks/Blocks.Build.cs @@ -6,6 +6,8 @@ public class Blocks : ModuleRules { public Blocks(ReadOnlyTargetRules Target) : base(Target) { + bEnableUndefinedIdentifierWarnings = false; + PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; bEnableExceptions = true; diff --git a/vehicle_apis/multirotor_api/include/arducopter_api.hpp b/vehicle_apis/multirotor_api/include/arducopter_api.hpp index 1bcc8569..9708d82c 100644 --- a/vehicle_apis/multirotor_api/include/arducopter_api.hpp +++ b/vehicle_apis/multirotor_api/include/arducopter_api.hpp @@ -126,8 +126,9 @@ class ArduCopterApi : public VTOLFWApiBase { * *********************************/ float GetCommandPeriod() const override; // time between two command required for drone in seconds - float GetTakeoffZ() - const override; // the height above ground for the drone after successful + bool SetTakeOffZ(float takeoffZ) override; + float GetTakeOffZ() + override; // the height above ground for the drone after successful // takeoff (Z above ground is negative due to NED // coordinate system). // noise in difference of two position coordinates. This is not GPS or diff --git a/vehicle_apis/multirotor_api/include/ilanding_gear_api.hpp b/vehicle_apis/multirotor_api/include/ilanding_gear_api.hpp new file mode 100644 index 00000000..930c2938 --- /dev/null +++ b/vehicle_apis/multirotor_api/include/ilanding_gear_api.hpp @@ -0,0 +1,23 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#ifndef MULTIROTOR_API_INCLUDE_ILANDING_GEAR_API_HPP_ //TO-DO: This is not only for multirotor. We need to do some refactoring here. +#define MULTIROTOR_API_INCLUDE_ILANDING_GEAR_API_HPP_ + + +namespace microsoft { +namespace projectairsim { + +// Represents public landing gear APIs intended for client +class ILandingGearApi { + public: + // return value of these functions is true if command was completed without + // interruption or timeouts + + virtual bool SetBrakes(float value) = 0; //value beteween 0 and 1 for all wheels + +}; + +} // namespace projectairsim +} // namespace microsoft + +#endif // MULTIROTOR_API_INCLUDE_ILANDING_GEAR_API_HPP_ \ No newline at end of file diff --git a/vehicle_apis/multirotor_api/include/imultirotor_api.hpp b/vehicle_apis/multirotor_api/include/imultirotor_api.hpp index 0c9abe70..7b6812df 100644 --- a/vehicle_apis/multirotor_api/include/imultirotor_api.hpp +++ b/vehicle_apis/multirotor_api/include/imultirotor_api.hpp @@ -123,6 +123,11 @@ class IMultirotorApi { // high level control APIs virtual bool Takeoff(float timeout_sec, int64_t command_start_time_nanos) = 0; + // the height above ground for the drone after successful + // takeoff (Z above ground is negative due to NED coordinate + // system). + virtual bool SetTakeOffZ(float z) = 0; + virtual float GetTakeOffZ() = 0; virtual bool Land(float timeout_sec, int64_t command_start_time_nanos) = 0; virtual bool GoHome(float timeout_sec, float velocity, int64_t command_start_time_nanos) = 0; diff --git a/vehicle_apis/multirotor_api/include/jsbsim_api.hpp b/vehicle_apis/multirotor_api/include/jsbsim_api.hpp new file mode 100644 index 00000000..4405d3d3 --- /dev/null +++ b/vehicle_apis/multirotor_api/include/jsbsim_api.hpp @@ -0,0 +1,192 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#ifndef MULTIROTOR_API_INCLUDE_JSBSIM_API_HPP_ +#define MULTIROTOR_API_INCLUDE_JSBSIM_API_HPP_ + +#include +#include +#include + +#include "core_sim/actor/robot.hpp" +#include "core_sim/runtime_components.hpp" +#include "core_sim/service_method.hpp" +#include "core_sim/transforms/transform_tree.hpp" +#include "simple_flight/AirSimSimpleFlightBoard.hpp" +#include "simple_flight/AirSimSimpleFlightCommLink.hpp" +#include "simple_flight/AirSimSimpleFlightEstimator.hpp" +#include "simple_flight/AirSimSimpleFlightEstimatorFW.hpp" +#include "simple_flight/firmware/Firmware.hpp" +#include "landing_gear_api_base.hpp" + + +namespace microsoft { +namespace projectairsim { + +// todo "firmware" / "firmware wrapper api" or "api" type (wrt px4 / mavlink) +//enum class MultirotorApiType { kJSBSim = 0 }; + +// JSBSimApi +// TODO: Should we use pimpl or some other pattern to hide the implementation? +class JSBSimApi : public LandingGearApiBase { + public: + JSBSimApi() {} + JSBSimApi(const Robot& robot, TransformTree* ptransformtree); + + virtual ~JSBSimApi() {} + + //--------------------------------------------------------------------------- + // IController overrides + + void BeginUpdate() override; + void EndUpdate() override; + void Reset() override; + void SetKinematics(const Kinematics* kinematics) override; + void Update() override; + std::vector GetControlSignals(const std::string& actuator_id) override; + + //--------------------------------------------------------------------------- + // IMultirotorApi overrides + + bool EnableApiControl() override; + bool DisableApiControl() override; + bool IsApiControlEnabled() override; + bool Arm(int64_t command_start_time_nanos) override; + bool Disarm() override; + bool CanArm() const override; + + float GetTakeOffZ() override; + bool SetTakeOffZ(float takeoffZ) override; + bool Takeoff( + float timeout_sec, TimeNano _service_method_start_time) override; + bool Land(float timeout_sec, int64_t command_start_time_nanos) override; + //SetBrakes + bool SetBrakes(float value); + bool MoveToZ(float z, float velocity, float timeout_sec, bool yaw_is_rate, + float yaw, float lookahead, float adaptive_lookahead, + int64_t command_start_time_nanos) override; + bool MoveToPosition(float x, float y, float z, float velocity, + float timeout_sec, DrivetrainType drivetrain, + bool yaw_is_rate, float yaw, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos) override; + bool MoveByHeading(float heading, float speed, + float vz, float duration, + float heading_margin, float yaw_rate, + float timeout_sec, + int64_t command_start_time_nanos) override; + float GetJSBSimProperty(const std::string& property); + bool SetJSBSimProperty(const std::string& property, float value); + + // Switched to using service method request-response for all control commands, + // but leaving the below pub-sub version commented out for reference in case + // it's needed in the future. + // void OnSetpointNEDvelocityYawrate(const Topic& topic, + // const Message& message) + // override; + + //--------------------------------------------------------------------------- + // IVTOLFWApi overrides + + protected: + //--------------------------------------------------------------------------- + // MultirotorApiBase overrides + std::string GetControllerName() const override { return "JSBSimApi"; } + + //--------------------------------------------------------------------------- + // Low level commands + + void CommandMotorPWMs(float front_right_pwm, float rear_left_pwm, + float front_left_pwm, float rear_right_pwm) override; + void CommandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, + float throttle) override; + void CommandRollPitchYawZ(float roll, float pitch, float yaw, + float z) override; + void CommandRollPitchYawThrottle(float roll, float pitch, float yaw, + float throttle) override; + void CommandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, + float z) override; + void CommandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, + float z) override; + void CommandAngleRatesThrottle(float roll_rate, float pitch_rate, + float yaw_rate, float throttle) override; + void CommandHeading(float heading, float speed, float vz) override; + void CommandVelocity(float vx, float vy, float vz, bool yaw_is_rate, + float yaw) override; + void CommandVelocityZ(float vx, float vy, float z, bool yaw_is_rate, + float yaw) override; + void CommandPosition(float x, float y, float z, bool yaw_is_rate, + float yaw) override; + void CommandVelocityBody(float vx, float vy, float vz, + bool yaw_is_rate, float yaw) override; + void CommandVelocityZBody(float vx, float vy, float z, + bool yaw_is_rate, float yaw) override; + + // controller configs + void SetControllerGains(uint8_t controllerType, const std::vector& kp, + const std::vector& ki, + const std::vector& kd) override; + ReadyState GetReadyState() const override; + GeoPoint GetGpsLocationEstimated() const override; + + /************************* State APIs *********************************/ + Kinematics GetKinematicsEstimated() const override; + LandedState GetLandedState() const override; + // GeoPoint GetGpsLocation() const override; + const MultirotorApiParams& GetMultirotorApiParams() const override; + + /************************* Basic Config APIs **************************/ + float GetCommandPeriod() + const override; // time between two command required for drone in seconds + // noise in difference of two position coordinates. This is not GPS or + // position accuracy which can be very low such as 1m. the difference between + // two position cancels out transitional errors. Typically this would be 0.1m + // or lower. + float GetDistanceAccuracy() const override; + + protected: + // optional overrides + Vector3 GetAngles() const override; + Vector3 GetPosition() const override; + Vector3 GetVelocity() const override; + Quaternion GetOrientation() const override; + + protected: + void RegisterServiceMethods(void) override; + + private: + // The kind of target vehicle + enum class VehicleKind { + Multirotor, // Multirotor helicopter (aka., multicopter) + VTOLTailsitter, // VTOL tail-sitting fixed-wing vehicle with multicopter + // and fixed-wing modes + VTOLTiltrotor, // VTOL tilt-rotor fixed-wing vehicle with multicopter + // and fixed-wing modes + FixedWing, // Fixed-wing aircraft + Other, + }; // enum class VehicleKind + + private: + void LoadSettings(const Robot& robot); + static void Break(std::shared_ptr model); + + private: + std::unordered_map actuator_id_to_output_idx_map_; + std::unique_ptr board_; + std::unique_ptr comm_link_; + std::unique_ptr estimator_; + std::unique_ptr estimator_fw_; + std::unique_ptr firmware_; + uint64_t millis_rc_input_last_update_ = 0; // Timestamp of when RC input was last received + std::unique_ptr params_; // todo params should become simple_flight_api_settings + Topic rc_input_topic_; // RC input topic + MultirotorApiParams safety_params_; + std::mutex update_lock_; + VehicleKind vehicle_kind_ = VehicleKind::Multirotor; // The type of vehicle being controlled + float takeoff_z_ = 100.0f; // The z-coordinate of the takeoff position + bool is_api_control_enabled_ = false; +}; + +} // namespace projectairsim +} // namespace microsoft + +#endif // MULTIROTOR_API_INCLUDE_SIMPLE_FLIGHT_API_HPP_ \ No newline at end of file diff --git a/vehicle_apis/multirotor_api/include/landing_gear_api_base.hpp b/vehicle_apis/multirotor_api/include/landing_gear_api_base.hpp new file mode 100644 index 00000000..2c1e16d1 --- /dev/null +++ b/vehicle_apis/multirotor_api/include/landing_gear_api_base.hpp @@ -0,0 +1,37 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#ifndef MULTIROTOR_API_INCLUDE_LANDING_GEAR_API_BASE_HPP_ +#define MULTIROTOR_API_INCLUDE_LANDING_GEAR_API_BASE_HPP_ + +#include +#include +#include +#include +#include + +#include "ilanding_gear_api.hpp" +#include "vtolfw_api_base.hpp" + +namespace microsoft { +namespace projectairsim { + +class LandingGearApiBase : public VTOLFWApiBase, public ILandingGearApi { + public: + LandingGearApiBase() {} + LandingGearApiBase(const Robot& robot, TransformTree* ptransformtree); + + virtual ~LandingGearApiBase() = default; + + protected: + void RegisterServiceMethods(void) override; + + //--------------------------------------------------------------------------- + // Service Method Wrappers + + bool SetBrakesServiceMethod(float value); +}; // class LandingGearApiBase + +} // namespace projectairsim +} // namespace microsoft + +#endif // MULTIROTOR_API_INCLUDE_LANDING_GEAR_API_BASE_HPP_ \ No newline at end of file diff --git a/vehicle_apis/multirotor_api/include/mavlink_api.hpp b/vehicle_apis/multirotor_api/include/mavlink_api.hpp index c311e761..21b79cff 100644 --- a/vehicle_apis/multirotor_api/include/mavlink_api.hpp +++ b/vehicle_apis/multirotor_api/include/mavlink_api.hpp @@ -65,6 +65,8 @@ class MavLinkApi : public VTOLFWApiBase { LandedState GetLandedState() const override; bool Takeoff(float timeout_sec, int64_t command_start_time_nanos) override; + float GetTakeOffZ() override; + bool SetTakeOffZ(float takeoffZ) override; bool Land(float timeout_sec, int64_t command_start_time_nanos) override; bool MoveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, @@ -134,10 +136,6 @@ class MavLinkApi : public VTOLFWApiBase { * *********************************/ float GetCommandPeriod() const override; // time between two command required for drone in seconds - float GetTakeoffZ() - const override; // the height above ground for the drone after successful - // takeoff (Z above ground is negative due to NED - // coordinate system). // noise in difference of two position coordinates. This is not GPS or // position accuracy which can be very low such as 1m. the difference between // two position cancels out transitional errors. Typically this would be 0.1m @@ -437,6 +435,8 @@ class MavLinkApi : public VTOLFWApiBase { mutable int state_version_; mutable mavlinkcom::VehicleState current_state_; + float takeoff_z_ = -10; + // Debug int heartbeat_messages_received_count_ = 0; bool first_message_sent_ = false; diff --git a/vehicle_apis/multirotor_api/include/multirotor_api_base.hpp b/vehicle_apis/multirotor_api/include/multirotor_api_base.hpp index e319719f..b02f1d82 100644 --- a/vehicle_apis/multirotor_api/include/multirotor_api_base.hpp +++ b/vehicle_apis/multirotor_api/include/multirotor_api_base.hpp @@ -274,10 +274,6 @@ class MultirotorApiBase : public IController, * *********************************/ virtual float GetCommandPeriod() const = 0; // time between two command required for drone in seconds - virtual float GetTakeoffZ() - const = 0; // the height above ground for the drone after successful - // takeoff (Z above ground is negative due to NED coordinate - // system). // noise in difference of two position coordinates. This is not GPS or // position accuracy which can be very low such as 1m. the difference between // two position cancels out transitional errors. Typically this would be 0.1m @@ -498,6 +494,10 @@ class MultirotorApiBase : public IController, bool TakeoffServiceMethod(float timeout_sec, TimeNano _service_method_start_time); + bool SetTakeOffZServiceMethod(float z); + + float GetTakeOffZServiceMethod(); + bool LandServiceMethod(float timeout_sec, TimeNano _service_method_start_time); @@ -556,6 +556,7 @@ class MultirotorApiBase : public IController, TransformTree* psim_transformtree_; // The transform tree containing the robot std::vector topics_; // Topics to advertise or subscribe to + float approx_zero_vel_ = 0.05f; private: // variables // TODO: Do we need pub/sub to publish any controller data? @@ -572,7 +573,6 @@ class MultirotorApiBase : public IController, // TODO: make this configurable? float landing_vel_ = 0.2f; // velocity to use for landing - float approx_zero_vel_ = 0.05f; float approx_zero_angular_vel_ = 0.01f; }; diff --git a/vehicle_apis/multirotor_api/include/simple_flight_api.hpp b/vehicle_apis/multirotor_api/include/simple_flight_api.hpp index f282a954..35c357e8 100644 --- a/vehicle_apis/multirotor_api/include/simple_flight_api.hpp +++ b/vehicle_apis/multirotor_api/include/simple_flight_api.hpp @@ -51,6 +51,8 @@ class SimpleFlightApi : public VTOLFWApiBase { bool IsApiControlEnabled() override; bool Arm(int64_t command_start_time_nanos) override; bool Disarm() override; + float GetTakeOffZ() override; + bool SetTakeOffZ(float takeoffZ) override; bool CanArm() const override; ReadyState GetReadyState() const override; Kinematics GetKinematicsEstimated() const override; @@ -121,10 +123,6 @@ class SimpleFlightApi : public VTOLFWApiBase { /************************* Basic Config APIs **************************/ float GetCommandPeriod() const override; // time between two command required for drone in seconds - float GetTakeoffZ() - const override; // the height above ground for the drone after successful - // takeoff (Z above ground is negative due to NED - // coordinate system). // noise in difference of two position coordinates. This is not GPS or // position accuracy which can be very low such as 1m. the difference between // two position cancels out transitional errors. Typically this would be 0.1m diff --git a/vehicle_apis/multirotor_api/src/CMakeLists.txt b/vehicle_apis/multirotor_api/src/CMakeLists.txt index 34647101..e570b86a 100644 --- a/vehicle_apis/multirotor_api/src/CMakeLists.txt +++ b/vehicle_apis/multirotor_api/src/CMakeLists.txt @@ -22,7 +22,9 @@ add_library( matlab_controller_api.cpp multirotor_api_base.cpp simple_flight_api.cpp + jsbsim_api.cpp vtolfw_api_base.cpp + landing_gear_api_base.cpp arducopter_api.cpp ) diff --git a/vehicle_apis/multirotor_api/src/arducopter_api.cpp b/vehicle_apis/multirotor_api/src/arducopter_api.cpp index 0d549e41..4530fb9e 100644 --- a/vehicle_apis/multirotor_api/src/arducopter_api.cpp +++ b/vehicle_apis/multirotor_api/src/arducopter_api.cpp @@ -228,7 +228,7 @@ std::vector ArduCopterApi::GetControlSignals(const std::string& actuator_ if (actuator_map_itr == actuator_id_to_output_idx_map_.end()) { GetLogger().LogWarning( GetControllerName(), - "ArduCopterApi::GetControlSignal() called for invalid actuator: %s", + "ArduCopterApi::GetControlSignals() called for invalid actuator: %s", actuator_id.c_str()); return std::vector(1,0.f); } @@ -500,8 +500,8 @@ float ArduCopterApi::GetCommandPeriod() const { return 1.0f / 50; // 50hz } -float ArduCopterApi::GetTakeoffZ() const { - AddStatusMessage("Not Implemented: GetTakeoffZ"); +float ArduCopterApi::GetTakeOffZ() { + AddStatusMessage("Not Implemented: GetTakeOffZ"); return 0; } @@ -580,6 +580,11 @@ void ArduCopterApi::ResetState() { CancelLastTask(); } +bool ArduCopterApi::SetTakeOffZ(float z) { + AddStatusMessage("Not Implemented: SetTakeOffZ"); + return true; +} + /* put ArduPilot in normal mode (i.e. non-simulation mode) void ArduCopterApi::SetNormalMode() { if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { diff --git a/vehicle_apis/multirotor_api/src/jsbsim_api.cpp b/vehicle_apis/multirotor_api/src/jsbsim_api.cpp new file mode 100644 index 00000000..7b2c2ec3 --- /dev/null +++ b/vehicle_apis/multirotor_api/src/jsbsim_api.cpp @@ -0,0 +1,543 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#include "jsbsim_api.hpp" + +#include +#ifdef LVMON_REPORTING +#include +#endif // LVMON_REPORTING + +#include + +#include "core_sim/message/flight_control_rc_input_message.hpp" +#include "core_sim/message/flight_control_setpoint_message.hpp" +#include "core_sim/transforms/transform_tree.hpp" +#include "core_sim/transforms/transform_utils.hpp" +#include "json.hpp" +#include "core_sim/service_method.hpp" +#include "core_sim/geodetic_converter.hpp" + +namespace microsoft { +namespace projectairsim { + +// ----------------------------------------------------------------------------- +// class JSBSimApi + +JSBSimApi::JSBSimApi(const Robot& robot, + TransformTree* ptransformtree) + : LandingGearApiBase(robot, ptransformtree) { + LoadSettings(robot); +} + +void JSBSimApi::LoadSettings(const Robot& robot) { + const std::string controller_settings = robot.GetControllerSettings(); + const json& controller_settings_json = json::parse(controller_settings); + + const std::string airframe_setup = + controller_settings_json.value("airframe-setup", ""); + + if (airframe_setup == "quadrotor-x") { + vehicle_kind_ = VehicleKind::Multirotor; + } else if (airframe_setup == "hexarotor-x") { + vehicle_kind_ = VehicleKind::Multirotor; + } else if (airframe_setup == "vtol-quad-x-tailsitter") { + vehicle_kind_ = VehicleKind::VTOLTailsitter; + } else if (airframe_setup == "vtol-quad-tiltrotor") {; + vehicle_kind_ = VehicleKind::VTOLTiltrotor; + } else if (airframe_setup == "fixed-wing") { //TODO: review if this variable is necesary, the number of motors is flexible + vehicle_kind_ = VehicleKind::FixedWing; + } else { + vehicle_kind_ = VehicleKind::Other; + } + + // GetJsonObject + //const json& jsbsim_api_settings_json = + // controller_settings_json.value("jsbsim-api-settings", "{ }"_json); + +} + +//--------------------------------------------------------------------------- +// IController overrides + +void JSBSimApi::BeginUpdate() { + MultirotorApiBase::BeginUpdate(); + + params_.reset(new simple_flight::Params); + params_->motor.motor_count = 4; + + if (vehicle_kind_ == VehicleKind::VTOLTailsitter) + params_->controller_type = + simple_flight::Params::ControllerType::kVFWTCascade; + else if (vehicle_kind_ == VehicleKind::VTOLTiltrotor) + params_->controller_type = + simple_flight::Params::ControllerType::kVTRCascade; + + board_.reset(new AirSimSimpleFlightBoard(params_.get())); + comm_link_.reset(new AirSimSimpleFlightCommLink()); + estimator_.reset(new AirSimSimpleFlightEstimator()); + estimator_fw_.reset(new AirSimSimpleFlightEstimatorFW()); + estimator_fw_->Initialize(estimator_.get()); + firmware_.reset( + new simple_flight::Firmware(params_.get(), board_.get(), comm_link_.get(), + estimator_.get(), estimator_fw_.get())); + + // Reset RC input timestamp--use uint64_t so wrap-around delta calculations + // work + { + auto millis_cur = board_->Millis(); + + millis_rc_input_last_update_ = *reinterpret_cast(&millis_cur); + } + + Reset(); + +#ifdef LVMON_REPORTING + LVMon::Set("Params/vtol/enable_fixed_wing", + params_->vtol.enable_fixed_wing_mode ? "true" : "false"); +#endif // LVMON_REPORTING +} + +void JSBSimApi::EndUpdate() { + MultirotorApiBase::EndUpdate(); + // TODO: Do we need any clean-up of the board, firmware? +} + +void JSBSimApi::Reset() { firmware_->Reset(); } + +void JSBSimApi::Update() { firmware_->Update(); } + +void JSBSimApi::SetKinematics(const Kinematics* kinematics) { + board_->SetGroundTruthKinematics(kinematics); + estimator_->SetGroundTruthKinematics(kinematics); +} + +// GetJSBSimProperty +float JSBSimApi::GetJSBSimProperty(const std::string& property) { + auto model = sim_robot_.GetJSBSimModel(); + auto value = model->GetPropertyValue(property); + return value; +} + +bool JSBSimApi::SetJSBSimProperty(const std::string& property, float value) { + auto model = sim_robot_.GetJSBSimModel(); + model->SetPropertyValue(property, value); + return true; +} + +void JSBSimApi::RegisterServiceMethods() { + LandingGearApiBase::RegisterServiceMethods(); + + //Register GetJSBSimProperty + auto method = ServiceMethod("GetJSBSimProperty", {"_property_name"}); + auto method_handler = + method.CreateMethodHandler(&JSBSimApi::GetJSBSimProperty, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); + + //Register SetJSBSimProperty + method = ServiceMethod("SetJSBSimProperty", {"_property_name","_value"}); + method_handler = + method.CreateMethodHandler(&JSBSimApi::SetJSBSimProperty, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); +} + +std::vector JSBSimApi::GetControlSignals(const std::string& actuator_id) { + auto model = sim_robot_.GetJSBSimModel(); + //add fcs/ to actuator_id + auto jsbsim_actuator_id = actuator_id; + std::vector return_vector{GetJSBSimProperty(jsbsim_actuator_id)}; + return return_vector; +} + +//--------------------------------------------------------------------------- + +bool JSBSimApi::Takeoff(float timeout_sec, TimeNano _service_method_start_time) { + // if this vehicle is not a fixed wing + if (vehicle_kind_ != VehicleKind::FixedWing) + return VTOLFWApiBase::Takeoff(timeout_sec, _service_method_start_time); + + // if this vehicle is a fixed wing + auto model = sim_robot_.GetJSBSimModel(); + + // A JSBSim vehicle must have the "airsim/takeoff" booleans defined + // that triggers all the necessary takeoff procedures + SetJSBSimProperty("airsim/takeoff", 1); + + auto success = RunFlightCommand( + [&]() { + float takeoff_ended = GetJSBSimProperty("airsim/takeoff-ended"); + return takeoff_ended > 0.99; + }, + timeout_sec, _service_method_start_time) + .IsComplete(); + + if (!success) + return false; + + success = RunFlightCommand( + [&]() { + float takeoff_success = GetJSBSimProperty("airsim/takeoff-success"); + return takeoff_success > 0.99; + }, + timeout_sec, _service_method_start_time) + .IsComplete(); + return success; +} + +bool JSBSimApi::Land(float timeout_sec, int64_t command_start_time_nanos) { + SingleTaskCall lock(this); + + auto model = sim_robot_.GetJSBSimModel(); + + // Wait until we stop moving vertically + { + // A JSBSim vehicle must have the "airsim/takeoff" boolean defined + // that triggers all the necessary takeoff procedures + SetJSBSimProperty("airsim/land", 1); + + return RunFlightCommand( + [&]() { + float land_ended = GetJSBSimProperty("airsim/land-ended"); + return static_cast(land_ended); + }, + timeout_sec, command_start_time_nanos) + .IsComplete(); + } +} + +bool JSBSimApi::EnableApiControl() { + is_api_control_enabled_ = true; + return true; +} + +bool JSBSimApi::DisableApiControl() { + firmware_->OffboardApi().ReleaseApiControl(); + is_api_control_enabled_ = false; + return true; +} + +bool JSBSimApi::IsApiControlEnabled() { + return is_api_control_enabled_; +} + +bool JSBSimApi::CanArm() const { + return true; +} + +void JSBSimApi::CommandVelocityBody(float vx, float vy, float vz, + bool yaw_is_rate, float yaw) { +} + +void JSBSimApi::CommandVelocityZBody(float vx, float vy, float z, + bool yaw_is_rate, float yaw) { +} + +GeoPoint JSBSimApi::GetGpsLocationEstimated() const +{ + return sim_robot_.GetEnvironment().home_geo_point.geo_point; +} + +ReadyState JSBSimApi::GetReadyState() const { + ReadyState state; + state.ReadyVal = true; + return state; +} + +bool JSBSimApi::SetBrakes(float value) { + //get jsbsim model + auto model = sim_robot_.GetJSBSimModel(); + SetJSBSimProperty("fcs/left-brake-cmd-norm", value); + SetJSBSimProperty("fcs/right-brake-cmd-norm", value); + SetJSBSimProperty("fcs/center-brake-cmd-norm", value); + return true; +} + + +bool JSBSimApi::Arm(int64_t /*command_start_time_nanos*/) { + //std::string message; + //return firmware_->OffboardApi().Arm(message); + //get jsbsim model + auto model = sim_robot_.GetJSBSimModel(); + SetJSBSimProperty("fcs/mixture-cmd-norm[0]", 1); + SetJSBSimProperty("fcs/mixture-cmd-norm[1]", 1); + SetJSBSimProperty("fcs/advance-cmd-norm[0]", 1); + SetJSBSimProperty("fcs/advance-cmd-norm[1]", 1); + SetJSBSimProperty("fcs/throttle-cmd-norm[0]", 0.01); + SetJSBSimProperty("fcs/throttle-cmd-norm[1]", 0.01); + SetJSBSimProperty("propulsion/magneto_cmd", 3); + SetJSBSimProperty("propulsion/starter_cmd", 1); + + + return true; +} + +bool JSBSimApi::Disarm() { + std::string message; + return firmware_->OffboardApi().Disarm(message); +} + +//--------------------------------------------------------------------------- +// Implementation for MultirotorApiBase + +void JSBSimApi::CommandMotorPWMs(float front_right_pwm, + float rear_left_pwm, + float front_left_pwm, + float rear_right_pwm) { + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode( + GoalModeType::kPassthrough, GoalModeType::kPassthrough, + GoalModeType::kPassthrough, GoalModeType::kPassthrough); + + simple_flight::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, + rear_right_pwm); + + std::string message; + firmware_->OffboardApi().SetGoalAndMode(&goal, &mode, message); +} + +void JSBSimApi::CommandRollPitchYawZ(float roll, float pitch, float yaw, + float z) { + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode( + GoalModeType::kAngleLevel, GoalModeType::kAngleLevel, + GoalModeType::kAngleLevel, GoalModeType::kPositionWorld); + + simple_flight::Axis4r goal(roll, pitch, yaw, z); + + std::string message; + firmware_->OffboardApi().SetGoalAndMode(&goal, &mode, message); +} + +void JSBSimApi::CommandRollPitchYawThrottle(float roll, float pitch, + float yaw, float throttle) { + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode( + GoalModeType::kAngleLevel, GoalModeType::kAngleLevel, + GoalModeType::kAngleLevel, GoalModeType::kPassthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw, throttle); + + std::string message; + firmware_->OffboardApi().SetGoalAndMode(&goal, &mode, message); +} + +void JSBSimApi::CommandRollPitchYawrateThrottle(float roll, float pitch, + float yaw_rate, + float throttle) { +} + +void JSBSimApi::CommandRollPitchYawrateZ(float roll, float pitch, + float yaw_rate, float z) { +} + +void JSBSimApi::CommandAngleRatesZ(float roll_rate, float pitch_rate, + float yaw_rate, float z) { +} + +void JSBSimApi::CommandAngleRatesThrottle(float roll_rate, + float pitch_rate, + float yaw_rate, + float throttle) { +} + +void JSBSimApi::CommandVelocity(float vx, float vy, float vz, + bool yaw_is_rate, float yaw) { +} + +void JSBSimApi::CommandVelocityZ(float vx, float vy, float z, + bool yaw_is_rate, float yaw) { +} + +void JSBSimApi::CommandPosition(float x, float y, float z, + bool yaw_is_rate, float yaw) { +} + + +bool JSBSimApi::MoveToPosition(float x, float y, float z, float velocity, + float timeout_sec, DrivetrainType drivetrain, + bool yaw_is_rate, float yaw, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos){ + + //get world coordinates from sim_robot_ + auto world_geopoint = sim_robot_.GetEnvironment().home_geo_point.geo_point; + // get coordinates for x,y,z + GeodeticConverter converter(world_geopoint.latitude, world_geopoint.longitude, + world_geopoint.altitude); + double lat, lon; + float alt; + converter.ned2Geodetic(x,y,z,&lat,&lon,&alt); + + SetJSBSimProperty("ap/altitude_setpoint", -z * MathUtils::meters_to_feets); // world is flat so z is altitude + SetJSBSimProperty("guidance/target_wp_latitude_rad", MathUtils::deg2Rad(lat)); + SetJSBSimProperty("guidance/target_wp_longitude_rad", MathUtils::deg2Rad(lon)); + SetJSBSimProperty("ap/heading-setpoint-select", 1); + SetJSBSimProperty("guidance/heading-selector-switch", 0); + SetJSBSimProperty("ap/heading_hold", 1); + + auto just_started = true; // to avoid checking distance on first iteration + + return RunFlightCommand( + [&]() { + if (just_started) { + just_started = false; + return false; + } + if (GetJSBSimProperty("guidance/wp-distance")<= lookahead * MathUtils::meters_to_feets) { + return true; + } else + return false; + }, + timeout_sec, command_start_time_nanos) + .IsComplete(); + +} + +const MultirotorApiBase::MultirotorApiParams& +JSBSimApi::GetMultirotorApiParams() const { + return safety_params_; +} + +void JSBSimApi::SetControllerGains(uint8_t controller_type, + const std::vector& kp, + const std::vector& ki, + const std::vector& kd) { +} + +Kinematics JSBSimApi::GetKinematicsEstimated() const { + return AirSimSimpleFlightCommon::ToKinematicsState3r( + firmware_->OffboardApi().GetStateEstimator().GetKinematicsEstimated()); +} + +Vector3 JSBSimApi::GetAngles() const { + const auto& val = firmware_->OffboardApi().GetStateEstimator().GetAngles(); + return AirSimSimpleFlightCommon::ToVector3(val); +} + +Vector3 JSBSimApi::GetPosition() const { + const auto& val = firmware_->OffboardApi().GetStateEstimator().GetPosition(); + return AirSimSimpleFlightCommon::ToVector3(val); +} + +Vector3 JSBSimApi::GetVelocity() const { + const auto& val = + firmware_->OffboardApi().GetStateEstimator().GetLinearVelocity(); + return AirSimSimpleFlightCommon::ToVector3(val); +} + +Quaternion JSBSimApi::GetOrientation() const { + const auto& val = + firmware_->OffboardApi().GetStateEstimator().GetOrientation(); + return AirSimSimpleFlightCommon::ToQuaternion(val); +} + +LandedState JSBSimApi::GetLandedState() const { + return firmware_->OffboardApi().GetLandedState() ? LandedState::Landed + : LandedState::Flying; +} + +float JSBSimApi::GetCommandPeriod() const { + return 1.0f / 50; // 50hz +} + +float JSBSimApi::GetTakeOffZ() { + return takeoff_z_; +} + +bool JSBSimApi::SetTakeOffZ(float z) { + takeoff_z_ = z; + return true; +} + +float JSBSimApi::GetDistanceAccuracy() const { + return 0.5f; // measured in simulator by firing commands "MoveToLocation -x 0 + // -y 0" multiple times and looking at distance traveled +} + +//--------------------------------------------------------------------------- +// Implementation for VTOLFWApiBase +bool JSBSimApi::MoveByHeading(float heading, float speed, float vz, + float duration, float heading_margin, + float yaw_rate, float timeout_sec, + int64_t command_start_time_nanos) { + SingleTaskCall lock(this); + + float yaw = MathUtils::NormalizeAngle(heading); + + return (RunFlightCommand( + [&]() { + // Command the heading and wait until we've reached it + if (RunFlightCommand( + [&]() { + if (IsYawWithinMargin(yaw, heading_margin)) + return (true); + else + CommandHeading(yaw, speed, vz); + + return (false); + }, + timeout_sec, command_start_time_nanos) + .IsTimeout()) { + return (false); + } + + // Keep commanding the heading for the requested duration + RunFlightCommand( + [&]() { + CommandHeading(yaw, speed, vz); + return (false); + }, + duration, SimClock::Get()->NowSimNanos()); + + return (true); + }, + timeout_sec, command_start_time_nanos) + .IsComplete()); +} + +void JSBSimApi::CommandHeading(float heading, float speed, float vz) { + auto setpoint_yaw_deg = MathUtils::rad2Deg(heading); + SetJSBSimProperty("ap/heading_setpoint", setpoint_yaw_deg); + SetJSBSimProperty("ap/heading-setpoint-select", 0); + SetJSBSimProperty("ap/heading_hold", 1); +} + +bool JSBSimApi::MoveToZ(float z, float velocity, float timeout_sec, bool yaw_is_rate, + float yaw, float lookahead, float adaptive_lookahead, + int64_t command_start_time_nanos) { + auto model = sim_robot_.GetJSBSimModel(); + + auto success = RunFlightCommand( + [&]() { + auto vel = GetVelocity().norm(); + if (vel >= 25.0f) { + SetJSBSimProperty("ap/altitude_setpoint", -z * MathUtils::meters_to_feets); + SetJSBSimProperty("ap/altitude_hold", 1); + return true; + } else + return false; + }, + timeout_sec, command_start_time_nanos) + .IsComplete(); + + if (!success) + return false; + + success = RunFlightCommand( + [&]() { + auto z_ = GetPosition().z(); + if (z_ <= z) { + return true; + } else + return false; + }, + timeout_sec, command_start_time_nanos) + .IsComplete(); + + return success; + +} + +//--------------------------------------------------------------------------- + +} // namespace projectairsim +} // namespace microsoft \ No newline at end of file diff --git a/vehicle_apis/multirotor_api/src/landing_gear_api_base.cpp b/vehicle_apis/multirotor_api/src/landing_gear_api_base.cpp new file mode 100644 index 00000000..2e715c17 --- /dev/null +++ b/vehicle_apis/multirotor_api/src/landing_gear_api_base.cpp @@ -0,0 +1,32 @@ +// Copyright (C) Microsoft Corporation. All rights reserved. + +#include "landing_gear_api_base.hpp" + +#include +#include +#include +#include +#include + +namespace microsoft { +namespace projectairsim { + +LandingGearApiBase::LandingGearApiBase(const Robot& robot, + TransformTree* ptransformtree) + : VTOLFWApiBase(robot, ptransformtree) {} + +bool LandingGearApiBase::SetBrakesServiceMethod(float value) { + return SetBrakes(value); +} + +void LandingGearApiBase::RegisterServiceMethods(void) { + VTOLFWApiBase::RegisterServiceMethods(); + + // Register SetBrakes + auto method = ServiceMethod("SetBrakes", {"_brakes_value"}); + auto method_handler = method.CreateMethodHandler(&LandingGearApiBase::SetBrakesServiceMethod, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); +} + +} // namespace projectairsim +} // namespace microsoft \ No newline at end of file diff --git a/vehicle_apis/multirotor_api/src/manual_controller_api.cpp b/vehicle_apis/multirotor_api/src/manual_controller_api.cpp index fff847c6..21827afc 100644 --- a/vehicle_apis/multirotor_api/src/manual_controller_api.cpp +++ b/vehicle_apis/multirotor_api/src/manual_controller_api.cpp @@ -36,7 +36,7 @@ std::vector ManualControllerApi::GetControlSignals(const std::string& act auto actuator_map_itr = actuator_id_to_output_idx_map_.find(actuator_id); if (actuator_map_itr == actuator_id_to_output_idx_map_.end()) { GetLogger().LogWarning("ManualControllerApi", - "ManualControllerApi::GetControlSignal() called for " + "ManualControllerApi::GetControlSignals() called for " "invalid actuator: %s", actuator_id.c_str()); return std::vector(1,0.0f); diff --git a/vehicle_apis/multirotor_api/src/matlab_controller_api.cpp b/vehicle_apis/multirotor_api/src/matlab_controller_api.cpp index db8b2132..47232d39 100644 --- a/vehicle_apis/multirotor_api/src/matlab_controller_api.cpp +++ b/vehicle_apis/multirotor_api/src/matlab_controller_api.cpp @@ -211,7 +211,7 @@ std::vector MatlabControllerApi::GetControlSignals(const std::string& act auto actuator_map_itr = actuator_id_to_output_idx_map_.find(actuator_id); if (actuator_map_itr == actuator_id_to_output_idx_map_.end()) { GetLogger().LogWarning("MatlabControllerApi", - "MatlabControllerApi::GetControlSignal() called for " + "MatlabControllerApi::GetControlSignals() called for " "invalid actuator: %s", actuator_id.c_str()); return std::vector(1, 0.f); diff --git a/vehicle_apis/multirotor_api/src/mavlink_api.cpp b/vehicle_apis/multirotor_api/src/mavlink_api.cpp index a21d9b26..445f1437 100644 --- a/vehicle_apis/multirotor_api/src/mavlink_api.cpp +++ b/vehicle_apis/multirotor_api/src/mavlink_api.cpp @@ -380,7 +380,7 @@ std::vector MavLinkApi::GetControlSignals(const std::string& actuator_id) if (actuator_map_itr == actuator_id_to_output_idx_map_.end()) { GetLogger().LogWarning( GetControllerName(), - "MavLinkApi::GetControlSignal() called for invalid actuator: %s", + "MavLinkApi::GetControlSignals() called for invalid actuator: %s", actuator_id.c_str()); return std::vector(1, 0.f); } @@ -569,7 +569,7 @@ bool MavLinkApi::Takeoff(float timeout_sec, int64_t command_start_time_nanos) { bool rc = false; auto vec = GetPosition(); auto yaw = current_state_.attitude.yaw; - float z = vec.z() + GetTakeoffZ(); + float z = vec.z() + GetTakeOffZ(); if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw) .wait(TimeoutToMilliseconds(timeout_sec), &rc)) { auto error_msg = "TakeOff command - timeout waiting for response"; @@ -978,10 +978,19 @@ float MavLinkApi::GetCommandPeriod() const { return 1.0f / 50; // 50hz } -float MavLinkApi::GetTakeoffZ() const { +float MavLinkApi::GetTakeOffZ() { + return takeoff_z_; +} + +bool MavLinkApi::SetTakeOffZ(float z) { // PX4's minimum take off altitude is 10.0m. Set negative due to NED // coordinate system. return -10.0f; + if (z >= -10.0f) { + return false; + } + takeoff_z_ = z; + return true; } float MavLinkApi::GetDistanceAccuracy() const { diff --git a/vehicle_apis/multirotor_api/src/multirotor_api_base.cpp b/vehicle_apis/multirotor_api/src/multirotor_api_base.cpp index 95bf0e2b..79ac95dc 100644 --- a/vehicle_apis/multirotor_api/src/multirotor_api_base.cpp +++ b/vehicle_apis/multirotor_api/src/multirotor_api_base.cpp @@ -253,6 +253,18 @@ void MultirotorApiBase::RegisterServiceMethods() { method_handler = method.CreateMethodHandler(&MultirotorApiBase::SetMissionMode, *this); sim_robot_.RegisterServiceMethod(method, method_handler); + + // Register GetTakeoffZ + method = ServiceMethod("GetTakeOffZ", {}); + method_handler = + method.CreateMethodHandler(&MultirotorApiBase::GetTakeOffZServiceMethod, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); + + // Register SetTakeoffZ + method = ServiceMethod("SetTakeOffZ", {"_take_off_z"}); + method_handler = + method.CreateMethodHandler(&MultirotorApiBase::SetTakeOffZServiceMethod, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); } bool MultirotorApiBase::TakeoffServiceMethod( @@ -260,6 +272,14 @@ bool MultirotorApiBase::TakeoffServiceMethod( return Takeoff(timeout_sec, _service_method_start_time); } +bool MultirotorApiBase::SetTakeOffZServiceMethod(float z){ + return SetTakeOffZ(z); +} + +float MultirotorApiBase::GetTakeOffZServiceMethod(){ + return GetTakeOffZ(); +} + bool MultirotorApiBase::Takeoff(float timeout_sec, int64_t command_start_time_nanos) { // TODO Parameterize take off altitude? @@ -276,7 +296,7 @@ bool MultirotorApiBase::Takeoff(float timeout_sec, bool ret = MoveToPosition(kinematics.pose.position.x(), kinematics.pose.position.y(), - kinematics.pose.position.z() + GetTakeoffZ(), 0.5f, + kinematics.pose.position.z() + GetTakeOffZ(), 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, true, 0.0, -1.0, 1.0, command_start_time_nanos); diff --git a/vehicle_apis/multirotor_api/src/simple_flight_api.cpp b/vehicle_apis/multirotor_api/src/simple_flight_api.cpp index 125276d9..a3c9de9e 100644 --- a/vehicle_apis/multirotor_api/src/simple_flight_api.cpp +++ b/vehicle_apis/multirotor_api/src/simple_flight_api.cpp @@ -154,7 +154,7 @@ std::vector SimpleFlightApi::GetControlSignals(const std::string& actuato if (actuator_map_itr == actuator_id_to_output_idx_map_.end()) { GetLogger().LogWarning( GetControllerName(), - "SimpleFlightApi::GetControlSignal() called for invalid actuator: %s", + "SimpleFlightApi::GetControlSignals() called for invalid actuator: %s", actuator_id.c_str()); return std::vector(1,0.f); } @@ -503,10 +503,15 @@ float SimpleFlightApi::GetCommandPeriod() const { return 1.0f / 50; // 50hz } -float SimpleFlightApi::GetTakeoffZ() const { +float SimpleFlightApi::GetTakeOffZ() { return params_->takeoff.takeoff_z; } +bool SimpleFlightApi::SetTakeOffZ(float z) { + params_->takeoff.takeoff_z = z; + return true; +} + float SimpleFlightApi::GetDistanceAccuracy() const { return 0.5f; // measured in simulator by firing commands "MoveToLocation -x 0 // -y 0" multiple times and looking at distance traveled