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 "../include/r2_joint_trajectory_action_controller.h"
00035 #include <sstream>
00036 #include "angles/angles.h"
00037 #include "pluginlib/class_list_macros.h"
00038
00039 PLUGINLIB_DECLARE_CLASS(r2_controllers_gazebo, R2JointTrajectoryActionController, r2_controller_ns::R2JointTrajectoryActionController, pr2_controller_interface::Controller)
00040
00041 namespace r2_controller_ns {
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 R2JointTrajectoryActionController::R2JointTrajectoryActionController()
00143 : loop_count_(0), robot_(NULL)
00144 {
00145 }
00146
00147 R2JointTrajectoryActionController::~R2JointTrajectoryActionController()
00148 {
00149 sub_command_.shutdown();
00150 serve_query_state_.shutdown();
00151 action_server_.reset();
00152 action_server_follow_.reset();
00153 }
00154
00155 bool R2JointTrajectoryActionController::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, &R2JointTrajectoryActionController::commandCB, this);
00277 serve_query_state_ = node_.advertiseService(
00278 "query_state", &R2JointTrajectoryActionController::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(&R2JointTrajectoryActionController::goalCB, this, _1),
00301 boost::bind(&R2JointTrajectoryActionController::cancelCB, this, _1)));
00302 action_server_follow_.reset(new FJTAS(node_, "follow_joint_trajectory",
00303 boost::bind(&R2JointTrajectoryActionController::goalCBFollow, this, _1),
00304 boost::bind(&R2JointTrajectoryActionController::cancelCBFollow, this, _1)));
00305
00306 return true;
00307 }
00308
00309 void R2JointTrajectoryActionController::starting()
00310 {
00311 last_time_ = robot_->getTime();
00312
00313 for (size_t i = 0; i < pids_.size(); ++i) {
00314 pids_[i].reset();
00315 proxies_[i].reset(joints_[i]->position_, joints_[i]->velocity_);
00316 }
00317
00318
00319 boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00320 SpecifiedTrajectory &hold = *hold_ptr;
00321 hold[0].start_time = last_time_.toSec() - 0.001;
00322 hold[0].duration = 0.0;
00323 hold[0].splines.resize(joints_.size());
00324 for (size_t j = 0; j < joints_.size(); ++j)
00325 hold[0].splines[j].coef[0] = joints_[j]->position_;
00326
00327 current_trajectory_box_.set(hold_ptr);
00328 }
00329
00330 void R2JointTrajectoryActionController::update()
00331 {
00332 ros::Time time = robot_->getTime();
00333 ros::Duration dt = time - last_time_;
00334 last_time_ = time;
00335
00336 boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
00337 boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);
00338
00339 boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00340 current_trajectory_box_.get(traj_ptr);
00341 if (!traj_ptr)
00342 ROS_FATAL("The current trajectory can never be null");
00343
00344
00345 const SpecifiedTrajectory &traj = *traj_ptr;
00346
00347
00348
00349
00350 int seg = -1;
00351 while (seg + 1 < (int)traj.size() &&
00352 traj[seg+1].start_time < time.toSec())
00353 {
00354 ++seg;
00355 }
00356
00357 if (seg == -1)
00358 {
00359 if (traj.size() == 0)
00360 ROS_ERROR("No segments in the trajectory");
00361 else
00362 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00363 return;
00364 }
00365
00366
00367
00368 for (size_t i = 0; i < q.size(); ++i)
00369 {
00370 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00371 time.toSec() - traj[seg].start_time,
00372 q[i], qd[i], qdd[i]);
00373 }
00374
00375
00376
00377 std::vector<double> error(joints_.size());
00378 std::vector<double> v_error(joints_.size());
00379 for (size_t i = 0; i < joints_.size(); ++i)
00380 {
00381 double effort;
00382
00383
00384
00385
00386 error[i] = joints_[i]->position_ - q[i];
00387 v_error[i] = joints_[i]->velocity_ - qd[i];
00388
00389
00390 if (proxies_enabled_[i]) {
00391 effort = proxies_[i].update(q[i], qd[i], qdd[i],
00392 joints_[i]->position_, joints_[i]->velocity_,
00393 dt.toSec());
00394 }
00395 else {
00396 effort = pids_[i].updatePid(error[i], v_error[i], dt);
00397
00398 double effort_unfiltered = effort;
00399 if (output_filters_[i])
00400 output_filters_[i]->update(effort_unfiltered, effort);
00401 }
00402
00403
00404 joints_[i]->commanded_effort_ = effort;
00405 }
00406
00407
00408
00409 if ((traj[seg].gh && traj[seg].gh == current_active_goal) ||
00410 (traj[seg].gh_follow && traj[seg].gh_follow == current_active_goal_follow))
00411 {
00412 ros::Time end_time(traj[seg].start_time + traj[seg].duration);
00413 if (time <= end_time)
00414 {
00415
00416 for (size_t j = 0; j < joints_.size(); ++j)
00417 {
00418 if (traj[seg].trajectory_tolerance[j].violated(error[j], v_error[j]))
00419 {
00420 if (traj[seg].gh)
00421 traj[seg].gh->setAborted();
00422 else if (traj[seg].gh_follow) {
00423 traj[seg].gh_follow->preallocated_result_->error_code =
00424 control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00425 traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
00426 }
00427 break;
00428 }
00429 }
00430 }
00431 else if (seg == (int)traj.size() - 1)
00432 {
00433
00434 bool inside_goal_constraints = true;
00435 for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
00436 {
00437 if (traj[seg].goal_tolerance[i].violated(error[i], v_error[i]))
00438 inside_goal_constraints = false;
00439 }
00440
00441 if (inside_goal_constraints)
00442 {
00443 rt_active_goal_.reset();
00444 rt_active_goal_follow_.reset();
00445 if (traj[seg].gh)
00446 traj[seg].gh->setSucceeded();
00447 else if (traj[seg].gh_follow) {
00448 traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00449 traj[seg].gh_follow->setSucceeded(traj[seg].gh_follow->preallocated_result_);
00450 }
00451
00452 }
00453 else if (time < end_time + ros::Duration(traj[seg].goal_time_tolerance))
00454 {
00455
00456 }
00457 else
00458 {
00459
00460 rt_active_goal_.reset();
00461 rt_active_goal_follow_.reset();
00462 if (traj[seg].gh)
00463 traj[seg].gh->setAborted();
00464 else if (traj[seg].gh_follow) {
00465 traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
00466 traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
00467 }
00468 }
00469 }
00470 }
00471
00472
00473
00474 if (loop_count_ % 10 == 0)
00475 {
00476 if (controller_state_publisher_ && controller_state_publisher_->trylock())
00477 {
00478 controller_state_publisher_->msg_.header.stamp = time;
00479 for (size_t j = 0; j < joints_.size(); ++j)
00480 {
00481 controller_state_publisher_->msg_.desired.positions[j] = q[j];
00482 controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
00483 controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
00484 controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
00485 controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
00486 controller_state_publisher_->msg_.error.positions[j] = error[j];
00487 controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
00488 }
00489 controller_state_publisher_->unlockAndPublish();
00490 }
00491 }
00492
00493 ++loop_count_;
00494 }
00495
00496 void R2JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
00497 {
00498 preemptActiveGoal();
00499 commandTrajectory(msg);
00500 }
00501
00502 void R2JointTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg,
00503 boost::shared_ptr<RTGoalHandle> gh,
00504 boost::shared_ptr<RTGoalHandleFollow> gh_follow)
00505 {
00506 assert(!gh || !gh_follow);
00507 ros::Time time = last_time_ + ros::Duration(0.01);
00508 ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00509 time.toSec(), msg->header.stamp.toSec());
00510
00511 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00512 SpecifiedTrajectory &new_traj = *new_traj_ptr;
00513
00514
00515
00516 if (msg->points.empty())
00517 {
00518 starting();
00519 return;
00520 }
00521
00522
00523
00524 std::vector<JointTolerance> trajectory_tolerance = default_trajectory_tolerance_;
00525 std::vector<JointTolerance> goal_tolerance = default_goal_tolerance_;
00526 double goal_time_tolerance = default_goal_time_constraint_;
00527
00528 if (gh_follow)
00529 {
00530 for (size_t j = 0; j < joints_.size(); ++j)
00531 {
00532
00533 for (size_t k = 0; k < gh_follow->gh_.getGoal()->path_tolerance.size(); ++k)
00534 {
00535 const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->path_tolerance[k];
00536 if (joints_[j]->joint_->name == tol.name)
00537 {
00538
00539
00540
00541
00542
00543 if (tol.position > 0)
00544 trajectory_tolerance[j].position = tol.position;
00545 else if (tol.position < 0)
00546 trajectory_tolerance[j].position = 0.0;
00547
00548 if (tol.velocity > 0)
00549 trajectory_tolerance[j].velocity = tol.velocity;
00550 else if (tol.velocity < 0)
00551 trajectory_tolerance[j].velocity = 0.0;
00552
00553 if (tol.acceleration > 0)
00554 trajectory_tolerance[j].acceleration = tol.acceleration;
00555 else if (tol.acceleration < 0)
00556 trajectory_tolerance[j].acceleration = 0.0;
00557
00558 break;
00559 }
00560 }
00561
00562
00563 for (size_t k = 0; k < gh_follow->gh_.getGoal()->goal_tolerance.size(); ++k)
00564 {
00565 const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->goal_tolerance[k];
00566 if (joints_[j]->joint_->name == tol.name)
00567 {
00568
00569
00570
00571
00572
00573 if (tol.position > 0)
00574 goal_tolerance[j].position = tol.position;
00575 else if (tol.position < 0)
00576 goal_tolerance[j].position = 0.0;
00577
00578 if (tol.velocity > 0)
00579 goal_tolerance[j].velocity = tol.velocity;
00580 else if (tol.velocity < 0)
00581 goal_tolerance[j].velocity = 0.0;
00582
00583 if (tol.acceleration > 0)
00584 goal_tolerance[j].acceleration = tol.acceleration;
00585 else if (tol.acceleration < 0)
00586 goal_tolerance[j].acceleration = 0.0;
00587
00588 break;
00589 }
00590 }
00591 }
00592
00593 const ros::Duration &tol = gh_follow->gh_.getGoal()->goal_time_tolerance;
00594 if (tol < ros::Duration(0.0))
00595 goal_time_tolerance = 0.0;
00596 else if (tol > ros::Duration(0.0))
00597 goal_time_tolerance = tol.toSec();
00598 }
00599
00600
00601
00602 std::vector<int> lookup(joints_.size(), -1);
00603 for (size_t j = 0; j < joints_.size(); ++j)
00604 {
00605 for (size_t k = 0; k < msg->joint_names.size(); ++k)
00606 {
00607 if (msg->joint_names[k] == joints_[j]->joint_->name)
00608 {
00609 lookup[j] = k;
00610 break;
00611 }
00612 }
00613
00614 if (lookup[j] == -1)
00615 {
00616 ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
00617 if (gh)
00618 gh->setAborted();
00619 else if (gh_follow) {
00620 gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00621 gh_follow->setAborted(gh_follow->preallocated_result_);
00622 }
00623 return;
00624 }
00625 }
00626
00627
00628
00629 boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
00630 current_trajectory_box_.get(prev_traj_ptr);
00631 if (!prev_traj_ptr)
00632 {
00633 ROS_FATAL("The current trajectory can never be null");
00634 return;
00635 }
00636 const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
00637
00638
00639
00640
00641 int first_useful = -1;
00642 while (first_useful + 1 < (int)prev_traj.size() &&
00643 prev_traj[first_useful + 1].start_time <= time.toSec())
00644 {
00645 ++first_useful;
00646 }
00647
00648
00649 int last_useful = -1;
00650 double msg_start_time;
00651 if (msg->header.stamp == ros::Time(0.0))
00652 msg_start_time = time.toSec();
00653 else
00654 msg_start_time = msg->header.stamp.toSec();
00655
00656
00657
00658
00659
00660 while (last_useful + 1 < (int)prev_traj.size() &&
00661 prev_traj[last_useful + 1].start_time < msg_start_time)
00662 {
00663 ++last_useful;
00664 }
00665
00666 if (last_useful < first_useful)
00667 first_useful = last_useful;
00668
00669
00670 for (int i = std::max(first_useful,0); i <= last_useful; ++i)
00671 {
00672 new_traj.push_back(prev_traj[i]);
00673 }
00674
00675
00676
00677 if (new_traj.size() == 0)
00678 new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00679
00680
00681
00682
00683 Segment &last = new_traj[new_traj.size() - 1];
00684 std::vector<double> prev_positions(joints_.size());
00685 std::vector<double> prev_velocities(joints_.size());
00686 std::vector<double> prev_accelerations(joints_.size());
00687
00688 double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec())
00689 - last.start_time;
00690 ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t);
00691 for (size_t i = 0; i < joints_.size(); ++i)
00692 {
00693 sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
00694 t,
00695 prev_positions[i], prev_velocities[i], prev_accelerations[i]);
00696 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00697 prev_accelerations[i], joints_[i]->joint_->name.c_str());
00698 }
00699
00700
00701
00702 std::vector<double> positions;
00703 std::vector<double> velocities;
00704 std::vector<double> accelerations;
00705
00706 std::vector<double> durations(msg->points.size());
00707 if (msg->points.size() > 0)
00708 durations[0] = msg->points[0].time_from_start.toSec();
00709 for (size_t i = 1; i < msg->points.size(); ++i)
00710 durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00711
00712
00713 std::vector<double> wrap(joints_.size(), 0.0);
00714 assert(!msg->points[0].positions.empty());
00715 for (size_t j = 0; j < joints_.size(); ++j)
00716 {
00717 if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
00718 {
00719 double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]);
00720 wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j];
00721 }
00722 }
00723
00724 for (size_t i = 0; i < msg->points.size(); ++i)
00725 {
00726 Segment seg;
00727
00728 if (msg->header.stamp == ros::Time(0.0))
00729 seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
00730 else
00731 seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00732 seg.duration = durations[i];
00733 seg.gh = gh;
00734 seg.gh_follow = gh_follow;
00735 seg.splines.resize(joints_.size());
00736
00737
00738
00739 if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
00740 {
00741 ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00742 return;
00743 }
00744 if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
00745 {
00746 ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00747 return;
00748 }
00749 if (msg->points[i].positions.size() != joints_.size())
00750 {
00751 ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00752 return;
00753 }
00754
00755
00756
00757 accelerations.resize(msg->points[i].accelerations.size());
00758 velocities.resize(msg->points[i].velocities.size());
00759 positions.resize(msg->points[i].positions.size());
00760 for (size_t j = 0; j < joints_.size(); ++j)
00761 {
00762 if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
00763 if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
00764 if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
00765 }
00766
00767
00768
00769 for (size_t j = 0; j < joints_.size(); ++j)
00770 {
00771 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00772 {
00773 getQuinticSplineCoefficients(
00774 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00775 positions[j], velocities[j], accelerations[j],
00776 durations[i],
00777 seg.splines[j].coef);
00778 }
00779 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00780 {
00781 getCubicSplineCoefficients(
00782 prev_positions[j], prev_velocities[j],
00783 positions[j], velocities[j],
00784 durations[i],
00785 seg.splines[j].coef);
00786 seg.splines[j].coef.resize(6, 0.0);
00787 }
00788 else
00789 {
00790 seg.splines[j].coef[0] = prev_positions[j];
00791 if (durations[i] == 0.0)
00792 seg.splines[j].coef[1] = 0.0;
00793 else
00794 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00795 seg.splines[j].coef[2] = 0.0;
00796 seg.splines[j].coef[3] = 0.0;
00797 seg.splines[j].coef[4] = 0.0;
00798 seg.splines[j].coef[5] = 0.0;
00799 }
00800 }
00801
00802
00803 seg.trajectory_tolerance = trajectory_tolerance;
00804 seg.goal_tolerance = goal_tolerance;
00805 seg.goal_time_tolerance = goal_time_tolerance;
00806
00807
00808
00809 new_traj.push_back(seg);
00810
00811
00812
00813 prev_positions = positions;
00814 prev_velocities = velocities;
00815 prev_accelerations = accelerations;
00816 }
00817
00818
00819
00820
00821
00822 if (!new_traj_ptr)
00823 {
00824 ROS_ERROR("The new trajectory was null!");
00825 return;
00826 }
00827
00828 current_trajectory_box_.set(new_traj_ptr);
00829 ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
00830 #if 0
00831 for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
00832 {
00833 ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
00834 for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
00835 {
00836 ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)",
00837 new_traj[i].splines[j].coef[0],
00838 new_traj[i].splines[j].coef[1],
00839 new_traj[i].splines[j].coef[2],
00840 new_traj[i].splines[j].coef[3],
00841 new_traj[i].splines[j].coef[4],
00842 new_traj[i].splines[j].coef[5],
00843 joints_[j]->joint_->name_.c_str());
00844 }
00845 }
00846 #endif
00847 }
00848
00849 bool R2JointTrajectoryActionController::queryStateService(
00850 pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00851 pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
00852 {
00853 boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00854 current_trajectory_box_.get(traj_ptr);
00855 if (!traj_ptr)
00856 {
00857 ROS_FATAL("The current trajectory can never be null");
00858 return false;
00859 }
00860 const SpecifiedTrajectory &traj = *traj_ptr;
00861
00862
00863 int seg = -1;
00864 while (seg + 1 < (int)traj.size() &&
00865 traj[seg+1].start_time < req.time.toSec())
00866 {
00867 ++seg;
00868 }
00869 if (seg == -1)
00870 return false;
00871
00872 for (size_t i = 0; i < q.size(); ++i)
00873 {
00874 }
00875
00876
00877 resp.name.resize(joints_.size());
00878 resp.position.resize(joints_.size());
00879 resp.velocity.resize(joints_.size());
00880 resp.acceleration.resize(joints_.size());
00881 for (size_t j = 0; j < joints_.size(); ++j)
00882 {
00883 resp.name[j] = joints_[j]->joint_->name;
00884 sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
00885 req.time.toSec() - traj[seg].start_time,
00886 resp.position[j], resp.velocity[j], resp.acceleration[j]);
00887 }
00888
00889 return true;
00890 }
00891
00892 void R2JointTrajectoryActionController::sampleSplineWithTimeBounds(
00893 const std::vector<double>& coefficients, double duration, double time,
00894 double& position, double& velocity, double& acceleration)
00895 {
00896 if (time < 0)
00897 {
00898 double _;
00899 sampleQuinticSpline(coefficients, 0.0, position, _, _);
00900 velocity = 0;
00901 acceleration = 0;
00902 }
00903 else if (time > duration)
00904 {
00905 double _;
00906 sampleQuinticSpline(coefficients, duration, position, _, _);
00907 velocity = 0;
00908 acceleration = 0;
00909 }
00910 else
00911 {
00912 sampleQuinticSpline(coefficients, time,
00913 position, velocity, acceleration);
00914 }
00915 }
00916
00917 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
00918 {
00919 if (a.size() != b.size())
00920 return false;
00921
00922 for (size_t i = 0; i < a.size(); ++i)
00923 {
00924 if (count(b.begin(), b.end(), a[i]) != 1)
00925 return false;
00926 }
00927 for (size_t i = 0; i < b.size(); ++i)
00928 {
00929 if (count(a.begin(), a.end(), b[i]) != 1)
00930 return false;
00931 }
00932
00933 return true;
00934 }
00935
00936 void R2JointTrajectoryActionController::preemptActiveGoal()
00937 {
00938 boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
00939 boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);
00940
00941
00942 if (current_active_goal)
00943 {
00944
00945 rt_active_goal_.reset();
00946 current_active_goal->gh_.setCanceled();
00947 }
00948 if (current_active_goal_follow)
00949 {
00950 rt_active_goal_follow_.reset();
00951 current_active_goal_follow->gh_.setCanceled();
00952 }
00953 }
00954
00955
00956 template <class Enclosure, class Member>
00957 static boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
00958 {
00959 actionlib::EnclosureDeleter<Enclosure> d(enclosure);
00960 boost::shared_ptr<Member> p(&member, d);
00961 return p;
00962 }
00963
00964 void R2JointTrajectoryActionController::goalCB(GoalHandle gh)
00965 {
00966 std::vector<std::string> joint_names(joints_.size());
00967 for (size_t j = 0; j < joints_.size(); ++j)
00968 joint_names[j] = joints_[j]->joint_->name;
00969
00970
00971 if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
00972 {
00973 ROS_ERROR("Joints on incoming goal don't match our joints");
00974 gh.setRejected();
00975 return;
00976 }
00977
00978 preemptActiveGoal();
00979
00980 gh.setAccepted();
00981 boost::shared_ptr<RTGoalHandle> rt_gh(new RTGoalHandle(gh));
00982
00983
00984 goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandle::runNonRT, rt_gh);
00985 commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory), rt_gh);
00986 rt_active_goal_ = rt_gh;
00987 goal_handle_timer_.start();
00988 }
00989
00990 void R2JointTrajectoryActionController::goalCBFollow(GoalHandleFollow gh)
00991 {
00992 std::vector<std::string> joint_names(joints_.size());
00993 for (size_t j = 0; j < joints_.size(); ++j)
00994 joint_names[j] = joints_[j]->joint_->name;
00995
00996
00997 if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
00998 {
00999 ROS_ERROR("Joints on incoming goal don't match our joints");
01000 control_msgs::FollowJointTrajectoryResult result;
01001 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
01002 gh.setRejected(result);
01003 return;
01004 }
01005
01006 preemptActiveGoal();
01007
01008 gh.setAccepted();
01009 boost::shared_ptr<RTGoalHandleFollow> rt_gh(new RTGoalHandleFollow(gh));
01010
01011
01012 goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandleFollow::runNonRT, rt_gh);
01013 commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory),
01014 boost::shared_ptr<RTGoalHandle>((RTGoalHandle*)NULL),
01015 rt_gh);
01016 rt_active_goal_follow_ = rt_gh;
01017 goal_handle_timer_.start();
01018 }
01019
01020 void R2JointTrajectoryActionController::cancelCB(GoalHandle gh)
01021 {
01022 boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
01023 if (current_active_goal && current_active_goal->gh_ == gh)
01024 {
01025 rt_active_goal_.reset();
01026
01027 trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
01028 empty->joint_names.resize(joints_.size());
01029 for (size_t j = 0; j < joints_.size(); ++j)
01030 empty->joint_names[j] = joints_[j]->joint_->name;
01031 commandTrajectory(empty);
01032
01033
01034 current_active_goal->gh_.setCanceled();
01035 }
01036 }
01037
01038 void R2JointTrajectoryActionController::cancelCBFollow(GoalHandleFollow gh)
01039 {
01040 boost::shared_ptr<RTGoalHandleFollow> current_active_goal(rt_active_goal_follow_);
01041 if (current_active_goal && current_active_goal->gh_ == gh)
01042 {
01043 rt_active_goal_follow_.reset();
01044
01045 trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
01046 empty->joint_names.resize(joints_.size());
01047 for (size_t j = 0; j < joints_.size(); ++j)
01048 empty->joint_names[j] = joints_[j]->joint_->name;
01049 commandTrajectory(empty);
01050
01051
01052 current_active_goal->gh_.setCanceled();
01053 }
01054 }
01055 }