diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy new file mode 100644 index 0000000000..287589a052 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy @@ -0,0 +1,138 @@ +#!/bin/sh +# +# @name BlueROV2 Heavy +# +# @type UUV +# +# @maintainer Pedro Roque +# @maintainer Nicola De Carli +# + +# Source PX4 parameters and configuration +. ${R}/etc/init.d/rc.uuv_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=underwater} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=uuv_bluerov2_heavy} + +param set-default SIM_GZ_EN 1 + +# Set parameters using `param` utility for SITL +param set-default CA_AIRFRAME 7 +param set-default CA_METHOD 0 +param set-default MAV_TYPE 12 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +param set-default CA_ROTOR0_AX -1 +param set-default CA_ROTOR0_AY 1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PX 0.14 +param set-default CA_ROTOR0_PY 0.10 +param set-default CA_ROTOR0_PZ 0.06 +#param set-default CA_ROTOR0_PZ 0.0 + +param set-default CA_ROTOR1_AX -1 +param set-default CA_ROTOR1_AY -1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PX 0.14 +param set-default CA_ROTOR1_PY -0.10 +param set-default CA_ROTOR1_PZ 0.06 +#param set-default CA_ROTOR1_PZ 0.0 + +param set-default CA_ROTOR2_AX 1 +param set-default CA_ROTOR2_AY 1 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX -0.14 +param set-default CA_ROTOR2_PY 0.10 +param set-default CA_ROTOR2_PZ 0.06 +#param set-default CA_ROTOR2_PZ 0.0 + +param set-default CA_ROTOR3_AX 1 +param set-default CA_ROTOR3_AY -1 +param set-default CA_ROTOR3_AZ 0 +param set-default CA_ROTOR3_KM 0 +param set-default CA_ROTOR3_PX -0.14 +param set-default CA_ROTOR3_PY -0.10 +param set-default CA_ROTOR3_PZ 0.06 + +param set-default CA_ROTOR4_AX 0 +param set-default CA_ROTOR4_AY 0 +param set-default CA_ROTOR4_AZ 1 +param set-default CA_ROTOR4_KM 0 +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY 0.22 +param set-default CA_ROTOR4_PZ 0 + +param set-default CA_ROTOR5_AX 0 +param set-default CA_ROTOR5_AY 0 +param set-default CA_ROTOR5_AZ 1 +param set-default CA_ROTOR5_KM 0 +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY -0.22 +param set-default CA_ROTOR5_PZ 0 + +param set-default CA_ROTOR6_AX 0 +param set-default CA_ROTOR6_AY 0 +param set-default CA_ROTOR6_AZ 1 +param set-default CA_ROTOR6_KM 0 +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY 0.22 +param set-default CA_ROTOR6_PZ 0 + +param set-default CA_ROTOR7_AX 0 +param set-default CA_ROTOR7_AY 0 +param set-default CA_ROTOR7_AZ 1 +param set-default CA_ROTOR7_KM 0 +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY -0.22 +param set-default CA_ROTOR7_PZ 0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_FUNC5 105 +param set-default SIM_GZ_EC_FUNC6 106 +param set-default SIM_GZ_EC_FUNC7 107 +param set-default SIM_GZ_EC_FUNC8 108 + +param set-default SIM_GZ_EC_MIN1 1100 +param set-default SIM_GZ_EC_MIN2 1100 +param set-default SIM_GZ_EC_MIN3 1100 +param set-default SIM_GZ_EC_MIN4 1100 +param set-default SIM_GZ_EC_MIN5 1100 +param set-default SIM_GZ_EC_MIN6 1100 +param set-default SIM_GZ_EC_MIN7 1100 +param set-default SIM_GZ_EC_MIN8 1100 + +param set-default SIM_GZ_EC_MAX1 1900 +param set-default SIM_GZ_EC_MAX2 1900 +param set-default SIM_GZ_EC_MAX3 1900 +param set-default SIM_GZ_EC_MAX4 1900 +param set-default SIM_GZ_EC_MAX5 1900 +param set-default SIM_GZ_EC_MAX6 1900 +param set-default SIM_GZ_EC_MAX7 1900 +param set-default SIM_GZ_EC_MAX8 1900 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index eb2fc90c49..59608118b0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -115,6 +115,8 @@ px4_add_romfs_files( 50000_gz_rover_differential + 60002_gz_uuv_bluerov2_heavy + 71001_gz_atmos 71002_gz_spacecraft_2d diff --git a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy index 1449a6d3ad..d397e5deae 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy @@ -22,7 +22,7 @@ . ${R}etc/init.d/rc.uuv_defaults -param set-default MAV_1_CONFIG 102 +# param set-default MAV_1_CONFIG 102 param set-default BAT1_A_PER_V 37.8798 param set-default BAT1_CAPACITY 18000 @@ -30,53 +30,93 @@ param set-default BAT1_V_DIV 11 param set-default BAT1_N_CELLS 4 param set-default BAT_V_OFFS_CURR 0.33 +# Set parameters using `param` utility for SITL param set-default CA_AIRFRAME 7 +param set-default CA_METHOD 0 +param set-default MAV_TYPE 12 + +# Board rotation: set to direct replacement of RPi with PX4 +# with forward facing vehicle forward +param set-default SENS_BOARD_ROT 16 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 + param set-default CA_ROTOR0_AX 1 param set-default CA_ROTOR0_AY -1 param set-default CA_ROTOR0_AZ 0 param set-default CA_ROTOR0_KM 0 -param set-default CA_ROTOR0_PX 0.5 -param set-default CA_ROTOR0_PY 0.3 -param set-default CA_ROTOR0_PZ 0.2 +param set-default CA_ROTOR0_PX 0.14 +param set-default CA_ROTOR0_PY 0.10 +param set-default CA_ROTOR0_PZ 0.06 + param set-default CA_ROTOR1_AX 1 param set-default CA_ROTOR1_AY 1 param set-default CA_ROTOR1_AZ 0 param set-default CA_ROTOR1_KM 0 -param set-default CA_ROTOR1_PX 0.5 -param set-default CA_ROTOR1_PY -0.3 -param set-default CA_ROTOR1_PZ 0.2 +param set-default CA_ROTOR1_PX 0.14 +param set-default CA_ROTOR1_PY -0.10 +param set-default CA_ROTOR1_PZ 0.06 + param set-default CA_ROTOR2_AX 1 param set-default CA_ROTOR2_AY 1 param set-default CA_ROTOR2_AZ 0 param set-default CA_ROTOR2_KM 0 -param set-default CA_ROTOR2_PX -0.5 -param set-default CA_ROTOR2_PY 0.3 -param set-default CA_ROTOR2_PZ 0.2 +param set-default CA_ROTOR2_PX -0.14 +param set-default CA_ROTOR2_PY 0.10 +param set-default CA_ROTOR2_PZ 0.06 + param set-default CA_ROTOR3_AX 1 param set-default CA_ROTOR3_AY -1 param set-default CA_ROTOR3_AZ 0 param set-default CA_ROTOR3_KM 0 -param set-default CA_ROTOR3_PX -0.5 -param set-default CA_ROTOR3_PY -0.3 -param set-default CA_ROTOR3_PZ 0.2 +param set-default CA_ROTOR3_PX -0.14 +param set-default CA_ROTOR3_PY -0.10 +param set-default CA_ROTOR3_PZ 0.06 + +param set-default CA_ROTOR4_AX 0 +param set-default CA_ROTOR4_AY 0 param set-default CA_ROTOR4_AZ -1 param set-default CA_ROTOR4_KM 0 -param set-default CA_ROTOR4_PX 0.5 -param set-default CA_ROTOR4_PY 0.5 +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY 0.22 +param set-default CA_ROTOR4_PZ 0 + +param set-default CA_ROTOR5_AX 0 +param set-default CA_ROTOR5_AY 0 param set-default CA_ROTOR5_AZ 1 param set-default CA_ROTOR5_KM 0 -param set-default CA_ROTOR5_PX 0.5 -param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY -0.22 +param set-default CA_ROTOR5_PZ 0 + +param set-default CA_ROTOR6_AX 0 +param set-default CA_ROTOR6_AY 0 param set-default CA_ROTOR6_AZ 1 param set-default CA_ROTOR6_KM 0 -param set-default CA_ROTOR6_PX -0.5 -param set-default CA_ROTOR6_PY 0.5 +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY 0.22 +param set-default CA_ROTOR6_PZ 0 + +param set-default CA_ROTOR7_AX 0 +param set-default CA_ROTOR7_AY 0 +param set-default CA_ROTOR7_AZ -1 param set-default CA_ROTOR7_KM 0 -param set-default CA_ROTOR7_PX -0.5 -param set-default CA_ROTOR7_PY -0.5 +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY -0.22 +param set-default CA_ROTOR7_PZ 0 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 @@ -86,3 +126,30 @@ param set-default PWM_MAIN_FUNC5 105 param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 + +param set-default PWM_MAIN_MIN1 1100 +param set-default PWM_MAIN_MIN2 1100 +param set-default PWM_MAIN_MIN3 1100 +param set-default PWM_MAIN_MIN4 1100 +param set-default PWM_MAIN_MIN5 1100 +param set-default PWM_MAIN_MIN6 1100 +param set-default PWM_MAIN_MIN7 1100 +param set-default PWM_MAIN_MIN8 1100 + +param set-default PWM_MAIN_MAX1 1900 +param set-default PWM_MAIN_MAX2 1900 +param set-default PWM_MAIN_MAX3 1900 +param set-default PWM_MAIN_MAX4 1900 +param set-default PWM_MAIN_MAX5 1900 +param set-default PWM_MAIN_MAX6 1900 +param set-default PWM_MAIN_MAX7 1900 +param set-default PWM_MAIN_MAX8 1900 + +param set-default PWM_MAIN_DIS1 1500 +param set-default PWM_MAIN_DIS2 1500 +param set-default PWM_MAIN_DIS3 1500 +param set-default PWM_MAIN_DIS4 1500 +param set-default PWM_MAIN_DIS5 1500 +param set-default PWM_MAIN_DIS6 1500 +param set-default PWM_MAIN_DIS7 1500 +param set-default PWM_MAIN_DIS8 1500 diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps index f604b64452..ae9cdb5b5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -16,6 +16,6 @@ control_allocator start uuv_att_control start # -# Start UUV Land Detector. +# Start UUV Position Controller. # -land_detector start rover +uuv_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults index 3398fac3f5..2bf3c60168 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults @@ -9,3 +9,21 @@ set VEHICLE_TYPE uuv # MAV_TYPE_SUBMARINE 12 param set-default MAV_TYPE 12 + +# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 +param set-default UXRCE_DDS_AG_IP -1062731775 + +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 +param set-default CBRK_SUPPLY_CHK 894281 +param set-default COM_ARM_HFLT_CHK 0 + +#Missing params +param set-default CP_DIST -1.0 + +# Default to MoCap fusion +param set-default ATT_EXT_HDG_M 2 +param set-default EKF2_EV_CTRL 15 +param set-default EKF2_EV_DELAY 5 +param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 3 diff --git a/boards/px4/fmu-v6x/uuv.px4board b/boards/px4/fmu-v6x/uuv.px4board new file mode 100644 index 0000000000..3a7ec000cb --- /dev/null +++ b/boards/px4/fmu-v6x/uuv.px4board @@ -0,0 +1,23 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_ROVER_ACKERMANN=n +CONFIG_MODULES_ROVER_DIFFERENTIAL=n +CONFIG_MODULES_ROVER_MECANUM=n +CONFIG_MODULES_SPACECRAFT=n +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/boards/px4/sitl/uuv.px4board b/boards/px4/sitl/uuv.px4board new file mode 100644 index 0000000000..3a7ec000cb --- /dev/null +++ b/boards/px4/sitl/uuv.px4board @@ -0,0 +1,23 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_ROVER_ACKERMANN=n +CONFIG_MODULES_ROVER_DIFFERENTIAL=n +CONFIG_MODULES_ROVER_MECANUM=n +CONFIG_MODULES_SPACECRAFT=n +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index bb5b576035..ecaddd5bca 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -574,7 +574,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low - && !is_ground_vehicle(_vehicle_status)) { + && ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) + ) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 48c9398249..6b1c8c1721 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -119,7 +119,8 @@ bool is_fixed_wing(const vehicle_status_s ¤t_status) bool is_ground_vehicle(const vehicle_status_s ¤t_status) { - return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); + return (current_status.system_type == VEHICLE_TYPE_BOAT || + current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); } // End time for currently blinking LED message, 0 if no blink message diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index 0001356a3a..ae0fd92ece 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -15,6 +15,8 @@ + + diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index 265bbd9599..1eb9f59886 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -56,12 +56,14 @@ if (gz-transport_FOUND) add_subdirectory(template_plugin) add_subdirectory(gstreamer) add_subdirectory(moving_platform_controller) + add_subdirectory(generic_motor) + add_subdirectory(buoyancy) add_subdirectory(spacecraft_thruster) # Add an alias target for each plugin if (TARGET GstCameraSystem) - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin) else() - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin) endif() endif() diff --git a/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.cpp b/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.cpp new file mode 100644 index 0000000000..d9fd3c200e --- /dev/null +++ b/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.cpp @@ -0,0 +1,699 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#include + +#include "gz/sim/components/CenterOfVolume.hh" +#include "gz/sim/components/Collision.hh" +#include "gz/sim/components/Gravity.hh" +#include "gz/sim/components/Inertial.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Volume.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +#include "BuoyancySystem.hpp" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::BuoyancyPrivate +{ +public: enum BuoyancyType { + /// \brief Applies same buoyancy to whole world. + UNIFORM_BUOYANCY, + /// \brief Uses z-axis to determine buoyancy of the world + /// This is useful for worlds where we want to simulate the ocean interface. + /// Or for instance if we want to simulate different levels of buoyancies + /// at different depths. + GRADED_BUOYANCY + }; +public: BuoyancyType buoyancyType{BuoyancyType::UNIFORM_BUOYANCY}; + /// \brief Get the fluid density based on a pose. + /// \param[in] _pose The pose to use when computing the fluid density. The + /// pose frame is left undefined because this function currently returns + /// a constant value, see the todo in the function implementation. + /// \return The fluid density at the givein pose. +public: double UniformFluidDensity(const math::Pose3d &_pose) const; + + /// \brief Get the resultant buoyant force on a shape. + /// \param[in] _pose World pose of the shape's origin. + /// \param[in] _shape The collision mesh of a shape. Currently must + /// be box or sphere. + /// \param[in] _gravity Gravity acceleration in the world frame. + /// Updates this->buoyancyForces containing {force, center_of_volume} to be + /// applied on the link. +public: + template + void GradedFluidDensity( + const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity); + + /// \brief Check for new links to apply buoyancy forces to. Calculates the + /// volume and center of volume for every new link and stages them to be + /// committed when `CommitNewEntities` is called. + /// \param[in] _ecm The Entity Component Manager. +public: void CheckForNewEntities(const EntityComponentManager &_ecm); + + /// \brief Commits the new entities to the ECM. + /// \param[in] _ecm The Entity Component Manager. +public: void CommitNewEntities(EntityComponentManager &_ecm); + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. +public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + + /// \brief Model interface +public: Entity world{kNullEntity}; + + /// \brief The density of the fluid in which the object is submerged in + /// kg/m^3. Defaults to 1000, the fluid density of water. +public: double fluidDensity{1000}; + + /// \brief When using GradedBuoyancy, we provide a different buoyancy for + /// each layer. The key on this map is height in meters and the value is fluid + /// density. I.E all the fluid between $key$m and $next_key$m has the density + /// $value$kg/m^3. Everything below the first key is considered as having + /// fluidDensity. +public: std::map layers; + + /// \brief Holds information about forces contributed by a single collision + /// shape. +public: struct BuoyancyActionPoint { + /// \brief The force to be applied, expressed in the world frame. + math::Vector3d force; + + /// \brief The point from which the force will be applied, expressed in + /// the collision's frame. + math::Vector3d point; + + /// \brief The world pose of the collision. + math::Pose3d pose; + }; + + /// \brief List of points from where the forces act. + /// This holds values refent to the current link being processed and must be + /// cleared between links. + /// \TODO(chapulina) It's dangerous to keep link-specific values in a member + /// variable. We should consider reducing the scope of this variable and pass + /// it across functions as needed. +public: std::vector buoyancyForces; + + /// \brief Resolve all forces as if they act as a Wrench from the give pose. + /// \param[in] _linkInWorld The point from which all poses are to be resolved. + /// This is the link's origin in the world frame. + /// \return A pair of {force, torque} describing the wrench to be applied + /// at _pose, expressed in the world frame. +public: std::pair ResolveForces( + const math::Pose3d &_linkInWorld); + + /// \brief Scoped names of entities that buoyancy should apply to. If empty, + /// all links will receive buoyancy. +public: std::unordered_set enabled; + + /// \brief Basic buoyancy enabler flag. This is used to set the buoyancy + /// equal to the gravity compensation of the base_link mass, with added noise + /// to simulate the effect of buoyancy. Useful for testing purposes. +public: bool basicBuoyancy{false}; + + /// \brief Center of volumes to be added on the next Pre-update +public: std::unordered_map centerOfVolumes; + + /// \brief Volumes to be added on the next. +public: std::unordered_map volumes; +}; + +////////////////////////////////////////////////// +double BuoyancyPrivate::UniformFluidDensity(const math::Pose3d &/*_pose*/) const +{ + return this->fluidDensity; +} + +////////////////////////////////////////////////// +template +void BuoyancyPrivate::GradedFluidDensity( + const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity) +{ + auto prevLayerFluidDensity = this->fluidDensity; + auto prevLayerVol = 0.0; + auto centerOfBuoyancy = math::Vector3d{0, 0, 0}; + + for (const auto &[height, currFluidDensity] : this->layers) { + // TODO(arjo): Transform plane and slice the shape + math::Planed plane{math::Vector3d{0, 0, 1}, height - _pose.Pos().Z()}; + auto vol = _shape.VolumeBelow(plane); + + // Short circuit. + if (vol <= 0) { + prevLayerFluidDensity = currFluidDensity; + continue; + } + + // Calculate point from which force is applied + auto cov = _shape.CenterOfVolumeBelow(plane); + + if (!cov.has_value()) { + prevLayerFluidDensity = currFluidDensity; + continue; + } + + // Archimedes principle for this layer + auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity; + + // Accumulate layers. + prevLayerFluidDensity = currFluidDensity; + + auto cob = (cov.value() * vol - centerOfBuoyancy * prevLayerVol) + / (vol - prevLayerVol); + centerOfBuoyancy = cov.value(); + auto buoyancyAction = BuoyancyActionPoint { + forceMag, + cob, + _pose + }; + this->buoyancyForces.push_back(buoyancyAction); + + prevLayerVol = vol; + } + + // For the rest of the layers. + auto vol = _shape.Volume(); + + // No force contributed by this layer. + if (std::abs(vol - prevLayerVol) < 1e-10) { + return; + } + + // Archimedes principle for this layer + auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity; + + // Calculate centre of buoyancy + auto cov = math::Vector3d{0, 0, 0}; + auto cob = + (cov * vol - centerOfBuoyancy * prevLayerVol) / (vol - prevLayerVol); + centerOfBuoyancy = cov; + auto buoyancyAction = BuoyancyActionPoint { + forceMag, + cob, + _pose + }; + this->buoyancyForces.push_back(buoyancyAction); +} + +////////////////////////////////////////////////// +std::pair BuoyancyPrivate::ResolveForces( + const math::Pose3d &_linkInWorld) +{ + auto force = math::Vector3d{0, 0, 0}; + auto torque = math::Vector3d{0, 0, 0}; + + for (const auto &b : this->buoyancyForces) { + force += b.force; + + // Pose offset from application point (COV) to collision origin, expressed + // in the collision frame + math::Pose3d pointInCol{b.point, math::Quaterniond::Identity}; + + // Application point in the world frame + auto pointInWorld = b.pose * pointInCol; + + // Offset between the link origin and the force application point + auto offset = _linkInWorld.Pos() - pointInWorld.Pos(); + + torque += b.force.Cross(offset); + } + + return {force, torque}; +} + +////////////////////////////////////////////////// +void BuoyancyPrivate::CheckForNewEntities(const EntityComponentManager &_ecm) +{ + // Compute the volume and center of volume for each new link + _ecm.EachNew( + [&](const Entity & _entity, + const components::Link *, + const components::Inertial *) -> bool { + if (this->basicBuoyancy == true) return true; + // Skip if the entity already has a volume and center of volume + if (_ecm.EntityHasComponentType(_entity, + components::CenterOfVolume().TypeId()) && + _ecm.EntityHasComponentType(_entity, + components::Volume().TypeId())) + { + return true; + } + + if (!this->IsEnabled(_entity, _ecm)) + { + return true; + } + + Link link(_entity); + + std::vector collisions = _ecm.ChildrenByComponents( + _entity, components::Collision()); + + double volumeSum = 0; + gz::math::Vector3d weightedPosInLinkSum = + gz::math::Vector3d::Zero; + + // Compute the volume of the link by iterating over all the collision + // elements and storing each geometry's volume. + for (const Entity &collision : collisions) + { + double volume = 0; + const components::CollisionElement *coll = + _ecm.Component(collision); + + if (!coll) { + gzerr << "Invalid collision pointer. This shouldn't happen\n"; + continue; + } + + switch (coll->Data().Geom()->Type()) { + case sdf::GeometryType::BOX: + volume = coll->Data().Geom()->BoxShape()->Shape().Volume(); + break; + + case sdf::GeometryType::SPHERE: + volume = coll->Data().Geom()->SphereShape()->Shape().Volume(); + break; + + case sdf::GeometryType::CYLINDER: + volume = coll->Data().Geom()->CylinderShape()->Shape().Volume(); + break; + + case sdf::GeometryType::PLANE: + // Ignore plane shapes. They have no volume and are not expected + // to be buoyant. + break; + + case sdf::GeometryType::MESH: { + std::string file = asFullPath( + coll->Data().Geom()->MeshShape()->Uri(), + coll->Data().Geom()->MeshShape()->FilePath()); + + if (common::MeshManager::Instance()->IsValidFilename(file)) { + const common::Mesh *mesh = + common::MeshManager::Instance()->Load(file); + + if (mesh) { + volume = mesh->Volume(); + + } else { + gzerr << "Unable to load mesh[" << file << "]\n"; + } + + } else { + gzerr << "Invalid mesh filename[" << file << "]\n"; + } + + break; + } + + default: + gzerr << "Unsupported collision geometry[" + << static_cast(coll->Data().Geom()->Type()) << "]\n"; + break; + } + + volumeSum += volume; + auto poseInLink = _ecm.Component(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); + } + + if (volumeSum > 0) + { + // Stage calculation results for future commit. We do this because + // during PostUpdate the ECM is const, so we can't modify it, + this->centerOfVolumes[_entity] = weightedPosInLinkSum / volumeSum; + this->volumes[_entity] = volumeSum; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void BuoyancyPrivate::CommitNewEntities(EntityComponentManager &_ecm) +{ + for (const auto [_entity, _cov] : this->centerOfVolumes) { + if (_ecm.HasEntity(_entity)) { + _ecm.CreateComponent(_entity, components::CenterOfVolume(_cov)); + } + } + + for (const auto [_entity, _vol] : this->volumes) { + if (_ecm.HasEntity(_entity)) { + _ecm.CreateComponent(_entity, components::Volume(_vol)); + } + } + + this->centerOfVolumes.clear(); + this->volumes.clear(); +} + +////////////////////////////////////////////////// +bool BuoyancyPrivate::IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const +{ + // If there's nothing enabled, all entities are enabled + if (this->enabled.empty()) { + return true; + } + + auto entity = _entity; + + while (entity != kNullEntity) { + // Fully scoped name + auto name = scopedName(entity, _ecm, "::", false); + + // Remove world name + name = removeParentScope(name, "::"); + + if (this->enabled.find(name) != this->enabled.end()) { + return true; + } + + // Check parent + auto parentComp = _ecm.Component(entity); + + if (nullptr == parentComp) { + return false; + } + + entity = parentComp->Data(); + } + + return false; +} + +////////////////////////////////////////////////// +Buoyancy::Buoyancy() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void Buoyancy::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + // Store the world. + this->dataPtr->world = _entity; + + // Get the gravity (defined in world frame) + const components::Gravity *gravity = _ecm.Component( + this->dataPtr->world); + + if (!gravity) { + gzerr << "Unable to get the gravity vector. Make sure this plugin is " + << "attached to a , not a ." << std::endl; + return; + } + + if (_sdf->HasElement("uniform_fluid_density")) { + this->dataPtr->fluidDensity = _sdf->Get("uniform_fluid_density"); + + } else if (_sdf->HasElement("graded_buoyancy")) { + this->dataPtr->buoyancyType = + BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY; + + auto gradedElement = _sdf->GetFirstElement(); + + if (gradedElement == nullptr) { + gzerr << "Unable to get element description" << std::endl; + return; + } + + auto argument = gradedElement->GetFirstElement(); + + while (argument != nullptr) { + if (argument->GetName() == "default_density") { + argument->GetValue()->Get(this->dataPtr->fluidDensity); + gzdbg << "Default density set to " + << this->dataPtr->fluidDensity << std::endl; + } + + if (argument->GetName() == "density_change") { + auto depth = argument->Get("above_depth", 0.0); + auto density = argument->Get("density", 0.0); + + if (!depth.second) { + gzwarn << "No tag was found as a " + << "child of " << std::endl; + } + + if (!density.second) { + gzwarn << "No tag was found as a " + << "child of " << std::endl; + } + + this->dataPtr->layers[depth.first] = density.first; + gzdbg << "Added layer at " << depth.first << ", " + << density.first << std::endl; + } + + argument = argument->GetNextElement(); + } + + } else if (_sdf->HasElement("basic_buoyancy")) { + // This is a legacy tag, which is equivalent to + this->dataPtr->basicBuoyancy = _sdf->Get("basic_buoyancy", false).first; + + } else { + gzwarn << + "Neither , , or were specified" + << std::endl + << "\tDefaulting to " + << std::endl; + this->dataPtr->basicBuoyancy = true; + } + + if (_sdf->HasElement("enable")) { + for (auto enableElem = _sdf->FindElement("enable"); + enableElem != nullptr; + enableElem = enableElem->GetNextElement("enable")) { + this->dataPtr->enabled.insert(enableElem->Get()); + } + } +} + +////////////////////////////////////////////////// +void Buoyancy::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("Buoyancy::PreUpdate"); + this->dataPtr->CheckForNewEntities(_ecm); + this->dataPtr->CommitNewEntities(_ecm); + + // Only update if not paused. + if (_info.paused) { + return; + } + + const components::Gravity *gravity = _ecm.Component( + this->dataPtr->world); + + if (!gravity) { + gzerr << "Unable to get the gravity vector. Has gravity been defined?" + << std::endl; + return; + } + + if (!this->dataPtr->basicBuoyancy) { + // Iterate over all links and apply buoyancy forces. + _ecm.Each( + [&](const Entity & _entity, + const components::Link *, + const components::Volume * _volume, + const components::CenterOfVolume * _centerOfVolume) -> bool { + // World pose of the link. + math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + + Link link(_entity); + + math::Vector3d buoyancy; + // By Archimedes' principle, + // buoyancy = -(mass*gravity)*fluid_density/object_density + // object_density = mass/volume, so the mass term cancels. + if (this->dataPtr->buoyancyType + == BuoyancyPrivate::BuoyancyType::UNIFORM_BUOYANCY) + { + buoyancy = + -this->dataPtr->UniformFluidDensity(linkWorldPose) * + _volume->Data() * gravity->Data(); + + // Convert the center of volume to the world frame + math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector( + _centerOfVolume->Data()); + // Compute the torque that should be applied due to buoyancy and + // the center of volume. + math::Vector3d torque = offsetWorld.Cross(buoyancy); + + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, buoyancy, torque); + + } else if (this->dataPtr->buoyancyType + == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY) + { + std::vector collisions = _ecm.ChildrenByComponents( + _entity, components::Collision()); + this->dataPtr->buoyancyForces.clear(); + + for (auto e : collisions) { + const components::CollisionElement *coll = + _ecm.Component(e); + + auto pose = worldPose(e, _ecm); + + if (!coll) { + gzerr << "Invalid collision pointer. This shouldn't happen\n"; + continue; + } + + switch (coll->Data().Geom()->Type()) { + case sdf::GeometryType::BOX: + this->dataPtr->GradedFluidDensity( + pose, + coll->Data().Geom()->BoxShape()->Shape(), + gravity->Data()); + break; + + case sdf::GeometryType::SPHERE: + this->dataPtr->GradedFluidDensity( + pose, + coll->Data().Geom()->SphereShape()->Shape(), + gravity->Data()); + break; + + default: { + static bool warned{false}; + + if (!warned) { + gzwarn << "Only and collisions are supported " + << "by the graded buoyancy option." << std::endl; + warned = true; + } + + break; + } + } + } + + auto [force, torque] = this->dataPtr->ResolveForces(linkWorldPose); + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, force, torque); + } + + return true; + }); + + } else { + // Apply simple buyancy to baselink with some white noise. + + // Collect all masses in the model + double mass = 0.0; + + _ecm.Each( + [&](const Entity & _entity, + const components::Link *, + const components::Inertial * _inertial) -> bool { + // Get all masses and add them up + Link link(_entity); + + if (_ecm.EntityHasComponentType(_entity, + components::Inertial().TypeId())) + { + const auto *inertialComp = + _ecm.Component(_entity); + + if (inertialComp) { + mass = inertialComp->Data().MassMatrix().Mass(); + // Apply the buoyancy force + math::Vector3d buoyancy = -gravity->Data() * mass; + math::Vector3d randVec(0.0, 0.0, -gravity->Data().Z() * mass * math::Rand::DblUniform(-0.01, 0.01)); + math::Vector3d force = buoyancy + randVec; + + link.AddWorldWrench(_ecm, force, math::Vector3d{0, 0, 0}); + } + } + + return true; + }); + } +} + +////////////////////////////////////////////////// +void Buoyancy::PostUpdate( + const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->CheckForNewEntities(_ecm); +} + +////////////////////////////////////////////////// +bool Buoyancy::IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const +{ + return this->dataPtr->IsEnabled(_entity, _ecm); +} + +GZ_ADD_PLUGIN(Buoyancy, + System, + Buoyancy::ISystemConfigure, + Buoyancy::ISystemPreUpdate, + Buoyancy::ISystemPostUpdate) + +GZ_ADD_PLUGIN_ALIAS(Buoyancy, + "gz::sim::systems::BuoyancySystem") diff --git a/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.hpp b/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.hpp new file mode 100644 index 0000000000..07599dfb9f --- /dev/null +++ b/src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.hpp @@ -0,0 +1,158 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_ +#define GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ +// Forward declaration +class BuoyancyPrivate; + +/// \brief A system that simulates buoyancy of objects immersed in fluid. +/// All SDF parameters are optional. This system must be attached to the +/// world and this system will apply buoyancy to all links that have collision +/// shapes. +/// +/// The volume and center of volume will be computed for each link, and +/// stored as components. During each iteration, Archimedes' principle is +/// applied to each link with a volume and center of volume component. +/// +/// Plane shapes are not handled by this plugin, and will not be affected +/// by buoyancy. +/// +/// ## System Parameters +/// +/// * `` sets the density of the fluid that surrounds +/// the buoyant object. [Units: kgm^-3] +/// * `` allows you to define a world where the buoyancy +/// changes along the Z axis. An example of such a world could be if we are +/// simulating an open ocean with its surface and under water behaviour. This +/// mode slices the volume of each collision mesh according to where the water +/// line is set. When defining a `` tag, one must also define +/// `` and `` tags. +/// * `` is the default fluid which the world should be +/// filled with. [Units: kgm^-3] +/// * `` allows you to define a new layer. +/// * `` a child property of ``. This determines +/// the height at which the next fluid layer should start. [Units: m] +/// * `` the density of the fluid in this layer. [Units: kgm^-3] +/// * `` used to indicate which models will have buoyancy. +/// Add one enable element per model or link. This element accepts names +/// scoped from the top level model (i.e. `::::`). +/// If there are no enabled entities, all models in simulation will be +/// affected by buoyancy. +/// +/// ## Examples +/// +/// ### uniform_fluid_density world +/// +/// The `buoyancy.sdf` SDF file contains three submarines. The first +/// submarine is neutrally buoyant, the second sinks, and the third +/// floats. To run: +/// +/// ``` +/// gz sim -v 4 buoyancy.sdf +/// ``` +/// +/// ### graded_buoyancy world +/// +/// Often when simulating a maritime environment one may need to simulate both +/// surface and underwater vessels. This means the buoyancy plugin needs to +/// take into account two different fluids. One being water with a density of +/// 1000kgm^-3 and another being air with a very light density of say 1kgm^-3. +/// An example for such a configuration may be found in the +/// `graded_buoyancy.sdf` world. +/// +/// ``` +/// gz sim -v 4 graded_buoyancy.sdf +/// ``` +/// +/// You should be able to see a sphere bobbing up and down undergoing simple +/// harmonic motion on the surface of the fluid (this is expected behaviour +/// as the SHM is usually damped by the hydrodynamic forces. See the hydro- +/// dynamics plugin for an example of how to use it). The key part of this is +/// +/// ``` +/// +/// 1000 +/// +/// 0 +/// 1 +/// +/// +/// ``` +/// The default density tag says that by default the world has a fluid density +/// of 1000kgm^-3. This essentially states that by default the world is filled +/// with dihydrogen monoxide (aka water). The `` tag +/// essentially establishes the fact that there is a nother fluid. The +/// `` tag says that above z=0 there is another fluid with a +/// different density. The density of that fluid is defined by the `` +/// tag. We will be simulating air with a fluid density of 1kgm^-3. +class Buoyancy + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate +{ + /// \brief Constructor +public: Buoyancy(); + + /// \brief Destructor +public: ~Buoyancy() override = default; + + // Documentation inherited +public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited +public: void PreUpdate( + const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited +public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. +public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer +private: std::unique_ptr dataPtr; +}; +} +} +} +} + +#endif diff --git a/src/modules/simulation/gz_plugins/buoyancy/CMakeLists.txt b/src/modules/simulation/gz_plugins/buoyancy/CMakeLists.txt new file mode 100644 index 0000000000..a89818eb11 --- /dev/null +++ b/src/modules/simulation/gz_plugins/buoyancy/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(BuoyancySystemPlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + BuoyancySystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR} + PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR} + PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR} + PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/generic_motor/CMakeLists.txt b/src/modules/simulation/gz_plugins/generic_motor/CMakeLists.txt new file mode 100644 index 0000000000..5753d45ca6 --- /dev/null +++ b/src/modules/simulation/gz_plugins/generic_motor/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(GenericMotorModelPlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + GenericMotorModel.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR} + PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR} + PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR} + PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.cpp b/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.cpp new file mode 100644 index 0000000000..b36d1d2fb1 --- /dev/null +++ b/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.cpp @@ -0,0 +1,931 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Benjamin Perseghetti, Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "GenericMotorModel.hpp" + +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include "gz/sim/components/Actuators.hh" +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/components/JointAxis.hh" +#include "gz/sim/components/JointVelocity.hh" +#include "gz/sim/components/JointVelocityCmd.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/ParentLinkName.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +// from rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h +/// \brief This class can be used to apply a first order filter on a signal. +/// It allows different acceleration and deceleration time constants. +/// \details +/// Short reveiw of discrete time implementation of first order system +/// Laplace: +/// X(s)/U(s) = 1/(tau*s + 1) +/// continous time system: +/// dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) +/// discretized system (ZoH): +/// x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) // NOLINT +template +class FirstOrderFilter +{ +public: + FirstOrderFilter(double _timeConstantUp, double _timeConstantDown, T _initialState): // NOLINT + timeConstantUp(_timeConstantUp), + timeConstantDown(_timeConstantDown), + previousState(_initialState) {} + + /// \brief This method will apply a first order filter on the _inputState. + T UpdateFilter(T _inputState, double _samplingTime) + { + T outputState; + + if (_inputState > previousState) { + // Calcuate the outputState if accelerating. + double alphaUp = exp(-_samplingTime / timeConstantUp); + // x(k+1) = Ad*x(k) + Bd*u(k) + outputState = alphaUp * previousState + (1 - alphaUp) * _inputState; + + } else { + // Calculate the outputState if decelerating. + double alphaDown = exp(-_samplingTime / timeConstantDown); + outputState = alphaDown * previousState + (1 - alphaDown) * _inputState; + } + + previousState = outputState; + return outputState; + } + + ~FirstOrderFilter() = default; + +protected: + double timeConstantUp; + double timeConstantDown; + T previousState; +}; + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Constants for specifying clockwise (kCw) and counter-clockwise (kCcw) +/// directions of rotation. +namespace turning_direction +{ +static const int kCcw = 1; +static const int kCw = -1; +} // namespace turning_direction + +/// \brief Type of input command to motor. +enum class MotorType { + kVelocity, + kPosition, + kForce, + kForcePolynomial +}; + +/// \brief Type of control methology +enum class ControlMethod { + kRPM, + kDutyCycle +}; + +class gz::sim::systems::GenericMotorModelPrivate +{ + /// \brief Callback for actuator commands. +public: void OnActuatorMsg(const msgs::Actuators &_msg); + + /// \brief Apply link forces and moments based on propeller state. +public: void UpdateForcesAndMoments(EntityComponentManager &_ecm); + + /// \brief Joint Entity +public: Entity jointEntity; + + /// \brief Joint name +public: std::string jointName; + + /// \brief Link Entity +public: Entity linkEntity; + + /// \brief Link name +public: std::string linkName; + + /// \brief Parent link Entity +public: Entity parentLinkEntity; + + /// \brief Parent link name +public: std::string parentLinkName; + + /// \brief Model interface +public: Model model{kNullEntity}; + + /// \brief Topic for actuator commands. +public: std::string commandSubTopic; + + /// \brief Topic namespace. +public: std::string robotNamespace; + + /// \brief Sampling time (from motor_model.hpp). +public: double samplingTime = 0.01; + + /// \brief Index of motor in Actuators msg on multirotor_base. +public: int actuatorNumber = 0; + + /// \brief Turning direction of the motor. +public: int turningDirection = turning_direction::kCw; + + /// \brief Type of input command to motor. +public: MotorType motorType = MotorType::kVelocity; + + /// \brief Type of input command to motor. +public: ControlMethod controlMethod = ControlMethod::kRPM; + + /// \brief Maximum rotational velocity command with units of rad/s. + /// The default value is taken from gazebo_motor_model.h + /// and is approximately 8000 revolutions / minute (rpm). +public: double maxRotVelocity = 838.0; + + /// \brief Minimum rotor velocity, below which the rotor is considered + /// to be stopped. Simulates minimum velocity achieved by the motor +public: double minCommand = 0.0; + + /// \brief Maximum duty cycle value +public: double maxDutyCycle = 0.0; + + /// \brief Minimum duty cycle value +public: double minDutyCycle = 0.0; + + /// \brief Bidirectional-capable motor. Set to true if it mimics an ESC + /// with bidirectional control. Zero input will be middle of max and min + /// duty cycles. +public: int bidirectionalMotor = 0; + + /// \brief Moment constant for computing drag torque based on thrust + /// with units of length (m). + /// The default value is taken from gazebo_motor_model.h +public: double momentConstant = 0.016; + + /// \brief Thrust coefficient for propeller with units of N / (rad/s)^2. + /// The default value is taken from gazebo_motor_model.h +public: double motorConstant = 8.54858e-06; + + /// \brief Reference input to motor. For MotorType kVelocity, this + /// is the reference angular velocity in rad/s. +public: double refMotorInput = 0.0; + + /// \brief Rolling moment coefficient with units of N*m / (m/s^2). + /// The default value is taken from gazebo_motor_model.h +public: double rollingMomentCoefficient = 1.0e-6; + + /// \brief Rotor drag coefficient for propeller with units of N / (m/s^2). + /// The default value is taken from gazebo_motor_model.h +public: double rotorDragCoefficient = 1.0e-4; + + /// \brief Large joint velocities can cause problems with aliasing, + /// so the joint velocity used by the physics engine is reduced + /// this factor, while the larger value is used for computing + /// propeller thrust. + /// The default value is taken from gazebo_motor_model.h +public: double rotorVelocitySlowdownSim = 10.0; + + /// \brief Time constant for rotor deceleration. + /// The default value is taken from gazebo_motor_model.h +public: double timeConstantDown = 1.0 / 40.0; + + /// \brief Time constant for rotor acceleration. + /// The default value is taken from gazebo_motor_model.h +public: double timeConstantUp = 1.0 / 80.0; + + /// \brief Filter on rotor velocity that has different time constants + /// for increasing and decreasing values. +public: std::unique_ptr> rotorVelocityFilter; + + /// \brief Received Actuators message. This is nullopt if no message has been + /// received. +public: std::optional recvdActuatorsMsg; + + /// \brief Mutex to protect recvdActuatorsMsg. +public: std::mutex recvdActuatorsMsgMutex; + + /// \brief Polynomial for RPM to Thrust conversion - admits 5 degrees with asymmetric behavior +public: std::vector positiveTorquePolynomial = {}; +public: std::vector positiveThrustPolynomial = {}; +public: std::vector negativeTorquePolynomial = {}; +public: std::vector negativeThrustPolynomial = {}; + + /// \brief Gazebo communication node. +public: transport::Node node; +}; + +////////////////////////////////////////////////// +GenericMotorModel::GenericMotorModel() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void GenericMotorModel::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) { + gzerr << "GenericMotorModel plugin should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } + + auto sdfClone = _sdf->Clone(); + + this->dataPtr->robotNamespace.clear(); + + if (sdfClone->HasElement("robotNamespace")) { + this->dataPtr->robotNamespace = + sdfClone->Get("robotNamespace"); + + } else { + gzwarn << "No robotNamespace set using entity name.\n"; + this->dataPtr->robotNamespace = this->dataPtr->model.Name(_ecm); + } + + // Get params from SDF + if (sdfClone->HasElement("jointName")) { + this->dataPtr->jointName = sdfClone->Get("jointName"); + } + + if (this->dataPtr->jointName.empty()) { + gzerr << "GenericMotorModel found an empty jointName parameter. " + << "Failed to initialize."; + return; + } + + if (sdfClone->HasElement("linkName")) { + this->dataPtr->linkName = sdfClone->Get("linkName"); + } + + if (this->dataPtr->linkName.empty()) { + gzerr << "GenericMotorModel found an empty linkName parameter. " + << "Failed to initialize."; + return; + } + + if (sdfClone->HasElement("actuator_number")) { + this->dataPtr->actuatorNumber = + sdfClone->GetElement("actuator_number")->Get(); + + } else if (sdfClone->HasElement("motorNumber")) { + this->dataPtr->actuatorNumber = + sdfClone->GetElement("motorNumber")->Get(); + gzwarn << " is depricated, " + << "please use .\n"; + + } else { + gzerr << "Please specify a actuator_number.\n"; + } + + if (sdfClone->HasElement("turningDirection")) { + auto turningDirection = + sdfClone->GetElement("turningDirection")->Get(); + + if (turningDirection == "cw") { + this->dataPtr->turningDirection = turning_direction::kCw; + + } else if (turningDirection == "ccw") { + this->dataPtr->turningDirection = turning_direction::kCcw; + + } else { + gzerr << "Please only use 'cw' or 'ccw' as turningDirection.\n"; + } + + } else { + gzerr << "Please specify a turning direction ('cw' or 'ccw').\n"; + } + + if (sdfClone->HasElement("controlMethod")) { + auto controlMethod = + sdfClone->GetElement("controlMethod")->Get(); + + if (controlMethod == "rpm") { + this->dataPtr->controlMethod = ControlMethod::kRPM; + + } else if (controlMethod == "duty_cycle") { + this->dataPtr->controlMethod = ControlMethod::kDutyCycle; + + } else { + gzerr << "Please only use 'rpm' or 'duty_cycle' as controlMethod.\n"; + } + + } else { + this->dataPtr->controlMethod = ControlMethod::kRPM; + } + + // Check required parameters for each control method type + if (this->dataPtr->controlMethod == ControlMethod::kDutyCycle) { + if (sdfClone->HasElement("minCommand")) { + this->dataPtr->minCommand = + sdfClone->GetElement("minCommand")->Get(); + + } else { + gzerr << "Please specify a minCommand.\n"; + } + + if (sdfClone->HasElement("maxDutyCycle")) { + this->dataPtr->maxDutyCycle = + sdfClone->GetElement("maxDutyCycle")->Get(); + + } else { + gzerr << "Please specify a maxDutyCycle.\n"; + } + + if (sdfClone->HasElement("minDutyCycle")) { + this->dataPtr->minDutyCycle = + sdfClone->GetElement("minDutyCycle")->Get(); + + } else { + gzerr << "Please specify a minDutyCycle.\n"; + } + + } + + if (sdfClone->HasElement("motorType")) { + auto motorType = sdfClone->GetElement("motorType")->Get(); + + if (motorType == "velocity") { + this->dataPtr->motorType = MotorType::kVelocity; + + } else if (motorType == "position") { + this->dataPtr->motorType = MotorType::kPosition; + gzerr << "motorType 'position' not implemented" << std::endl; + + } else if (motorType == "force") { + this->dataPtr->motorType = MotorType::kForce; + gzerr << "motorType 'force' not implemented" << std::endl; + + } else if (motorType == "force_polynomial") { + this->dataPtr->motorType = MotorType::kForcePolynomial; + + // Check validity of required parameters + if (sdfClone->HasElement("positiveThrustPolynomial")) { + std::string raw_coeffs = sdfClone->GetElement("positiveThrustPolynomial")->Get(); + std::vector coeffs = this->ParsePolynomial(raw_coeffs); + + if (coeffs.size() < 4) { + gzerr << "positiveThrustPolynomial must have at least 4 elements, but got " + << coeffs.size() << std::endl; + } + + for (const auto &coeff : coeffs) { + this->dataPtr->positiveThrustPolynomial.push_back(coeff); + } + + } else { + gzerr << "Please specify a positiveThrustPolynomial.\n"; + } + + if (sdfClone->HasElement("negativeThrustPolynomial")) { + std::string raw_coeffs = sdfClone->GetElement("negativeThrustPolynomial")->Get(); + std::vector coeffs = this->ParsePolynomial(raw_coeffs); + + if (coeffs.size() < 4) { + gzerr << "negativeThrustPolynomial must have at least 4 elements, but got " + << coeffs.size() << std::endl; + } + + for (const auto &coeff : coeffs) { + this->dataPtr->negativeThrustPolynomial.push_back(coeff); + } + + } else { + this->dataPtr->negativeThrustPolynomial = this->dataPtr->positiveThrustPolynomial; + gzwarn << "Using positive thrust polynomial as negative thrust polynomial (symmetric performance)." << std::endl; + } + + if (sdfClone->HasElement("positiveTorquePolynomial")) { + std::string raw_coeffs = sdfClone->GetElement("positiveTorquePolynomial")->Get(); + std::vector coeffs = this->ParsePolynomial(raw_coeffs); + + if (coeffs.size() < 4 || coeffs.size() > 6) { + gzerr << "positiveTorquePolynomial must have between 4 and 6 elements, but got " + << coeffs.size() << std::endl; + } + + for (const auto &coeff : coeffs) { + this->dataPtr->positiveTorquePolynomial.push_back(coeff); + } + + } else { + gzdbg << "No positive torque polynomial. Using zero.\n" << std::endl; + this->dataPtr->positiveTorquePolynomial = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + } + + if (sdfClone->HasElement("negativeTorquePolynomial")) { + std::string raw_coeffs = sdfClone->GetElement("negativeTorquePolynomial")->Get(); + std::vector coeffs = this->ParsePolynomial(raw_coeffs); + + if (coeffs.size() < 4 || coeffs.size() > 6) { + gzerr << "negativeTorquePolynomial must have between 4 and 6 elements, but got " + << coeffs.size() << std::endl; + } + + for (const auto &coeff : coeffs) { + this->dataPtr->negativeTorquePolynomial.push_back(coeff); + } + + } else { + this->dataPtr->negativeTorquePolynomial = this->dataPtr->positiveTorquePolynomial; + gzwarn << "Using positive torque polynomial as negative torque polynomial (symmetric performance)." << std::endl; + } + + // print final polynomial values + gzdbg << "Added motor " << this->dataPtr->actuatorNumber << ". Polynomials:\n - " + << "Positive thrust polynomial: "; + + for (const auto &val : this->dataPtr->positiveThrustPolynomial) { + gzdbg << val << " "; + } + + gzdbg << "\n - Negative thrust polynomial: "; + + for (const auto &val : this->dataPtr->negativeThrustPolynomial) { + gzdbg << val << " "; + } + + gzdbg << "\n - Positive torque polynomial: "; + + for (const auto &val : this->dataPtr->positiveTorquePolynomial) { + gzdbg << val << " "; + } + + gzdbg << "\n - Negative torque polynomial: "; + + for (const auto &val : this->dataPtr->negativeTorquePolynomial) { + gzdbg << val << " "; + } + + gzdbg << std::endl; + + } else { + gzerr << "Please only use 'velocity', 'position', " + "'force' or 'force_polynomial' as motorType.\n" + << std::endl; + } + + } else { + gzwarn << "motorType not specified, using velocity.\n"; + this->dataPtr->motorType = MotorType::kVelocity; + } + + sdfClone->Get("commandSubTopic", + this->dataPtr->commandSubTopic, this->dataPtr->commandSubTopic); + + sdfClone->Get("rotorDragCoefficient", + this->dataPtr->rotorDragCoefficient, + this->dataPtr->rotorDragCoefficient); + sdfClone->Get("rollingMomentCoefficient", + this->dataPtr->rollingMomentCoefficient, + this->dataPtr->rollingMomentCoefficient); + sdfClone->Get("maxRotVelocity", + this->dataPtr->maxRotVelocity, this->dataPtr->maxRotVelocity); + sdfClone->Get("motorConstant", + this->dataPtr->motorConstant, this->dataPtr->motorConstant); + sdfClone->Get("momentConstant", + this->dataPtr->momentConstant, this->dataPtr->momentConstant); + sdfClone->Get("bidirectionalMotor", + this->dataPtr->bidirectionalMotor, this->dataPtr->bidirectionalMotor); + + sdfClone->Get("timeConstantUp", + this->dataPtr->timeConstantUp, this->dataPtr->timeConstantUp); + sdfClone->Get("timeConstantDown", + this->dataPtr->timeConstantDown, this->dataPtr->timeConstantDown); + sdfClone->Get("rotorVelocitySlowdownSim", + this->dataPtr->rotorVelocitySlowdownSim, 10); + + // Create the first order filter. + this->dataPtr->rotorVelocityFilter = + std::make_unique>( + this->dataPtr->timeConstantUp, this->dataPtr->timeConstantDown, + this->dataPtr->refMotorInput); + + // Subscribe to actuator command messages + std::string topic = transport::TopicUtils::AsValidTopic( + this->dataPtr->robotNamespace + "/" + this->dataPtr->commandSubTopic); + + if (topic.empty()) { + gzerr << "Failed to create topic for [" << this->dataPtr->robotNamespace + << "]" << std::endl; + return; + + } else { + gzdbg << "Listening to topic: " << topic << std::endl; + } + + this->dataPtr->node.Subscribe(topic, + &GenericMotorModelPrivate::OnActuatorMsg, this->dataPtr.get()); +} + +////////////////////////////////////////////////// +void GenericMotorModel::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("GenericMotorModel::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) { + gzwarn << "Detected jump back in time [" + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // If the joint or links haven't been identified yet, look for them + if (this->dataPtr->jointEntity == kNullEntity) { + this->dataPtr->jointEntity = + this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); + + const auto parentLinkName = _ecm.Component( + this->dataPtr->jointEntity); + + if (parentLinkName) { + this->dataPtr->parentLinkName = parentLinkName->Data(); + } + } + + if (this->dataPtr->linkEntity == kNullEntity) { + this->dataPtr->linkEntity = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + } + + if (this->dataPtr->parentLinkEntity == kNullEntity) { + this->dataPtr->parentLinkEntity = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->parentLinkName); + } + + if (this->dataPtr->jointEntity == kNullEntity || + this->dataPtr->linkEntity == kNullEntity || + this->dataPtr->parentLinkEntity == kNullEntity) { + return; + } + + // skip UpdateForcesAndMoments if needed components are missing + bool doUpdateForcesAndMoments = true; + + const auto jointVelocity = _ecm.Component( + this->dataPtr->jointEntity); + + if (!jointVelocity) { + _ecm.CreateComponent(this->dataPtr->jointEntity, + components::JointVelocity()); + doUpdateForcesAndMoments = false; + + } else if (jointVelocity->Data().empty()) { + doUpdateForcesAndMoments = false; + } + + if (!_ecm.Component( + this->dataPtr->jointEntity)) { + _ecm.CreateComponent(this->dataPtr->jointEntity, + components::JointVelocityCmd({0})); + doUpdateForcesAndMoments = false; + } + + if (!_ecm.Component(this->dataPtr->linkEntity)) { + _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose()); + doUpdateForcesAndMoments = false; + } + + if (!_ecm.Component( + this->dataPtr->linkEntity)) { + _ecm.CreateComponent(this->dataPtr->linkEntity, + components::WorldLinearVelocity()); + doUpdateForcesAndMoments = false; + } + + if (!_ecm.Component(this->dataPtr->parentLinkEntity)) { + _ecm.CreateComponent(this->dataPtr->parentLinkEntity, + components::WorldPose()); + doUpdateForcesAndMoments = false; + } + + // Nothing left to do if paused. + if (_info.paused) { + return; + } + + this->dataPtr->samplingTime = + std::chrono::duration(_info.dt).count(); + + if (doUpdateForcesAndMoments) { + this->dataPtr->UpdateForcesAndMoments(_ecm); + } +} + +////////////////////////////////////////////////// +void GenericMotorModelPrivate::OnActuatorMsg( + const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->recvdActuatorsMsgMutex); + this->recvdActuatorsMsg = _msg; +} + +////////////////////////////////////////////////// +void GenericMotorModelPrivate::UpdateForcesAndMoments( + EntityComponentManager &_ecm) +{ + GZ_PROFILE("GenericMotorModelPrivate::UpdateForcesAndMoments"); + + std::optional msg; + auto actuatorMsgComp = + _ecm.Component(this->model.Entity()); + + // Actuators messages can come in from transport or via a component. If a + // component is available, it takes precedence. + if (actuatorMsgComp) { + msg = actuatorMsgComp->Data(); + + } else { + std::lock_guard lock(this->recvdActuatorsMsgMutex); + + if (this->recvdActuatorsMsg.has_value()) { + msg = *this->recvdActuatorsMsg; + this->recvdActuatorsMsg.reset(); + } + } + + if (msg.has_value()) { + if (this->actuatorNumber > msg->velocity_size() - 1) { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator array which is of size " + << std::max(msg->velocity_size(), msg->normalized_size()) << std::endl; + return; + } + + if (this->motorType == MotorType::kVelocity) { + this->refMotorInput = std::min( + static_cast(msg->velocity(this->actuatorNumber)), + static_cast(this->maxRotVelocity)); + + } else if (this->motorType == MotorType::kForcePolynomial && + this->controlMethod == ControlMethod::kRPM) { + // Uses RPM as reference + this->refMotorInput = std::min( + static_cast(msg->velocity(this->actuatorNumber)), + static_cast(this->maxRotVelocity)); + + } else if (this->motorType == MotorType::kForcePolynomial && + this->controlMethod == ControlMethod::kDutyCycle && + msg->velocity(this->actuatorNumber) >= this->minDutyCycle) { + // Create normalized reference based on the duty cycle + this->refMotorInput = ((msg->velocity(this->actuatorNumber) - (this->minDutyCycle + + (this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0)) / ((this->maxDutyCycle - + this->minDutyCycle) - (this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0)); + + } else { + this->refMotorInput = msg->velocity(this->actuatorNumber); + } + } + + switch (this->motorType) { + case (MotorType::kPosition): { + // TODO: Integrate + // double err = joint_->GetAngle(0).Radian() - this->refMotorInput; + // double force = pids_.Update(err, this->samplingTime); + // joint_->SetForce(0, force); + break; + } + + case (MotorType::kForce): { + // TODO: Integrate + // joint_->SetForce(0, this->refMotorInput); + break; + } + + case (MotorType::kForcePolynomial): { + sim::Link link(this->linkEntity); + const auto worldPose = link.WorldPose(_ecm); + using Vector3 = math::Vector3d; + + // Compute thrust + double thrust = 0.0; + + if (this->refMotorInput >= this->minCommand) { + for (unsigned int i = 0; i < this->positiveThrustPolynomial.size(); i++) { + thrust += this->positiveThrustPolynomial[i] * std::pow(this->refMotorInput, i); + } + + } else if (this->refMotorInput <= -this->minCommand) { + for (unsigned int i = 0; i < this->negativeThrustPolynomial.size(); i++) { + thrust -= this->negativeThrustPolynomial[i] * std::pow(std::abs(this->refMotorInput), i); + } + + } else { + thrust = 0.0; + } + + link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, thrust))); + + // Compute torques + double torque = 0.0; + + if (this->refMotorInput >= this->minCommand) { + for (unsigned int i = 0; i < this->positiveTorquePolynomial.size(); i++) { + torque += this->positiveTorquePolynomial[i] * std::pow(this->refMotorInput, i); + } + + } else if (this->refMotorInput <= -this->minCommand) { + for (unsigned int i = 0; i < this->negativeTorquePolynomial.size(); i++) { + torque -= this->negativeTorquePolynomial[i] * std::pow(std::abs(this->refMotorInput), i); + } + + } else { + torque = 0.0; + } + + link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, torque))); + + // Set joint velocities + const auto jointVelCmd = _ecm.Component( + this->jointEntity); + *jointVelCmd = components::JointVelocityCmd({this->refMotorInput / this->rotorVelocitySlowdownSim}); + break; + } + + default: { // MotorType::kVelocity + const auto jointVelocity = _ecm.Component( + this->jointEntity); + double motorRotVel = jointVelocity->Data()[0]; + + if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime)) { + gzerr << "Aliasing on motor [" << this->actuatorNumber + << "] might occur. Consider making smaller simulation time " + "steps or raising the rotorVelocitySlowdownSim param.\n"; + } + + double realMotorVelocity = + motorRotVel * this->rotorVelocitySlowdownSim; + // Get the direction of the rotor rotation. + int realMotorVelocitySign = + (realMotorVelocity > 0) - (realMotorVelocity < 0); + // Assuming symmetric propellers (or rotors) for the thrust calculation. + double thrust = this->turningDirection * realMotorVelocitySign * + realMotorVelocity * realMotorVelocity * + this->motorConstant; + + using Pose = math::Pose3d; + using Vector3 = math::Vector3d; + + Link link(this->linkEntity); + const auto worldPose = link.WorldPose(_ecm); + + // Apply a force to the link. + link.AddWorldForce(_ecm, + worldPose->Rot().RotateVector(Vector3(0, 0, thrust))); + + const auto jointPose = _ecm.Component( + this->jointEntity); + + if (!jointPose) { + gzerr << "joint " << this->jointName << " has no Pose" + << "component" << std::endl; + return; + } + + // computer joint world pose by multiplying child link WorldPose + // with joint Pose + Pose jointWorldPose = *worldPose * jointPose->Data(); + + const auto jointAxisComp = _ecm.Component( + this->jointEntity); + + if (!jointAxisComp) { + gzerr << "joint " << this->jointName << " has no JointAxis" + << "component" << std::endl; + return; + } + + const auto worldLinearVel = link.WorldLinearVelocity(_ecm); + + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + auto windLinearVel = + _ecm.Component(windEntity); + Vector3 windSpeedWorld = windLinearVel->Data(); + + // Forces from Philppe Martin's and Erwan Salaun's + // 2010 IEEE Conference on Robotics and Automation paper + // The True Role of Accelerometer Feedback in Quadrotor Control + // - \omega * \lambda_1 * V_A^{\perp} + Vector3 jointAxis = + jointWorldPose.Rot().RotateVector(jointAxisComp->Data().Xyz()); + Vector3 bodyVelocityWorld = *worldLinearVel; + Vector3 relativeWindVelocityWorld = bodyVelocityWorld - windSpeedWorld; + Vector3 bodyVelocityPerpendicular = + relativeWindVelocityWorld - + (relativeWindVelocityWorld.Dot(jointAxis) * jointAxis); + Vector3 airDrag = -std::abs(realMotorVelocity) * + this->rotorDragCoefficient * + bodyVelocityPerpendicular; + + // Apply air drag to link. + link.AddWorldForce(_ecm, airDrag); + // Moments get the parent link, such that the resulting torques can be + // applied. + Vector3 parentWorldTorque; + auto parentWrenchComp = + _ecm.Component( + this->parentLinkEntity); + // gazebo_motor_model.cpp subtracts the GetWorldCoGPose() of the + // child link from the parent but only uses the rotation component. + // Since GetWorldCoGPose() uses the link frame orientation, it + // is equivalent to use WorldPose().Rot(). + Link parentLink(this->parentLinkEntity); + const auto parentWorldPose = parentLink.WorldPose(_ecm); + // The transformation from the parent_link to the link_. + // Pose poseDifference = + // parent_links.at(0)->GetWorldCoGPose().Inverse() + // * link_->GetWorldCoGPose() + Pose poseDifference = (*parentWorldPose).Inverse() * (*worldPose); + Vector3 dragTorque( + 0, 0, -this->turningDirection * thrust * this->momentConstant); + // Transforming the drag torque into the parent frame to handle + // arbitrary rotor orientations. + Vector3 dragTorqueParentFrame = + poseDifference.Rot().RotateVector(dragTorque); + parentWorldTorque = + parentWorldPose->Rot().RotateVector(dragTorqueParentFrame); + + Vector3 rollingMoment; + // - \omega * \mu_1 * V_A^{\perp} + rollingMoment = -std::abs(realMotorVelocity) * + this->rollingMomentCoefficient * + bodyVelocityPerpendicular; + parentWorldTorque += rollingMoment; + + if (!parentWrenchComp) { + components::ExternalWorldWrenchCmd wrench; + msgs::Set(wrench.Data().mutable_torque(), parentWorldTorque); + _ecm.CreateComponent(this->parentLinkEntity, wrench); + + } else { + msgs::Set(parentWrenchComp->Data().mutable_torque(), + msgs::Convert(parentWrenchComp->Data().torque()) + parentWorldTorque); + } + + // Apply the filter on the motor's velocity. + double refMotorRotVel; + refMotorRotVel = this->rotorVelocityFilter->UpdateFilter( + this->refMotorInput, this->samplingTime); + + const auto jointVelCmd = _ecm.Component( + this->jointEntity); + *jointVelCmd = components::JointVelocityCmd({ + this->turningDirection *refMotorRotVel + / this->rotorVelocitySlowdownSim}); + } + } +} + +GZ_ADD_PLUGIN(GenericMotorModel, + System, + GenericMotorModel::ISystemConfigure, + GenericMotorModel::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(GenericMotorModel, + "gz::sim::systems::GenericMotorModel") diff --git a/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.hpp b/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.hpp new file mode 100644 index 0000000000..25c49be661 --- /dev/null +++ b/src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_ +#define GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_ + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ +// Forward declaration +class GenericMotorModelPrivate; + +/// \brief This system applies a thrust force to models with spinning +/// propellers. See examples/worlds/quadcopter.sdf for a demonstration. +class GenericMotorModel + : public System, + public ISystemConfigure, + public ISystemPreUpdate +{ + /// \brief Constructor +public: GenericMotorModel(); + + /// \brief Destructor +public: ~GenericMotorModel() override = default; + + // Documentation inherited +public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited +public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer +private: std::unique_ptr dataPtr; +private: + std::vector ParsePolynomial(const std::string &input) + { + std::vector result; + std::string trimmed = input; + + // Optional: remove brackets + trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), '['), trimmed.end()); + trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), ']'), trimmed.end()); + + std::stringstream ss(trimmed); + std::string token; + + while (std::getline(ss, token, ',')) { + try { + result.push_back(std::stod(token)); + + } catch (const std::invalid_argument &e) { + gzerr << "[YourPlugin] Invalid number: " << token << std::endl; + } + } + + return result; + } + +}; +} +} +} +} + +#endif diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 52baccb8dd..c7186fc487 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,9 +38,12 @@ * * All the acknowledgments and credits for the fw wing/rover app are reported in those files. * + * 2025: refactoring of the mode settings: attitude, rate and manual control now working. + * * @author Daniel Duecker * @author Philipp Hastedt * @author Tim Hansen + * @author Pedro Roque */ #include "uuv_att_control.hpp" @@ -94,7 +97,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u float thrust_x, float thrust_y, float thrust_z) { if (PX4_ISFINITE(roll_u)) { - roll_u = math::constrain(roll_u, -1.0f, 1.0f); + roll_u = math::constrain(roll_u, -_param_torque_sat.get(), _param_torque_sat.get()); _vehicle_torque_setpoint.xyz[0] = roll_u; } else { @@ -102,7 +105,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u } if (PX4_ISFINITE(pitch_u)) { - pitch_u = math::constrain(pitch_u, -1.0f, 1.0f); + pitch_u = math::constrain(pitch_u, -_param_torque_sat.get(), _param_torque_sat.get()); _vehicle_torque_setpoint.xyz[1] = pitch_u; } else { @@ -110,7 +113,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u } if (PX4_ISFINITE(yaw_u)) { - yaw_u = math::constrain(yaw_u, -1.0f, 1.0f); + yaw_u = math::constrain(yaw_u, -_param_torque_sat.get(), _param_torque_sat.get()); _vehicle_torque_setpoint.xyz[2] = yaw_u; } else { @@ -118,17 +121,33 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u } if (PX4_ISFINITE(thrust_x)) { - thrust_x = math::constrain(thrust_x, -1.0f, 1.0f); + thrust_x = math::constrain(thrust_x, -_param_thrust_sat.get(), _param_thrust_sat.get()); _vehicle_thrust_setpoint.xyz[0] = thrust_x; } else { _vehicle_thrust_setpoint.xyz[0] = 0.0f; } + + if (PX4_ISFINITE(thrust_y)) { + thrust_y = math::constrain(thrust_y, -_param_thrust_sat.get(), _param_thrust_sat.get()); + _vehicle_thrust_setpoint.xyz[1] = thrust_y; + + } else { + _vehicle_thrust_setpoint.xyz[1] = 0.0f; + } + + if (PX4_ISFINITE(thrust_z)) { + thrust_z = math::constrain(thrust_z, -_param_thrust_sat.get(), _param_thrust_sat.get()); + _vehicle_thrust_setpoint.xyz[2] = thrust_z; + + } else { + _vehicle_thrust_setpoint.xyz[2] = 0.0f; + } } void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity, - const vehicle_rates_setpoint_s &rates_setpoint) + const vehicle_rates_setpoint_s &rates_setpoint, bool attitude_control_enabled) { /** Geometric Controller * @@ -156,6 +175,7 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude Vector3f e_R_vec; Vector3f torques; + torques.setZero(); /* Compute matrix: attitude error */ Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5; @@ -171,29 +191,109 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude omega(2) -= yaw_rate_desired; /**< P-Control */ - torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */ - torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */ - torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */ + if (attitude_control_enabled) { + torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */ + torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */ + torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */ - /**< PD-Control */ - torques(0) = torques(0) - omega(0) * _param_roll_d.get(); /**< Roll */ - torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */ - torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */ + // take thrust from attitude message + float thrust_x = attitude_setpoint.thrust_body[0]; + float thrust_y = attitude_setpoint.thrust_body[1]; + float thrust_z = attitude_setpoint.thrust_body[2]; - float roll_u = torques(0); - float pitch_u = torques(1); - float yaw_u = torques(2); + /**< PD-Control */ + torques(0) -= omega(0) * _param_roll_d.get(); /**< Roll */ + torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */ + torques(2) -= omega(2) * _param_yaw_d.get(); /**< Yaw */ - // take thrust as - float thrust_x = attitude_setpoint.thrust_body[0]; - float thrust_y = attitude_setpoint.thrust_body[1]; - float thrust_z = attitude_setpoint.thrust_body[2]; + float roll_u = torques(0); + float pitch_u = torques(1); + float yaw_u = torques(2); - constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); + constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); + + } else { + // take thrust from rates message + float thrust_x = _rates_setpoint.thrust_body[0]; + float thrust_y = _rates_setpoint.thrust_body[1]; + float thrust_z = _rates_setpoint.thrust_body[2]; + + /**< PD-Control */ + torques(0) -= omega(0) * _param_roll_d.get(); /**< Roll */ + torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */ + torques(2) -= omega(2) * _param_yaw_d.get(); /**< Yaw */ + + float roll_u = torques(0); + float pitch_u = torques(1); + float yaw_u = torques(2); + + + constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); + } + /* Geometric Controller END*/ } +void UUVAttitudeControl::generate_attitude_setpoint(float dt) +{ + // Avoid accumulating absolute yaw error with arming stick gesture + float roll = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).phi(); + float pitch = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).theta(); + float yaw = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).psi(); + + // Integrate manual control inputs + float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get(); + float roll_setpoint = roll + _manual_control_setpoint.roll * dt * _param_sgm_roll.get(); + float pitch_setpoint = pitch + -_manual_control_setpoint.pitch * dt * _param_sgm_pitch.get(); + + // Generate target quaternion + Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint); + Quatf q_sp = euler_sp; + + // Normalize the quaternion to avoid numerical issues + q_sp.normalize(); + + q_sp.copyTo(_attitude_setpoint.q_d); + + _attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_sgm_thrtl.get(); + + _attitude_setpoint.timestamp = hrt_absolute_time(); +} + +void UUVAttitudeControl::generate_rates_setpoint(float dt) +{ + // Integrate manual control inputs + _rates_setpoint.roll = _manual_control_setpoint.roll * dt * _param_rgm_roll.get(); + _rates_setpoint.pitch = -_manual_control_setpoint.pitch * dt * _param_rgm_pitch.get(); + _rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get(); + + _rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_rgm_thrtl.get(); + _rates_setpoint.timestamp = hrt_absolute_time(); + +} + +void UUVAttitudeControl::check_setpoint_validity(vehicle_attitude_s &v_att) +{ + const float _setpoint_age = (hrt_absolute_time() - _attitude_setpoint.timestamp) * 1e-6f; + + if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) { + reset_attitude_setpoint(v_att); + } +} + +void UUVAttitudeControl::reset_attitude_setpoint(vehicle_attitude_s &v_att) +{ + _attitude_setpoint.timestamp = hrt_absolute_time(); + _attitude_setpoint.q_d[0] = v_att.q[0]; + _attitude_setpoint.q_d[1] = v_att.q[1]; + _attitude_setpoint.q_d[2] = v_att.q[2]; + _attitude_setpoint.q_d[3] = v_att.q[3]; + _attitude_setpoint.thrust_body[0] = 0.f; + _attitude_setpoint.thrust_body[1] = 0.f; + _attitude_setpoint.thrust_body[2] = 0.f; +} + void UUVAttitudeControl::Run() { if (should_exit()) { @@ -214,65 +314,82 @@ void UUVAttitudeControl::Run() /* only run controller if attitude changed */ if (_vehicle_attitude_sub.update(&attitude)) { + const float dt = math::constrain(((attitude.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = attitude.timestamp_sample; + vehicle_angular_velocity_s angular_velocity {}; _angular_velocity_sub.copy(&angular_velocity); - /* Run geometric attitude controllers if NOT manual mode*/ - if (!_vcontrol_mode.flag_control_manual_enabled - && _vcontrol_mode.flag_control_attitude_enabled - && _vcontrol_mode.flag_control_rates_enabled) { + /* Check that we are not in position / velocity / altitude modes + and that we are using manual inputs */ + if (_vcontrol_mode.flag_control_manual_enabled + && !_vcontrol_mode.flag_control_position_enabled + && !_vcontrol_mode.flag_control_velocity_enabled + && !_vcontrol_mode.flag_control_altitude_enabled) { - int input_mode = _param_input_mode.get(); + /* Update manual setpoints */ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); - _vehicle_attitude_setpoint_sub.update(&_attitude_setpoint); - _vehicle_rates_setpoint_sub.update(&_rates_setpoint); + if (_vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + /* Run stabilized mode */ + _vehicle_rates_setpoint_sub.update(&_rates_setpoint); - if (input_mode == 1) { // process manual data - Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get())); - attitude_setpoint.copyTo(_attitude_setpoint.q_d); - _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); - _attitude_setpoint.thrust_body[1] = 0.f; - _attitude_setpoint.thrust_body[2] = 0.f; + // Check setpoint validty + check_setpoint_validity(attitude); + + /* Generate atttiude setpoint from sticks */ + generate_attitude_setpoint(dt); + + control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true); + + } else if (!_vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + /* Run Rate mode */ + generate_rates_setpoint(dt); + + control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false); + + } else if (!_vcontrol_mode.flag_control_attitude_enabled + && !_vcontrol_mode.flag_control_rates_enabled) { + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + constrain_actuator_commands(_manual_control_setpoint.roll * _param_mgm_roll.get(), + -_manual_control_setpoint.pitch * _param_mgm_pitch.get(), + _manual_control_setpoint.yaw * _param_mgm_yaw.get(), + _manual_control_setpoint.throttle * _param_mgm_thrtl.get(), + 0.f, + 0.f); } - /* Geometric Control*/ - int skip_controller = _param_skip_ctrl.get(); - - if (skip_controller) { - constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw, - _rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]); + } else { + if (_vcontrol_mode.flag_control_attitude_enabled) { + /* Get attitude and rate setpoints and control system */ + _vehicle_attitude_setpoint_sub.update(&_attitude_setpoint); + _vehicle_rates_setpoint_sub.update(&_rates_setpoint); + control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true); } else { - control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint); + /* Get rate setpoints and control system */ + _vehicle_rates_setpoint_sub.update(&_rates_setpoint); + control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false); } } - } - /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ - if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { - // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep - // returning immediately and this loop will eat up resources. - if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { - /* manual/direct control */ - constrain_actuator_commands(_manual_control_setpoint.roll, -_manual_control_setpoint.pitch, - _manual_control_setpoint.yaw, - _manual_control_setpoint.throttle, 0.f, 0.f); + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + + _vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint.timestamp_sample = 0.f; + _vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint); + + _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint.timestamp_sample = 0.f; + _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); } } - /* Only publish if any of the proper modes are enabled */ - if (_vcontrol_mode.flag_control_manual_enabled || - _vcontrol_mode.flag_control_attitude_enabled) { - - _vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint.timestamp_sample = 0.f; - _vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint); - - _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint.timestamp_sample = 0.f; - _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); - } - perf_end(_loop_perf); } diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index 7559ef4701..76588848a3 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -75,7 +75,10 @@ using matrix::Eulerf; using matrix::Quatf; using matrix::Matrix3f; using matrix::Vector3f; +using matrix::Vector2f; using matrix::Dcmf; +using matrix::AxisAnglef; +using matrix::AxisAngle; using uORB::SubscriptionData; @@ -122,6 +125,7 @@ private: vehicle_control_mode_s _vcontrol_mode{}; perf_counter_t _loop_perf; + hrt_abstime _last_run{0}; DEFINE_PARAMETERS( (ParamFloat) _param_roll_p, @@ -130,14 +134,23 @@ private: (ParamFloat) _param_pitch_d, (ParamFloat) _param_yaw_p, (ParamFloat) _param_yaw_d, - // control/input modes - (ParamInt) _param_input_mode, - (ParamInt) _param_skip_ctrl, - // direct access to inputs - (ParamFloat) _param_direct_roll, - (ParamFloat) _param_direct_pitch, - (ParamFloat) _param_direct_yaw, - (ParamFloat) _param_direct_thrust + // gains for the different modes + (ParamFloat) _param_sgm_roll, + (ParamFloat) _param_sgm_pitch, + (ParamFloat) _param_sgm_yaw, + (ParamFloat) _param_sgm_thrtl, + (ParamFloat) _param_rgm_roll, + (ParamFloat) _param_rgm_pitch, + (ParamFloat) _param_rgm_yaw, + (ParamFloat) _param_rgm_thrtl, + (ParamFloat) _param_mgm_roll, + (ParamFloat) _param_mgm_pitch, + (ParamFloat) _param_mgm_yaw, + (ParamFloat) _param_mgm_thrtl, + (ParamFloat) _param_torque_sat, + (ParamFloat) _param_thrust_sat, + (ParamFloat) _param_setpoint_max_age + ) void Run() override; @@ -150,7 +163,12 @@ private: * Control Attitude */ void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint, - const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint); + const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint, + bool attitude_control_enabled); void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_x, float thrust_y, float thrust_z); + void generate_attitude_setpoint(float dt); + void generate_rates_setpoint(float dt); + void reset_attitude_setpoint(vehicle_attitude_s &v_att); + void check_setpoint_validity(vehicle_attitude_s &v_att); }; diff --git a/src/modules/uuv_att_control/uuv_att_control_params.c b/src/modules/uuv_att_control/uuv_att_control_params.c index fdad0b3d88..08dea0ef41 100644 --- a/src/modules/uuv_att_control/uuv_att_control_params.c +++ b/src/modules/uuv_att_control/uuv_att_control_params.c @@ -42,6 +42,7 @@ * All the ackowledgments and credits for the fw wing/rover app are reported in those files. * * @author Daniel Duecker + * @author Pedro Roque */ /* @@ -103,48 +104,138 @@ PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f); PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f); -// Input Modes +// Gains for Manual Inputs in different Modes /** - * Select Input Mode + * Roll gain for manual inputs in attitude control mode * - * @value 0 use Attitude Setpoints - * @value 1 Direct Feedthrough * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 */ -PARAM_DEFINE_INT32(UUV_INPUT_MODE, 0); +PARAM_DEFINE_FLOAT(UUV_SGM_ROLL, 0.5f); /** - * Skip the controller + * Pitch gain for manual inputs in attitude control mode * - * @value 0 use the module's controller - * @value 1 skip the controller and feedthrough the setpoints + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 */ -PARAM_DEFINE_INT32(UUV_SKIP_CTRL, 0); +PARAM_DEFINE_FLOAT(UUV_SGM_PITCH, 0.5f); /** - * Direct roll input + * Yaw gain for manual inputs in attitude control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_SGM_YAW, 0.5f); + +/** + * Throttle gain for manual inputs in attitude control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_SGM_THRTL, 0.1f); + +/** + * Roll gain for manual inputs in rate control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_RGM_ROLL, 100.0f); + +/** + * Pitch gain for manual inputs in rate control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_RGM_PITCH, 100.0f); + +/** + * Yaw gain for manual inputs in rate control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_RGM_YAW, 100.0f); + +/** + * Throttle gain for manual inputs in rate control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_RGM_THRTL, 10.0f); + +/** + * Roll gain for manual inputs in manual control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_MGM_ROLL, 0.05f); + +/** + * Pitch gain for manual inputs in manual control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_MGM_PITCH, 0.05f); + +/** + * Yaw gain for manual inputs in manual control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_MGM_YAW, 0.05f); + +/** + * Throttle gain for manual inputs in manual control mode + * + * @group UUV Attitude Control + * @min 0.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_MGM_THRTL, 0.1f); + +/** + * UUV Torque setpoint Saturation + * + * @group UUV Attitude Control + * @min 0.0 + * @max 1.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_TORQUE_SAT, 0.3f); + +/** + * UUV Thrust setpoint Saturation + * + * @group UUV Attitude Control + * @min 0.0 + * @max 1.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_THRUST_SAT, 0.1f); + +/** + * Maximum time (in seconds) before resetting setpoint * * @group UUV Attitude Control */ -PARAM_DEFINE_FLOAT(UUV_DIRCT_ROLL, 0.0f); - -/** - * Direct pitch input - * - * @group UUV Attitude Control - */ -PARAM_DEFINE_FLOAT(UUV_DIRCT_PITCH, 0.0f); - -/** - * Direct yaw input - * - * @group UUV Attitude Control - */ -PARAM_DEFINE_FLOAT(UUV_DIRCT_YAW, 0.0f); - -/** - * Direct thrust input - * - * @group UUV Attitude Control - */ -PARAM_DEFINE_FLOAT(UUV_DIRCT_THRUST, 0.0f); +PARAM_DEFINE_FLOAT(UUV_SP_MAX_AGE, 2.0f); diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index 272ea56def..109563c52d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -90,61 +90,172 @@ void UUVPOSControl::parameters_update(bool force) } } -void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, - const float roll_des, const float pitch_des, const float yaw_des) -{ - //watch if inputs are not to high - vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; - vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); - - const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des)); - attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d); - - vehicle_attitude_setpoint.thrust_body[0] = thrust_x; - vehicle_attitude_setpoint.thrust_body[1] = thrust_y; - vehicle_attitude_setpoint.thrust_body[2] = thrust_z; - - - _att_sp_pub.publish(vehicle_attitude_setpoint); -} - -void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des, - const float roll_des, const float pitch_des, const float yaw_des, - vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude, + vehicle_local_position_s &vlocal_pos, bool altitude_mode) { //get current rotation of vehicle Quatf q_att(vehicle_attitude.q); + // Assumes target 0 velocity Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get() * vlocal_pos.vx, _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy, _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz); - Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body) + if (altitude_mode) { + // In altitude mode, we only control the z-axis + p_control_output(0) = 0.0f; + p_control_output(1) = 0.0f; + } - publish_attitude_setpoint(rotated_input(0), - rotated_input(1), - rotated_input(2), - roll_des, pitch_des, yaw_des); + Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output); //rotate the coord.sys (from global to body) + _attitude_setpoint.timestamp = hrt_absolute_time(); + _attitude_setpoint.q_d[0] = _trajectory_setpoint.quaternion[0]; + _attitude_setpoint.q_d[1] = _trajectory_setpoint.quaternion[1]; + _attitude_setpoint.q_d[2] = _trajectory_setpoint.quaternion[2]; + _attitude_setpoint.q_d[3] = _trajectory_setpoint.quaternion[3]; + _attitude_setpoint.thrust_body[0] = rotated_input(0); + _attitude_setpoint.thrust_body[1] = rotated_input(1); + _attitude_setpoint.thrust_body[2] = rotated_input(2); } -void UUVPOSControl::stabilization_controller_6dof(const Vector3f &pos_des, - const float roll_des, const float pitch_des, const float yaw_des, - vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +void UUVPOSControl::check_setpoint_validity(vehicle_local_position_s &vlocal_pos) { - //get current rotation of vehicle + const float _setpoint_age = (hrt_absolute_time() - _trajectory_setpoint.timestamp) * 1e-6f; + + if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) { + reset_trajectory_setpoint(vlocal_pos); + } + + if (!PX4_ISFINITE(_trajectory_setpoint.position[0]) || + !PX4_ISFINITE(_trajectory_setpoint.position[1]) || + !PX4_ISFINITE(_trajectory_setpoint.position[2]) || + !PX4_ISFINITE(_trajectory_setpoint.quaternion[0]) || + !PX4_ISFINITE(_trajectory_setpoint.quaternion[1]) || + !PX4_ISFINITE(_trajectory_setpoint.quaternion[2]) || + !PX4_ISFINITE(_trajectory_setpoint.quaternion[3])) { + reset_trajectory_setpoint(vlocal_pos); + } +} + +void UUVPOSControl::generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos, + vehicle_attitude_s &vehicle_attitude, + float dt) +{ + float roll = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).phi(); + float pitch = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).theta(); + float yaw = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).psi(); + + // Integrate manual control inputs + // Info: + // - throttle is Z, roll is Y, pitch is X + // - if param_stab_mode == 1: + // - roll = 0 + // - pitch = 0 + // - if param_stab_mode == 0: + // - roll can be updated with D-pad (joystick) + // - pitch can be updated with D-pad (joystick) + float roll_setpoint = roll; + float pitch_setpoint = pitch; + + if (_param_stab_mode.get()) { + roll_setpoint = 0.0; + pitch_setpoint = 0.0; + + } else { + // Update target roll and pitch setpoint with D-pad + switch (_manual_control_setpoint.buttons) { + case 2048: + pitch_setpoint -= dt * _param_sgm_pitch.get(); + break; + + case 4096: + pitch_setpoint += dt * _param_sgm_pitch.get(); + break; + + case 8192: + roll_setpoint -= dt * _param_sgm_roll.get(); + break; + + case 16384: + roll_setpoint += dt * _param_sgm_roll.get(); + break; + + default: + break; + } + } + + float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get(); + + // Update position setpoints based on manual control inputs + float vx_sp = 0.0; + + if (_manual_control_setpoint.pitch > _param_pos_stick_db.get() + || _manual_control_setpoint.pitch < -_param_pos_stick_db.get()) { + // If pitch is not zero, we use it to set the roll setpoint + vx_sp = _manual_control_setpoint.pitch * _param_pgm_vel.get(); + } + + float vy_sp = 0.0; + + if (_manual_control_setpoint.roll > _param_pos_stick_db.get() + || _manual_control_setpoint.roll < -_param_pos_stick_db.get()) { + // If roll is not zero, we use it to set the pitch setpoint + vy_sp = _manual_control_setpoint.roll * _param_pgm_vel.get(); + } + + float vz_sp = 0.0; + + if (_manual_control_setpoint.throttle > _param_pos_stick_db.get() + || _manual_control_setpoint.throttle < -_param_pos_stick_db.get()) { + // If throttle is not zero, we use it to set the vertical velocity + vz_sp = -_manual_control_setpoint.throttle * _param_pgm_vel.get(); + } + + // rotate velocity setpoint in body frame to global frame + Vector3f velocity_setpoint(vx_sp, vy_sp, vz_sp); Quatf q_att(vehicle_attitude.q); + Vector3f rotated_velocity_setpoint = q_att.rotateVector(velocity_setpoint); - Vector3f p_control_output = Vector3f(0, - 0, - _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z)); - //potential d controller missing - Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body) + // Generate target quaternion + Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint); + Quatf q_sp = euler_sp; - publish_attitude_setpoint(rotated_input(0) + pos_des(0), rotated_input(1) + pos_des(1), rotated_input(2), - roll_des, pitch_des, yaw_des); + // Normalize the quaternion to avoid numerical issues + q_sp.normalize(); + q_sp.copyTo(_trajectory_setpoint.quaternion); + + _trajectory_setpoint.timestamp = hrt_absolute_time(); + + if (!_param_pos_mode.get()) { + _trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + vx_sp * dt; // X in world frame + _trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + vy_sp * dt; // Y in world frame + _trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + vz_sp * dt; // Z in world frame + + } else { + _trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + rotated_velocity_setpoint( + 0) * dt; // X in body frame + _trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + rotated_velocity_setpoint( + 1) * dt; // Y in body frame + _trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + rotated_velocity_setpoint( + 2) * dt; // Z in body frame + } +} + +void UUVPOSControl::reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos) +{ + // Reset trajectory setpoint to current position and attitude + _trajectory_setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint.position[0] = vlocal_pos.x; + _trajectory_setpoint.position[1] = vlocal_pos.y; + _trajectory_setpoint.position[2] = vlocal_pos.z; + _trajectory_setpoint.quaternion[0] = _vehicle_attitude.q[0]; + _trajectory_setpoint.quaternion[1] = _vehicle_attitude.q[1]; + _trajectory_setpoint.quaternion[2] = _vehicle_attitude.q[2]; + _trajectory_setpoint.quaternion[3] = _vehicle_attitude.q[3]; } void UUVPOSControl::Run() @@ -160,7 +271,6 @@ void UUVPOSControl::Run() /* check vehicle control mode for changes to publication state */ _vcontrol_mode_sub.update(&_vcontrol_mode); - /* update parameters from storage */ parameters_update(); @@ -169,34 +279,58 @@ void UUVPOSControl::Run() /* only run controller if local_pos changed */ if (_vehicle_local_position_sub.update(&vlocal_pos)) { + const float dt = math::constrain(((vlocal_pos.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = vlocal_pos.timestamp_sample; - /* Run geometric attitude controllers if NOT manual mode*/ - if (!_vcontrol_mode.flag_control_manual_enabled - && _vcontrol_mode.flag_control_attitude_enabled - && _vcontrol_mode.flag_control_rates_enabled) { + // Update vehicle attitude + _vehicle_attitude_sub.update(&_vehicle_attitude); - _vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude + /* Run position or altitude mode from manual setpoints*/ + if (_vcontrol_mode.flag_control_manual_enabled + && (_vcontrol_mode.flag_control_altitude_enabled + || _vcontrol_mode.flag_control_position_enabled) + && _vcontrol_mode.flag_armed) { + /* Update manual setpoints */ + + const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled + && ! _vcontrol_mode.flag_control_position_enabled; + + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + + // Ensure no nan and sufficiently recent setpoint + check_setpoint_validity(vlocal_pos); + + // Generate _trajectory_setpoint -> creates _trajectory_setpoint + generate_trajectory_setpoint(vlocal_pos, _vehicle_attitude, dt); + + pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude, + vlocal_pos, altitude_only_flag); + + } else if (!_vcontrol_mode.flag_control_manual_enabled + && (_vcontrol_mode.flag_control_altitude_enabled + || _vcontrol_mode.flag_control_position_enabled) + && _vcontrol_mode.flag_armed) { + /* Autonomous position mode - no manual inputs are used */ + const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled + && ! _vcontrol_mode.flag_control_position_enabled; + + // get manual control setpoint _trajectory_setpoint_sub.update(&_trajectory_setpoint); - float roll_des = 0; - float pitch_des = 0; - float yaw_des = _trajectory_setpoint.yaw; + pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude, + vlocal_pos, altitude_only_flag); - //stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw) - if (_param_stabilization.get() == 0) { - pose_controller_6dof(Vector3f(_trajectory_setpoint.position), - roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); - - } else { - stabilization_controller_6dof(Vector3f(_trajectory_setpoint.position), - roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); - } + } else { + // Reset if not in a valid mode (like attitude, rate, manual) to clear setpoint + check_setpoint_validity(vlocal_pos); } } /* Only publish if any of the proper modes are enabled */ - if (_vcontrol_mode.flag_control_manual_enabled || - _vcontrol_mode.flag_control_attitude_enabled) { + if (_vcontrol_mode.flag_control_position_enabled || + _vcontrol_mode.flag_control_altitude_enabled) { + // Print attitude setpoint + _att_sp_pub.publish(_attitude_setpoint); } perf_end(_loop_perf); diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index 192365a98d..4d2d4944df 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -59,8 +59,9 @@ #include #include #include +#include #include -#include +#include #include #include #include @@ -101,16 +102,20 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; vehicle_attitude_s _vehicle_attitude{}; - trajectory_setpoint_s _trajectory_setpoint{}; + trajectory_setpoint6dof_s _trajectory_setpoint{}; + manual_control_setpoint_s _manual_control_setpoint{}; vehicle_control_mode_s _vcontrol_mode{}; + vehicle_attitude_setpoint_s _attitude_setpoint{}; perf_counter_t _loop_perf; + hrt_abstime _last_run{0}; DEFINE_PARAMETERS( (ParamFloat) _param_pose_gain_x, @@ -119,10 +124,14 @@ private: (ParamFloat) _param_pose_gain_d_x, (ParamFloat) _param_pose_gain_d_y, (ParamFloat) _param_pose_gain_d_z, - - (ParamInt) _param_input_mode, - (ParamInt) _param_stabilization, - (ParamInt) _param_skip_ctrl + (ParamInt) _param_stab_mode, + (ParamFloat) _param_pos_stick_db, + (ParamFloat) _param_pgm_vel, + (ParamFloat) _param_sgm_roll, + (ParamFloat) _param_sgm_pitch, + (ParamFloat) _param_sgm_yaw, + (ParamFloat) _param_setpoint_max_age, + (ParamInt) _param_pos_mode ) void Run() override; @@ -136,10 +145,12 @@ private: */ void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, const float roll_des, const float pitch_des, const float yaw_des); - void pose_controller_6dof(const Vector3f &pos_des, - const float roll_des, const float pitch_des, const float yaw_des, - vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); + void pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude, + vehicle_local_position_s &vlocal_pos, bool altitude_mode); void stabilization_controller_6dof(const Vector3f &pos_des, const float roll_des, const float pitch_des, const float yaw_des, vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); + void generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos, vehicle_attitude_s &vehicle_attitude, float dt); + void reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos); + void check_setpoint_validity(vehicle_local_position_s &vlocal_pos); }; diff --git a/src/modules/uuv_pos_control/uuv_pos_control_params.c b/src/modules/uuv_pos_control/uuv_pos_control_params.c index d3532edd43..c5b0705d74 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control_params.c +++ b/src/modules/uuv_pos_control/uuv_pos_control_params.c @@ -90,8 +90,31 @@ PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f); /** * Stabilization mode(1) or Position Control(0) * - * @value 0 Position Control - * @value 1 Stabilization Mode + * @value 0 Tracks previous attitude setpoint + * @value 1 Tracks horizontal attitude (allows yaw change) * @group UUV Position Control */ PARAM_DEFINE_INT32(UUV_STAB_MODE, 1); + +/** + * Deadband for changing position setpoint + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_POS_STICK_DB, 0.1f); + +/** + * Gain for position control velocity setpoint update + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_PGM_VEL, 0.5f); + +/** + * Stabilization mode(1) or Position Control(0) + * + * @value 0 Moves position setpoint in world frame + * @value 1 Moves position setpoint in body frame + * @group UUV Position Control + */ +PARAM_DEFINE_INT32(UUV_POS_MODE, 1);