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