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 <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)
00036 {
00037
00038 GRKAPoint default_point = {0.0, 0.0};
00039 current_point_ = default_point;
00040 last_desired_point_ = default_point;
00041
00042
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
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 trajectory 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
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
00136 ROS_DEBUG("Still have some time left to make it.");
00137
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
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
00183 if (has_active_goal_)
00184 {
00185
00186 trajectory_finished_ = true;
00187
00188
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
00198 this->setCurrentTrajectory(active_goal_.getGoal()->trajectory);
00199 }
00200
00201 void KatanaGripperJointTrajectoryController::cancelCB(GoalHandle gh)
00202 {
00203 if (active_goal_ == gh)
00204 {
00205
00206 trajectory_finished_ = true;
00207
00208
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
00224
00225
00226
00227
00228 this->current_traj_ = traj;
00229
00230 this->trajectory_finished_ = false;
00231
00232 }
00233
00234 GRKAPoint KatanaGripperJointTrajectoryController::getNextDesiredPoint(ros::Time time)
00235 {
00236
00237
00238 trajectory_msgs::JointTrajectory traj = current_traj_;
00239
00240
00241 if (trajectory_finished_)
00242 {
00243
00244 return current_point_;
00245 }
00246
00247
00248 if (time.toSec() < traj.header.stamp.toSec())
00249 {
00250
00251 return current_point_;
00252 }
00253
00254 ros::Duration relative_time = ros::Duration(time.toSec() - traj.header.stamp.toSec());
00255
00256
00257
00258
00259
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
00274
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
00281 trajectory_finished_ = true;
00282
00283
00284 return last_desired_point_;
00285 }
00286
00287
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
00294
00295
00296
00297 double end_pos = traj.points[end_index].positions[0];
00298 double end_vel = traj.points[end_index].velocities[0];
00299
00300
00301
00302
00303 double time_from_start = traj.points[end_index].time_from_start.toSec();
00304
00305
00306
00307
00308
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
00315 spline_smoother::sampleCubicSpline(coefficients, relative_time.toSec(), sample_pos, sample_vel, sample_acc);
00316
00317
00318
00319
00320 GRKAPoint point = {sample_pos, sample_vel};
00321
00322
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 }
00343