Skip to content

Commit

Permalink
Lift Drag Bug Fix (#2189)
Browse files Browse the repository at this point in the history 
Signed-off-by: frederik <[email protected]>
  • Loading branch information
@frede791
frede791 authored Oct 9, 2023
1 parent f6efeef commit 2881041
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 36 deletions.
42 changes: 33 additions & 9 deletions src/systems/lift_drag/LiftDrag.cc
View file
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){
// 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
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

0 comments on commit 2881041

Please sign in to comment.