00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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
00043 GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController()
00044 {
00045 event::Events::DisconnectWorldUpdateStart(updateConnection);
00046
00047 node_handle_->shutdown();
00048 delete node_handle_;
00049 }
00050
00052
00053 void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00054 {
00055 world = _model->GetWorld();
00056
00057
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
00107 inertia = link->GetInertial()->GetPrincipalMoments();
00108 mass = link->GetInertial()->GetMass();
00109
00110 node_handle_ = new ros::NodeHandle(namespace_);
00111
00112
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
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
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
00147
00148
00149 Reset();
00150
00151
00152
00153
00154 controlTimer.Load(world, _sdf);
00155 updateConnection = event::Events::ConnectWorldUpdateStart(
00156 boost::bind(&GazeboQuadrotorSimpleController::Update, this));
00157 }
00158
00160
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
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
00198 void GazeboQuadrotorSimpleController::Update()
00199 {
00200
00201 callback_queue_.callAvailable();
00202
00203 double dt;
00204 if (controlTimer.update(dt) && dt > 0.0) {
00205
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
00217
00218
00219
00220
00221
00222
00223
00224
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);
00228
00229
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
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
00243
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
00250
00251
00252
00253
00254
00255
00256
00257 }
00258
00259
00260 link->AddRelativeForce(force);
00261 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().GetCrossProd(force));
00262 }
00263
00265
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
00279 pose.Reset();
00280 velocity.Set();
00281 angular_velocity.Set();
00282 acceleration.Set();
00283 euler.Set();
00284 state_stamp = ros::Time();
00285 }
00286
00288
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
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
00317 if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit;
00318
00319
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
00326 p = input - x;
00327 d = dinput - dx;
00328 i = i + dt * p;
00329
00330
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
00342 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorSimpleController)
00343
00344 }