$search
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 * katana_gripper_joint_trajectory_controller.h 00020 * 00021 * Created on: 20.10.2011 00022 * Author: Karl Glatz <glatz@hs-weingarten.de> 00023 * Ravensburg-Weingarten, University of Applied Sciences 00024 * 00025 * based on joint_trajectory_action/src/joint_trajectory_action.cpp 00026 * 00027 */ 00028 00029 #include <katana_gazebo_plugins/katana_gripper_joint_trajectory_controller.h> 00030 00031 namespace katana_gazebo_plugins 00032 { 00033 00034 KatanaGripperJointTrajectoryController::KatanaGripperJointTrajectoryController(ros::NodeHandle pn) : 00035 has_active_goal_(false), trajectory_finished_(true) /* c++0x: , current_point_({0.0, 0.0}), last_desired_point_( {0.0, 0.0})*/ 00036 { 00037 00038 GRKAPoint default_point = {0.0, 0.0}; 00039 current_point_ = default_point; 00040 last_desired_point_ = default_point; 00041 00042 // set the joints fixed here 00043 joint_names_.push_back((std::string)"katana_r_finger_joint"); 00044 joint_names_.push_back((std::string)"katana_l_finger_joint"); 00045 00046 pn.param("constraints/goal_time", goal_time_constraint_, 5.0); 00047 00048 // Gets the constraints for each joint. 00049 for (size_t i = 0; i < joint_names_.size(); ++i) 00050 { 00051 std::string ns = std::string("constraints/") + joint_names_[i]; 00052 double g, t; 00053 pn.param(ns + "/goal", g, -1.0); 00054 pn.param(ns + "/trajectory", t, -1.0); 00055 goal_constraints_[joint_names_[i]] = g; 00056 trajectory_constraints_[joint_names_[i]] = t; 00057 } 00058 pn.param("constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01); 00059 00060 ros::NodeHandle action_node("katana_arm_controller"); 00061 00062 action_server_ = new JTAS(action_node, "gripper_joint_trajectory_action", 00063 boost::bind(&KatanaGripperJointTrajectoryController::goalCB, this, _1), 00064 boost::bind(&KatanaGripperJointTrajectoryController::cancelCB, this, _1), false); 00065 00066 action_server_->start(); 00067 ROS_INFO( 00068 "katana gripper joint trajctory action server started on topic katana_arm_controller/gripper_joint_trajectory_action"); 00069 00070 } 00071 00072 KatanaGripperJointTrajectoryController::~KatanaGripperJointTrajectoryController() 00073 { 00074 delete action_server_; 00075 } 00076 00077 bool KatanaGripperJointTrajectoryController::setsEqual(const std::vector<std::string> &a, 00078 const std::vector<std::string> &b) 00079 { 00080 if (a.size() != b.size()) 00081 return false; 00082 00083 for (size_t i = 0; i < a.size(); ++i) 00084 { 00085 if (count(b.begin(), b.end(), a[i]) != 1) 00086 return false; 00087 } 00088 for (size_t i = 0; i < b.size(); ++i) 00089 { 00090 if (count(a.begin(), a.end(), b[i]) != 1) 00091 return false; 00092 } 00093 00094 return true; 00095 } 00096 00097 void KatanaGripperJointTrajectoryController::checkGoalStatus() 00098 { 00099 00100 ros::Time now = ros::Time::now(); 00101 00102 if (!has_active_goal_) 00103 return; 00104 if (current_traj_.points.empty()) 00105 return; 00106 00107 // time left? 00108 if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start) 00109 return; 00110 00111 int last = current_traj_.points.size() - 1; 00112 ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start; 00113 00114 bool inside_goal_constraints = false; 00115 00116 if (this->isTrajectoryFinished()) 00117 { 00118 00119 if (this->currentIsDesiredAngle()) 00120 { 00121 inside_goal_constraints = true; 00122 } 00123 00124 } 00125 00126 if (inside_goal_constraints) 00127 { 00128 ROS_DEBUG("Goal Succeeded!"); 00129 active_goal_.setSucceeded(); 00130 has_active_goal_ = false; 00131 ROS_INFO("last_desired_point_.position: %f current_point_.position: %f", last_desired_point_.position, current_point_.position); 00132 } 00133 else if (now < end_time + ros::Duration(goal_time_constraint_)) 00134 { 00135 // Still have some time left to make it. 00136 ROS_DEBUG("Still have some time left to make it."); 00137 //ROS_INFO("Still have some time left to make it. current_point_.position: %f ", current_point_.position); 00138 } 00139 else 00140 { 00141 ROS_WARN( 00142 "Aborting because we wound up outside the goal constraints [current_angle: %f last_desired_angle: %f ]", current_point_.position, last_desired_point_.position); 00143 active_goal_.setAborted(); 00144 has_active_goal_ = false; 00145 } 00146 00147 } 00148 00149 bool KatanaGripperJointTrajectoryController::currentIsDesiredAngle() 00150 { 00151 00152 double current_angle_ = current_point_.position; 00153 double desired_angle_ = last_desired_point_.position; 00154 00155 ROS_DEBUG("current_angle_: %f desired_angle_: %f", current_angle_, desired_angle_); 00156 00157 return ((current_angle_ - GRIPPER_ANGLE_THRESHOLD) <= desired_angle_ 00158 && (current_angle_ + GRIPPER_ANGLE_THRESHOLD) >= desired_angle_); 00159 00160 } 00161 00162 void KatanaGripperJointTrajectoryController::goalCB(GoalHandle gh) 00163 { 00164 00165 ROS_DEBUG("KatanaGripperJointTrajectoryController::goalCB"); 00166 00167 // Ensures that the joints in the goal match the joints we are commanding. 00168 if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names)) 00169 { 00170 ROS_ERROR("KatanaGripperJointTrajectoryController::goalCB: Joints on incoming goal don't match our joints"); 00171 gh.setRejected(); 00172 return; 00173 } 00174 00175 double desired_start_pos = gh.getGoal()->trajectory.points[0].positions[0]; 00176 if (fabs(desired_start_pos - current_point_.position) > 0.05) { 00177 ROS_ERROR("Input trajectory is invalid (difference between desired and current point too high: %f). This might crash Gazebo with error \"The minimum corner of the box must be less than or equal to maximum corner\".", fabs(desired_start_pos - current_point_.position)); 00178 gh.setRejected(); 00179 return; 00180 } 00181 00182 // Cancels the currently active goal. 00183 if (has_active_goal_) 00184 { 00185 // Stops the controller. 00186 trajectory_finished_ = true; 00187 00188 // Marks the current goal as canceled. 00189 active_goal_.setCanceled(); 00190 has_active_goal_ = false; 00191 } 00192 00193 gh.setAccepted(); 00194 active_goal_ = gh; 00195 has_active_goal_ = true; 00196 00197 // Sends the trajectory "along to the controller" 00198 this->setCurrentTrajectory(active_goal_.getGoal()->trajectory); 00199 } 00200 00201 void KatanaGripperJointTrajectoryController::cancelCB(GoalHandle gh) 00202 { 00203 if (active_goal_ == gh) 00204 { 00205 // stop sending points 00206 trajectory_finished_ = true; 00207 00208 // Marks the current goal as canceled. 00209 active_goal_.setCanceled(); 00210 has_active_goal_ = false; 00211 } 00212 } 00213 00214 void KatanaGripperJointTrajectoryController::setCurrentTrajectory(trajectory_msgs::JointTrajectory traj) 00215 { 00216 00217 if (traj.points.empty()) 00218 { 00219 ROS_WARN("KatanaGripperJointTrajectoryController::setCurrentTrajectory: Empty trajectory"); 00220 return; 00221 } 00222 00223 //TODO: check current position of the gripper to avoid too big efforts 00224 00225 // traj.points.resize(traj.points.size()+1); 00226 00227 00228 this->current_traj_ = traj; 00229 // set the finished flag to false for this new trajectory 00230 this->trajectory_finished_ = false; 00231 00232 } 00233 00234 GRKAPoint KatanaGripperJointTrajectoryController::getNextDesiredPoint(ros::Time time) 00235 { 00236 //ROS_INFO("getNextDesiredPoint"); 00237 00238 trajectory_msgs::JointTrajectory traj = current_traj_; 00239 00240 // is there a active trajectory? 00241 if (trajectory_finished_) 00242 { 00243 // just send the last point (default 0.0) 00244 return current_point_; 00245 } 00246 00247 // should we start already?? 00248 if (time.toSec() < traj.header.stamp.toSec()) 00249 { 00250 // just send the last point (default 0.0) 00251 return current_point_; 00252 } 00253 00254 ros::Duration relative_time = ros::Duration(time.toSec() - traj.header.stamp.toSec()); 00255 00256 //ROS_INFO("time: %f | header.stamp %f", time.toSec(), traj.header.stamp.toSec()); 00257 //ROS_INFO("relative_time %f", relative_time.toSec()); 00258 00259 // search for correct trajectory segment 00260 size_t traj_segment = 0; 00261 bool found_traj_seg = false; 00262 size_t numof_points = traj.points.size(); 00263 for (size_t i = 1; i < numof_points; i++) 00264 { 00265 if (traj.points[i].time_from_start >= relative_time) 00266 { 00267 traj_segment = i; 00268 found_traj_seg = true; 00269 break; 00270 } 00271 } 00272 00273 // segment found? 00274 // not found happens only if the time is beyond of any time_from_start values of the points in the trajectory 00275 if (!found_traj_seg) 00276 { 00277 ROS_INFO( 00278 "Trajectory finished (requested time %f time_from_start[last_point]: %f)", relative_time.toSec(), traj.points[traj.points.size()-1].time_from_start.toSec()); 00279 00280 // set trajectory to finished 00281 trajectory_finished_ = true; 00282 00283 // stay at the last point 00284 return last_desired_point_; 00285 } 00286 00287 // sample one point at current time 00288 size_t start_index = traj_segment - 1; 00289 size_t end_index = traj_segment; 00290 00291 double start_pos = traj.points[start_index].positions[0]; 00292 double start_vel = traj.points[start_index].velocities[0]; 00293 // double start_acc = traj.points[start_index].accelerations[0]; 00294 00295 //ROS_INFO("start_index %i: start_pos %f start_vel %f", start_index, start_pos, start_vel); 00296 00297 double end_pos = traj.points[end_index].positions[0]; 00298 double end_vel = traj.points[end_index].velocities[0]; 00299 // double end_acc = traj.points[end_index].accelerations[0]; 00300 00301 //ROS_INFO("end_index %i: end_pos %f end_vel %f", end_index, end_pos, end_vel); 00302 00303 double time_from_start = traj.points[end_index].time_from_start.toSec(); 00304 // double duration = traj.points[end_index].time_from_start.toSec() - traj.points[start_index].time_from_start.toSec(); 00305 00306 //ROS_INFO("time_from_start %f | relative_time.toSec() %f", time_from_start, relative_time.toSec()); 00307 00308 //TODO: save the coefficients for each segment 00309 std::vector<double> coefficients; 00310 00311 spline_smoother::getCubicSplineCoefficients(start_pos, start_vel, end_pos, end_vel, time_from_start, coefficients); 00312 00313 double sample_pos = 0, sample_vel = 0, sample_acc = 0; 00314 // katana::sampleSplineWithTimeBounds(coefficients, duration, relative_time.toSec(), sample_pos, sample_vel, sample_acc); 00315 spline_smoother::sampleCubicSpline(coefficients, relative_time.toSec(), sample_pos, sample_vel, sample_acc); 00316 00317 //ROS_INFO("sample_pos: %f, sample_vel: %f", sample_pos, sample_vel); 00318 //ROS_INFO("current_point_.position: %f ", current_point_.position); 00319 00320 GRKAPoint point = {sample_pos, sample_vel}; 00321 00322 // set the last desired point 00323 last_desired_point_ = point; 00324 00325 return point; 00326 00327 } 00328 00329 bool KatanaGripperJointTrajectoryController::isTrajectoryFinished() 00330 { 00331 return trajectory_finished_; 00332 } 00333 00334 00335 void KatanaGripperJointTrajectoryController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) { 00336 p = 6.0; 00337 i = 0.1; 00338 d = 0.1; 00339 } 00340 00341 00342 } // end namespace 00343