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 all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
0491e4e
lift drag bug fix
fredmarkus Oct 3, 2023
8388f9a
updated formatting as per comment
fredmarkus Oct 3, 2023
d7c2e57
adding wind to the lift drag plugin
fredmarkus Oct 3, 2023
256157a
additional formatting
fredmarkus Oct 3, 2023
8861db1
Merge pull request #1 from frede791/wind_inclusion
fredmarkus Oct 3, 2023
96ad4b4
fixing trailing spaces and long lines
fredmarkus Oct 5, 2023
3aedcbb
Merge remote-tracking branch 'refs/remotes/origin/lift_drag_improveme…
fredmarkus Oct 5, 2023
b9838e7
updated formatting with long lines
fredmarkus Oct 5, 2023
9efa921
added nullptr checks to windvel and kNullEntity check to windentity
fredmarkus Oct 6, 2023
99e8ca7
put gzdbg comments back in
fredmarkus 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: 33 additions & 9 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 All @@ -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;
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -155,6 +161,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;

// blade forward (-drag) direction in link frame
this->forward =
Expand Down Expand Up @@ -206,7 +213,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 @@ -259,6 +265,13 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm)
const auto worldPose =
_ecm.Component<components::WorldPose>(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<components::WorldLinearVelocity>(windEntity);
}
components::JointPosition *controlJointPosition = nullptr;
if (this->controlJointEntity != kNullEntity)
{
Expand All @@ -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;
Expand All @@ -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){
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
// Only calculate lift or drag if the wind relative velocity
// is in the same direction
return;
}

math::Vector3d upwardI;
if (this->radialSymmetry)
{
Expand All @@ -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
Expand Down Expand Up @@ -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 =
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
57 changes: 30 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,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)
/// - `<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
/// surface deflection angle
class LiftDrag
: public System,
public ISystemConfigure,
Expand Down