quadrotor_simple_controller.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_quadrotor_gazebo_plugins/quadrotor_simple_controller.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 
00033 #include <cmath>
00034 
00035 namespace gazebo {
00036 
00037 GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController()
00038 {
00039 }
00040 
00042 // Destructor
00043 GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController()
00044 {
00045   event::Events::DisconnectWorldUpdateStart(updateConnection);
00046 
00047   node_handle_->shutdown();
00048   delete node_handle_;
00049 }
00050 
00052 // Load the controller
00053 void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00054 {
00055   world = _model->GetWorld();
00056 
00057   // load parameters
00058   if (!_sdf->HasElement("robotNamespace") || !_sdf->GetElement("robotNamespace")->GetValue())
00059     namespace_.clear();
00060   else
00061     namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00062 
00063   if (!_sdf->HasElement("topicName") || !_sdf->GetElement("topicName")->GetValue())
00064     velocity_topic_ = "cmd_vel";
00065   else
00066     velocity_topic_ = _sdf->GetElement("topicName")->GetValueString();
00067 
00068   if (!_sdf->HasElement("imuTopic") || !_sdf->GetElement("imuTopic")->GetValue())
00069     imu_topic_.clear();
00070   else
00071     imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString();
00072 
00073   if (!_sdf->HasElement("stateTopic") || !_sdf->GetElement("stateTopic")->GetValue())
00074     state_topic_.clear();
00075   else
00076     state_topic_ = _sdf->GetElement("stateTopic")->GetValueString();
00077 
00078   if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
00079   {
00080     link = _model->GetLink();
00081     link_name_ = link->GetName();
00082   }
00083   else {
00084     link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00085     link = _model->GetLink(link_name_);
00086   }
00087 
00088   if (!link)
00089   {
00090     ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00091     return;
00092   }
00093 
00094   if (!_sdf->HasElement("maxForce") || !_sdf->GetElement("maxForce")->GetValue())
00095     max_force_ = -1;
00096   else
00097     max_force_ = _sdf->GetElement("maxForce")->GetValueDouble();
00098 
00099   controllers_.roll.Load(_sdf, "rollpitch");
00100   controllers_.pitch.Load(_sdf, "rollpitch");
00101   controllers_.yaw.Load(_sdf, "yaw");
00102   controllers_.velocity_x.Load(_sdf, "velocityXY");
00103   controllers_.velocity_y.Load(_sdf, "velocityXY");
00104   controllers_.velocity_z.Load(_sdf, "velocityZ");
00105 
00106   // Get inertia and mass of quadrotor body
00107   inertia = link->GetInertial()->GetPrincipalMoments();
00108   mass = link->GetInertial()->GetMass();
00109 
00110   node_handle_ = new ros::NodeHandle(namespace_);
00111 
00112   // subscribe command
00113   if (!velocity_topic_.empty())
00114   {
00115     ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
00116       velocity_topic_, 1,
00117       boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1),
00118       ros::VoidPtr(), &callback_queue_);
00119     velocity_subscriber_ = node_handle_->subscribe(ops);
00120   }
00121 
00122   // subscribe imu
00123   if (!imu_topic_.empty())
00124   {
00125     ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
00126       imu_topic_, 1,
00127       boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1),
00128       ros::VoidPtr(), &callback_queue_);
00129     imu_subscriber_ = node_handle_->subscribe(ops);
00130 
00131     ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
00132   }
00133 
00134   // subscribe state
00135   if (!state_topic_.empty())
00136   {
00137     ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
00138       state_topic_, 1,
00139       boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1),
00140       ros::VoidPtr(), &callback_queue_);
00141     state_subscriber_ = node_handle_->subscribe(ops);
00142 
00143     ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
00144   }
00145 
00146   // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorSimpleController::CallbackQueueThread,this ) );
00147 
00148 
00149   Reset();
00150 
00151   // New Mechanism for Updating every World Cycle
00152   // Listen to the update event. This event is broadcast every
00153   // simulation iteration.
00154   controlTimer.Load(world, _sdf);
00155   updateConnection = event::Events::ConnectWorldUpdateStart(
00156       boost::bind(&GazeboQuadrotorSimpleController::Update, this));
00157 }
00158 
00160 // Callbacks
00161 void GazeboQuadrotorSimpleController::VelocityCallback(const geometry_msgs::TwistConstPtr& velocity)
00162 {
00163   velocity_command_ = *velocity;
00164 }
00165 
00166 void GazeboQuadrotorSimpleController::ImuCallback(const sensor_msgs::ImuConstPtr& imu)
00167 {
00168   pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
00169   euler = pose.rot.GetAsEuler();
00170   angular_velocity = pose.rot.RotateVector(math::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z));
00171 }
00172 
00173 void GazeboQuadrotorSimpleController::StateCallback(const nav_msgs::OdometryConstPtr& state)
00174 {
00175   math::Vector3 velocity1(velocity);
00176 
00177   if (imu_topic_.empty()) {
00178     pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z);
00179     pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z);
00180     euler = pose.rot.GetAsEuler();
00181     angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z);
00182   }
00183 
00184   velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z);
00185 
00186   // calculate acceleration
00187   double dt = !state_stamp.isZero() ? (state->header.stamp - state_stamp).toSec() : 0.0;
00188   state_stamp = state->header.stamp;
00189   if (dt > 0.0) {
00190     acceleration = (velocity - velocity1) / dt;
00191   } else {
00192     acceleration.Set();
00193   }
00194 }
00195 
00197 // Update the controller
00198 void GazeboQuadrotorSimpleController::Update()
00199 {
00200   // Get new commands/state
00201   callback_queue_.callAvailable();
00202 
00203   double dt;
00204   if (controlTimer.update(dt) && dt > 0.0) {
00205     // Get Pose/Orientation from Gazebo (if no state subscriber is active)
00206     if (imu_topic_.empty()) {
00207       pose = link->GetWorldPose();
00208       angular_velocity = link->GetWorldAngularVel();
00209       euler = pose.rot.GetAsEuler();
00210     }
00211     if (state_topic_.empty()) {
00212       acceleration = (link->GetWorldLinearVel() - velocity) / dt;
00213       velocity = link->GetWorldLinearVel();
00214     }
00215 
00216   //  static Time lastDebug;
00217   //  if ((world->GetSimTime() - lastDebug).Double() > 0.5) {
00218   //    ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Velocity:         gazebo = [" << link->GetWorldLinearVel()   << "], state = [" << velocity << "]");
00219   //    ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Acceleration:     gazebo = [" << link->GetWorldLinearAccel() << "], state = [" << acceleration << "]");
00220   //    ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Angular Velocity: gazebo = [" << link->GetWorldAngularVel() << "], state = [" << angular_velocity << "]");
00221   //    lastDebug = world->GetSimTime();
00222   //  }
00223 
00224     // Get gravity
00225     math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity());
00226     double gravity = gravity_body.GetLength();
00227     double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().GetDotProd(gravity_body);  // Get gravity
00228 
00229     // Rotate vectors to coordinate frames relevant for control
00230     math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
00231     math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
00232     math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
00233     math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
00234 
00235     // update controllers
00236     force.Set(0.0, 0.0, 0.0);
00237     torque.Set(0.0, 0.0, 0.0);
00238     double pitch_command =  controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity;
00239     double roll_command  = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity;
00240     torque.x = inertia.x *  controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt);
00241     torque.y = inertia.y *  controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt);
00242     // torque.x = inertia.x *  controllers_.roll.update(-velocity_command_.linear.y/gravity, euler.x, angular_velocity_body.x, dt);
00243     // torque.y = inertia.y *  controllers_.pitch.update(velocity_command_.linear.x/gravity, euler.y, angular_velocity_body.y, dt);
00244     torque.z = inertia.z *  controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt);
00245     force.z  = mass      * (controllers_.velocity_z.update(velocity_command_.linear.z,  velocity.z, acceleration.z, dt) + load_factor * gravity);
00246     if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_;
00247     if (force.z < 0.0) force.z = 0.0;
00248 
00249   //  static double lastDebugOutput = 0.0;
00250   //  if (last_time.Double() - lastDebugOutput > 0.1) {
00251   //    ROS_DEBUG_NAMED("quadrotor_simple_controller", "Velocity = [%g %g %g], Acceleration = [%g %g %g]", velocity.x, velocity.y, velocity.z, acceleration.x, acceleration.y, acceleration.z);
00252   //    ROS_DEBUG_NAMED("quadrotor_simple_controller", "Command: linear = [%g %g %g], angular = [%g %g %g], roll/pitch = [%g %g]", velocity_command_.linear.x, velocity_command_.linear.y, velocity_command_.linear.z, velocity_command_.angular.x*180/M_PI, velocity_command_.angular.y*180/M_PI, velocity_command_.angular.z*180/M_PI, roll_command*180/M_PI, pitch_command*180/M_PI);
00253   //    ROS_DEBUG_NAMED("quadrotor_simple_controller", "Mass: %g kg, Inertia: [%g %g %g], Load: %g g", mass, inertia.x, inertia.y, inertia.z, load_factor);
00254   //    ROS_DEBUG_NAMED("quadrotor_simple_controller", "Force: [%g %g %g], Torque: [%g %g %g]", force.x, force.y, force.z, torque.x, torque.y, torque.z);
00255   //    lastDebugOutput = last_time.Double();
00256   //  }
00257   }
00258 
00259   // set force and torque in gazebo
00260   link->AddRelativeForce(force);
00261   link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().GetCrossProd(force));
00262 }
00263 
00265 // Reset the controller
00266 void GazeboQuadrotorSimpleController::Reset()
00267 {
00268   controllers_.roll.reset();
00269   controllers_.pitch.reset();
00270   controllers_.yaw.reset();
00271   controllers_.velocity_x.reset();
00272   controllers_.velocity_y.reset();
00273   controllers_.velocity_z.reset();
00274 
00275   force.Set();
00276   torque.Set();
00277 
00278   // reset state
00279   pose.Reset();
00280   velocity.Set();
00281   angular_velocity.Set();
00282   acceleration.Set();
00283   euler.Set();
00284   state_stamp = ros::Time();
00285 }
00286 
00288 // PID controller implementation
00289 GazeboQuadrotorSimpleController::PIDController::PIDController()
00290 {
00291 }
00292 
00293 GazeboQuadrotorSimpleController::PIDController::~PIDController()
00294 {
00295 }
00296 
00297 void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix)
00298 {
00299   gain_p = 0.0;
00300   gain_d = 0.0;
00301   gain_i = 0.0;
00302   time_constant = 0.0;
00303   limit = -1.0;
00304 
00305   if (!_sdf) return;
00306   // _sdf->PrintDescription(_sdf->GetName());
00307   if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->GetValueDouble();
00308   if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->GetValueDouble();
00309   if (_sdf->HasElement(prefix + "IntegralGain"))     gain_i = _sdf->GetElement(prefix + "IntegralGain")->GetValueDouble();
00310   if (_sdf->HasElement(prefix + "TimeConstant"))     time_constant = _sdf->GetElement(prefix + "TimeConstant")->GetValueDouble();
00311   if (_sdf->HasElement(prefix + "Limit"))            limit = _sdf->GetElement(prefix + "Limit")->GetValueDouble();
00312 }
00313 
00314 double GazeboQuadrotorSimpleController::PIDController::update(double new_input, double x, double dx, double dt)
00315 {
00316   // limit command
00317   if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit;
00318 
00319   // filter command
00320   if (dt + time_constant > 0.0) {
00321     dinput = (new_input - input) / (dt + time_constant);
00322     input  = (dt * new_input + time_constant * input) / (dt + time_constant);
00323   }
00324 
00325   // update proportional, differential and integral errors
00326   p = input - x;
00327   d = dinput - dx;
00328   i = i + dt * p;
00329 
00330   // update control output
00331   output = gain_p * p + gain_d * d + gain_i * i;
00332   return output;
00333 }
00334 
00335 void GazeboQuadrotorSimpleController::PIDController::reset()
00336 {
00337   input = dinput = 0;
00338   p = i = d = output = 0;
00339 }
00340 
00341 // Register this plugin with the simulator
00342 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorSimpleController)
00343 
00344 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_quadrotor_gazebo_plugins
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:48:25