00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
00044
00045
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
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
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
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
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
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
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
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
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
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
00349 const SpecifiedTrajectory &traj = *traj_ptr;
00350
00351
00352
00353
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
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
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
00388
00389
00390 error[i] = q[i] - joints_[i]->position_;
00391 v_error[i] = qd[i] - joints_[i]->velocity_;
00392
00393
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
00408 joints_[i]->commanded_effort_ = effort;
00409 }
00410
00411
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
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
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
00456 }
00457 else if (time < end_time + ros::Duration(traj[seg].goal_time_tolerance))
00458 {
00459
00460 }
00461 else
00462 {
00463
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
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
00549
00550 if (msg->points.empty())
00551 {
00552 starting();
00553 return;
00554 }
00555
00556
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
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
00573
00574
00575
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
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
00603
00604
00605
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
00635
00636 std::vector<int> lookup(joints_.size(), -1);
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
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
00673
00674
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
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
00691
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
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
00710
00711 if (new_traj.size() == 0)
00712 new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00713
00714
00715
00716
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
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
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
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
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
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
00837 seg.trajectory_tolerance = trajectory_tolerance;
00838 seg.goal_tolerance = goal_tolerance;
00839 seg.goal_time_tolerance = goal_time_tolerance;
00840
00841
00842
00843 new_traj.push_back(seg);
00844
00845
00846
00847 prev_positions = positions;
00848 prev_velocities = velocities;
00849 prev_accelerations = accelerations;
00850 }
00851
00852
00853
00854
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
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
00976 if (current_active_goal)
00977 {
00978
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
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
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
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
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
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
01086 current_active_goal->gh_.setCanceled();
01087 }
01088 }
01089 }