From 0491e4ef9e29b0e2fcc3d310a0f455c505caa974 Mon Sep 17 00:00:00 2001 From: frederik Date: Tue, 3 Oct 2023 14:36:07 +0200 Subject: [PATCH 1/8] lift drag bug fix Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 42 ++++++----------------- src/systems/lift_drag/LiftDrag.hh | 56 ++++++++++++++++--------------- 2 files changed, 39 insertions(+), 59 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index f465e04789..cc96a3027e 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 @@ -87,6 +88,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 +160,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 +212,6 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, return; } - if (_sdf->HasElement("control_joint_name")) { auto controlJointName = _sdf->Get("control_joint_name"); @@ -304,7 +309,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 +341,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 +440,13 @@ 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 + 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; @@ -469,30 +472,6 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto totalTorque = torque + cpWorld.Cross(force); Link link(this->linkEntity); link.AddWorldWrench(_ecm, force, totalTorque); - - // Debug - // auto linkName = _ecm.Component(this->linkEntity)->Data(); - // gzdbg << "=============================\n"; - // gzdbg << "Link: [" << linkName << "] pose: [" << pose - // << "] dynamic pressure: [" << q << "]\n"; - // gzdbg << "spd: [" << vel.Length() << "] vel: [" << vel << "]\n"; - // gzdbg << "LD plane spd: [" << velInLDPlane.Length() << "] vel : [" - // << velInLDPlane << "]\n"; - // gzdbg << "forward (inertial): " << forwardI << "\n"; - // gzdbg << "upward (inertial): " << upwardI << "\n"; - // gzdbg << "q: " << q << "\n"; - // gzdbg << "cl: " << cl << "\n"; - // gzdbg << "lift dir (inertial): " << liftI << "\n"; - // gzdbg << "Span direction (normal to LD plane): " << spanwiseI << "\n"; - // gzdbg << "sweep: " << sweep << "\n"; - // gzdbg << "alpha: " << alpha << "\n"; - // gzdbg << "lift: " << lift << "\n"; - // gzdbg << "drag: " << drag << " cd: " << cd << " cda: " - // << this->cda << "\n"; - // gzdbg << "moment: " << moment << "\n"; - // gzdbg << "force: " << force << "\n"; - // gzdbg << "torque: " << torque << "\n"; - // gzdbg << "totalTorque: " << totalTorque << "\n"; } ////////////////////////////////////////////////// @@ -530,7 +509,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..b2cbf67232 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,34 +35,36 @@ 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, From 8388f9af4f2ac6c117ab5f8c801629f237372988 Mon Sep 17 00:00:00 2001 From: frederik Date: Tue, 3 Oct 2023 15:33:10 +0200 Subject: [PATCH 2/8] updated formatting as per comment Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 2 +- src/systems/lift_drag/LiftDrag.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index cc96a3027e..eef4fc8b14 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -160,7 +160,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; + this->cm_delta = _sdf->Get("cm_delta", this->cm_delta).first; // blade forward (-drag) direction in link frame this->forward = diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index b2cbf67232..9163fd8662 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,7 +35,7 @@ namespace systems /// \brief The LiftDrag system computes lift and drag forces enabling /// simulation of aerodynamic robots. /// - /// ##System Parameters + /// ## System Parameters /// /// - ``: Name of the link affected by the group of lift/drag /// properties. This can be a scoped name to reference links in From d7c2e5754d680b71f1af2466d14649308e704321 Mon Sep 17 00:00:00 2001 From: frederik Date: Tue, 3 Oct 2023 17:54:53 +0200 Subject: [PATCH 3/8] adding wind to the lift drag plugin Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index eef4fc8b14..04e1f826ee 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -41,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; @@ -264,6 +265,11 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto worldPose = _ecm.Component(this->linkEntity); + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + auto windLinearVel = + _ecm.Component(windEntity); + + components::JointPosition *controlJointPosition = nullptr; if (this->controlJointEntity != kNullEntity) { @@ -276,7 +282,7 @@ 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); + const auto vel = worldLinVel->Data() + worldAngVel->Data().Cross(cpWorld) - windLinearVel->Data(); if (vel.Length() <= 0.01) return; @@ -286,6 +292,11 @@ 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) { From 256157a5327a51d57828451c74442669dc68936d Mon Sep 17 00:00:00 2001 From: frederik Date: Tue, 3 Oct 2023 18:23:56 +0200 Subject: [PATCH 4/8] additional formatting Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 04e1f826ee..ff7f65b3a1 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -265,11 +265,11 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto worldPose = _ecm.Component(this->linkEntity); + // get wind as a component from the _ecm Entity windEntity = _ecm.EntityByComponents(components::Wind()); auto windLinearVel = _ecm.Component(windEntity); - components::JointPosition *controlJointPosition = nullptr; if (this->controlJointEntity != kNullEntity) { From 96ad4b4cd45deed32124aa1a638bb8bb13b979b5 Mon Sep 17 00:00:00 2001 From: frederik Date: Thu, 5 Oct 2023 13:10:39 +0200 Subject: [PATCH 5/8] fixing trailing spaces and long lines Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 2 +- src/systems/lift_drag/LiftDrag.hh | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index eef4fc8b14..2b869cb46b 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -88,7 +88,7 @@ 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 + /// \brief How much Cm changes with a change in control /// surface deflection angle public: double cm_delta = 0.0; diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 9163fd8662..a84475854d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -52,18 +52,19 @@ namespace systems /// 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 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. + /// - ``: 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 + /// - ``: 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, From b9838e749e08970459af266e2f61030fe130180c Mon Sep 17 00:00:00 2001 From: frederik Date: Thu, 5 Oct 2023 13:17:05 +0200 Subject: [PATCH 6/8] updated formatting with long lines Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 2adea5a616..774c0c8b75 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -282,7 +282,8 @@ 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) - windLinearVel->Data(); + const auto vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld) - windLinearVel->Data(); if (vel.Length() <= 0.01) return; @@ -293,7 +294,8 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) 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 + // Only calculate lift or drag if the wind relative velocity + // is in the same direction return; } From 9efa921dc810d1088ad78b1b97ac23957e809b7e Mon Sep 17 00:00:00 2001 From: frederik Date: Fri, 6 Oct 2023 10:01:02 +0200 Subject: [PATCH 7/8] added nullptr checks to windvel and kNullEntity check to windentity Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 774c0c8b75..6ee52c89c6 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -266,10 +266,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) _ecm.Component(this->linkEntity); // get wind as a component from the _ecm - Entity windEntity = _ecm.EntityByComponents(components::Wind()); - auto windLinearVel = - _ecm.Component(windEntity); - + 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) { @@ -282,8 +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( + 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; @@ -454,7 +460,10 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) cm = this->cma * alpha * cosSweepAngle; // Take into account the effect of control surface deflection angle to cm - cm += this->cm_delta * controlJointPosition->Data()[0]; + if (controlJointPosition && !controlJointPosition->Data().empty()) + { + cm += this->cm_delta * controlJointPosition->Data()[0]; + } // compute moment (torque) at cp // spanwiseI used to be momentDirection From 99e8ca7a2ca7a15db858a11803933d5c0c4901d4 Mon Sep 17 00:00:00 2001 From: frederik Date: Fri, 6 Oct 2023 10:16:48 +0200 Subject: [PATCH 8/8] put gzdbg comments back in Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 6ee52c89c6..cd58c9f22c 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -494,6 +494,30 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto totalTorque = torque + cpWorld.Cross(force); Link link(this->linkEntity); link.AddWorldWrench(_ecm, force, totalTorque); + + // Debug + // auto linkName = _ecm.Component(this->linkEntity)->Data(); + // gzdbg << "=============================\n"; + // gzdbg << "Link: [" << linkName << "] pose: [" << pose + // << "] dynamic pressure: [" << q << "]\n"; + // gzdbg << "spd: [" << vel.Length() << "] vel: [" << vel << "]\n"; + // gzdbg << "LD plane spd: [" << velInLDPlane.Length() << "] vel : [" + // << velInLDPlane << "]\n"; + // gzdbg << "forward (inertial): " << forwardI << "\n"; + // gzdbg << "upward (inertial): " << upwardI << "\n"; + // gzdbg << "q: " << q << "\n"; + // gzdbg << "cl: " << cl << "\n"; + // gzdbg << "lift dir (inertial): " << liftI << "\n"; + // gzdbg << "Span direction (normal to LD plane): " << spanwiseI << "\n"; + // gzdbg << "sweep: " << sweep << "\n"; + // gzdbg << "alpha: " << alpha << "\n"; + // gzdbg << "lift: " << lift << "\n"; + // gzdbg << "drag: " << drag << " cd: " << cd << " cda: " + // << this->cda << "\n"; + // gzdbg << "moment: " << moment << "\n"; + // gzdbg << "force: " << force << "\n"; + // gzdbg << "torque: " << torque << "\n"; + // gzdbg << "totalTorque: " << totalTorque << "\n"; } //////////////////////////////////////////////////