//////////////////////////////////////////////////////////////////////////////// // 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; }