servo_plugin.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_gazebo_plugins/servo_plugin.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 #include <gazebo/gazebo_config.h>
00033 
00034 #include <sensor_msgs/JointState.h>
00035 
00036 #if (GAZEBO_MAJOR_VERSION > 1) || (GAZEBO_MINOR_VERSION >= 2)
00037   #define RADIAN Radian
00038 #else
00039   #define RADIAN GetAsRadian
00040 #endif
00041 
00042 namespace gazebo {
00043 
00044 GZ_REGISTER_MODEL_PLUGIN(ServoPlugin)
00045 
00046 enum
00047 {
00048   FIRST = 0, SECOND = 1, THIRD = 2
00049 };
00050 
00051 enum
00052 {
00053   xyz, zyx
00054 };
00055 
00056 // Constructor
00057 ServoPlugin::ServoPlugin()
00058 {
00059   rosnode_ = 0;
00060   transform_listener_ = 0;
00061 }
00062 
00063 // Destructor
00064 ServoPlugin::~ServoPlugin()
00065 {
00066   event::Events::DisconnectWorldUpdateBegin(updateConnection);
00067   delete transform_listener_;
00068   rosnode_->shutdown();
00069   delete rosnode_;
00070 }
00071 
00072 // Load the controller
00073 void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00074 {
00075   // Get the world name.
00076   world = _model->GetWorld();
00077 
00078   // default parameters
00079   topicName = "drive";
00080   jointStateName = "joint_states";
00081   robotNamespace.clear();
00082   controlPeriod = 0;
00083   proportionalControllerGain = 8.0;
00084   derivativeControllerGain = 0.0;
00085   maximumVelocity = 0.0;
00086   maximumTorque = 0.0;
00087 
00088   // load parameters
00089   if (_sdf->HasElement("robotNamespace")) robotNamespace = _sdf->Get<std::string>("robotNamespace");
00090   if (_sdf->HasElement("topicName")) topicName = _sdf->Get<std::string>("topicName");
00091   if (_sdf->HasElement("jointStateName")) jointStateName = _sdf->Get<std::string>("jointStateName");
00092   if (_sdf->HasElement("firstServoName")) servo[FIRST].name = _sdf->Get<std::string>("firstServoName");
00093   if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<math::Vector3>("firstServoAxis");
00094   if (_sdf->HasElement("secondServoName")) servo[SECOND].name = _sdf->Get<std::string>("secondServoName");
00095   if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<math::Vector3>("secondServoAxis");
00096   if (_sdf->HasElement("thirdServoName")) servo[THIRD].name = _sdf->Get<std::string>("thirdServoName");
00097   if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<math::Vector3>("thirdServoAxis");
00098   if (_sdf->HasElement("proportionalControllerGain")) proportionalControllerGain = _sdf->Get<double>("proportionalControllerGain");
00099   if (_sdf->HasElement("derivativeControllerGain")) derivativeControllerGain = _sdf->Get<double>("derivativeControllerGain");
00100   if (_sdf->HasElement("maxVelocity")) maximumVelocity = _sdf->Get<double>("maxVelocity");
00101   if (_sdf->HasElement("torque")) maximumTorque = _sdf->Get<double>("torque");
00102 
00103   double controlRate = 0.0;
00104   if (_sdf->HasElement("controlRate")) controlRate = _sdf->Get<double>("controlRate");
00105   controlPeriod = controlRate > 0.0 ? 1.0/controlRate : 0.0;
00106 
00107   servo[FIRST].joint  = _model->GetJoint(servo[FIRST].name);
00108   servo[SECOND].joint = _model->GetJoint(servo[SECOND].name);
00109   servo[THIRD].joint  = _model->GetJoint(servo[THIRD].name);
00110 
00111   if (!servo[FIRST].joint)
00112     gzthrow("The controller couldn't get first joint");
00113 
00114   countOfServos = 1;
00115   if (servo[SECOND].joint) {
00116     countOfServos = 2;
00117     if (servo[THIRD].joint) {
00118       countOfServos = 3;
00119     }
00120   }
00121   else {
00122     if (servo[THIRD].joint) {
00123       gzthrow("The controller couldn't get second joint, but third joint was loaded");
00124     }
00125   }
00126 
00127   if (!ros::isInitialized()) {
00128     int argc = 0;
00129     char** argv = NULL;
00130     ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
00131   }
00132 
00133   rosnode_ = new ros::NodeHandle(robotNamespace);
00134 
00135   transform_listener_ = new tf::TransformListener();
00136   transform_listener_->setExtrapolationLimit(ros::Duration(1.0));
00137 
00138   if (!topicName.empty()) {
00139     ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::QuaternionStamped>(topicName, 1,
00140                                                                                                boost::bind(&ServoPlugin::cmdCallback, this, _1),
00141                                                                                                ros::VoidPtr(), &queue_);
00142     sub_ = rosnode_->subscribe(so);
00143   }
00144 
00145   if (!jointStateName.empty()) {
00146     jointStatePub_ = rosnode_->advertise<sensor_msgs::JointState>(jointStateName, 10);
00147   }
00148 
00149   joint_state.header.frame_id = transform_listener_->resolve(_model->GetLink()->GetName());
00150 
00151   // New Mechanism for Updating every World Cycle
00152   // Listen to the update event. This event is broadcast every
00153   // simulation iteration.
00154   updateConnection = event::Events::ConnectWorldUpdateBegin(
00155       boost::bind(&ServoPlugin::Update, this));
00156 }
00157 
00158 // Initialize the controller
00159 void ServoPlugin::Init()
00160 {
00161   Reset();
00162 }
00163 
00164 // Reset
00165 void ServoPlugin::Reset()
00166 {
00167   // Reset orientation
00168   current_cmd.reset();
00169 
00170   enableMotors = true;
00171 
00172   servo[FIRST].velocity = 0;
00173   servo[SECOND].velocity = 0;
00174   servo[THIRD].velocity = 0;
00175 
00176   prevUpdateTime = world->GetSimTime();
00177 }
00178 
00179 // Update the controller
00180 void ServoPlugin::Update()
00181 {
00182   // handle callbacks
00183   queue_.callAvailable();
00184 
00185   common::Time stepTime;
00186   stepTime = world->GetSimTime() - prevUpdateTime;
00187 
00188   if (controlPeriod == 0.0 || stepTime > controlPeriod) {
00189     CalculateVelocities();
00190     publish_joint_states();
00191     prevUpdateTime = world->GetSimTime();
00192   }
00193 
00194   if (enableMotors)
00195   {
00196     servo[FIRST].joint->SetVelocity(0, servo[FIRST].velocity);
00197     if (countOfServos > 1) {
00198       servo[SECOND].joint->SetVelocity(0, servo[SECOND].velocity);
00199       if (countOfServos > 2) {
00200         servo[THIRD].joint->SetVelocity(0, servo[THIRD].velocity);
00201       }
00202     }
00203 
00204 #if (GAZEBO_MAJOR_VERSION > 4)
00205     servo[FIRST].joint->SetEffortLimit(0, maximumTorque);
00206     if (countOfServos > 1) {
00207       servo[SECOND].joint->SetEffortLimit(0, maximumTorque);
00208       if (countOfServos > 2) {
00209         servo[THIRD].joint->SetEffortLimit(0, maximumTorque);
00210       }
00211     }
00212 #else
00213     servo[FIRST].joint->SetMaxForce(0, maximumTorque);
00214     if (countOfServos > 1) {
00215       servo[SECOND].joint->SetMaxForce(0, maximumTorque);
00216       if (countOfServos > 2) {
00217         servo[THIRD].joint->SetMaxForce(0, maximumTorque);
00218       }
00219     }
00220 #endif
00221   } else {
00222 #if (GAZEBO_MAJOR_VERSION > 4)
00223     servo[FIRST].joint->SetEffortLimit(0, 0.0);
00224     if (countOfServos > 1) {
00225       servo[SECOND].joint->SetEffortLimit(0, 0.0);
00226       if (countOfServos > 2) {
00227         servo[THIRD].joint->SetEffortLimit(0, 0.0);
00228       }
00229     }
00230 #else
00231     servo[FIRST].joint->SetMaxForce(0, 0.0);
00232     if (countOfServos > 1) {
00233       servo[SECOND].joint->SetMaxForce(0, 0.0);
00234       if (countOfServos > 2) {
00235         servo[THIRD].joint->SetMaxForce(0, 0.0);
00236       }
00237     }
00238 #endif
00239   }
00240 }
00241 
00242 void ServoPlugin::CalculateVelocities()
00243 {
00244   tf::StampedTransform transform;
00245   boost::mutex::scoped_lock lock(mutex);
00246 
00247   if(!current_cmd){
00248     geometry_msgs::QuaternionStamped *default_cmd = new geometry_msgs::QuaternionStamped;
00249     default_cmd->header.frame_id = "base_stabilized";
00250     default_cmd->quaternion.w = 1;
00251     current_cmd.reset(default_cmd);
00252   }
00253 
00254   try{
00255     // ros::Time simTime(world->GetSimTime().sec, world->GetSimTime().nsec);
00256     transform_listener_->lookupTransform("base_link", current_cmd->header.frame_id, ros::Time(0), transform);
00257   }
00258   catch (tf::TransformException ex){
00259     ROS_DEBUG("%s",ex.what());
00260     servo[FIRST].velocity = 0.0;
00261     servo[SECOND].velocity = 0.0;
00262     servo[THIRD].velocity = 0.0;
00263     return;
00264   }
00265 
00266   rotation_.Set(current_cmd->quaternion.w, current_cmd->quaternion.x, current_cmd->quaternion.y, current_cmd->quaternion.z);
00267 
00268   math::Quaternion quat(transform.getRotation().getW(),transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ());
00269 
00270   rotation_ = quat * rotation_;
00271 
00272   double temp[5];
00273   double desAngle[3];
00274   double actualAngle[3] = {0.0, 0.0, 0.0};
00275   double actualVel[3] = {0.0, 0.0, 0.0};
00276 
00277   //TODO use countOfServos for calculation
00278   rotationConv = 99;
00279   orderOfAxes[0] = 99;
00280   orderOfAxes[1] = 99;
00281   orderOfAxes[2] = 99;
00282 
00283   switch(countOfServos) {
00284     case 2:
00285       if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1)) {
00286         rotationConv = zyx;
00287         orderOfAxes[0] = 0;
00288         orderOfAxes[1] = 1;
00289       }
00290       else {
00291         if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1)) {
00292           rotationConv = xyz;
00293           orderOfAxes[0] = 0;
00294           orderOfAxes[1] = 1;
00295         }
00296       }
00297       break;
00298 
00299     case 3:
00300       if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.x == 1)) {
00301         rotationConv = zyx;
00302         orderOfAxes[0] = 0;
00303         orderOfAxes[1] = 1;
00304         orderOfAxes[2] = 2;
00305       }
00306       else if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.z == 1)) {
00307         rotationConv = xyz;
00308         orderOfAxes[0] = 0;
00309         orderOfAxes[1] = 1;
00310         orderOfAxes[2] = 2;
00311       }
00312       break;
00313 
00314     case 1:
00315       if (servo[FIRST].axis.y == 1) {
00316          rotationConv = xyz;
00317          orderOfAxes[0] = 1;
00318       }
00319       break;
00320 
00321     default:
00322       gzthrow("Something went wrong. The count of servos is greater than 3");
00323       break;
00324   }
00325 
00326   switch(rotationConv)  {
00327     case zyx:
00328       temp[0] =  2*(rotation_.x*rotation_.y + rotation_.w*rotation_.z);
00329       temp[1] =     rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
00330       temp[2] = -2*(rotation_.x*rotation_.z - rotation_.w*rotation_.y);
00331       temp[3] =  2*(rotation_.y*rotation_.z + rotation_.w*rotation_.x);
00332       temp[4] =     rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
00333       break;
00334 
00335     case xyz:
00336       temp[0] =  -2*(rotation_.y*rotation_.z - rotation_.w*rotation_.x);
00337       temp[1] =      rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
00338       temp[2] =   2*(rotation_.x*rotation_.z + rotation_.w*rotation_.y);
00339       temp[3] =  -2*(rotation_.x*rotation_.y - rotation_.w*rotation_.z);
00340       temp[4] =      rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
00341       break;
00342 
00343     default:
00344       gzthrow("joint order " << rotationConv << " not supported");
00345       break;
00346   }
00347 
00348   desAngle[0] = atan2(temp[0], temp[1]);
00349   desAngle[1] = asin(temp[2]);
00350   desAngle[2] = atan2(temp[3], temp[4]);
00351 
00352   actualAngle[FIRST] = servo[FIRST].joint->GetAngle(0).RADIAN();
00353   actualVel[FIRST] = servo[FIRST].joint->GetVelocity(0);
00354   ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[FIRST].name.c_str(), desAngle[orderOfAxes[FIRST]], actualAngle[FIRST]);
00355   servo[FIRST].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[FIRST]] - actualAngle[FIRST]) - derivativeControllerGain*actualVel[FIRST]);
00356   if (maximumVelocity > 0.0 && fabs(servo[FIRST].velocity) > maximumVelocity) servo[FIRST].velocity = (servo[FIRST].velocity > 0 ? maximumVelocity : -maximumVelocity);
00357 
00358   if (countOfServos > 1) {
00359     actualAngle[SECOND] = servo[SECOND].joint->GetAngle(0).RADIAN();
00360     actualVel[SECOND] = servo[SECOND].joint->GetVelocity(0);
00361     ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[SECOND].name.c_str(), desAngle[orderOfAxes[SECOND]], actualAngle[SECOND]);
00362     servo[SECOND].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[SECOND]] - actualAngle[SECOND]) - derivativeControllerGain*actualVel[SECOND]);
00363     if (maximumVelocity > 0.0 && fabs(servo[SECOND].velocity) > maximumVelocity) servo[SECOND].velocity = (servo[SECOND].velocity > 0 ? maximumVelocity : -maximumVelocity);
00364 
00365     if (countOfServos == 3) {
00366       actualAngle[THIRD] = servo[THIRD].joint->GetAngle(0).RADIAN();
00367       actualVel[THIRD] = servo[THIRD].joint->GetVelocity(0);
00368       ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[THIRD].name.c_str(), desAngle[orderOfAxes[THIRD]], actualAngle[THIRD]);
00369       servo[THIRD].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[THIRD]] - actualAngle[THIRD]) - derivativeControllerGain*actualVel[THIRD]);
00370       if (maximumVelocity > 0.0 && fabs(servo[THIRD].velocity) > maximumVelocity) servo[THIRD].velocity = (servo[THIRD].velocity > 0 ? maximumVelocity : -maximumVelocity);
00371     }
00372   }
00373 
00374   // Changed motors to be always on, which is probably what we want anyway
00375   enableMotors = true; //myIface->data->cmdEnableMotors > 0;
00376 }
00377 
00378 // NEW: Store the velocities from the ROS message
00379 void ServoPlugin::cmdCallback(const geometry_msgs::QuaternionStamped::ConstPtr& cmd_msg)
00380 {
00381   boost::mutex::scoped_lock lock(mutex);
00382   current_cmd = cmd_msg;
00383 }
00384 
00385 void ServoPlugin::publish_joint_states()
00386 {
00387   if (!jointStatePub_) return;
00388 
00389   joint_state.header.stamp.sec = (world->GetSimTime()).sec;
00390   joint_state.header.stamp.nsec = (world->GetSimTime()).nsec;
00391 
00392   joint_state.name.resize(countOfServos);
00393   joint_state.position.resize(countOfServos);
00394   joint_state.velocity.resize(countOfServos);
00395   joint_state.effort.resize(countOfServos);
00396 
00397   for (unsigned int i = 0; i < countOfServos; i++) {
00398     joint_state.name[i] = servo[i].joint->GetName();
00399     joint_state.position[i] = servo[i].joint->GetAngle(0).RADIAN();
00400     joint_state.velocity[i] = servo[i].joint->GetVelocity(0);
00401     joint_state.effort[i] = servo[i].joint->GetForce(0u);
00402   }
00403 
00404   jointStatePub_.publish(joint_state);
00405 }
00406 
00407 } // namespace gazebo


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Sat Jun 8 2019 19:52:36