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_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
00057 ServoPlugin::ServoPlugin()
00058 {
00059 rosnode_ = 0;
00060 transform_listener_ = 0;
00061 }
00062
00063
00064 ServoPlugin::~ServoPlugin()
00065 {
00066 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00067 delete transform_listener_;
00068 rosnode_->shutdown();
00069 delete rosnode_;
00070 }
00071
00072
00073 void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00074 {
00075
00076 world = _model->GetWorld();
00077
00078
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
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
00152
00153
00154 updateConnection = event::Events::ConnectWorldUpdateBegin(
00155 boost::bind(&ServoPlugin::Update, this));
00156 }
00157
00158
00159 void ServoPlugin::Init()
00160 {
00161 Reset();
00162 }
00163
00164
00165 void ServoPlugin::Reset()
00166 {
00167
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
00180 void ServoPlugin::Update()
00181 {
00182
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
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
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
00375 enableMotors = true;
00376 }
00377
00378
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 }