Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Lift Drag Bug Fix #2189

Merged
merged 10 commits into from
Oct 9, 2023
Merged

Lift Drag Bug Fix #2189

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
0491e4e
lift drag bug fix
frede791 Oct 3, 2023
8388f9a
updated formatting as per comment
frede791 Oct 3, 2023
d7c2e57
adding wind to the lift drag plugin
frede791 Oct 3, 2023
256157a
additional formatting
frede791 Oct 3, 2023
8861db1
Merge pull request #1 from frede791/wind_inclusion
frede791 Oct 3, 2023
96ad4b4
fixing trailing spaces and long lines
frede791 Oct 5, 2023
3aedcbb
Merge remote-tracking branch 'refs/remotes/origin/lift_drag_improveme…
frede791 Oct 5, 2023
b9838e7
updated formatting with long lines
frede791 Oct 5, 2023
9efa921
added nullptr checks to windvel and kNullEntity check to windentity
frede791 Oct 6, 2023
99e8ca7
put gzdbg comments back in
frede791 Oct 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 10 additions & 32 deletions src/systems/lift_drag/LiftDrag.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <algorithm>
#include <string>
#include <vector>
#include <cmath>

#include <gz/common/Profiler.hh>
#include <gz/plugin/Register.hh>
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -155,6 +160,7 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm,
this->area = _sdf->Get<double>("area", this->area).first;
this->alpha0 = _sdf->Get<double>("a0", this->alpha0).first;
this->cp = _sdf->Get<math::Vector3d>("cp", this->cp).first;
this->cm_delta = _sdf->Get<double>("cm_delta",this->cm_delta).first;
frede791 marked this conversation as resolved.
Show resolved Hide resolved

// blade forward (-drag) direction in link frame
this->forward =
Expand Down Expand Up @@ -206,7 +212,6 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm,
return;
}


if (_sdf->HasElement("control_joint_name"))
{
auto controlJointName = _sdf->Get<std::string>("control_joint_name");
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 =
Expand Down Expand Up @@ -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];
frede791 marked this conversation as resolved.
Show resolved Hide resolved

// 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;
Expand All @@ -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);

frede791 marked this conversation as resolved.
Show resolved Hide resolved
// Debug
// auto linkName = _ecm.Component<components::Name>(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";
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -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);
Expand Down
56 changes: 29 additions & 27 deletions src/systems/lift_drag/LiftDrag.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
frede791 marked this conversation as resolved.
Show resolved Hide resolved
///
/// 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)
/// - `<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)
/// - `<cm_delta>`: How much Cm changes with a change in control
frede791 marked this conversation as resolved.
Show resolved Hide resolved
/// surface deflection angle
class LiftDrag
: public System,
public ISystemConfigure,
Expand Down
Loading