21 #include "gazebo/common/Assert.hh" 22 #include "gazebo/physics/physics.hh" 23 #include "gazebo/sensors/SensorManager.hh" 24 #include "gazebo/transport/transport.hh" 25 #include "gazebo/msgs/msgs.hh" 35 this->cp = ignition::math::Vector3d (0, 0, 0);
36 this->forward = ignition::math::Vector3d (1, 0, 0);
37 this->upward = ignition::math::Vector3d (0, 0, 1);
42 this->velocityStall = 0.0;
45 this->alphaStall = 0.5*
M_PI;
48 this->radialSymmetry =
false;
55 this->controlJointRadToCL = 4.0;
59 LiftDragPlugin::~LiftDragPlugin()
67 GZ_ASSERT(_model,
"LiftDragPlugin _model pointer is NULL");
68 GZ_ASSERT(_sdf,
"LiftDragPlugin _sdf pointer is NULL");
73 GZ_ASSERT(this->
world,
"LiftDragPlugin world pointer is NULL");
76 GZ_ASSERT(this->
physics,
"LiftDragPlugin physics pointer is NULL");
78 GZ_ASSERT(_sdf,
"LiftDragPlugin _sdf pointer is NULL");
80 if (_sdf->HasElement(
"radial_symmetry"))
83 if (_sdf->HasElement(
"a0"))
84 this->
alpha0 = _sdf->Get<
double>(
"a0");
86 if (_sdf->HasElement(
"cla"))
87 this->
cla = _sdf->Get<
double>(
"cla");
89 if (_sdf->HasElement(
"cda"))
90 this->
cda = _sdf->Get<
double>(
"cda");
92 if (_sdf->HasElement(
"cma"))
93 this->
cma = _sdf->Get<
double>(
"cma");
95 if (_sdf->HasElement(
"alpha_stall"))
96 this->
alphaStall = _sdf->Get<
double>(
"alpha_stall");
98 if (_sdf->HasElement(
"cla_stall"))
99 this->
claStall = _sdf->Get<
double>(
"cla_stall");
101 if (_sdf->HasElement(
"cda_stall"))
102 this->
cdaStall = _sdf->Get<
double>(
"cda_stall");
104 if (_sdf->HasElement(
"cma_stall"))
105 this->
cmaStall = _sdf->Get<
double>(
"cma_stall");
107 if (_sdf->HasElement(
"cp"))
108 this->
cp = _sdf->Get<ignition::math::Vector3d >(
"cp");
111 if (_sdf->HasElement(
"forward"))
112 this->
forward = _sdf->Get<ignition::math::Vector3d >(
"forward");
116 if (_sdf->HasElement(
"upward"))
117 this->
upward = _sdf->Get<ignition::math::Vector3d >(
"upward");
120 if (_sdf->HasElement(
"area"))
121 this->
area = _sdf->Get<
double>(
"area");
123 if (_sdf->HasElement(
"air_density"))
124 this->
rho = _sdf->Get<
double>(
"air_density");
126 if (_sdf->HasElement(
"link_name"))
128 sdf::ElementPtr elem = _sdf->GetElement(
"link_name");
130 std::string linkName = elem->Get<std::string>();
131 this->
link = this->
model->GetLink(linkName);
136 gzerr <<
"Link with name[" << linkName <<
"] not found. " 137 <<
"The LiftDragPlugin will not generate forces\n";
146 if (_sdf->HasElement(
"control_joint_name"))
148 std::string controlJointName = _sdf->Get<std::string>(
"control_joint_name");
150 if (!this->controlJoint)
152 gzerr <<
"Joint with name[" << controlJointName <<
"] does not exist.\n";
156 if (_sdf->HasElement(
"control_joint_rad_to_cl"))
163 GZ_ASSERT(this->
link,
"Link was NULL");
165 ignition::math::Vector3d vel = this->
link->WorldLinearVel(this->
cp);
166 ignition::math::Vector3d velI = vel;
174 if (vel.Length() <= 0.01)
178 ignition::math::Pose3d pose = this->
link->WorldPose();
181 ignition::math::Vector3d forwardI = pose.Rot().RotateVector(this->
forward);
183 ignition::math::Vector3d upwardI;
188 ignition::math::Vector3d tmp = forwardI.Cross(velI);
189 upwardI = forwardI.Cross(tmp).Normalize();
193 upwardI = pose.Rot().RotateVector(this->
upward);
197 ignition::math::Vector3d spanwiseI = forwardI.Cross(upwardI).Normalize();
199 const double minRatio = -1.0;
200 const double maxRatio = 1.0;
202 double sinSweepAngle = ignition::math::clamp(
203 spanwiseI.Dot(velI), minRatio, maxRatio);
206 double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle;
223 ignition::math::Vector3d velInLDPlane = vel - vel.Dot(spanwiseI)*velI;
226 ignition::math::Vector3d dragDirection = -velInLDPlane;
227 dragDirection.Normalize();
230 ignition::math::Vector3d liftI = spanwiseI.Cross(velInLDPlane);
234 ignition::math::Vector3d momentDirection = spanwiseI;
241 double cosAlpha = ignition::math::clamp(liftI.Dot(upwardI), minRatio, maxRatio);
247 if (liftI.Dot(forwardI) >= 0.0)
253 while (fabs(this->
alpha) > 0.5 * M_PI)
255 : this->
alpha + M_PI;
258 double speedInLDPlane = velInLDPlane.Length();
259 double q = 0.5 * this->
rho * speedInLDPlane * speedInLDPlane;
269 cl = std::max(0.0, cl);
273 cl = (-this->
cla * this->alphaStall +
277 cl = std::min(0.0, cl);
280 cl = this->
cla * this->
alpha * cosSweepAngle;
291 ignition::math::Vector3d lift = cl * q * this->
area * liftI;
295 if (this->
alpha > this->alphaStall)
297 cd = (this->
cda * this->alphaStall +
301 else if (this->alpha < -this->alphaStall)
303 cd = (-this->
cda * this->alphaStall +
308 cd = (this->
cda * this->
alpha) * cosSweepAngle;
314 ignition::math::Vector3d drag = cd * q * this->
area * dragDirection;
318 if (this->
alpha > this->alphaStall)
320 cm = (this->
cma * this->alphaStall +
324 cm = std::max(0.0, cm);
326 else if (this->alpha < -this->alphaStall)
328 cm = (-this->
cma * this->alphaStall +
332 cm = std::min(0.0, cm);
335 cm = this->
cma * this->
alpha * cosSweepAngle;
342 ignition::math::Vector3d moment = cm * q * this->
area * momentDirection;
345 ignition::math::Vector3d momentArm = pose.Rot().RotateVector(
346 this->
cp - this->
link->GetInertial()->CoG());
350 ignition::math::Vector3d force = lift + drag;
353 ignition::math::Vector3d torque = moment;
364 gzdbg <<
"=============================\n";
365 gzdbg <<
"sensor: [" << this->GetHandle() <<
"]\n";
366 gzdbg <<
"Link: [" << this->
link->GetName()
367 <<
"] pose: [" << pose
368 <<
"] dynamic pressure: [" << q <<
"]\n";
369 gzdbg <<
"spd: [" << vel.Length()
370 <<
"] vel: [" << vel <<
"]\n";
371 gzdbg <<
"LD plane spd: [" << velInLDPlane.Length()
372 <<
"] vel : [" << velInLDPlane <<
"]\n";
373 gzdbg <<
"forward (inertial): " << forwardI <<
"\n";
374 gzdbg <<
"upward (inertial): " << upwardI <<
"\n";
375 gzdbg <<
"lift dir (inertial): " << liftI <<
"\n";
376 gzdbg <<
"Span direction (normal to LD plane): " << spanwiseI <<
"\n";
377 gzdbg <<
"sweep: " << this->
sweep <<
"\n";
378 gzdbg <<
"alpha: " << this->
alpha <<
"\n";
379 gzdbg <<
"lift: " << lift <<
"\n";
380 gzdbg <<
"drag: " << drag <<
" cd: " 381 << cd <<
" cda: " << this->
cda <<
"\n";
382 gzdbg <<
"moment: " << moment <<
"\n";
383 gzdbg <<
"cp momentArm: " << momentArm <<
"\n";
384 gzdbg <<
"force: " << force <<
"\n";
385 gzdbg <<
"torque: " << torque <<
"\n";
394 this->
link->AddForceAtRelativePosition(force, this->
cp);
395 this->
link->AddTorque(torque);
physics::LinkPtr link
Pointer to link currently targeted by mud joint.
LiftDragPlugin()
Constructor.
double area
effective planeform surface area
double alpha
angle of attack
ignition::math::Vector3d upward
A vector in the lift/drag plane, perpendicular to the forward vector. Inflow velocity orthogonal to f...
double sweep
angle of sweep
physics::ModelPtr model
Pointer to model containing plugin.
double cla
Coefficient of Lift / alpha slope. Lift = C_L * q * S where q (dynamic pressure) = 0...
virtual void OnUpdate()
Callback for World Update events.
sdf::ElementPtr sdf
SDF for this plugin;.
double alphaStall
angle of attach when airfoil stalls
ignition::math::Vector3d forward
Normally, this is taken as a direction parallel to the chord of the airfoil in zero angle of attack f...
double controlJointRadToCL
how much to change CL per radian of control surface joint value.
double rho
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...
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
double cdaStall
Cd-alpha rate after stall.
physics::WorldPtr world
Pointer to world.
event::ConnectionPtr updateConnection
Connection to World Update events.
physics::JointPtr controlJoint
Pointer to a joint that actuates a control surface for this lifting body.
double cma
Coefficient of Moment / alpha slope. Moment = C_M * q * S where q (dynamic pressure) = 0...
double cmaStall
Cm-alpha rate after stall.
ignition::math::Vector3d cp
center of pressure in link local coordinates
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
A plugin that simulates lift and drag.
bool radialSymmetry
if the shape is aerodynamically radially symmetric about the forward direction. Defaults to false for...
double cda
Coefficient of Drag / alpha slope. Drag = C_D * q * S where q (dynamic pressure) = 0...
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
double claStall
Cl-alpha rate after stall.
physics::PhysicsEnginePtr physics
Pointer to physics engine.
double alpha0
initial angle of attack