joint_trajectory_action_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 #include "robot_mechanism_controllers/joint_trajectory_action_controller.h"
00035 #include <sstream>
00036 #include "angles/angles.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 PLUGINLIB_EXPORT_CLASS( controller::JointTrajectoryActionController, pr2_controller_interface::Controller)
00040 
00041 namespace controller {
00042 
00043 // These functions are pulled from the spline_smoother package.
00044 // They've been moved here to avoid depending on packages that aren't
00045 // mature yet.
00046 
00047 
00048 static inline void generatePowers(int n, double x, double* powers)
00049 {
00050   powers[0] = 1.0;
00051   for (int i=1; i<=n; i++)
00052   {
00053     powers[i] = powers[i-1]*x;
00054   }
00055 }
00056 
00057 static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
00058     double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
00059 {
00060   coefficients.resize(6);
00061 
00062   if (time == 0.0)
00063   {
00064     coefficients[0] = end_pos;
00065     coefficients[1] = end_vel;
00066     coefficients[2] = 0.5*end_acc;
00067     coefficients[3] = 0.0;
00068     coefficients[4] = 0.0;
00069     coefficients[5] = 0.0;
00070   }
00071   else
00072   {
00073     double T[6];
00074     generatePowers(5, time, T);
00075 
00076     coefficients[0] = start_pos;
00077     coefficients[1] = start_vel;
00078     coefficients[2] = 0.5*start_acc;
00079     coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
00080                        12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
00081     coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
00082                        16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
00083     coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
00084                        6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
00085   }
00086 }
00087 
00091 static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
00092     double& position, double& velocity, double& acceleration)
00093 {
00094   // create powers of time:
00095   double t[6];
00096   generatePowers(5, time, t);
00097 
00098   position = t[0]*coefficients[0] +
00099       t[1]*coefficients[1] +
00100       t[2]*coefficients[2] +
00101       t[3]*coefficients[3] +
00102       t[4]*coefficients[4] +
00103       t[5]*coefficients[5];
00104 
00105   velocity = t[0]*coefficients[1] +
00106       2.0*t[1]*coefficients[2] +
00107       3.0*t[2]*coefficients[3] +
00108       4.0*t[3]*coefficients[4] +
00109       5.0*t[4]*coefficients[5];
00110 
00111   acceleration = 2.0*t[0]*coefficients[2] +
00112       6.0*t[1]*coefficients[3] +
00113       12.0*t[2]*coefficients[4] +
00114       20.0*t[3]*coefficients[5];
00115 }
00116 
00117 static void getCubicSplineCoefficients(double start_pos, double start_vel,
00118     double end_pos, double end_vel, double time, std::vector<double>& coefficients)
00119 {
00120   coefficients.resize(4);
00121 
00122   if (time == 0.0)
00123   {
00124     coefficients[0] = end_pos;
00125     coefficients[1] = end_vel;
00126     coefficients[2] = 0.0;
00127     coefficients[3] = 0.0;
00128   }
00129   else
00130   {
00131     double T[4];
00132     generatePowers(3, time, T);
00133 
00134     coefficients[0] = start_pos;
00135     coefficients[1] = start_vel;
00136     coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
00137     coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
00138   }
00139 }
00140 
00141 
00142 JointTrajectoryActionController::JointTrajectoryActionController()
00143   : loop_count_(0), robot_(NULL)
00144 {
00145 }
00146 
00147 JointTrajectoryActionController::~JointTrajectoryActionController()
00148 {
00149   sub_command_.shutdown();
00150   serve_query_state_.shutdown();
00151   action_server_.reset();
00152   action_server_follow_.reset();
00153 }
00154 
00155 bool JointTrajectoryActionController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00156 {
00157   using namespace XmlRpc;
00158   node_ = n;
00159   robot_ = robot;
00160 
00161   // Gets all of the joints
00162   XmlRpc::XmlRpcValue joint_names;
00163   if (!node_.getParam("joints", joint_names))
00164   {
00165     ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
00166     return false;
00167   }
00168   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00169   {
00170     ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
00171     return false;
00172   }
00173   for (int i = 0; i < joint_names.size(); ++i)
00174   {
00175     XmlRpcValue &name_value = joint_names[i];
00176     if (name_value.getType() != XmlRpcValue::TypeString)
00177     {
00178       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00179                 node_.getNamespace().c_str());
00180       return false;
00181     }
00182 
00183     pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
00184     if (!j) {
00185       ROS_ERROR("Joint not found: %s. (namespace: %s)",
00186                 ((std::string)name_value).c_str(), node_.getNamespace().c_str());
00187       return false;
00188     }
00189     joints_.push_back(j);
00190   }
00191 
00192   // Ensures that all the joints are calibrated.
00193   for (size_t i = 0; i < joints_.size(); ++i)
00194   {
00195     if (!joints_[i]->calibrated_)
00196     {
00197       ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
00198                 joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
00199       return false;
00200     }
00201   }
00202 
00203   // Sets up pid controllers for all of the joints
00204   std::string gains_ns;
00205   if (!node_.getParam("gains", gains_ns))
00206     gains_ns = node_.getNamespace() + "/gains";
00207   masses_.resize(joints_.size());
00208   pids_.resize(joints_.size());
00209   for (size_t i = 0; i < joints_.size(); ++i)
00210   {
00211     ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
00212     joint_nh.param("mass", masses_[i], 0.0);
00213     if (!pids_[i].init(joint_nh))
00214       return false;
00215   }
00216 
00217   // Sets up the proxy controllers for each joint
00218   proxies_enabled_.resize(joints_.size());
00219   proxies_.resize(joints_.size());
00220   for (size_t i = 0; i < joints_.size(); ++i)
00221   {
00222     ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
00223     if (joint_nh.hasParam("proxy")) {
00224       proxies_enabled_[i] = true;
00225       joint_nh.param("proxy/lambda", proxies_[i].lambda_proxy_, 1.0);
00226       joint_nh.param("proxy/acc_converge", proxies_[i].acc_converge_, 0.0);
00227       joint_nh.param("proxy/vel_limit", proxies_[i].vel_limit_, 0.0);
00228       joint_nh.param("proxy/effort_limit", proxies_[i].effort_limit_, 0.0);
00229       proxies_[i].mass_ = masses_[i];
00230       double _;
00231       pids_[i].getGains(proxies_[i].Kp_, proxies_[i].Ki_, proxies_[i].Kd_, proxies_[i].Ficl_, _);
00232     }
00233     else {
00234       proxies_enabled_[i] = false;
00235     }
00236   }
00237 
00238   // Trajectory and goal tolerances
00239   default_trajectory_tolerance_.resize(joints_.size());
00240   default_goal_tolerance_.resize(joints_.size());
00241   node_.param("joint_trajectory_action_node/constraints/goal_time", default_goal_time_constraint_, 0.0);
00242   double stopped_velocity_tolerance;
00243   node_.param("joint_trajectory_action_node/constraints/stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
00244   for (size_t i = 0; i < default_goal_tolerance_.size(); ++i)
00245     default_goal_tolerance_[i].velocity = stopped_velocity_tolerance;
00246   for (size_t i = 0; i < joints_.size(); ++i)
00247   {
00248     std::string ns = std::string("joint_trajectory_action_node/constraints") + joints_[i]->joint_->name;
00249     node_.param(ns + "/goal", default_goal_tolerance_[i].position, 0.0);
00250     node_.param(ns + "/trajectory", default_trajectory_tolerance_[i].position, 0.0);
00251   }
00252 
00253   // Output filters
00254   output_filters_.resize(joints_.size());
00255   for (size_t i = 0; i < joints_.size(); ++i)
00256   {
00257     std::string p = "output_filters/" + joints_[i]->joint_->name;
00258     if (node_.hasParam(p))
00259     {
00260       output_filters_[i].reset(new filters::FilterChain<double>("double"));
00261       if (!output_filters_[i]->configure(node_.resolveName(p)))
00262         output_filters_[i].reset();
00263     }
00264   }
00265 
00266   // Creates a dummy trajectory
00267   boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
00268   SpecifiedTrajectory &traj = *traj_ptr;
00269   traj[0].start_time = robot_->getTime().toSec();
00270   traj[0].duration = 0.0;
00271   traj[0].splines.resize(joints_.size());
00272   for (size_t j = 0; j < joints_.size(); ++j)
00273     traj[0].splines[j].coef[0] = 0.0;
00274   current_trajectory_box_.set(traj_ptr);
00275 
00276   sub_command_ = node_.subscribe("command", 1, &JointTrajectoryActionController::commandCB, this);
00277   serve_query_state_ = node_.advertiseService(
00278     "query_state", &JointTrajectoryActionController::queryStateService, this);
00279 
00280   q.resize(joints_.size());
00281   qd.resize(joints_.size());
00282   qdd.resize(joints_.size());
00283 
00284   controller_state_publisher_.reset(
00285     new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
00286     (node_, "state", 1));
00287   controller_state_publisher_->lock();
00288   for (size_t j = 0; j < joints_.size(); ++j)
00289     controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
00290   controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
00291   controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
00292   controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
00293   controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
00294   controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
00295   controller_state_publisher_->msg_.error.positions.resize(joints_.size());
00296   controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
00297   controller_state_publisher_->unlock();
00298 
00299   action_server_.reset(new JTAS(node_, "joint_trajectory_action",
00300                                 boost::bind(&JointTrajectoryActionController::goalCB, this, _1),
00301                                 boost::bind(&JointTrajectoryActionController::cancelCB, this, _1),
00302                                 false));
00303   action_server_follow_.reset(new FJTAS(node_, "follow_joint_trajectory",
00304                                         boost::bind(&JointTrajectoryActionController::goalCBFollow, this, _1),
00305                                         boost::bind(&JointTrajectoryActionController::cancelCBFollow, this, _1),
00306                                         false));
00307   action_server_->start();
00308   action_server_follow_->start();
00309 
00310   return true;
00311 }
00312 
00313 void JointTrajectoryActionController::starting()
00314 {
00315   last_time_ = robot_->getTime();
00316 
00317   for (size_t i = 0; i < pids_.size(); ++i) {
00318     pids_[i].reset();
00319     proxies_[i].reset(joints_[i]->position_, joints_[i]->velocity_);
00320   }
00321 
00322   // Creates a "hold current position" trajectory.
00323   boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00324   SpecifiedTrajectory &hold = *hold_ptr;
00325   hold[0].start_time = last_time_.toSec() - 0.001;
00326   hold[0].duration = 0.0;
00327   hold[0].splines.resize(joints_.size());
00328   for (size_t j = 0; j < joints_.size(); ++j)
00329     hold[0].splines[j].coef[0] = joints_[j]->position_;
00330 
00331   current_trajectory_box_.set(hold_ptr);
00332 }
00333 
00334 void JointTrajectoryActionController::update()
00335 {
00336   ros::Time time = robot_->getTime();
00337   ros::Duration dt = time - last_time_;
00338   last_time_ = time;
00339 
00340   boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
00341   boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);
00342 
00343   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00344   current_trajectory_box_.get(traj_ptr);
00345   if (!traj_ptr)
00346     ROS_FATAL("The current trajectory can never be null");
00347 
00348   // Only because this is what the code originally looked like.
00349   const SpecifiedTrajectory &traj = *traj_ptr;
00350 
00351   // ------ Finds the current segment
00352 
00353   // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
00354   int seg = -1;
00355   while (seg + 1 < (int)traj.size() &&
00356          traj[seg+1].start_time < time.toSec())
00357   {
00358     ++seg;
00359   }
00360 
00361   if (seg == -1)
00362   {
00363     if (traj.size() == 0)
00364       ROS_ERROR("No segments in the trajectory");
00365     else
00366       ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00367     return;
00368   }
00369 
00370   // ------ Trajectory Sampling
00371 
00372   for (size_t i = 0; i < q.size(); ++i)
00373   {
00374     sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00375                                time.toSec() - traj[seg].start_time,
00376                                q[i], qd[i], qdd[i]);
00377   }
00378 
00379   // ------ Trajectory Following
00380 
00381   std::vector<double> error(joints_.size());
00382   std::vector<double> v_error(joints_.size());
00383   for (size_t i = 0; i < joints_.size(); ++i)
00384   {
00385     double effort;
00386 
00387     // Compute the errors with respect to the desired trajectory
00388     // (whether or not using the proxy controller).  They are also
00389     // used later to determine reaching the goal.
00390     error[i] = q[i] - joints_[i]->position_;
00391     v_error[i] = qd[i] - joints_[i]->velocity_;
00392 
00393     // Use the proxy controller (if enabled)
00394     if (proxies_enabled_[i]) {
00395       effort = proxies_[i].update(q[i], qd[i], qdd[i],
00396                                   joints_[i]->position_, joints_[i]->velocity_,
00397                                   dt.toSec());
00398     }
00399     else {
00400       effort = pids_[i].computeCommand(error[i], v_error[i], dt);
00401 
00402       double effort_unfiltered = effort;
00403       if (output_filters_[i])
00404         output_filters_[i]->update(effort_unfiltered, effort);
00405     }
00406 
00407     // Apply the effort.  WHY IS THIS ADDITIVE?
00408     joints_[i]->commanded_effort_ = effort;
00409   }
00410 
00411   // ------ Determines if the goal has failed or succeeded
00412 
00413   if ((traj[seg].gh && traj[seg].gh == current_active_goal) ||
00414       (traj[seg].gh_follow && traj[seg].gh_follow == current_active_goal_follow))
00415   {
00416     ros::Time end_time(traj[seg].start_time + traj[seg].duration);
00417     if (time <= end_time)
00418     {
00419       // Verifies trajectory tolerances
00420       for (size_t j = 0; j < joints_.size(); ++j)
00421       {
00422         if (traj[seg].trajectory_tolerance[j].violated(error[j], v_error[j]))
00423         {
00424           if (traj[seg].gh)
00425             traj[seg].gh->setAborted();
00426           else if (traj[seg].gh_follow) {
00427             traj[seg].gh_follow->preallocated_result_->error_code =
00428               control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00429             traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
00430           }
00431           break;
00432         }
00433       }
00434     }
00435     else if (seg == (int)traj.size() - 1)
00436     {
00437       // Checks that we have ended inside the goal tolerances
00438       bool inside_goal_constraints = true;
00439       for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
00440       {
00441         if (traj[seg].goal_tolerance[i].violated(error[i], v_error[i]))
00442           inside_goal_constraints = false;
00443       }
00444 
00445       if (inside_goal_constraints)
00446       {
00447         rt_active_goal_.reset();
00448         rt_active_goal_follow_.reset();
00449         if (traj[seg].gh)
00450           traj[seg].gh->setSucceeded();
00451         else if (traj[seg].gh_follow) {
00452           traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00453           traj[seg].gh_follow->setSucceeded(traj[seg].gh_follow->preallocated_result_);
00454         }
00455         //ROS_ERROR("Success!  (%s)", traj[seg].gh->gh_.getGoalID().id.c_str());
00456       }
00457       else if (time < end_time + ros::Duration(traj[seg].goal_time_tolerance))
00458       {
00459         // Still have some time left to make it.
00460       }
00461       else
00462       {
00463         //ROS_WARN("Aborting because we wound up outside the goal constraints");
00464         rt_active_goal_.reset();
00465         rt_active_goal_follow_.reset();
00466         if (traj[seg].gh)
00467           traj[seg].gh->setAborted();
00468         else if (traj[seg].gh_follow) {
00469           traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
00470           traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
00471         }
00472       }
00473     }
00474   }
00475 
00476   // ------ State publishing
00477 
00478   if (loop_count_ % 10 == 0)
00479   {
00480     if (controller_state_publisher_ && controller_state_publisher_->trylock())
00481     {
00482       controller_state_publisher_->msg_.header.stamp = time;
00483       for (size_t j = 0; j < joints_.size(); ++j)
00484       {
00485         controller_state_publisher_->msg_.desired.positions[j] = q[j];
00486         controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
00487         controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
00488         controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
00489         controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
00490         controller_state_publisher_->msg_.error.positions[j] = error[j];
00491         controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
00492       }
00493       controller_state_publisher_->unlockAndPublish();
00494     }
00495     if (current_active_goal_follow)
00496     {
00497       current_active_goal_follow->preallocated_feedback_->header.stamp = time;
00498       current_active_goal_follow->preallocated_feedback_->joint_names.resize(joints_.size());
00499       current_active_goal_follow->preallocated_feedback_->desired.positions.resize(joints_.size());
00500       current_active_goal_follow->preallocated_feedback_->desired.velocities.resize(joints_.size());
00501       current_active_goal_follow->preallocated_feedback_->desired.accelerations.resize(joints_.size());
00502       current_active_goal_follow->preallocated_feedback_->actual.positions.resize(joints_.size());
00503       current_active_goal_follow->preallocated_feedback_->actual.velocities.resize(joints_.size());
00504       current_active_goal_follow->preallocated_feedback_->error.positions.resize(joints_.size());
00505       current_active_goal_follow->preallocated_feedback_->error.velocities.resize(joints_.size());
00506       for (size_t j = 0; j < joints_.size(); ++j)
00507       {
00508         current_active_goal_follow->preallocated_feedback_->joint_names[j] = joints_[j]->joint_->name;
00509         current_active_goal_follow->preallocated_feedback_->desired.positions[j] = q[j];
00510         current_active_goal_follow->preallocated_feedback_->desired.velocities[j] = qd[j];
00511         current_active_goal_follow->preallocated_feedback_->desired.accelerations[j] = qdd[j];
00512         current_active_goal_follow->preallocated_feedback_->actual.positions[j] = joints_[j]->position_;
00513         current_active_goal_follow->preallocated_feedback_->actual.velocities[j] = joints_[j]->velocity_;
00514         current_active_goal_follow->preallocated_feedback_->error.positions[j] = error[j];
00515         current_active_goal_follow->preallocated_feedback_->error.velocities[j] = joints_[j]->velocity_ - qd[j];
00516       }
00517       const actionlib_msgs::GoalID goalID = current_active_goal_follow->gh_.getGoalID();
00518       ros::Duration time_from_start = time - goalID.stamp;
00519       current_active_goal_follow->preallocated_feedback_->desired.time_from_start = time_from_start;
00520       current_active_goal_follow->preallocated_feedback_->actual.time_from_start = time_from_start;
00521       current_active_goal_follow->preallocated_feedback_->error.time_from_start = time_from_start;
00522       current_active_goal_follow->gh_.publishFeedback(*current_active_goal_follow->preallocated_feedback_);
00523 
00524     }
00525   }
00526 
00527   ++loop_count_;
00528 }
00529 
00530 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
00531 {
00532   preemptActiveGoal();
00533   commandTrajectory(msg);
00534 }
00535 
00536 void JointTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg,
00537                                                         boost::shared_ptr<RTGoalHandle> gh,
00538                                                         boost::shared_ptr<RTGoalHandleFollow> gh_follow)
00539 {
00540   assert(!gh || !gh_follow);
00541   ros::Time time = last_time_ + ros::Duration(0.01);
00542   ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00543             time.toSec(), msg->header.stamp.toSec());
00544 
00545   boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00546   SpecifiedTrajectory &new_traj = *new_traj_ptr;
00547 
00548   // ------ If requested, performs a stop
00549 
00550   if (msg->points.empty())
00551   {
00552     starting();
00553     return;
00554   }
00555 
00556   // ------ Computes the tolerances for following the trajectory
00557   
00558   std::vector<JointTolerance> trajectory_tolerance = default_trajectory_tolerance_;
00559   std::vector<JointTolerance> goal_tolerance = default_goal_tolerance_;
00560   double goal_time_tolerance = default_goal_time_constraint_;
00561 
00562   if (gh_follow)
00563   {
00564     for (size_t j = 0; j < joints_.size(); ++j)
00565     {
00566       // Extracts the path tolerance from the command
00567       for (size_t k = 0; k < gh_follow->gh_.getGoal()->path_tolerance.size(); ++k)
00568       {
00569         const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->path_tolerance[k];
00570         if (joints_[j]->joint_->name == tol.name)
00571         {
00572           // If the commanded tolerances are positive, overwrite the
00573           // existing tolerances.  If they are -1, remove any existing
00574           // tolerance.  If they are 0, leave the default tolerance in
00575           // place.
00576           
00577           if (tol.position > 0)
00578             trajectory_tolerance[j].position = tol.position;
00579           else if (tol.position < 0)
00580             trajectory_tolerance[j].position = 0.0;
00581 
00582           if (tol.velocity > 0)
00583             trajectory_tolerance[j].velocity = tol.velocity;
00584           else if (tol.velocity < 0)
00585             trajectory_tolerance[j].velocity = 0.0;
00586 
00587           if (tol.acceleration > 0)
00588             trajectory_tolerance[j].acceleration = tol.acceleration;
00589           else if (tol.acceleration < 0)
00590             trajectory_tolerance[j].acceleration = 0.0;
00591           
00592           break;
00593         }
00594       }
00595 
00596       // Extracts the goal tolerance from the command
00597       for (size_t k = 0; k < gh_follow->gh_.getGoal()->goal_tolerance.size(); ++k)
00598       {
00599         const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->goal_tolerance[k];
00600         if (joints_[j]->joint_->name == tol.name)
00601         {
00602           // If the commanded tolerances are positive, overwrite the
00603           // existing tolerances.  If they are -1, remove any existing
00604           // tolerance.  If they are 0, leave the default tolerance in
00605           // place.
00606           
00607           if (tol.position > 0)
00608             goal_tolerance[j].position = tol.position;
00609           else if (tol.position < 0)
00610             goal_tolerance[j].position = 0.0;
00611 
00612           if (tol.velocity > 0)
00613             goal_tolerance[j].velocity = tol.velocity;
00614           else if (tol.velocity < 0)
00615             goal_tolerance[j].velocity = 0.0;
00616 
00617           if (tol.acceleration > 0)
00618             goal_tolerance[j].acceleration = tol.acceleration;
00619           else if (tol.acceleration < 0)
00620             goal_tolerance[j].acceleration = 0.0;
00621           
00622           break;
00623         }
00624       }
00625     }
00626     
00627     const ros::Duration &tol = gh_follow->gh_.getGoal()->goal_time_tolerance;
00628     if (tol < ros::Duration(0.0))
00629       goal_time_tolerance = 0.0;
00630     else if (tol > ros::Duration(0.0))
00631       goal_time_tolerance = tol.toSec();
00632   }
00633 
00634   // ------ Correlates the joints we're commanding to the joints in the message
00635 
00636   std::vector<int> lookup(joints_.size(), -1);  // Maps from an index in joints_ to an index in the msg
00637   for (size_t j = 0; j < joints_.size(); ++j)
00638   {
00639     for (size_t k = 0; k < msg->joint_names.size(); ++k)
00640     {
00641       if (msg->joint_names[k] == joints_[j]->joint_->name)
00642       {
00643         lookup[j] = k;
00644         break;
00645       }
00646     }
00647 
00648     if (lookup[j] == -1)
00649     {
00650       ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
00651       if (gh)
00652         gh->setAborted();
00653       else if (gh_follow) {
00654         gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00655         gh_follow->setAborted(gh_follow->preallocated_result_);
00656       }
00657       return;
00658     }
00659   }
00660 
00661   // ------ Grabs the trajectory that we're currently following.
00662 
00663   boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
00664   current_trajectory_box_.get(prev_traj_ptr);
00665   if (!prev_traj_ptr)
00666   {
00667     ROS_FATAL("The current trajectory can never be null");
00668     return;
00669   }
00670   const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
00671 
00672   // ------ Copies over the segments from the previous trajectory that are still useful.
00673 
00674   // Useful segments are still relevant after the current time.
00675   int first_useful = -1;
00676   while (first_useful + 1 < (int)prev_traj.size() &&
00677          prev_traj[first_useful + 1].start_time <= time.toSec())
00678   {
00679     ++first_useful;
00680   }
00681 
00682   // Useful segments are not going to be completely overwritten by the message's splines.
00683   int last_useful = -1;
00684   double msg_start_time;
00685   if (msg->header.stamp == ros::Time(0.0))
00686     msg_start_time = time.toSec();
00687   else
00688     msg_start_time = msg->header.stamp.toSec();
00689   /*
00690   if (msg->points.size() > 0)
00691     msg_start_time += msg->points[0].time_from_start.toSec();
00692   */
00693 
00694   while (last_useful + 1 < (int)prev_traj.size() &&
00695          prev_traj[last_useful + 1].start_time < msg_start_time)
00696   {
00697     ++last_useful;
00698   }
00699 
00700   if (last_useful < first_useful)
00701     first_useful = last_useful;
00702 
00703   // Copies over the old segments that were determined to be useful.
00704   for (int i = std::max(first_useful,0); i <= last_useful; ++i)
00705   {
00706     new_traj.push_back(prev_traj[i]);
00707   }
00708 
00709   // We always save the last segment so that we know where to stop if
00710   // there are no new segments.
00711   if (new_traj.size() == 0)
00712     new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00713 
00714   // ------ Determines when and where the new segments start
00715 
00716   // Finds the end conditions of the final segment
00717   Segment &last = new_traj[new_traj.size() - 1];
00718   std::vector<double> prev_positions(joints_.size());
00719   std::vector<double> prev_velocities(joints_.size());
00720   std::vector<double> prev_accelerations(joints_.size());
00721 
00722   double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec())
00723     - last.start_time;
00724   ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t);
00725   for (size_t i = 0; i < joints_.size(); ++i)
00726   {
00727     sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
00728                                t,
00729                                prev_positions[i], prev_velocities[i], prev_accelerations[i]);
00730     ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
00731               prev_accelerations[i], joints_[i]->joint_->name.c_str());
00732   }
00733 
00734   // ------ Tacks on the new segments
00735 
00736   std::vector<double> positions;
00737   std::vector<double> velocities;
00738   std::vector<double> accelerations;
00739 
00740   std::vector<double> durations(msg->points.size());
00741   if (msg->points.size() > 0)
00742     durations[0] = msg->points[0].time_from_start.toSec();
00743   for (size_t i = 1; i < msg->points.size(); ++i)
00744     durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00745 
00746   // Checks if we should wrap
00747   std::vector<double> wrap(joints_.size(), 0.0);
00748   assert(!msg->points[0].positions.empty());
00749   for (size_t j = 0; j < joints_.size(); ++j)
00750   {
00751     if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
00752     {
00753       double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[lookup[j]]);
00754       wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[lookup[j]];
00755     }
00756   }
00757 
00758   for (size_t i = 0; i < msg->points.size(); ++i)
00759   {
00760     Segment seg;
00761 
00762     if (msg->header.stamp == ros::Time(0.0))
00763       seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
00764     else
00765       seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00766     seg.duration = durations[i];
00767     seg.gh = gh;
00768     seg.gh_follow = gh_follow;
00769     seg.splines.resize(joints_.size());
00770 
00771     // Checks that the incoming segment has the right number of elements.
00772 
00773     if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
00774     {
00775       ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00776       return;
00777     }
00778     if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
00779     {
00780       ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00781       return;
00782     }
00783     if (msg->points[i].positions.size() != joints_.size())
00784     {
00785       ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00786       return;
00787     }
00788 
00789     // Re-orders the joints in the command to match the internal joint order.
00790 
00791     accelerations.resize(msg->points[i].accelerations.size());
00792     velocities.resize(msg->points[i].velocities.size());
00793     positions.resize(msg->points[i].positions.size());
00794     for (size_t j = 0; j < joints_.size(); ++j)
00795     {
00796       if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
00797       if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
00798       if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
00799     }
00800 
00801     // Converts the boundary conditions to splines.
00802 
00803     for (size_t j = 0; j < joints_.size(); ++j)
00804     {
00805       if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00806       {
00807         getQuinticSplineCoefficients(
00808           prev_positions[j], prev_velocities[j], prev_accelerations[j],
00809           positions[j], velocities[j], accelerations[j],
00810           durations[i],
00811           seg.splines[j].coef);
00812       }
00813       else if (prev_velocities.size() > 0 && velocities.size() > 0)
00814       {
00815         getCubicSplineCoefficients(
00816           prev_positions[j], prev_velocities[j],
00817           positions[j], velocities[j],
00818           durations[i],
00819           seg.splines[j].coef);
00820         seg.splines[j].coef.resize(6, 0.0);
00821       }
00822       else
00823       {
00824         seg.splines[j].coef[0] = prev_positions[j];
00825         if (durations[i] == 0.0)
00826           seg.splines[j].coef[1] = 0.0;
00827         else
00828           seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00829         seg.splines[j].coef[2] = 0.0;
00830         seg.splines[j].coef[3] = 0.0;
00831         seg.splines[j].coef[4] = 0.0;
00832         seg.splines[j].coef[5] = 0.0;
00833       }
00834     }
00835 
00836     // Writes the tolerances into the segment
00837     seg.trajectory_tolerance = trajectory_tolerance;
00838     seg.goal_tolerance = goal_tolerance;
00839     seg.goal_time_tolerance = goal_time_tolerance;
00840 
00841     // Pushes the splines onto the end of the new trajectory.
00842 
00843     new_traj.push_back(seg);
00844 
00845     // Computes the starting conditions for the next segment
00846 
00847     prev_positions = positions;
00848     prev_velocities = velocities;
00849     prev_accelerations = accelerations;
00850   }
00851 
00852   //ROS_ERROR("Last segment goal id: %s", new_traj[new_traj.size()-1].gh->gh_.getGoalID().id.c_str());
00853 
00854   // ------ Commits the new trajectory
00855 
00856   if (!new_traj_ptr)
00857   {
00858     ROS_ERROR("The new trajectory was null!");
00859     return;
00860   }
00861 
00862   current_trajectory_box_.set(new_traj_ptr);
00863   ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
00864 #if 0
00865   for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
00866   {
00867     ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
00868     for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
00869     {
00870       ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf , %.2lf  %.2lf(%s)",
00871                 new_traj[i].splines[j].coef[0],
00872                 new_traj[i].splines[j].coef[1],
00873                 new_traj[i].splines[j].coef[2],
00874                 new_traj[i].splines[j].coef[3],
00875                 new_traj[i].splines[j].coef[4],
00876                 new_traj[i].splines[j].coef[5],
00877                 joints_[j]->joint_->name_.c_str());
00878     }
00879   }
00880 #endif
00881 }
00882 
00883 bool JointTrajectoryActionController::queryStateService(
00884   pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00885   pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
00886 {
00887   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00888   current_trajectory_box_.get(traj_ptr);
00889   if (!traj_ptr)
00890   {
00891     ROS_FATAL("The current trajectory can never be null");
00892     return false;
00893   }
00894   const SpecifiedTrajectory &traj = *traj_ptr;
00895 
00896   // Determines which segment of the trajectory to use
00897   int seg = -1;
00898   while (seg + 1 < (int)traj.size() &&
00899          traj[seg+1].start_time < req.time.toSec())
00900   {
00901     ++seg;
00902   }
00903   if (seg == -1)
00904     return false;
00905 
00906   for (size_t i = 0; i < q.size(); ++i)
00907   {
00908   }
00909 
00910 
00911   resp.name.resize(joints_.size());
00912   resp.position.resize(joints_.size());
00913   resp.velocity.resize(joints_.size());
00914   resp.acceleration.resize(joints_.size());
00915   for (size_t j = 0; j < joints_.size(); ++j)
00916   {
00917     resp.name[j] = joints_[j]->joint_->name;
00918     sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
00919                                req.time.toSec() - traj[seg].start_time,
00920                                resp.position[j], resp.velocity[j], resp.acceleration[j]);
00921   }
00922 
00923   return true;
00924 }
00925 
00926 void JointTrajectoryActionController::sampleSplineWithTimeBounds(
00927   const std::vector<double>& coefficients, double duration, double time,
00928   double& position, double& velocity, double& acceleration)
00929 {
00930   if (time < 0)
00931   {
00932     double _;
00933     sampleQuinticSpline(coefficients, 0.0, position, _, _);
00934     velocity = 0;
00935     acceleration = 0;
00936   }
00937   else if (time > duration)
00938   {
00939     double _;
00940     sampleQuinticSpline(coefficients, duration, position, _, _);
00941     velocity = 0;
00942     acceleration = 0;
00943   }
00944   else
00945   {
00946     sampleQuinticSpline(coefficients, time,
00947                         position, velocity, acceleration);
00948   }
00949 }
00950 
00951 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
00952 {
00953   if (a.size() != b.size())
00954     return false;
00955 
00956   for (size_t i = 0; i < a.size(); ++i)
00957   {
00958     if (count(b.begin(), b.end(), a[i]) != 1)
00959       return false;
00960   }
00961   for (size_t i = 0; i < b.size(); ++i)
00962   {
00963     if (count(a.begin(), a.end(), b[i]) != 1)
00964       return false;
00965   }
00966 
00967   return true;
00968 }
00969 
00970 void JointTrajectoryActionController::preemptActiveGoal()
00971 {
00972   boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
00973   boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);
00974   
00975   // Cancels the currently active goal.
00976   if (current_active_goal)
00977   {
00978     // Marks the current goal as canceled.
00979     rt_active_goal_.reset();
00980     current_active_goal->gh_.setCanceled();
00981   }
00982   if (current_active_goal_follow)
00983   {
00984     rt_active_goal_follow_.reset();
00985     current_active_goal_follow->gh_.setCanceled();
00986   }
00987 }
00988 
00989 
00990 template <class Enclosure, class Member>
00991 static boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
00992 {
00993   actionlib::EnclosureDeleter<Enclosure> d(enclosure);
00994   boost::shared_ptr<Member> p(&member, d);
00995   return p;
00996 }
00997 
00998 void JointTrajectoryActionController::goalCB(GoalHandle gh)
00999 {
01000   std::vector<std::string> joint_names(joints_.size());
01001   for (size_t j = 0; j < joints_.size(); ++j)
01002     joint_names[j] = joints_[j]->joint_->name;
01003 
01004   // Ensures that the joints in the goal match the joints we are commanding.
01005   if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
01006   {
01007     ROS_ERROR("Joints on incoming goal don't match our joints");
01008     gh.setRejected();
01009     return;
01010   }
01011 
01012   preemptActiveGoal();
01013 
01014   gh.setAccepted();
01015   boost::shared_ptr<RTGoalHandle> rt_gh(new RTGoalHandle(gh));
01016 
01017   // Sends the trajectory along to the controller
01018   goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandle::runNonRT, rt_gh);
01019   commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory), rt_gh);
01020   rt_active_goal_ = rt_gh;
01021   goal_handle_timer_.start();
01022 }
01023 
01024 void JointTrajectoryActionController::goalCBFollow(GoalHandleFollow gh)
01025 {
01026   std::vector<std::string> joint_names(joints_.size());
01027   for (size_t j = 0; j < joints_.size(); ++j)
01028     joint_names[j] = joints_[j]->joint_->name;
01029 
01030   // Ensures that the joints in the goal match the joints we are commanding.
01031   if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
01032   {
01033     ROS_ERROR("Joints on incoming goal don't match our joints");
01034     control_msgs::FollowJointTrajectoryResult result;
01035     result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
01036     gh.setRejected(result);
01037     return;
01038   }
01039 
01040   preemptActiveGoal();
01041 
01042   gh.setAccepted();
01043   boost::shared_ptr<RTGoalHandleFollow> rt_gh(new RTGoalHandleFollow(gh));
01044 
01045   // Sends the trajectory along to the controller
01046   goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandleFollow::runNonRT, rt_gh);
01047   commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory),
01048                     boost::shared_ptr<RTGoalHandle>((RTGoalHandle*)NULL),
01049                     rt_gh);
01050   rt_active_goal_follow_ = rt_gh;
01051   goal_handle_timer_.start();
01052 }
01053 
01054 void JointTrajectoryActionController::cancelCB(GoalHandle gh)
01055 {
01056   boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
01057   if (current_active_goal && current_active_goal->gh_ == gh)
01058   {
01059     rt_active_goal_.reset();
01060 
01061     trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
01062     empty->joint_names.resize(joints_.size());
01063     for (size_t j = 0; j < joints_.size(); ++j)
01064       empty->joint_names[j] = joints_[j]->joint_->name;
01065     commandTrajectory(empty);
01066 
01067     // Marks the current goal as canceled.
01068     current_active_goal->gh_.setCanceled();
01069   }
01070 }
01071 
01072 void JointTrajectoryActionController::cancelCBFollow(GoalHandleFollow gh)
01073 {
01074   boost::shared_ptr<RTGoalHandleFollow> current_active_goal(rt_active_goal_follow_);
01075   if (current_active_goal && current_active_goal->gh_ == gh)
01076   {
01077     rt_active_goal_follow_.reset();
01078 
01079     trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
01080     empty->joint_names.resize(joints_.size());
01081     for (size_t j = 0; j < joints_.size(); ++j)
01082       empty->joint_names[j] = joints_[j]->joint_->name;
01083     commandTrajectory(empty);
01084 
01085     // Marks the current goal as canceled.
01086     current_active_goal->gh_.setCanceled();
01087   }
01088 }
01089 }


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Aug 27 2015 14:22:45