From 03c19a1633344b916a03ae8d64dbb8bc020973b6 Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 5 Nov 2019 16:36:53 -0800 Subject: [PATCH] [simpleflight] add / fix unintuitive lower level APIs - RPYZ|Throttle, AngleRateZ|Throttle --- .../multirotor/api/MultirotorApiBase.hpp | 24 ++- .../multirotor/api/MultirotorRpcLibClient.hpp | 8 +- .../mavlink/MavLinkMultirotorApi.hpp | 42 +++- .../simple_flight/SimpleFlightApi.hpp | 64 +++++- .../multirotor/api/MultirotorApiBase.cpp | 92 +++++++- .../multirotor/api/MultirotorRpcLibClient.cpp | 32 ++- .../multirotor/api/MultirotorRpcLibServer.cpp | 30 ++- DroneShell/src/main.cpp | 16 +- PythonClient/airsim/client.py | 198 +++++++++++++++++- 9 files changed, 460 insertions(+), 46 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index b42473f9ca..97fe42ae05 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -24,8 +24,12 @@ class MultirotorApiBase : public VehicleApiBase { protected: //must be implemented /************************* low level move APIs *********************************/ - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; @@ -81,8 +85,12 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); - virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); - virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, @@ -127,11 +135,15 @@ class MultirotorApiBase : public VehicleApiBase { typedef std::function WaitFunction; //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); - virtual void moveByRollPitchZInternal(float pitch, float roll, float z, float yaw); - virtual void moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate); /************* safety checks & emergency maneuvers ************/ virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index b90177e2ff..bc832b5bbc 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -23,8 +23,12 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 363d6f7dfb..64e107c2e8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -473,7 +473,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase } protected: //methods - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { if (target_height_ != -z) { // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best @@ -487,11 +487,47 @@ class MavLinkMultirotorApi : public MultirotorApiBase float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } checkValidVehicle(); - mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust); } + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + checkValidVehicle(); + // todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale? + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle); + } + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + checkValidVehicle(); + // todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale? + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle); + } + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust); + } + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + checkValidVehicle(); + // todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale? + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle); + } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { checkValidVehicle(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 71dfa99105..ca619faf06 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -195,9 +195,35 @@ class SimpleFlightApi : public MultirotorApiBase { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { - //Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + //Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll, pitch, yaw, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); @@ -208,14 +234,40 @@ class SimpleFlightApi : public MultirotorApiBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override { - //Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); - simple_flight::Axis4r goal(roll, pitch, yaw, z); + simple_flight::Axis4r goal(roll, pitch, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); std::string message; firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index be32d215fd..75dc37e9dd 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -72,7 +72,7 @@ bool MultirotorApiBase::goHome(float timeout_sec) return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); } -bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) +bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) { SingleTaskCall lock(this); @@ -80,12 +80,12 @@ bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw return true; return waitForFunction([&]() { - moveByRollPitchZInternal(pitch, roll, z, yaw); + moveByRollPitchYawZInternal(roll, pitch, yaw, z); return false; //keep moving until timeout }, duration).isTimeout(); } -bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) +bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) { SingleTaskCall lock(this); @@ -93,7 +93,59 @@ bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throt return true; return waitForFunction([&]() { - moveByRollPitchThrottleInternal(pitch, roll, throttle, yaw_rate); + moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); return false; //keep moving until timeout }, duration).isTimeout(); } @@ -451,16 +503,40 @@ void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMo commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); } -void MultirotorApiBase::moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate) +void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawZ(roll, pitch, yaw, z); +} + +void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawThrottle(roll, pitch, yaw, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); +} + +void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); + commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); } -void MultirotorApiBase::moveByRollPitchZInternal(float pitch, float roll, float z, float yaw) +void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchZ(pitch, roll, z, yaw); + commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); } //executes a given function until it returns true. Each execution is spaced apart at command period. diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 01f17fcd52..7e2cf9869b 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -75,15 +75,39 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, c return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleZ", pitch, roll, z, yaw, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); return this; } diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 7051bc58da..edd460b77a 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -53,14 +53,34 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->goHome(timeout_sec); }); - (static_cast(getServer()))-> - bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) -> - bool { return getVehicleApi(vehicle_name)->moveByAngleZ(pitch, roll, z, yaw, duration); }); + bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> + bool { return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); + }); (static_cast(getServer()))-> - bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration, + bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); + return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 791ae73bf7..b6ca98d556 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -505,7 +505,7 @@ class MoveByAngleZCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration); }); return false; @@ -534,7 +534,7 @@ class MoveByAngleThrottleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleThrottleAsync(pitch, roll, throttle, yaw_rate, duration); + context->client.moveByRollPitchYawrateThrottleAsync(roll, pitch, yaw_rate, throttle, duration); }); return false; @@ -693,13 +693,13 @@ class BackForthByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration); if (!context->client.waitOnLastTask()) { throw std::runtime_error("BackForthByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, duration); if (!context->client.waitOnLastTask()){ throw std::runtime_error("BackForthByAngleCommand canceled"); } @@ -782,28 +782,28 @@ class SquareByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(pitch, -roll, z, yaw, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-pitch, roll, z, yaw, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0); if (!context->client.waitOnLastTask()){ throw std::runtime_error("SquareByAngleCommand canceled"); } diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index d2aebcec47..2cd647a271 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -247,10 +247,200 @@ def goHome(self): raise Exception("goHome API is deprecated. Please use goHomeAsync() API." + self.upgrade_api_help) def hover(self): raise Exception("hover API is deprecated. Please use hoverAsync() API." + self.upgrade_api_help) - def moveByAngleZ(self, pitch, roll, z, yaw, duration): - raise Exception("moveByAngleZ API is deprecated. Please use moveByAngleZAsync() API." + self.upgrade_api_help) - def moveByAngleThrottle(self, pitch, roll, throttle, yaw_rate, duration): - raise Exception("moveByAngleThrottle API is deprecated. Please use moveByAngleThrottleAsync() API." + self.upgrade_api_help) + + # low-level control API + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw angle are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name) + def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): raise Exception("moveByVelocity API is deprecated. Please use moveByVelocityAsync() API." + self.upgrade_api_help) def moveByVelocityZ(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()):