joint_spline_trajectory_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_spline_trajectory_controller.h"
00035 #include <sstream>
00036 #include "angles/angles.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 PLUGINLIB_EXPORT_CLASS( controller::JointSplineTrajectoryController, 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 JointSplineTrajectoryController::JointSplineTrajectoryController()
00143   : loop_count_(0), robot_(NULL)
00144 {
00145 }
00146 
00147 JointSplineTrajectoryController::~JointSplineTrajectoryController()
00148 {
00149   sub_command_.shutdown();
00150   serve_query_state_.shutdown();
00151 }
00152 
00153 bool JointSplineTrajectoryController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00154 {
00155   using namespace XmlRpc;
00156   node_ = n;
00157   robot_ = robot;
00158 
00159   // Gets all of the joints
00160   XmlRpc::XmlRpcValue joint_names;
00161   if (!node_.getParam("joints", joint_names))
00162   {
00163     ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
00164     return false;
00165   }
00166   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00167   {
00168     ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
00169     return false;
00170   }
00171   for (int i = 0; i < joint_names.size(); ++i)
00172   {
00173     XmlRpcValue &name_value = joint_names[i];
00174     if (name_value.getType() != XmlRpcValue::TypeString)
00175     {
00176       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00177                 node_.getNamespace().c_str());
00178       return false;
00179     }
00180 
00181     pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
00182     if (!j) {
00183       ROS_ERROR("Joint not found: %s. (namespace: %s)",
00184                 ((std::string)name_value).c_str(), node_.getNamespace().c_str());
00185       return false;
00186     }
00187     joints_.push_back(j);
00188   }
00189 
00190   // Ensures that all the joints are calibrated.
00191   for (size_t i = 0; i < joints_.size(); ++i)
00192   {
00193     if (!joints_[i]->calibrated_)
00194     {
00195       ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
00196                 joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
00197       return false;
00198     }
00199   }
00200 
00201   // Sets up pid controllers for all of the joints
00202   std::string gains_ns;
00203   if (!node_.getParam("gains", gains_ns))
00204     gains_ns = node_.getNamespace() + "/gains";
00205   pids_.resize(joints_.size());
00206   for (size_t i = 0; i < joints_.size(); ++i)
00207     if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
00208       return false;
00209 
00210   // Creates a dummy trajectory
00211   boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
00212   SpecifiedTrajectory &traj = *traj_ptr;
00213   traj[0].start_time = robot_->getTime().toSec();
00214   traj[0].duration = 0.0;
00215   traj[0].splines.resize(joints_.size());
00216   for (size_t j = 0; j < joints_.size(); ++j)
00217     traj[0].splines[j].coef[0] = 0.0;
00218   current_trajectory_box_.set(traj_ptr);
00219 
00220   sub_command_ = node_.subscribe("command", 1, &JointSplineTrajectoryController::commandCB, this);
00221   serve_query_state_ = node_.advertiseService(
00222     "query_state", &JointSplineTrajectoryController::queryStateService, this);
00223 
00224   q.resize(joints_.size());
00225   qd.resize(joints_.size());
00226   qdd.resize(joints_.size());
00227 
00228   controller_state_publisher_.reset(
00229     new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
00230     (node_, "state", 1));
00231   controller_state_publisher_->lock();
00232   for (size_t j = 0; j < joints_.size(); ++j)
00233     controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
00234   controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
00235   controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
00236   controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
00237   controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
00238   controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
00239   controller_state_publisher_->msg_.error.positions.resize(joints_.size());
00240   controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
00241   controller_state_publisher_->unlock();
00242 
00243 
00244   return true;
00245 }
00246 
00247 void JointSplineTrajectoryController::starting()
00248 {
00249   last_time_ = robot_->getTime();
00250 
00251   for (size_t i = 0; i < pids_.size(); ++i)
00252     pids_[i].reset();
00253 
00254   // Creates a "hold current position" trajectory.
00255   boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00256   SpecifiedTrajectory &hold = *hold_ptr;
00257   hold[0].start_time = last_time_.toSec() - 0.001;
00258   hold[0].duration = 0.0;
00259   hold[0].splines.resize(joints_.size());
00260   for (size_t j = 0; j < joints_.size(); ++j)
00261     hold[0].splines[j].coef[0] = joints_[j]->position_;
00262 
00263   current_trajectory_box_.set(hold_ptr);
00264 }
00265 
00266 void JointSplineTrajectoryController::update()
00267 {
00268   // Checks if all the joints are calibrated.
00269 
00270   ros::Time time = robot_->getTime();
00271   ros::Duration dt = time - last_time_;
00272   last_time_ = time;
00273 
00274   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00275   current_trajectory_box_.get(traj_ptr);
00276   if (!traj_ptr)
00277     ROS_FATAL("The current trajectory can never be null");
00278 
00279   // Only because this is what the code originally looked like.
00280   const SpecifiedTrajectory &traj = *traj_ptr;
00281 
00282   // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
00283   int seg = -1;
00284   while (seg + 1 < (int)traj.size() &&
00285          traj[seg+1].start_time < time.toSec())
00286   {
00287     ++seg;
00288   }
00289 
00290   if (seg == -1)
00291   {
00292     if (traj.size() == 0)
00293       ROS_ERROR("No segments in the trajectory");
00294     else
00295       ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00296     return;
00297   }
00298 
00299   // ------ Trajectory Sampling
00300 
00301   for (size_t i = 0; i < q.size(); ++i)
00302   {
00303     sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00304                                time.toSec() - traj[seg].start_time,
00305                                q[i], qd[i], qdd[i]);
00306   }
00307 
00308   // ------ Trajectory Following
00309 
00310   std::vector<double> error(joints_.size());
00311   for (size_t i = 0; i < joints_.size(); ++i)
00312   {
00313     error[i] = q[i] - joints_[i]->position_;
00314     joints_[i]->commanded_effort_ += pids_[i].computeCommand(error[i],
00315           joints_[i]->velocity_ - qd[i], dt);
00316   }
00317 
00318   // ------ State publishing
00319 
00320   if (loop_count_ % 10 == 0)
00321   {
00322     if (controller_state_publisher_ && controller_state_publisher_->trylock())
00323     {
00324       controller_state_publisher_->msg_.header.stamp = time;
00325       for (size_t j = 0; j < joints_.size(); ++j)
00326       {
00327         controller_state_publisher_->msg_.desired.positions[j] = q[j];
00328         controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
00329         controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
00330         controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
00331         controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
00332         controller_state_publisher_->msg_.error.positions[j] = error[j];
00333         controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
00334       }
00335       controller_state_publisher_->unlockAndPublish();
00336     }
00337   }
00338 
00339   ++loop_count_;
00340 }
00341 
00342 void JointSplineTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00343 {
00344   ros::Time time = last_time_;
00345   ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00346             time.toSec(), msg->header.stamp.toSec());
00347 
00348   boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00349   SpecifiedTrajectory &new_traj = *new_traj_ptr;
00350 
00351   // ------ If requested, performs a stop
00352 
00353   if (msg->points.empty())
00354   {
00355     starting();
00356     return;
00357   }
00358 
00359   // ------ Correlates the joints we're commanding to the joints in the message
00360 
00361   std::vector<int> lookup(joints_.size(), -1);  // Maps from an index in joints_ to an index in the msg
00362   for (size_t j = 0; j < joints_.size(); ++j)
00363   {
00364     for (size_t k = 0; k < msg->joint_names.size(); ++k)
00365     {
00366       if (msg->joint_names[k] == joints_[j]->joint_->name)
00367       {
00368         lookup[j] = k;
00369         break;
00370       }
00371     }
00372 
00373     if (lookup[j] == -1)
00374     {
00375       ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
00376       return;
00377     }
00378   }
00379 
00380   // ------ Grabs the trajectory that we're currently following.
00381 
00382   boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
00383   current_trajectory_box_.get(prev_traj_ptr);
00384   if (!prev_traj_ptr)
00385   {
00386     ROS_FATAL("The current trajectory can never be null");
00387     return;
00388   }
00389   const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
00390 
00391   // ------ Copies over the segments from the previous trajectory that are still useful.
00392 
00393   // Useful segments are still relevant after the current time.
00394   int first_useful = -1;
00395   while (first_useful + 1 < (int)prev_traj.size() &&
00396          prev_traj[first_useful + 1].start_time <= time.toSec())
00397   {
00398     ++first_useful;
00399   }
00400 
00401   // Useful segments are not going to be completely overwritten by the message's splines.
00402   int last_useful = -1;
00403   double msg_start_time;
00404   if (msg->points.size() > 0)
00405     msg_start_time = (msg->header.stamp + msg->points[0].time_from_start).toSec();
00406   else
00407     msg_start_time = std::max(time.toSec(), msg->header.stamp.toSec());
00408 
00409   while (last_useful + 1 < (int)prev_traj.size() &&
00410          prev_traj[last_useful + 1].start_time < msg_start_time)
00411   {
00412     ++last_useful;
00413   }
00414 
00415   if (last_useful < first_useful)
00416     first_useful = last_useful;
00417 
00418   // Copies over the old segments that were determined to be useful.
00419   for (int i = std::max(first_useful,0); i <= last_useful; ++i)
00420   {
00421     new_traj.push_back(prev_traj[i]);
00422   }
00423 
00424   // We always save the last segment so that we know where to stop if
00425   // there are no new segments.
00426   if (new_traj.size() == 0)
00427     new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00428 
00429   // ------ Determines when and where the new segments start
00430 
00431   // Finds the end conditions of the final segment
00432   Segment &last = new_traj[new_traj.size() - 1];
00433   std::vector<double> prev_positions(joints_.size());
00434   std::vector<double> prev_velocities(joints_.size());
00435   std::vector<double> prev_accelerations(joints_.size());
00436 
00437   ROS_DEBUG("Initial conditions for new set of splines:");
00438   for (size_t i = 0; i < joints_.size(); ++i)
00439   {
00440     sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
00441                                msg->header.stamp.toSec() - last.start_time,
00442                                prev_positions[i], prev_velocities[i], prev_accelerations[i]);
00443     ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
00444               prev_accelerations[i], joints_[i]->joint_->name.c_str());
00445   }
00446 
00447   // ------ Tacks on the new segments
00448 
00449   std::vector<double> positions;
00450   std::vector<double> velocities;
00451   std::vector<double> accelerations;
00452 
00453   std::vector<double> durations(msg->points.size());
00454   durations[0] = msg->points[0].time_from_start.toSec();
00455   for (size_t i = 1; i < msg->points.size(); ++i)
00456     durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00457 
00458   // Checks if we should wrap
00459   std::vector<double> wrap(joints_.size(), 0.0);
00460   assert(!msg->points[0].positions.empty());
00461   for (size_t j = 0; j < joints_.size(); ++j)
00462   {
00463     if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
00464     {
00465       double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]);
00466       wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j];
00467     }
00468   }
00469 
00470   for (size_t i = 0; i < msg->points.size(); ++i)
00471   {
00472     Segment seg;
00473 
00474     seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00475     seg.duration = durations[i];
00476     seg.splines.resize(joints_.size());
00477 
00478     // Checks that the incoming segment has the right number of elements.
00479 
00480     if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
00481     {
00482       ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00483       return;
00484     }
00485     if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
00486     {
00487       ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00488       return;
00489     }
00490     if (msg->points[i].positions.size() != joints_.size())
00491     {
00492       ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00493       return;
00494     }
00495 
00496     // Re-orders the joints in the command to match the interal joint order.
00497 
00498     accelerations.resize(msg->points[i].accelerations.size());
00499     velocities.resize(msg->points[i].velocities.size());
00500     positions.resize(msg->points[i].positions.size());
00501     for (size_t j = 0; j < joints_.size(); ++j)
00502     {
00503       if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
00504       if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
00505       if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
00506     }
00507 
00508     // Converts the boundary conditions to splines.
00509 
00510     for (size_t j = 0; j < joints_.size(); ++j)
00511     {
00512       if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00513       {
00514         getQuinticSplineCoefficients(
00515           prev_positions[j], prev_velocities[j], prev_accelerations[j],
00516           positions[j], velocities[j], accelerations[j],
00517           durations[i],
00518           seg.splines[j].coef);
00519       }
00520       else if (prev_velocities.size() > 0 && velocities.size() > 0)
00521       {
00522         getCubicSplineCoefficients(
00523           prev_positions[j], prev_velocities[j],
00524           positions[j], velocities[j],
00525           durations[i],
00526           seg.splines[j].coef);
00527         seg.splines[j].coef.resize(6, 0.0);
00528       }
00529       else
00530       {
00531         seg.splines[j].coef[0] = prev_positions[j];
00532         if (durations[i] == 0.0)
00533           seg.splines[j].coef[1] = 0.0;
00534         else
00535           seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00536         seg.splines[j].coef[2] = 0.0;
00537         seg.splines[j].coef[3] = 0.0;
00538         seg.splines[j].coef[4] = 0.0;
00539         seg.splines[j].coef[5] = 0.0;
00540       }
00541     }
00542 
00543     // Pushes the splines onto the end of the new trajectory.
00544 
00545     new_traj.push_back(seg);
00546 
00547     // Computes the starting conditions for the next segment
00548 
00549     prev_positions = positions;
00550     prev_velocities = velocities;
00551     prev_accelerations = accelerations;
00552   }
00553 
00554   // ------ Commits the new trajectory
00555 
00556   if (!new_traj_ptr)
00557   {
00558     ROS_ERROR("The new trajectory was null!");
00559     return;
00560   }
00561 
00562   current_trajectory_box_.set(new_traj_ptr);
00563   ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
00564 #if 0
00565   for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
00566   {
00567     ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
00568     for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
00569     {
00570       ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf , %.2lf  %.2lf(%s)",
00571                 new_traj[i].splines[j].coef[0],
00572                 new_traj[i].splines[j].coef[1],
00573                 new_traj[i].splines[j].coef[2],
00574                 new_traj[i].splines[j].coef[3],
00575                 new_traj[i].splines[j].coef[4],
00576                 new_traj[i].splines[j].coef[5],
00577                 joints_[j]->joint_->name_.c_str());
00578     }
00579   }
00580 #endif
00581 }
00582 
00583 bool JointSplineTrajectoryController::queryStateService(
00584   pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00585   pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
00586 {
00587   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00588   current_trajectory_box_.get(traj_ptr);
00589   if (!traj_ptr)
00590   {
00591     ROS_FATAL("The current trajectory can never be null");
00592     return false;
00593   }
00594   const SpecifiedTrajectory &traj = *traj_ptr;
00595 
00596   // Determines which segment of the trajectory to use
00597   int seg = -1;
00598   while (seg + 1 < (int)traj.size() &&
00599          traj[seg+1].start_time < req.time.toSec())
00600   {
00601     ++seg;
00602   }
00603   if (seg == -1)
00604     return false;
00605 
00606   for (size_t i = 0; i < q.size(); ++i)
00607   {
00608   }
00609 
00610 
00611   resp.name.resize(joints_.size());
00612   resp.position.resize(joints_.size());
00613   resp.velocity.resize(joints_.size());
00614   resp.acceleration.resize(joints_.size());
00615   for (size_t j = 0; j < joints_.size(); ++j)
00616   {
00617     resp.name[j] = joints_[j]->joint_->name;
00618     sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
00619                                req.time.toSec() - traj[seg].start_time,
00620                                resp.position[j], resp.velocity[j], resp.acceleration[j]);
00621   }
00622 
00623   return true;
00624 }
00625 
00626 void JointSplineTrajectoryController::sampleSplineWithTimeBounds(
00627   const std::vector<double>& coefficients, double duration, double time,
00628   double& position, double& velocity, double& acceleration)
00629 {
00630   if (time < 0)
00631   {
00632     double _;
00633     sampleQuinticSpline(coefficients, 0.0, position, _, _);
00634     velocity = 0;
00635     acceleration = 0;
00636   }
00637   else if (time > duration)
00638   {
00639     double _;
00640     sampleQuinticSpline(coefficients, duration, position, _, _);
00641     velocity = 0;
00642     acceleration = 0;
00643   }
00644   else
00645   {
00646     sampleQuinticSpline(coefficients, time,
00647                         position, velocity, acceleration);
00648   }
00649 }
00650 
00651 }


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Sat Jun 8 2019 20:49:25