gazebo_ros_katana_gripper.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2011  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * gazebo_ros_katana_gripper.cpp
00020  *
00021  *  Created on: 29.08.2011
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana_gazebo_plugins/gazebo_ros_katana_gripper.h>
00026 #include <sensor_msgs/JointState.h>
00027 
00028 #include <ros/time.h>
00029 
00030 using namespace gazebo;
00031 
00032 GazeboRosKatanaGripper::GazeboRosKatanaGripper() :
00033     publish_counter_(0)
00034 {
00035   this->spinner_thread_ = new boost::thread(boost::bind(&GazeboRosKatanaGripper::spin, this));
00036 
00037   for (size_t i = 0; i < NUM_JOINTS; ++i)
00038   {
00039     joints_[i].reset();
00040   }
00041 
00042 }
00043 
00044 GazeboRosKatanaGripper::~GazeboRosKatanaGripper()
00045 {
00046   // delete all gripper actions
00047   for (std::size_t i = 0; i != gripper_action_list_.size(); i++)
00048   {
00049     /* delete object at pointer */
00050     delete gripper_action_list_[i];
00051   }
00052 
00053   rosnode_->shutdown();
00054   this->spinner_thread_->join();
00055   delete this->spinner_thread_;
00056   delete rosnode_;
00057 }
00058 
00059 void GazeboRosKatanaGripper::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00060 {
00061   this->my_world_ = _parent->GetWorld();
00062 
00063   this->my_parent_ = _parent;
00064   if (!this->my_parent_)
00065   {
00066     ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
00067     return;
00068   }
00069 
00070   this->node_namespace_ = "katana/";
00071   if (_sdf->HasElement("robotNamespace"))
00072     this->node_namespace_ = _sdf->Get<std::string>("node_namespace") + "/";
00073 
00074   torque_ = 0.5;
00075   if (_sdf->HasElement("max_torque"))
00076     torque_ = _sdf->Get<float>("max_torque");
00077 
00078   joint_names_.resize(NUM_JOINTS);
00079   joint_names_[0] = "katana_r_finger_joint";
00080   if (_sdf->HasElement("r_finger_joint"))
00081     joint_names_[0] = _sdf->Get<std::string>("r_finger_joint");
00082 
00083   joint_names_[1] = "katana_r_finger_joint";
00084   if (_sdf->HasElement("l_finger_joint"))
00085     joint_names_[1] = _sdf->Get<std::string>("l_finger_joint");
00086 
00087   if (!ros::isInitialized())
00088   {
00089     int argc = 0;
00090     char** argv = NULL;
00091     ros::init(argc, argv, "gazebo_ros_katana_gripper",
00092               ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
00093   }
00094 
00095   rosnode_ = new ros::NodeHandle(node_namespace_);
00096 
00097   controller_state_pub_ = rosnode_->advertise<katana_msgs::GripperControllerState>("gripper_controller_state", 1);
00098 
00099   for (size_t i = 0; i < NUM_JOINTS; ++i)
00100   {
00101     joints_[i] = my_parent_->GetJoint(joint_names_[i]);
00102     if (!joints_[i])
00103       gzthrow("The controller couldn't get joint " << joint_names_[i]);
00104   }
00105 
00106   // construct pid controller
00107   if (!pid_controller_.init(ros::NodeHandle(*rosnode_, "gripper_pid")))
00108   {
00109     ROS_FATAL("gazebo_ros_katana_gripper could not construct PID controller!");
00110   }
00111 
00112   // create gripper actions
00113   katana_gazebo_plugins::IGazeboRosKatanaGripperAction* gripper_grasp_controller_ =
00114       new katana_gazebo_plugins::KatanaGripperGraspController(ros::NodeHandle(node_namespace_));
00115   katana_gazebo_plugins::IGazeboRosKatanaGripperAction* gripper_jt_controller_ =
00116       new katana_gazebo_plugins::KatanaGripperJointTrajectoryController(ros::NodeHandle(node_namespace_));
00117 
00118   // "register" gripper actions
00119   gripper_action_list_.push_back(gripper_grasp_controller_);
00120   gripper_action_list_.push_back(gripper_jt_controller_);
00121 
00122   // set default action
00123   active_gripper_action_ = gripper_grasp_controller_;
00124   updateGains();
00125 
00126   // Get the name of the parent model
00127   std::string modelName = _sdf->GetParent()->Get<std::string>("name");
00128 
00129   // Listen to the update event. This event is broadcast every
00130   // simulation iteration.
00131   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00132       boost::bind(&GazeboRosKatanaGripper::UpdateChild, this));
00133   gzdbg << "plugin model name: " << modelName << "\n";
00134 
00135   ROS_INFO("gazebo_ros_katana_gripper plugin initialized");
00136 }
00137 
00138 void GazeboRosKatanaGripper::InitChild()
00139 {
00140   pid_controller_.reset();
00141   prev_update_time_ = this->my_world_->GetSimTime();
00142 }
00143 
00144 void GazeboRosKatanaGripper::FiniChild()
00145 {
00146   rosnode_->shutdown();
00147 }
00148 
00149 void GazeboRosKatanaGripper::UpdateChild()
00150 {
00151   // --------------- command joints  ---------------
00152   common::Time time_now = this->my_world_->GetSimTime();
00153   common::Time step_time = time_now - prev_update_time_;
00154   prev_update_time_ = time_now;
00155   ros::Duration dt = ros::Duration(step_time.Double());
00156 
00157   double desired_pos[NUM_JOINTS];
00158   double actual_pos[NUM_JOINTS];
00159   double commanded_effort[NUM_JOINTS];
00160 
00161   // check for new goals, this my change the active_gripper_action_
00162   this->updateActiveGripperAction();
00163 
00164   for (size_t i = 0; i < NUM_JOINTS; ++i)
00165   {
00166     // desired_pos = 0.3 * sin(0.25 * this->my_world_->GetSimTime());
00167     //if ((prev_update_time_.sec / 6) % 2 == 0)
00168     //  desired_pos[i] = 0.3;
00169     //else
00170     //  desired_pos[i] = -0.44;
00171 
00172     desired_pos[i] = active_gripper_action_->getNextDesiredPoint(ros::Time::now()).position;
00173     actual_pos[i] = joints_[i]->GetAngle(0).Radian();
00174 
00175     commanded_effort[i] = pid_controller_.computeCommand(desired_pos[i] - actual_pos[i], -joints_[i]->GetVelocity(0), dt);
00176 
00177     if (commanded_effort[i] > torque_)
00178       commanded_effort[i] = torque_;
00179     else if (commanded_effort[i] < -torque_)
00180       commanded_effort[i] = -torque_;
00181 
00182     joints_[i]->SetForce(0, commanded_effort[i]);
00183 
00184     // TODO: ensure that both joints are always having (approximately)
00185     // the same joint position, even if one is blocked by an object
00186   }
00187   if (fabs(commanded_effort[0]) > 0.001)
00188     ROS_DEBUG("efforts: r %f, l %f (max: %f)", commanded_effort[0], commanded_effort[1], torque_);
00189 
00190   // --------------- update gripper_grasp_controller  ---------------
00191   for (size_t i = 0; i < NUM_JOINTS; ++i)
00192   {
00193 
00194     // update all actions
00195     for (std::size_t i = 0; i != gripper_action_list_.size(); i++)
00196     {
00197       gripper_action_list_[i]->setCurrentPoint(joints_[i]->GetAngle(0).Radian(), joints_[i]->GetVelocity(0));
00198     }
00199 
00200   }
00201 
00202   // --------------- limit publishing frequency to 25 Hz ---------------
00203   publish_counter_ = ((publish_counter_ + 1) % 40);
00204 
00205   if (publish_counter_ == 0)
00206   {
00207     // --------------- publish gripper controller state  ---------------
00208     katana_msgs::GripperControllerState controller_state;
00209     controller_state.header.stamp = ros::Time::now();
00210     for (size_t i = 0; i < NUM_JOINTS; ++i)
00211     {
00212       controller_state.name.push_back(joints_[i]->GetName());
00213       controller_state.actual.push_back(actual_pos[i]);
00214       controller_state.desired.push_back(desired_pos[i]);
00215       controller_state.error.push_back(desired_pos[i] - actual_pos[i]);
00216     }
00217 
00218     controller_state_pub_.publish(controller_state);
00219 
00220     // don't publish joint states: The pr2_controller_manager publishes joint states for
00221     // ALL joints, not just the ones it controls.
00222     //
00223     //    // --------------- publish joint states ---------------
00224     //    js_.header.stamp = ros::Time::now();
00225     //
00226     //    for (size_t i = 0; i < NUM_JOINTS; ++i)
00227     //    {
00228     //      js_.position[i] = joints_[i]->GetAngle(0).Radian();
00229     //      js_.velocity[i] = joints_[i]->GetVelocity(0);
00230     //      js_.effort[i] = commanded_effort[i];
00231     //
00232     //      ROS_DEBUG("publishing gripper joint state %d (effort: %f)", i, commanded_effort[i]);
00233     //    }
00234     //
00235     //    joint_state_pub_.publish(js_);
00236   }
00237 }
00238 
00242 void GazeboRosKatanaGripper::updateActiveGripperAction()
00243 {
00244   //TODO: improve the selection of the action, maybe prefer newer started actions (but how to know?)
00245   //      atm the list gives some kind of priority, and it its impossible to cancel a goal
00246   //      by submitting a new one to another action (but you can cancel the goal via a message)
00247 
00248   // search for a new action if the current (or default on) is finished with its goal
00249   // if we cant find a new action, just use the current one
00250   if (!active_gripper_action_->hasActiveGoal())
00251   {
00252 
00253     // find a new action with a goal
00254     for (std::size_t i = 0; i != gripper_action_list_.size(); i++)
00255     {
00256       // just use the first found
00257       if (gripper_action_list_[i]->hasActiveGoal())
00258       {
00259         // change the active gripper action
00260         active_gripper_action_ = gripper_action_list_[i];
00261         updateGains();
00262 
00263         break;
00264       }
00265     }
00266   }
00267 }
00268 
00269 void GazeboRosKatanaGripper::updateGains()
00270 {
00271   // PID Controller parameters (gains)
00272   double p, i, d, i_max, i_min;
00273   // get current values
00274   pid_controller_.getGains(p, i, d, i_max, i_min);
00275   // overwrite with defaults from the active action
00276   active_gripper_action_->getGains(p, i, d, i_max, i_min);
00277   // set changed values
00278   pid_controller_.setGains(p, i, d, i_max, i_min);
00279 }
00280 
00281 void GazeboRosKatanaGripper::spin()
00282 {
00283   while (ros::ok())
00284     ros::spinOnce();
00285 }
00286 
00287 GZ_REGISTER_MODEL_PLUGIN(GazeboRosKatanaGripper);


katana_gazebo_plugins
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:23