$search
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_controller/quadrotor_simple_controller.h> 00030 00031 #include <gazebo/Sensor.hh> 00032 #include <gazebo/Global.hh> 00033 #include <gazebo/XMLConfig.hh> 00034 #include <gazebo/Simulator.hh> 00035 #include <gazebo/gazebo.h> 00036 #include <gazebo/World.hh> 00037 #include <gazebo/PhysicsEngine.hh> 00038 #include <gazebo/GazeboError.hh> 00039 #include <gazebo/ControllerFactory.hh> 00040 00041 #include <cmath> 00042 00043 using namespace gazebo; 00044 00045 GZ_REGISTER_DYNAMIC_CONTROLLER("hector_gazebo_quadrotor_simple_controller", GazeboQuadrotorSimpleController) 00046 00047 GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController(Entity *parent) 00048 : Controller(parent) 00049 , controllers_(parameters) 00050 { 00051 parent_ = dynamic_cast<Model*>(parent); 00052 if (!parent_) gzthrow("GazeboQuadrotorSimpleController controller requires a Model as its parent"); 00053 00054 if (!ros::isInitialized()) 00055 { 00056 int argc = 0; 00057 char** argv = NULL; 00058 ros::init(argc,argv, "gazebo", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); 00059 } 00060 00061 Param::Begin(¶meters); 00062 namespace_param_ = new ParamT<std::string>("robotNamespace", "", false); 00063 body_name_param_ = new ParamT<std::string>("bodyName", "", true); 00064 velocity_topic_param_ = new ParamT<std::string>("topicName", "cmd_vel", false); 00065 imu_topic_param_ = new ParamT<std::string>("imuTopic", "", false); 00066 state_topic_param_ = new ParamT<std::string>("stateTopic", "", false); 00067 max_force_param_ = new ParamT<double>("maxForce", -1.0, false); 00068 Param::End(); 00069 } 00070 00072 // Destructor 00073 GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController() 00074 { 00075 delete namespace_param_; 00076 delete body_name_param_; 00077 delete velocity_topic_param_; 00078 delete state_topic_param_; 00079 delete imu_topic_param_; 00080 delete max_force_param_; 00081 } 00082 00084 // Load the controller 00085 void GazeboQuadrotorSimpleController::LoadChild(XMLConfigNode *node) 00086 { 00087 namespace_param_->Load(node); 00088 namespace_ = namespace_param_->GetValue(); 00089 00090 body_name_param_->Load(node); 00091 body_name_ = body_name_param_->GetValue(); 00092 00093 // assert that the body by body_name_ exists 00094 body_ = dynamic_cast<Body*>(parent_->GetBody(body_name_)); 00095 if (!body_) gzthrow("gazebo_quadrotor_simple_controller plugin error: body_name_: " << body_name_ << "does not exist\n"); 00096 00097 // check update rate against world physics update rate 00098 // should be equal or higher to guarantee the wrench applied is not "diluted" 00099 if (this->updatePeriod > 0 && 00100 (gazebo::World::Instance()->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod)) 00101 ROS_ERROR_NAMED(this->GetName(), "gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)"); 00102 00103 velocity_topic_param_->Load(node); 00104 velocity_topic_ = velocity_topic_param_->GetValue(); 00105 imu_topic_param_->Load(node); 00106 imu_topic_ = imu_topic_param_->GetValue(); 00107 state_topic_param_->Load(node); 00108 state_topic_ = state_topic_param_->GetValue(); 00109 00110 controllers_.roll.LoadChild(node->GetChild("rollpitch")); 00111 controllers_.pitch.LoadChild(node->GetChild("rollpitch")); 00112 controllers_.yaw.LoadChild(node->GetChild("yaw")); 00113 controllers_.velocity_x.LoadChild(node->GetChild("velocity_xy")); 00114 controllers_.velocity_y.LoadChild(node->GetChild("velocity_xy")); 00115 controllers_.velocity_z.LoadChild(node->GetChild("velocity_z")); 00116 00117 max_force_param_->Load(node); 00118 max_force_ = max_force_param_->GetValue(); 00119 00120 // Get inertia and mass of quadrotor body 00121 inertia = body_->GetMass().GetPrincipalMoments(); 00122 mass = body_->GetMass().GetAsDouble(); 00123 } 00124 00126 // Initialize the controller 00127 void GazeboQuadrotorSimpleController::InitChild() 00128 { 00129 node_handle_ = new ros::NodeHandle(namespace_); 00130 00131 // subscribe command 00132 if (!velocity_topic_.empty()) 00133 { 00134 ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>( 00135 velocity_topic_, 1, 00136 boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1), 00137 ros::VoidPtr(), &callback_queue_); 00138 velocity_subscriber_ = node_handle_->subscribe(ops); 00139 } 00140 00141 // subscribe imu 00142 if (!imu_topic_.empty()) 00143 { 00144 ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>( 00145 imu_topic_, 1, 00146 boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1), 00147 ros::VoidPtr(), &callback_queue_); 00148 imu_subscriber_ = node_handle_->subscribe(ops); 00149 00150 ROS_INFO_NAMED(this->GetName(), "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str()); 00151 } 00152 00153 // subscribe state 00154 if (!state_topic_.empty()) 00155 { 00156 ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>( 00157 state_topic_, 1, 00158 boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1), 00159 ros::VoidPtr(), &callback_queue_); 00160 state_subscriber_ = node_handle_->subscribe(ops); 00161 00162 ROS_INFO_NAMED(this->GetName(), "Using state information on topic %s as source of state information.", state_topic_.c_str()); 00163 } 00164 00165 // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorSimpleController::CallbackQueueThread,this ) ); 00166 } 00167 00169 // Callbacks 00170 void GazeboQuadrotorSimpleController::VelocityCallback(const geometry_msgs::TwistConstPtr& velocity) 00171 { 00172 velocity_command_ = *velocity; 00173 } 00174 00175 void GazeboQuadrotorSimpleController::ImuCallback(const sensor_msgs::ImuConstPtr& imu) 00176 { 00177 pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z); 00178 euler = pose.rot.GetAsEuler(); 00179 angular_velocity = pose.rot.RotateVector(Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z)); 00180 } 00181 00182 void GazeboQuadrotorSimpleController::StateCallback(const nav_msgs::OdometryConstPtr& state) 00183 { 00184 Vector3 velocity1(velocity); 00185 00186 if (imu_topic_.empty()) { 00187 pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z); 00188 pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z); 00189 euler = pose.rot.GetAsEuler(); 00190 angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z); 00191 } 00192 00193 velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z); 00194 00195 // calculate acceleration 00196 double dt = !state_stamp.isZero() ? (state->header.stamp - state_stamp).toSec() : 0.0; 00197 state_stamp = state->header.stamp; 00198 if (dt > 0.0) { 00199 acceleration = (velocity - velocity1) / dt; 00200 } else { 00201 acceleration.Set(); 00202 } 00203 } 00204 00206 // Update the controller 00207 void GazeboQuadrotorSimpleController::UpdateChild() 00208 { 00209 Vector3 force, torque; 00210 00211 // Get new commands/state 00212 callback_queue_.callAvailable(); 00213 00214 // Get simulator time 00215 double dt = Simulator::Instance()->GetSimTime() - lastUpdate; 00216 if (dt == 0.0) return; 00217 00218 // Get Pose/Orientation from Gazebo (if no state subscriber is active) 00219 if (imu_topic_.empty()) { 00220 pose = body_->GetWorldPose(); 00221 angular_velocity = body_->GetWorldAngularVel(); 00222 euler = pose.rot.GetAsEuler(); 00223 } 00224 if (state_topic_.empty()) { 00225 acceleration = (body_->GetWorldLinearVel() - velocity) / dt; 00226 velocity = body_->GetWorldLinearVel(); 00227 } 00228 00229 // static Time lastDebug; 00230 // if ((Simulator::Instance()->GetSimTime() - lastDebug).Double() > 0.5) { 00231 // ROS_DEBUG_STREAM_NAMED(this->GetName(), "Velocity: gazebo = [" << body_->GetWorldLinearVel() << "], state = [" << velocity << "]"); 00232 // ROS_DEBUG_STREAM_NAMED(this->GetName(), "Acceleration: gazebo = [" << body_->GetWorldLinearAccel() << "], state = [" << acceleration << "]"); 00233 // ROS_DEBUG_STREAM_NAMED(this->GetName(), "Angular Velocity: gazebo = [" << body_->GetWorldAngularVel() << "], state = [" << angular_velocity << "]"); 00234 // lastDebug = Simulator::Instance()->GetSimTime(); 00235 // } 00236 00237 // Get gravity 00238 Vector3 gravity_body = pose.rot.RotateVector(World::Instance()->GetPhysicsEngine()->GetGravity()); 00239 double gravity = gravity_body.GetLength(); 00240 double load_factor = gravity * gravity / World::Instance()->GetPhysicsEngine()->GetGravity().GetDotProd(gravity_body); // Get gravity 00241 00242 // Rotate vectors to coordinate frames relevant for control 00243 Quatern heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2)); 00244 Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity); 00245 Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration); 00246 Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity); 00247 00248 // update controllers 00249 force.Set(0.0, 0.0, 0.0); 00250 torque.Set(0.0, 0.0, 0.0); 00251 double pitch_command = controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity; 00252 double roll_command = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity; 00253 torque.x = inertia.x * controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt); 00254 torque.y = inertia.y * controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt); 00255 // torque.x = inertia.x * controllers_.roll.update(-velocity_command_.linear.y/gravity, euler.x, angular_velocity_body.x, dt); 00256 // torque.y = inertia.y * controllers_.pitch.update(velocity_command_.linear.x/gravity, euler.y, angular_velocity_body.y, dt); 00257 torque.z = inertia.z * controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt); 00258 force.z = mass * (controllers_.velocity_z.update(velocity_command_.linear.z, velocity.z, acceleration.z, dt) + load_factor * gravity); 00259 if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_; 00260 if (force.z < 0.0) force.z = 0.0; 00261 00262 // static double lastDebugOutput = 0.0; 00263 // if (lastUpdate.Double() - lastDebugOutput > 0.1) { 00264 // ROS_DEBUG_NAMED(this->GetName(), "Velocity = [%g %g %g], Acceleration = [%g %g %g]", velocity.x, velocity.y, velocity.z, acceleration.x, acceleration.y, acceleration.z); 00265 // ROS_DEBUG_NAMED(this->GetName(), "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); 00266 // ROS_DEBUG_NAMED(this->GetName(), "Mass: %g kg, Inertia: [%g %g %g], Load: %g g", mass, inertia.x, inertia.y, inertia.z, load_factor); 00267 // ROS_DEBUG_NAMED(this->GetName(), "Force: [%g %g %g], Torque: [%g %g %g]", force.x, force.y, force.z, torque.x, torque.y, torque.z); 00268 // lastDebugOutput = lastUpdate.Double(); 00269 // } 00270 00271 // set force and torque in gazebo 00272 body_->SetForce(force); 00273 body_->SetTorque(torque - body_->GetMass().GetCoG().GetCrossProd(force)); 00274 } 00275 00277 // Finalize the controller 00278 void GazeboQuadrotorSimpleController::FiniChild() 00279 { 00280 node_handle_->shutdown(); 00281 delete node_handle_; 00282 } 00283 00285 // Reset the controller 00286 void GazeboQuadrotorSimpleController::ResetChild() 00287 { 00288 controllers_.roll.reset(); 00289 controllers_.pitch.reset(); 00290 controllers_.yaw.reset(); 00291 controllers_.velocity_x.reset(); 00292 controllers_.velocity_y.reset(); 00293 controllers_.velocity_z.reset(); 00294 00295 body_->SetForce(Vector3(0,0,0)); 00296 body_->SetTorque(Vector3(0,0,0)); 00297 00298 // reset state 00299 pose.Reset(); 00300 velocity.Set(); 00301 angular_velocity.Set(); 00302 acceleration.Set(); 00303 euler.Set(); 00304 state_stamp = ros::Time(); 00305 } 00306 00308 // PID controller implementation 00309 GazeboQuadrotorSimpleController::PIDController::PIDController(std::vector<Param*>& parameters) 00310 { 00311 Param::Begin(¶meters); 00312 gain_p_param_ = new ParamT<double>("proportionalGain", 0.0, false); 00313 gain_d_param_ = new ParamT<double>("differentialGain", 0.0, false); 00314 gain_i_param_ = new ParamT<double>("integralGain", 0.0, false); 00315 time_constant_param_ = new ParamT<double>("timeConstant", 0.0, false); 00316 limit_param_ = new ParamT<double>("limit", -1.0, false); 00317 Param::End(); 00318 00319 reset(); 00320 } 00321 00322 GazeboQuadrotorSimpleController::PIDController::~PIDController() 00323 { 00324 delete gain_p_param_; 00325 delete gain_d_param_; 00326 delete gain_i_param_; 00327 delete time_constant_param_; 00328 delete limit_param_; 00329 } 00330 00331 void GazeboQuadrotorSimpleController::PIDController::LoadChild(XMLConfigNode *node) 00332 { 00333 if (!node) return; 00334 gain_p_param_->Load(node); 00335 gain_p = gain_p_param_->GetValue(); 00336 gain_d_param_->Load(node); 00337 gain_d = gain_d_param_->GetValue(); 00338 gain_i_param_->Load(node); 00339 gain_i = gain_i_param_->GetValue(); 00340 time_constant_param_->Load(node); 00341 time_constant = time_constant_param_->GetValue(); 00342 limit_param_->Load(node); 00343 limit = limit_param_->GetValue(); 00344 } 00345 00346 double GazeboQuadrotorSimpleController::PIDController::update(double new_input, double x, double dx, double dt) 00347 { 00348 // limit command 00349 if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit; 00350 00351 // filter command 00352 if (dt + time_constant > 0.0) { 00353 dinput = (new_input - input) / (dt + time_constant); 00354 input = (dt * new_input + time_constant * input) / (dt + time_constant); 00355 } 00356 00357 // update proportional, differential and integral errors 00358 p = input - x; 00359 d = dinput - dx; 00360 i = i + dt * p; 00361 00362 // update control output 00363 output = gain_p * p + gain_d * d + gain_i * i; 00364 return output; 00365 } 00366 00367 void GazeboQuadrotorSimpleController::PIDController::reset() 00368 { 00369 input = dinput = 0; 00370 p = i = d = output = 0; 00371 }