From 9b98cb34ea74235eaa0b3a71c6b3751ffc1dd48b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 28 May 2015 18:12:46 -0700 Subject: [PATCH] Plugin to make Cessna fly and twweaks in the CessnaGUIPlugin. --- gazebo/msgs/cessna.proto | 35 ++++- plugins/CessnaGUIPlugin.cc | 253 ++++++++++++++++++++++++++++++------- plugins/CessnaGUIPlugin.hh | 40 ++++-- plugins/CessnaPlugin.cc | 49 ++++--- plugins/LiftDragPlugin.cc | 119 +++++++++++------ plugins/LiftDragPlugin.hh | 31 ++++- worlds/cessna_demo.world | 129 ++++++++++++++++++- 7 files changed, 533 insertions(+), 123 deletions(-) diff --git a/gazebo/msgs/cessna.proto b/gazebo/msgs/cessna.proto index e96ac9da9d..886bf1ae56 100644 --- a/gazebo/msgs/cessna.proto +++ b/gazebo/msgs/cessna.proto @@ -6,24 +6,45 @@ package gazebo.msgs; message Cessna { - /// \brief Target RPM of the propeller. + /// \brief Current RPM of the propeller. optional float propeller_speed = 1; - /// \brief Target left aileron angle in rads. + /// \brief Current left aileron angle in rads. optional float left_aileron = 2; - /// \brief Target left flap angle in rads. + /// \brief Current left flap angle in rads. optional float left_flap = 3; - /// \brief Target right aileron angle in rads. + /// \brief Current right aileron angle in rads. optional float right_aileron = 4; - /// \brief Target right flap angle in rads. + /// \brief Current right flap angle in rads. optional float right_flap = 5; - /// \brief Target elevators angle in rads. + /// \brief Current elevators angle in rads. optional float elevators = 6; - /// \brief Target ruddle angle in rads. + /// \brief Current ruddle angle in rads. optional float rudder = 7; + + /// \brief Target RPM of the propeller. + optional float cmd_propeller_speed = 8; + + /// \brief Target left aileron angle in rads. + optional float cmd_left_aileron = 9; + + /// \brief Target left flap angle in rads. + optional float cmd_left_flap = 10; + + /// \brief Target right aileron angle in rads. + optional float cmd_right_aileron = 11; + + /// \brief Target right flap angle in rads. + optional float cmd_right_flap = 12; + + /// \brief Target elevators angle in rads. + optional float cmd_elevators = 13; + + /// \brief Target ruddle angle in rads. + optional float cmd_rudder = 14; } diff --git a/plugins/CessnaGUIPlugin.cc b/plugins/CessnaGUIPlugin.cc index 26db329ec4..4e61828f16 100644 --- a/plugins/CessnaGUIPlugin.cc +++ b/plugins/CessnaGUIPlugin.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include #include "CessnaGUIPlugin.hh" @@ -40,39 +41,62 @@ CessnaGUIPlugin::CessnaGUIPlugin() this->gzNode->Init(); this->controlPub = this->gzNode->Advertise("~/cessna_c172/control"); + this->stateSub = this->gzNode->Subscribe( + "~/cessna_c172/control", &CessnaGUIPlugin::OnState, this); - // Connect hotkeys - QShortcut *increaseThrust = new QShortcut(QKeySequence("a"), this); + // Connect hotkeys. + QShortcut *increaseThrust = new QShortcut(QKeySequence("w"), this); QObject::connect(increaseThrust, SIGNAL(activated()), this, SLOT(OnIncreaseThrust())); - QShortcut *decreaseThrust = new QShortcut(QKeySequence("z"), this); + QShortcut *decreaseThrust = new QShortcut(QKeySequence("s"), this); QObject::connect(decreaseThrust, SIGNAL(activated()), this, SLOT(OnDecreaseThrust())); - QShortcut *increaseFlaps = new QShortcut(QKeySequence("s"), this); + QShortcut *increaseFlaps = new QShortcut(QKeySequence("g"), this); QObject::connect(increaseFlaps, SIGNAL(activated()), this, SLOT(OnIncreaseFlaps())); - QShortcut *decreaseFlaps = new QShortcut(QKeySequence("x"), this); + QShortcut *decreaseFlaps = new QShortcut(QKeySequence("b"), this); QObject::connect(decreaseFlaps, SIGNAL(activated()), this, SLOT(OnDecreaseFlaps())); - QShortcut *increaseElevators = new QShortcut(QKeySequence("d"), this); + QShortcut *increaseRoll = new QShortcut(QKeySequence(Qt::Key_Left), this); + QObject::connect(increaseRoll, SIGNAL(activated()), this, + SLOT(OnIncreaseRoll())); + + QShortcut *decreaseRoll = new QShortcut(QKeySequence(Qt::Key_Right), this); + QObject::connect(decreaseRoll, SIGNAL(activated()), this, + SLOT(OnDecreaseRoll())); + + QShortcut *increaseElevators = + new QShortcut(QKeySequence(Qt::Key_Down), this); QObject::connect(increaseElevators, SIGNAL(activated()), this, SLOT(OnIncreaseElevators())); - QShortcut *decreaseElevators = new QShortcut(QKeySequence("c"), this); + QShortcut *decreaseElevators = new QShortcut(QKeySequence(Qt::Key_Up), this); QObject::connect(decreaseElevators, SIGNAL(activated()), this, SLOT(OnDecreaseElevators())); - QShortcut *increaseRudder = new QShortcut(QKeySequence("f"), this); + QShortcut *increaseRudder = new QShortcut(QKeySequence("a"), this); QObject::connect(increaseRudder, SIGNAL(activated()), this, SLOT(OnIncreaseRudder())); - QShortcut *decreaseRudder = new QShortcut(QKeySequence("v"), this); + QShortcut *decreaseRudder = new QShortcut(QKeySequence("d"), this); QObject::connect(decreaseRudder, SIGNAL(activated()), this, SLOT(OnDecreaseRudder())); + + QShortcut *presetTakeOff = new QShortcut(QKeySequence('1'), this); + QObject::connect(presetTakeOff, SIGNAL(activated()), this, + SLOT(OnPresetTakeOff())); + + QShortcut *presetCruise = new QShortcut(QKeySequence('2'), this); + QObject::connect(presetCruise, SIGNAL(activated()), this, + SLOT(OnPresetCruise())); + + QShortcut *presetLanding = new QShortcut(QKeySequence('3'), this); + QObject::connect(presetLanding, SIGNAL(activated()), this, + SLOT(OnPresetLanding())); } ///////////////////////////////////////////////// @@ -80,92 +104,231 @@ CessnaGUIPlugin::~CessnaGUIPlugin() { } +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnState(ConstCessnaPtr &_msg) +{ + std::lock_guard lock(this->mutex); + + // Refresh the state. + this->state = *_msg; +} + ///////////////////////////////////////////////// void CessnaGUIPlugin::OnIncreaseThrust() { + float thrust; + { + std::lock_guard lock(this->mutex); + thrust = this->state.cmd_propeller_speed(); + } + msgs::Cessna msg; - this->targetThrust = std::min(this->targetThrust + 0.01f, 1.0f); - msg.set_propeller_speed(this->targetThrust); + thrust = std::min(thrust + 0.1f, 1.0f); + msg.set_cmd_propeller_speed(thrust); this->controlPub->Publish(msg); } ///////////////////////////////////////////////// void CessnaGUIPlugin::OnDecreaseThrust() { + float thrust; + { + std::lock_guard lock(this->mutex); + thrust = this->state.cmd_propeller_speed(); + } + msgs::Cessna msg; - this->targetThrust = std::max(this->targetThrust - 0.01f, 0.0f); - msg.set_propeller_speed(this->targetThrust); + thrust = std::max(thrust - 0.1f, 0.0f); + msg.set_cmd_propeller_speed(thrust); this->controlPub->Publish(msg); } ///////////////////////////////////////////////// void CessnaGUIPlugin::OnIncreaseFlaps() { - msgs::Cessna msg; - if (this->targetFlaps.Degree() < 30) - this->targetFlaps += this->angleStep; + math::Angle flap; + { + std::lock_guard lock(this->mutex); + flap.SetFromRadian(this->state.cmd_left_flap()); + } - msg.set_left_aileron(this->targetFlaps.Radian()); - msg.set_left_flap(this->targetFlaps.Radian()); - msg.set_right_aileron(this->targetFlaps.Radian()); - msg.set_right_flap(this->targetFlaps.Radian()); - this->controlPub->Publish(msg); + msgs::Cessna msg; + if (flap.Degree() < 30) + { + flap += this->angleStep; + msg.set_cmd_left_flap(flap.Radian()); + msg.set_cmd_right_flap(flap.Radian()); + this->controlPub->Publish(msg); + } } ///////////////////////////////////////////////// void CessnaGUIPlugin::OnDecreaseFlaps() { + math::Angle flap; + { + std::lock_guard lock(this->mutex); + flap.SetFromRadian(this->state.cmd_left_flap()); + } + msgs::Cessna msg; - if (this->targetFlaps.Degree() > -30) - this->targetFlaps -= this->angleStep; + if (flap.Degree() > -30) + { + flap -= this->angleStep; + msg.set_cmd_left_flap(flap.Radian()); + msg.set_cmd_right_flap(flap.Radian()); + this->controlPub->Publish(msg); + } +} - msg.set_left_aileron(this->targetFlaps.Radian()); - msg.set_left_flap(this->targetFlaps.Radian()); - msg.set_right_aileron(this->targetFlaps.Radian()); - msg.set_right_flap(this->targetFlaps.Radian()); - this->controlPub->Publish(msg); +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnIncreaseRoll() +{ + math::Angle aileron; + { + std::lock_guard lock(this->mutex); + aileron.SetFromRadian(this->state.cmd_left_aileron()); + } + + msgs::Cessna msg; + if (aileron.Degree() < 30) + { + aileron += this->angleStep; + msg.set_cmd_left_aileron(aileron.Radian()); + msg.set_cmd_right_aileron(-aileron.Radian()); + this->controlPub->Publish(msg); + } } ///////////////////////////////////////////////// -void CessnaGUIPlugin::OnIncreaseElevators() +void CessnaGUIPlugin::OnDecreaseRoll() { + math::Angle aileron; + { + std::lock_guard lock(this->mutex); + aileron.SetFromRadian(this->state.cmd_left_aileron()); + } + msgs::Cessna msg; - if (this->targetElevators.Degree() < 30) - this->targetElevators += this->angleStep; + if (aileron.Degree() > -30) + { + aileron -= this->angleStep; + msg.set_cmd_left_aileron(aileron.Radian()); + msg.set_cmd_right_aileron(-aileron.Radian()); + this->controlPub->Publish(msg); + } +} - msg.set_elevators(this->targetElevators.Radian()); - this->controlPub->Publish(msg); +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnIncreaseElevators() +{ + math::Angle elevators; + { + std::lock_guard lock(this->mutex); + elevators.SetFromRadian(this->state.cmd_elevators()); + } + + msgs::Cessna msg; + if (elevators.Degree() < 30) + { + elevators += this->angleStep; + msg.set_cmd_elevators(elevators.Radian()); + this->controlPub->Publish(msg); + } } ///////////////////////////////////////////////// void CessnaGUIPlugin::OnDecreaseElevators() { - msgs::Cessna msg; - if (this->targetElevators.Degree() > -30) - this->targetElevators -= this->angleStep; + math::Angle elevators; + { + std::lock_guard lock(this->mutex); + elevators.SetFromRadian(this->state.cmd_elevators()); + } - msg.set_elevators(this->targetElevators.Radian()); - this->controlPub->Publish(msg); + msgs::Cessna msg; + if (elevators.Degree() > -30) + { + elevators -= this->angleStep; + msg.set_cmd_elevators(elevators.Radian()); + this->controlPub->Publish(msg); + } } ///////////////////////////////////////////////// void CessnaGUIPlugin::OnIncreaseRudder() { + math::Angle rudder; + { + std::lock_guard lock(this->mutex); + rudder.SetFromRadian(this->state.cmd_rudder()); + } + + msgs::Cessna msg; + if (rudder.Degree() < 30) + { + rudder += this->angleStep; + msg.set_cmd_elevators(rudder.Radian()); + this->controlPub->Publish(msg); + } +} + +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnDecreaseRudder() +{ + math::Angle rudder; + { + std::lock_guard lock(this->mutex); + rudder.SetFromRadian(this->state.cmd_rudder()); + } + msgs::Cessna msg; - if (this->targetRudder.Degree() < 30) - this->targetRudder += this->angleStep; + if (rudder.Degree() < -30) + { + rudder += this->angleStep; + msg.set_cmd_elevators(rudder.Radian()); + this->controlPub->Publish(msg); + } +} - msg.set_rudder(this->targetRudder.Radian()); +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnPresetTakeOff() +{ + msgs::Cessna msg; + msg.set_cmd_propeller_speed(0.8); + msg.set_cmd_left_aileron(-0.017); + msg.set_cmd_right_aileron(0.017); + msg.set_cmd_left_flap(0); + msg.set_cmd_right_flap(0); + msg.set_cmd_elevators(0.033); + msg.set_cmd_rudder(-0.035); this->controlPub->Publish(msg); } ///////////////////////////////////////////////// -void CessnaGUIPlugin::OnDecreaseRudder() +void CessnaGUIPlugin::OnPresetCruise() { msgs::Cessna msg; - if (this->targetRudder.Degree() > -30) - this->targetRudder -= this->angleStep; + msg.set_cmd_propeller_speed(0.6); + msg.set_cmd_left_aileron(0); + msg.set_cmd_right_aileron(0); + msg.set_cmd_left_flap(0); + msg.set_cmd_right_flap(0); + msg.set_cmd_elevators(0.12); + msg.set_cmd_rudder(-0.035); + this->controlPub->Publish(msg); +} - msg.set_rudder(this->targetRudder.Radian()); +///////////////////////////////////////////////// +void CessnaGUIPlugin::OnPresetLanding() +{ + msgs::Cessna msg; + msg.set_cmd_propeller_speed(0.3); + msg.set_cmd_left_aileron(0); + msg.set_cmd_right_aileron(0); + msg.set_cmd_left_flap(0); + msg.set_cmd_right_flap(0); + msg.set_cmd_elevators(0.16); + msg.set_cmd_rudder(-0.035); this->controlPub->Publish(msg); } diff --git a/plugins/CessnaGUIPlugin.hh b/plugins/CessnaGUIPlugin.hh index 049c9a7512..56db65aec4 100644 --- a/plugins/CessnaGUIPlugin.hh +++ b/plugins/CessnaGUIPlugin.hh @@ -17,6 +17,7 @@ #ifndef _GAZEBO_GUI_CESSNA_PLUGIN_HH_ #define _GAZEBO_GUI_CESSNA_PLUGIN_HH_ +#include #include #include #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 @@ -39,6 +40,9 @@ namespace gazebo /// \brief Destructor. public: virtual ~CessnaGUIPlugin(); + ///////////////////////////////////////////////// + private: void OnState(ConstCessnaPtr &_msg); + /// \brief Increase the propeller RPMs. private slots: void OnIncreaseThrust(); @@ -51,7 +55,13 @@ namespace gazebo /// \brief Decrease the flaps angle. private slots: void OnDecreaseFlaps(); - /// \brief Increase the elevators angle + /// \brief Increase Roll. + private slots: void OnIncreaseRoll(); + + /// \brief Decrease Roll. + private slots: void OnDecreaseRoll(); + + /// \brief Increase the elevators angle. private slots: void OnIncreaseElevators(); /// \brief Decrease the elevators angle. @@ -63,6 +73,15 @@ namespace gazebo /// \brief Decrease the rudder angle. private slots: void OnDecreaseRudder(); + /// \brief Take-off preset. + private slots: void OnPresetTakeOff(); + + /// \brief Cruise preset. + private slots: void OnPresetCruise(); + + /// \brief Landing preset. + private slots: void OnPresetLanding(); + /// \brief SDF for this plugin. private: sdf::ElementPtr sdf; @@ -72,20 +91,17 @@ namespace gazebo /// \brief Control publisher. private: transport::PublisherPtr controlPub; - /// \brief Target thrust percentage. - private: float targetThrust = 0.0f; - - /// \brief Target flaps angle in degrees. - private: math::Angle targetFlaps = math::Angle::Zero; - - /// \brief Target elevators angle in degrees. - private: math::Angle targetElevators = math::Angle::Zero; - - /// \brief Target rudder angle in degrees. - private: math::Angle targetRudder = math::Angle::Zero; + /// \brief State subscriber. + private: transport::SubscriberPtr stateSub; /// \brief Angle increment/decrement each time a key is pressed; private: math::Angle angleStep; + + /// \brief State received from the Cessna plugin. + private: msgs::Cessna state; + + /// \brief Protection. + private: std::mutex mutex; }; } diff --git a/plugins/CessnaPlugin.cc b/plugins/CessnaPlugin.cc index 256bfad4bb..d26cff0a99 100644 --- a/plugins/CessnaPlugin.cc +++ b/plugins/CessnaPlugin.cc @@ -33,7 +33,7 @@ GZ_REGISTER_MODEL_PLUGIN(CessnaPlugin) CessnaPlugin::CessnaPlugin() { // PID default parameters. - this->propellerPID.Init(50.0, 0.1, 1, 0.0, 0.0, 20.0, -20.0); + this->propellerPID.Init(50.0, 0.1, 1, 0.0, 0.0, 20000.0, -20000.0); this->propellerPID.SetCmd(0.0); for (auto &pid : this->controlSurfacesPID) @@ -170,20 +170,23 @@ void CessnaPlugin::OnControl(ConstCessnaPtr &_msg) { std::lock_guard lock(this->mutex); - if (_msg->has_propeller_speed() && std::abs(_msg->propeller_speed()) <= 1) - this->cmds[kPropeller] = _msg->propeller_speed(); - if (_msg->has_left_aileron()) - this->cmds[kLeftAileron] = _msg->left_aileron(); + if (_msg->has_cmd_propeller_speed() && + std::abs(_msg->cmd_propeller_speed()) <= 1) + { + this->cmds[kPropeller] = _msg->cmd_propeller_speed(); + } + if (_msg->has_cmd_left_aileron()) + this->cmds[kLeftAileron] = _msg->cmd_left_aileron(); if (_msg->has_left_flap()) - this->cmds[kLeftFlap] = _msg->left_flap(); - if (_msg->has_right_aileron()) - this->cmds[kRightAileron] = _msg->right_aileron(); - if (_msg->has_right_flap()) - this->cmds[kRightFlap] = _msg->right_flap(); - if (_msg->has_elevators()) - this->cmds[kElevators] = _msg->elevators(); - if (_msg->has_rudder()) - this->cmds[kRudder] = _msg->rudder(); + this->cmds[kLeftFlap] = _msg->cmd_left_flap(); + if (_msg->has_cmd_right_aileron()) + this->cmds[kRightAileron] = _msg->cmd_right_aileron(); + if (_msg->has_cmd_right_flap()) + this->cmds[kRightFlap] = _msg->cmd_right_flap(); + if (_msg->has_cmd_elevators()) + this->cmds[kElevators] = _msg->cmd_elevators(); + if (_msg->has_cmd_rudder()) + this->cmds[kRudder] = _msg->cmd_rudder(); } ///////////////////////////////////////////////// @@ -191,7 +194,8 @@ void CessnaPlugin::UpdatePIDs(double _dt) { // Velocity PID for the propeller. double vel = this->joints[kPropeller]->GetVelocity(0); - double target = this->propellerMaxRpm * this->cmds[kPropeller]; + double maxVel = this->propellerMaxRpm*2.0*M_PI/60.0; + double target = maxVel * this->cmds[kPropeller]; double error = vel - target; double force = this->propellerPID.Update(error, _dt); this->joints[kPropeller]->SetForce(0, force); @@ -210,8 +214,9 @@ void CessnaPlugin::UpdatePIDs(double _dt) void CessnaPlugin::PublishState() { // Read the current state. - double propellerRpms = this->joints[kPropeller]->GetVelocity(0); - int32_t propellerSpeed = 100.0 * propellerRpms / this->propellerMaxRpm; + double propellerRpms = this->joints[kPropeller]->GetVelocity(0) + /(2.0*M_PI)*60.0; + float propellerSpeed = propellerRpms / this->propellerMaxRpm; float leftAileron = this->joints[kLeftAileron]->GetAngle(0).Radian(); float leftFlap = this->joints[kLeftFlap]->GetAngle(0).Radian(); float rightAileron = this->joints[kRightAileron]->GetAngle(0).Radian(); @@ -220,6 +225,7 @@ void CessnaPlugin::PublishState() float rudder = this->joints[kRudder]->GetAngle(0).Radian(); msgs::Cessna msg; + // Set the observed state. msg.set_propeller_speed(propellerSpeed); msg.set_left_aileron(leftAileron); msg.set_left_flap(leftFlap); @@ -228,5 +234,14 @@ void CessnaPlugin::PublishState() msg.set_elevators(elevators); msg.set_rudder(rudder); + // Set the target state. + msg.set_cmd_propeller_speed(this->cmds[kPropeller]); + msg.set_cmd_left_aileron(this->cmds[kLeftAileron]); + msg.set_cmd_left_flap(this->cmds[kLeftFlap]); + msg.set_cmd_right_aileron(this->cmds[kRightAileron]); + msg.set_cmd_right_flap(this->cmds[kRightFlap]); + msg.set_cmd_elevators(this->cmds[kElevators]); + msg.set_cmd_rudder(this->cmds[kRudder]); + this->statePub->Publish(msg); } diff --git a/plugins/LiftDragPlugin.cc b/plugins/LiftDragPlugin.cc index ea92636ec4..cc1ad9ae8b 100644 --- a/plugins/LiftDragPlugin.cc +++ b/plugins/LiftDragPlugin.cc @@ -44,13 +44,24 @@ LiftDragPlugin::LiftDragPlugin() : cla(1.0), cda(0.01), cma(0.01), rho(1.2041) this->alphaStall = 0.5*M_PI; this->claStall = 0.0; + this->radialSymmetry = false; + /// \TODO: what's flat plate drag? this->cdaStall = 1.0; this->cmaStall = 0.0; + + /// how much to change CL per every radian of the control joint value + this->controlJointRadToCL = 4.0; +} + +///////////////////////////////////////////////// +LiftDragPlugin::~LiftDragPlugin() +{ } ///////////////////////////////////////////////// -void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +void LiftDragPlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { GZ_ASSERT(_model, "LiftDragPlugin _model pointer is NULL"); GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL"); @@ -66,6 +77,9 @@ void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL"); + if (_sdf->HasElement("radial_symmetry")) + this->radialSymmetry = _sdf->Get("radial_symmetry"); + if (_sdf->HasElement("a0")) this->alpha0 = _sdf->Get("a0"); @@ -113,8 +127,19 @@ void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) GZ_ASSERT(elem, "Element link_name doesn't exist!"); this->linkName = elem->Get(); this->link = this->model->GetLink(this->linkName); - GZ_ASSERT(link, "Link was NULL"); + GZ_ASSERT(this->link, "Link was NULL"); + } + + if (_sdf->HasElement("control_joint_name")) + { + sdf::ElementPtr elem = _sdf->GetElement("control_joint_name"); + GZ_ASSERT(elem, "Element control_joint_name doesn't exist!"); + this->controlJointName = elem->Get(); + this->controlJoint = this->model->GetJoint(this->controlJointName); } + + if (_sdf->HasElement("control_joint_rad_to_cl")) + this->controlJointRadToCL = _sdf->Get("control_joint_rad_to_cl"); } ///////////////////////////////////////////////// @@ -137,14 +162,14 @@ void LiftDragPlugin::OnUpdate() { GZ_ASSERT(this->link, "Link was NULL"); // get linear velocity at cp in inertial frame - math::Vector3 vel = this->link->GetWorldLinearVel(this->cp); + math::Vector3 velI = this->link->GetWorldLinearVel(this->cp); // smoothing // double e = 0.8; - // this->velSmooth = e*vel + (1.0 - e)*velSmooth; - // vel = this->velSmooth; + // this->velSmooth = e*velI + (1.0 - e)*velSmooth; + // velI = this->velSmooth; - if (vel.GetLength() <= 0.01) + if (velI.GetLength() <= 0.01) return; // pose of body @@ -152,19 +177,31 @@ void LiftDragPlugin::OnUpdate() // rotate forward and upward vectors into inertial frame math::Vector3 forwardI = pose.rot.RotateVector(this->forward); - math::Vector3 upwardI = pose.rot.RotateVector(this->upward); - // ldNormal vector to lift-drag-plane described in inertial frame - math::Vector3 ldNormal = forwardI.Cross(upwardI).Normalize(); + math::Vector3 upwardI; + if (this->radialSymmetry) + { + // use inflow velocity to determine upward direction + // which is the component of inflow perpendicular to forward direction. + math::Vector3 tmp = forwardI.Cross(velI); + upwardI = forwardI.Cross(tmp).Normalize(); + } + else + { + upwardI = pose.rot.RotateVector(this->upward); + } + + // spanwiseI: a vector normal to lift-drag-plane described in inertial frame + math::Vector3 spanwiseI = forwardI.Cross(upwardI).Normalize(); - double min = -1; - double max = 1; - // check sweep (angle between vel and lift-drag-plane) - double sinSweepAngle = - math::clamp(ldNormal.Dot(vel) / vel.GetLength(), min, max); + const double minRatio = -1.0; + const double maxRatio = 1.0; + // check sweep (angle between velI and lift-drag-plane) + double sinSweepAngle = math::clamp( + spanwiseI.Dot(velI) / velI.GetLength(), minRatio, maxRatio); // get cos from trig identity - double cosSweepAngle2 = 1.0 - sinSweepAngle * sinSweepAngle; + double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; this->sweep = asin(sinSweepAngle); // truncate sweep to within +/-90 deg @@ -173,30 +210,30 @@ void LiftDragPlugin::OnUpdate() : this->sweep + M_PI; // angle of attack is the angle between - // vel projected into lift-drag plane + // velI projected into lift-drag plane // and // forward vector // - // projected = ldNormal Xcross ( vector Xcross ldNormal) + // projected = spanwiseI Xcross ( vector Xcross spanwiseI) // // so, // velocity in lift-drag plane (expressed in inertial frame) is: - math::Vector3 velInLDPlane = ldNormal.Cross(vel.Cross(ldNormal)); + math::Vector3 velInLDPlane = spanwiseI.Cross(velI.Cross(spanwiseI)); // get direction of drag math::Vector3 dragDirection = -velInLDPlane; dragDirection.Normalize(); // get direction of lift - math::Vector3 liftDirection = ldNormal.Cross(velInLDPlane); + math::Vector3 liftDirection = spanwiseI.Cross(velInLDPlane); liftDirection.Normalize(); // get direction of moment - math::Vector3 momentDirection = ldNormal; + math::Vector3 momentDirection = spanwiseI; double forwardVelocity = forwardI.GetLength() * velInLDPlane.GetLength(); double cosAlpha = math::clamp( - forwardI.Dot(velInLDPlane) / forwardVelocity, min, max); + forwardI.Dot(velInLDPlane) / forwardVelocity, minRatio, maxRatio); // gzerr << "ca " << forwardI.Dot(velInLDPlane) / // (forwardI.GetLength() * velInLDPlane.GetLength()) << "\n"; @@ -227,7 +264,7 @@ void LiftDragPlugin::OnUpdate() { cl = (this->cla * this->alphaStall + this->claStall * (this->alpha - this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; // make sure cl is still great than 0 cl = std::max(0.0, cl); } @@ -235,12 +272,20 @@ void LiftDragPlugin::OnUpdate() { cl = (-this->cla * this->alphaStall + this->claStall * (this->alpha + this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; // make sure cl is still less than 0 cl = std::min(0.0, cl); } else - cl = this->cla * this->alpha * cosSweepAngle2; + cl = this->cla * this->alpha * cosSweepAngle; + + // modify cl per control joint value + if (this->controlJoint) + { + double controlAngle = this->controlJoint->GetAngle(0).Radian(); + cl = cl + this->controlJointRadToCL * controlAngle; + /// \TODO: also change cm and cd + } // compute lift force at cp math::Vector3 lift = cl * q * this->area * liftDirection; @@ -251,16 +296,16 @@ void LiftDragPlugin::OnUpdate() { cd = (this->cda * this->alphaStall + this->cdaStall * (this->alpha - this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; } else if (this->alpha < -this->alphaStall) { cd = (-this->cda * this->alphaStall + this->cdaStall * (this->alpha + this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; } else - cd = (this->cda * this->alpha) * cosSweepAngle2; + cd = (this->cda * this->alpha) * cosSweepAngle; // make sure drag is positive cd = fabs(cd); @@ -274,7 +319,7 @@ void LiftDragPlugin::OnUpdate() { cm = (this->cma * this->alphaStall + this->cmaStall * (this->alpha - this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; // make sure cm is still great than 0 cm = std::max(0.0, cm); } @@ -282,14 +327,15 @@ void LiftDragPlugin::OnUpdate() { cm = (-this->cma * this->alphaStall + this->cmaStall * (this->alpha + this->alphaStall)) - * cosSweepAngle2; + * cosSweepAngle; // make sure cm is still less than 0 cm = std::min(0.0, cm); } else - cm = this->cma * this->alpha * cosSweepAngle2; + cm = this->cma * this->alpha * cosSweepAngle; - // reset cm to zero, as cm needs testing + /// \TODO: implement cm + /// for now, reset cm to zero, as cm needs testing cm = 0.0; // compute moment (torque) at cp @@ -311,21 +357,22 @@ void LiftDragPlugin::OnUpdate() // // if ((this->link->GetName() == "wing_1" || // this->link->GetName() == "wing_2") && - // (vel.GetLength() > 50.0 && - // vel.GetLength() < 50.0)) + // (velI.GetLength() > 50.0 && + // velI.GetLength() < 50.0)) if (0) { gzerr << "=============================\n"; gzerr << "Link: [" << this->link->GetName() << "] pose: [" << pose << "] dynamic pressure: [" << q << "]\n"; - gzerr << "spd: [" << vel.GetLength() << "] vel: [" << vel << "]\n"; + gzerr << "spd: [" << velI.GetLength() + << "] velI: [" << velI << "]\n"; gzerr << "spd sweep: [" << velInLDPlane.GetLength() - << "] vel in LD: [" << velInLDPlane << "]\n"; + << "] velI in LD plane: [" << velInLDPlane << "]\n"; gzerr << "forward (inertial): " << forwardI << "\n"; gzerr << "upward (inertial): " << upwardI << "\n"; gzerr << "lift dir (inertial): " << liftDirection << "\n"; - gzerr << "LD Normal: " << ldNormal << "\n"; + gzerr << "Span direction (normal to LD plane): " << spanwiseI << "\n"; gzerr << "sweep: " << this->sweep << "\n"; gzerr << "alpha: " << this->alpha << "\n"; gzerr << "lift: " << lift << "\n"; diff --git a/plugins/LiftDragPlugin.hh b/plugins/LiftDragPlugin.hh index c3f0c4c9d5..615c90b2d2 100644 --- a/plugins/LiftDragPlugin.hh +++ b/plugins/LiftDragPlugin.hh @@ -32,6 +32,9 @@ namespace gazebo /// \brief Constructor. public: LiftDragPlugin(); + /// \brief Destructor. + public: ~LiftDragPlugin(); + // Documentation Inherited. public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); @@ -91,6 +94,12 @@ namespace gazebo /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. protected: double rho; + /// \brief if the shape is aerodynamically radially symmetric about + /// the forward direction. Defaults to false for wing shapes. + /// If set to true, the upward direction is determined by the + /// angle of attack. + protected: bool radialSymmetry; + /// \brief effective planeform surface area protected: double area; @@ -106,14 +115,16 @@ namespace gazebo /// \brief center of pressure in link local coordinates protected: math::Vector3 cp; - /// \brief forward flight direction in link local coordinates + /// \brief Normally, this is taken as a direction parallel to the chord + /// of the airfoil in zero angle of attack forward flight. protected: math::Vector3 forward; - /// \brief A vector in the lift/drag plane, anything orthogonal to it - /// is considered wing sweep. + /// \brief A vector in the lift/drag plane, perpendicular to the forward + /// vector. Inflow velocity orthogonal to forward and upward vectors + /// is considered flow in the wing sweep direction. protected: math::Vector3 upward; - /// \brief Smooth velocity + /// \brief Smoothed velocity protected: math::Vector3 velSmooth; /// \brief Names of allowed target links, specified in sdf parameters. @@ -122,6 +133,18 @@ namespace gazebo /// \brief Pointer to link currently targeted by mud joint. protected: physics::LinkPtr link; + /// \brief Names of a joint that actuates a control surface for + /// this lifting body + protected: std::string controlJointName; + + /// \brief Pointer to a joint that actuates a control surface for + /// this lifting body + protected: physics::JointPtr controlJoint; + + /// \brief how much to change CL per radian of control surface joint + /// value. + protected: double controlJointRadToCL; + /// \brief SDF for this plugin; protected: sdf::ElementPtr sdf; }; diff --git a/worlds/cessna_demo.world b/worlds/cessna_demo.world index 007cf9d375..30ee638483 100644 --- a/worlds/cessna_demo.world +++ b/worlds/cessna_demo.world @@ -75,7 +75,17 @@ model://cessna - + + 1.79 0 1.350 0 0 0 + 0 + + + + 0 0 0.495 0 0 0 + 0 + + + cessna_c172::propeller_joint 2500 @@ -85,8 +95,123 @@ cessna_c172::right_flap_joint cessna_c172::elevators_joint cessna_c172::rudder_joint + 10000 + 0 + 0 + 2000 + 0 + 0 + + + 0.4 + 4.752798721 + 0.6417112299 + 0 + 1.5 + -3.85 + -0.9233984055 + 0 + -0.37 0 0.77 + 0.1 + 1.2041 + 0 -1 0 + 1 0 0 + cessna_c172::propeller + + + 0.4 + 4.752798721 + 0.6417112299 + 0 + 1.5 + -3.85 + -0.9233984055 + 0 + -0.37 0 -0.77 + 0.1 + 1.2041 + 0 1 0 + 1 0 0 + cessna_c172::propeller - + + 0.05984281113 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -1 2.205 1.5 + 8.08255 + 1.2041 + 1 0 0 + 0 0 1 + cessna_c172::body + cessna_c172::left_aileron_joint + -2.0 + + + + 0.05984281113 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -1 -2.205 1.5 + 8.08255 + 1.2041 + 1 0 0 + 0 0 1 + cessna_c172::body + + cessna_c172::right_aileron_joint + + -2.0 + + + + -0.2 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -5.45 0 0.55 + 2.03458 + 1.2041 + 1 0 0 + 0 0 1 + cessna_c172::body + cessna_c172::elevators_joint + -4.0 + + + + 0 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -6 0 1.55 + 1.5329 + 1.2041 + 1 0 0 + 0 1 0 + cessna_c172::body + cessna_c172::rudder_joint + 4.0 + +