////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorAerodynamics::Update()
{
// Get simulator time
Time current_time = world->GetSimTime();
Time dt = current_time - last_time_;
last_time_ = current_time;
if (dt <= 0.0) return;
// Get new commands/state
callback_queue_.callAvailable();
// fill input vector u for drag model
geometry_msgs::Quaternion orientation;
fromQuaternion(link->GetWorldPose().rot, orientation);
model_.setOrientation(orientation);
geometry_msgs::Twist twist;
fromVector(link->GetWorldLinearVel(), twist.linear);
fromVector(link->GetWorldAngularVel(), twist.angular);
model_.setTwist(twist);
// update the model
model_.update(dt.Double());
// get wrench from model
Vector3 force, torque;
toVector(model_.getWrench().force, force);
toVector(model_.getWrench().torque, torque);
// set force and torque in gazebo
link->AddRelativeForce(force);
link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
}
RotMatrix<3>::RotMatrix(const Quaternion& q,
const bool not_flip) : m_flip(false), m_valid(false),
m_age(0)
{
fromQuaternion(q, not_flip);
}
/**
@brief Default constructor with presets.
@param quaternion Calculate starting values from this quaternion
*/
explicit Euler(const Quaternion &quaternion) {
fromQuaternion(quaternion);
}
/// Copy assignment operator (Quaternion)
inline Euler& operator=(const Quaternion &quaternion) {
fromQuaternion(quaternion);
return *this;
}
