$search
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_DECLARE_CLASS(robot_mechanism_controllers, JointSplineTrajectoryController, 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] = joints_[i]->position_ - q[i]; 00314 joints_[i]->commanded_effort_ += pids_[i].updatePid(error[i], joints_[i]->velocity_ - qd[i], dt); 00315 } 00316 00317 // ------ State publishing 00318 00319 if (loop_count_ % 10 == 0) 00320 { 00321 if (controller_state_publisher_ && controller_state_publisher_->trylock()) 00322 { 00323 controller_state_publisher_->msg_.header.stamp = time; 00324 for (size_t j = 0; j < joints_.size(); ++j) 00325 { 00326 controller_state_publisher_->msg_.desired.positions[j] = q[j]; 00327 controller_state_publisher_->msg_.desired.velocities[j] = qd[j]; 00328 controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j]; 00329 controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_; 00330 controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_; 00331 controller_state_publisher_->msg_.error.positions[j] = error[j]; 00332 controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j]; 00333 } 00334 controller_state_publisher_->unlockAndPublish(); 00335 } 00336 } 00337 00338 ++loop_count_; 00339 } 00340 00341 void JointSplineTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) 00342 { 00343 ros::Time time = last_time_; 00344 ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf", 00345 time.toSec(), msg->header.stamp.toSec()); 00346 00347 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory); 00348 SpecifiedTrajectory &new_traj = *new_traj_ptr; 00349 00350 // ------ If requested, performs a stop 00351 00352 if (msg->points.empty()) 00353 { 00354 starting(); 00355 return; 00356 } 00357 00358 // ------ Correlates the joints we're commanding to the joints in the message 00359 00360 std::vector<int> lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg 00361 for (size_t j = 0; j < joints_.size(); ++j) 00362 { 00363 for (size_t k = 0; k < msg->joint_names.size(); ++k) 00364 { 00365 if (msg->joint_names[k] == joints_[j]->joint_->name) 00366 { 00367 lookup[j] = k; 00368 break; 00369 } 00370 } 00371 00372 if (lookup[j] == -1) 00373 { 00374 ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str()); 00375 return; 00376 } 00377 } 00378 00379 // ------ Grabs the trajectory that we're currently following. 00380 00381 boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr; 00382 current_trajectory_box_.get(prev_traj_ptr); 00383 if (!prev_traj_ptr) 00384 { 00385 ROS_FATAL("The current trajectory can never be null"); 00386 return; 00387 } 00388 const SpecifiedTrajectory &prev_traj = *prev_traj_ptr; 00389 00390 // ------ Copies over the segments from the previous trajectory that are still useful. 00391 00392 // Useful segments are still relevant after the current time. 00393 int first_useful = -1; 00394 while (first_useful + 1 < (int)prev_traj.size() && 00395 prev_traj[first_useful + 1].start_time <= time.toSec()) 00396 { 00397 ++first_useful; 00398 } 00399 00400 // Useful segments are not going to be completely overwritten by the message's splines. 00401 int last_useful = -1; 00402 double msg_start_time; 00403 if (msg->points.size() > 0) 00404 msg_start_time = (msg->header.stamp + msg->points[0].time_from_start).toSec(); 00405 else 00406 msg_start_time = std::max(time.toSec(), msg->header.stamp.toSec()); 00407 00408 while (last_useful + 1 < (int)prev_traj.size() && 00409 prev_traj[last_useful + 1].start_time < msg_start_time) 00410 { 00411 ++last_useful; 00412 } 00413 00414 if (last_useful < first_useful) 00415 first_useful = last_useful; 00416 00417 // Copies over the old segments that were determined to be useful. 00418 for (int i = std::max(first_useful,0); i <= last_useful; ++i) 00419 { 00420 new_traj.push_back(prev_traj[i]); 00421 } 00422 00423 // We always save the last segment so that we know where to stop if 00424 // there are no new segments. 00425 if (new_traj.size() == 0) 00426 new_traj.push_back(prev_traj[prev_traj.size() - 1]); 00427 00428 // ------ Determines when and where the new segments start 00429 00430 // Finds the end conditions of the final segment 00431 Segment &last = new_traj[new_traj.size() - 1]; 00432 std::vector<double> prev_positions(joints_.size()); 00433 std::vector<double> prev_velocities(joints_.size()); 00434 std::vector<double> prev_accelerations(joints_.size()); 00435 00436 ROS_DEBUG("Initial conditions for new set of splines:"); 00437 for (size_t i = 0; i < joints_.size(); ++i) 00438 { 00439 sampleSplineWithTimeBounds(last.splines[i].coef, last.duration, 00440 msg->header.stamp.toSec() - last.start_time, 00441 prev_positions[i], prev_velocities[i], prev_accelerations[i]); 00442 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i], 00443 prev_accelerations[i], joints_[i]->joint_->name.c_str()); 00444 } 00445 00446 // ------ Tacks on the new segments 00447 00448 std::vector<double> positions; 00449 std::vector<double> velocities; 00450 std::vector<double> accelerations; 00451 00452 std::vector<double> durations(msg->points.size()); 00453 durations[0] = msg->points[0].time_from_start.toSec(); 00454 for (size_t i = 1; i < msg->points.size(); ++i) 00455 durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec(); 00456 00457 // Checks if we should wrap 00458 std::vector<double> wrap(joints_.size(), 0.0); 00459 assert(!msg->points[0].positions.empty()); 00460 for (size_t j = 0; j < joints_.size(); ++j) 00461 { 00462 if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS) 00463 { 00464 double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]); 00465 wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j]; 00466 } 00467 } 00468 00469 for (size_t i = 0; i < msg->points.size(); ++i) 00470 { 00471 Segment seg; 00472 00473 seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i]; 00474 seg.duration = durations[i]; 00475 seg.splines.resize(joints_.size()); 00476 00477 // Checks that the incoming segment has the right number of elements. 00478 00479 if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size()) 00480 { 00481 ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size()); 00482 return; 00483 } 00484 if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size()) 00485 { 00486 ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size()); 00487 return; 00488 } 00489 if (msg->points[i].positions.size() != joints_.size()) 00490 { 00491 ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size()); 00492 return; 00493 } 00494 00495 // Re-orders the joints in the command to match the interal joint order. 00496 00497 accelerations.resize(msg->points[i].accelerations.size()); 00498 velocities.resize(msg->points[i].velocities.size()); 00499 positions.resize(msg->points[i].positions.size()); 00500 for (size_t j = 0; j < joints_.size(); ++j) 00501 { 00502 if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]]; 00503 if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]]; 00504 if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j]; 00505 } 00506 00507 // Converts the boundary conditions to splines. 00508 00509 for (size_t j = 0; j < joints_.size(); ++j) 00510 { 00511 if (prev_accelerations.size() > 0 && accelerations.size() > 0) 00512 { 00513 getQuinticSplineCoefficients( 00514 prev_positions[j], prev_velocities[j], prev_accelerations[j], 00515 positions[j], velocities[j], accelerations[j], 00516 durations[i], 00517 seg.splines[j].coef); 00518 } 00519 else if (prev_velocities.size() > 0 && velocities.size() > 0) 00520 { 00521 getCubicSplineCoefficients( 00522 prev_positions[j], prev_velocities[j], 00523 positions[j], velocities[j], 00524 durations[i], 00525 seg.splines[j].coef); 00526 seg.splines[j].coef.resize(6, 0.0); 00527 } 00528 else 00529 { 00530 seg.splines[j].coef[0] = prev_positions[j]; 00531 if (durations[i] == 0.0) 00532 seg.splines[j].coef[1] = 0.0; 00533 else 00534 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i]; 00535 seg.splines[j].coef[2] = 0.0; 00536 seg.splines[j].coef[3] = 0.0; 00537 seg.splines[j].coef[4] = 0.0; 00538 seg.splines[j].coef[5] = 0.0; 00539 } 00540 } 00541 00542 // Pushes the splines onto the end of the new trajectory. 00543 00544 new_traj.push_back(seg); 00545 00546 // Computes the starting conditions for the next segment 00547 00548 prev_positions = positions; 00549 prev_velocities = velocities; 00550 prev_accelerations = accelerations; 00551 } 00552 00553 // ------ Commits the new trajectory 00554 00555 if (!new_traj_ptr) 00556 { 00557 ROS_ERROR("The new trajectory was null!"); 00558 return; 00559 } 00560 00561 current_trajectory_box_.set(new_traj_ptr); 00562 ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size()); 00563 #if 0 00564 for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i) 00565 { 00566 ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration); 00567 for (size_t j = 0; j < new_traj[i].splines.size(); ++j) 00568 { 00569 ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)", 00570 new_traj[i].splines[j].coef[0], 00571 new_traj[i].splines[j].coef[1], 00572 new_traj[i].splines[j].coef[2], 00573 new_traj[i].splines[j].coef[3], 00574 new_traj[i].splines[j].coef[4], 00575 new_traj[i].splines[j].coef[5], 00576 joints_[j]->joint_->name_.c_str()); 00577 } 00578 } 00579 #endif 00580 } 00581 00582 bool JointSplineTrajectoryController::queryStateService( 00583 pr2_controllers_msgs::QueryTrajectoryState::Request &req, 00584 pr2_controllers_msgs::QueryTrajectoryState::Response &resp) 00585 { 00586 boost::shared_ptr<const SpecifiedTrajectory> traj_ptr; 00587 current_trajectory_box_.get(traj_ptr); 00588 if (!traj_ptr) 00589 { 00590 ROS_FATAL("The current trajectory can never be null"); 00591 return false; 00592 } 00593 const SpecifiedTrajectory &traj = *traj_ptr; 00594 00595 // Determines which segment of the trajectory to use 00596 int seg = -1; 00597 while (seg + 1 < (int)traj.size() && 00598 traj[seg+1].start_time < req.time.toSec()) 00599 { 00600 ++seg; 00601 } 00602 if (seg == -1) 00603 return false; 00604 00605 for (size_t i = 0; i < q.size(); ++i) 00606 { 00607 } 00608 00609 00610 resp.name.resize(joints_.size()); 00611 resp.position.resize(joints_.size()); 00612 resp.velocity.resize(joints_.size()); 00613 resp.acceleration.resize(joints_.size()); 00614 for (size_t j = 0; j < joints_.size(); ++j) 00615 { 00616 resp.name[j] = joints_[j]->joint_->name; 00617 sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, 00618 req.time.toSec() - traj[seg].start_time, 00619 resp.position[j], resp.velocity[j], resp.acceleration[j]); 00620 } 00621 00622 return true; 00623 } 00624 00625 void JointSplineTrajectoryController::sampleSplineWithTimeBounds( 00626 const std::vector<double>& coefficients, double duration, double time, 00627 double& position, double& velocity, double& acceleration) 00628 { 00629 if (time < 0) 00630 { 00631 double _; 00632 sampleQuinticSpline(coefficients, 0.0, position, _, _); 00633 velocity = 0; 00634 acceleration = 0; 00635 } 00636 else if (time > duration) 00637 { 00638 double _; 00639 sampleQuinticSpline(coefficients, duration, position, _, _); 00640 velocity = 0; 00641 acceleration = 0; 00642 } 00643 else 00644 { 00645 sampleQuinticSpline(coefficients, time, 00646 position, velocity, acceleration); 00647 } 00648 } 00649 00650 }