diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index f465e04789..cd58c9f22c 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ExternalWorldWrenchCmd.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" using namespace gz; using namespace sim; @@ -87,6 +89,10 @@ class gz::sim::systems::LiftDragPrivate /// \brief Cm-alpha rate after stall public: double cmaStall = 0.0; + /// \brief How much Cm changes with a change in control + /// surface deflection angle + public: double cm_delta = 0.0; + /// \brief air density /// at 25 deg C it's about 1.1839 kg/m^3 /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. @@ -155,6 +161,7 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, this->area = _sdf->Get("area", this->area).first; this->alpha0 = _sdf->Get("a0", this->alpha0).first; this->cp = _sdf->Get("cp", this->cp).first; + this->cm_delta = _sdf->Get("cm_delta", this->cm_delta).first; // blade forward (-drag) direction in link frame this->forward = @@ -206,7 +213,6 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, return; } - if (_sdf->HasElement("control_joint_name")) { auto controlJointName = _sdf->Get("control_joint_name"); @@ -259,6 +265,13 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto worldPose = _ecm.Component(this->linkEntity); + // get wind as a component from the _ecm + components::WorldLinearVelocity *windLinearVel = nullptr; + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + windLinearVel = + _ecm.Component(windEntity); + } components::JointPosition *controlJointPosition = nullptr; if (this->controlJointEntity != kNullEntity) { @@ -271,7 +284,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto &pose = worldPose->Data(); const auto cpWorld = pose.Rot().RotateVector(this->cp); - const auto vel = worldLinVel->Data() + worldAngVel->Data().Cross(cpWorld); + auto vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld); + if (windLinearVel != nullptr){ + vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld) - windLinearVel->Data(); + } if (vel.Length() <= 0.01) return; @@ -281,6 +299,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // rotate forward and upward vectors into world frame const auto forwardI = pose.Rot().RotateVector(this->forward); + if (forwardI.Dot(vel) <= 0.0){ + // Only calculate lift or drag if the wind relative velocity + // is in the same direction + return; + } + math::Vector3d upwardI; if (this->radialSymmetry) { @@ -304,7 +328,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) spanwiseI.Dot(velI), minRatio, maxRatio); // get cos from trig identity - const double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; + double cosSweepAngle = sqrt(1.0 - sinSweepAngle * sinSweepAngle); double sweep = std::asin(sinSweepAngle); // truncate sweep to within +/-90 deg @@ -336,7 +360,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // compute angle between upwardI and liftI // in general, given vectors a and b: - // cos(theta) = a.Dot(b)/(a.Length()*b.Lenghth()) + // cos(theta) = a.Dot(b)/(a.Length()*b.Length()) // given upwardI and liftI are both unit vectors, we can drop the denominator // cos(theta) = a.Dot(b) const double cosAlpha = @@ -435,15 +459,16 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) else cm = this->cma * alpha * cosSweepAngle; - /// \todo(anyone): implement cm - /// for now, reset cm to zero, as cm needs testing - cm = 0.0; + // Take into account the effect of control surface deflection angle to cm + if (controlJointPosition && !controlJointPosition->Data().empty()) + { + cm += this->cm_delta * controlJointPosition->Data()[0]; + } // compute moment (torque) at cp // spanwiseI used to be momentDirection math::Vector3d moment = cm * q * this->area * spanwiseI; - // force and torque about cg in world frame math::Vector3d force = lift + drag; math::Vector3d torque = moment; @@ -530,7 +555,6 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); this->dataPtr->initialized = true; - if (this->dataPtr->validConfig) { Link link(this->dataPtr->linkEntity); diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 8c7cba996d..a84475854d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,34 +35,37 @@ namespace systems /// \brief The LiftDrag system computes lift and drag forces enabling /// simulation of aerodynamic robots. /// - /// The following parameters are used by the system: + /// ## System Parameters /// - /// link_name : Name of the link affected by the group of lift/drag - /// properties. This can be a scoped name to reference links in - /// nested models. \sa entitiesFromScopedName to learn more - /// about scoped names. - /// air_density : Density of the fluid this model is suspended in. - /// area : Surface area of the link. - /// a0 : The initial "alpha" or initial angle of attack. a0 is also - /// the y-intercept of the alpha-lift coefficient curve. - /// cla : The ratio of the coefficient of lift and alpha slope before - /// stall. Slope of the first portion of the alpha-lift - /// coefficient curve. - /// cda : The ratio of the coefficient of drag and alpha slope before - /// stall. - /// cp : Center of pressure. The forces due to lift and drag will be - /// applied here. - /// forward : 3-vector representing the forward direction of motion in the - /// link frame. - /// upward : 3-vector representing the direction of lift or drag. - /// alpha_stall : Angle of attack at stall point; the peak angle of attack. - /// cla_stall : The ratio of coefficient of lift and alpha slope after - /// stall. Slope of the second portion of the alpha-lift - /// coefficient curve. - /// cda_stall : The ratio of coefficient of drag and alpha slope after - /// stall. - /// control_joint_name: Name of joint that actuates a control surface for this - /// lifting body (Optional) + /// - ``: Name of the link affected by the group of lift/drag + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. + /// - ``: Density of the fluid this model is suspended in. + /// - ``: Surface area of the link. + /// - ``: The initial "alpha" or initial angle of attack. a0 is also + /// the y-intercept of the alpha-lift coefficient curve. + /// - ``: The ratio of the coefficient of lift and alpha slope before + /// stall. Slope of the first portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of the coefficient of drag and alpha slope before + /// stall. + /// - ``: Center of pressure. The forces due to lift and drag will be + /// applied here. + /// - ``: 3-vector representing the forward direction of motion + /// in the link frame. + /// - ``: 3-vector representing the direction of lift or drag. + /// - ``: Angle of attack at stall point; the peak angle + /// of attack. + /// - ``: The ratio of coefficient of lift and alpha slope after + /// stall. Slope of the second portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of coefficient of drag and alpha slope after + /// stall. + /// - ``: Name of joint that actuates a control surface + /// for this lifting body (Optional) + /// - ``: How much Cm changes with a change in control + /// surface deflection angle class LiftDrag : public System, public ISystemConfigure,