joint_trajectory_action_controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Stuart Glaser
32  */
33 
35 #include <sstream>
36 #include "angles/angles.h"
38 
40 
41 namespace controller {
42 
43 // These functions are pulled from the spline_smoother package.
44 // They've been moved here to avoid depending on packages that aren't
45 // mature yet.
46 
47 
48 static inline void generatePowers(int n, double x, double* powers)
49 {
50  powers[0] = 1.0;
51  for (int i=1; i<=n; i++)
52  {
53  powers[i] = powers[i-1]*x;
54  }
55 }
56 
57 static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
58  double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
59 {
60  coefficients.resize(6);
61 
62  if (time == 0.0)
63  {
64  coefficients[0] = end_pos;
65  coefficients[1] = end_vel;
66  coefficients[2] = 0.5*end_acc;
67  coefficients[3] = 0.0;
68  coefficients[4] = 0.0;
69  coefficients[5] = 0.0;
70  }
71  else
72  {
73  double T[6];
74  generatePowers(5, time, T);
75 
76  coefficients[0] = start_pos;
77  coefficients[1] = start_vel;
78  coefficients[2] = 0.5*start_acc;
79  coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
80  12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
81  coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
82  16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
83  coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
84  6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
85  }
86 }
87 
91 static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
92  double& position, double& velocity, double& acceleration)
93 {
94  // create powers of time:
95  double t[6];
96  generatePowers(5, time, t);
97 
98  position = t[0]*coefficients[0] +
99  t[1]*coefficients[1] +
100  t[2]*coefficients[2] +
101  t[3]*coefficients[3] +
102  t[4]*coefficients[4] +
103  t[5]*coefficients[5];
104 
105  velocity = t[0]*coefficients[1] +
106  2.0*t[1]*coefficients[2] +
107  3.0*t[2]*coefficients[3] +
108  4.0*t[3]*coefficients[4] +
109  5.0*t[4]*coefficients[5];
110 
111  acceleration = 2.0*t[0]*coefficients[2] +
112  6.0*t[1]*coefficients[3] +
113  12.0*t[2]*coefficients[4] +
114  20.0*t[3]*coefficients[5];
115 }
116 
117 static void getCubicSplineCoefficients(double start_pos, double start_vel,
118  double end_pos, double end_vel, double time, std::vector<double>& coefficients)
119 {
120  coefficients.resize(4);
121 
122  if (time == 0.0)
123  {
124  coefficients[0] = end_pos;
125  coefficients[1] = end_vel;
126  coefficients[2] = 0.0;
127  coefficients[3] = 0.0;
128  }
129  else
130  {
131  double T[4];
132  generatePowers(3, time, T);
133 
134  coefficients[0] = start_pos;
135  coefficients[1] = start_vel;
136  coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
137  coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
138  }
139 }
140 
141 
142 JointTrajectoryActionController::JointTrajectoryActionController()
143  : loop_count_(0), robot_(NULL)
144 {
145 }
146 
148 {
151  action_server_.reset();
152  action_server_follow_.reset();
153 }
154 
156 {
157  using namespace XmlRpc;
158  node_ = n;
159  robot_ = robot;
160 
161  // Gets all of the joints
162  XmlRpc::XmlRpcValue joint_names;
163  if (!node_.getParam("joints", joint_names))
164  {
165  ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
166  return false;
167  }
168  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
169  {
170  ROS_ERROR("Malformed joint specification. (namespace: %s)", node_.getNamespace().c_str());
171  return false;
172  }
173  for (int i = 0; i < joint_names.size(); ++i)
174  {
175  XmlRpcValue &name_value = joint_names[i];
176  if (name_value.getType() != XmlRpcValue::TypeString)
177  {
178  ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)",
179  node_.getNamespace().c_str());
180  return false;
181  }
182 
183  pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
184  if (!j) {
185  ROS_ERROR("Joint not found: %s. (namespace: %s)",
186  ((std::string)name_value).c_str(), node_.getNamespace().c_str());
187  return false;
188  }
189  joints_.push_back(j);
190  }
191 
192  // Ensures that all the joints are calibrated.
193  for (size_t i = 0; i < joints_.size(); ++i)
194  {
195  if (!joints_[i]->calibrated_)
196  {
197  ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
198  joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
199  return false;
200  }
201  }
202 
203  // Sets up pid controllers for all of the joints
204  std::string gains_ns;
205  if (!node_.getParam("gains", gains_ns))
206  gains_ns = node_.getNamespace() + "/gains";
207  masses_.resize(joints_.size());
208  pids_.resize(joints_.size());
209  for (size_t i = 0; i < joints_.size(); ++i)
210  {
211  ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
212  joint_nh.param("mass", masses_[i], 0.0);
213  if (!pids_[i].init(joint_nh))
214  return false;
215  }
216 
217  // Sets up the proxy controllers for each joint
218  proxies_enabled_.resize(joints_.size());
219  proxies_.resize(joints_.size());
220  for (size_t i = 0; i < joints_.size(); ++i)
221  {
222  ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
223  if (joint_nh.hasParam("proxy")) {
224  proxies_enabled_[i] = true;
225  joint_nh.param("proxy/lambda", proxies_[i].lambda_proxy_, 1.0);
226  joint_nh.param("proxy/acc_converge", proxies_[i].acc_converge_, 0.0);
227  joint_nh.param("proxy/vel_limit", proxies_[i].vel_limit_, 0.0);
228  joint_nh.param("proxy/effort_limit", proxies_[i].effort_limit_, 0.0);
229  proxies_[i].mass_ = masses_[i];
230  double _;
231  pids_[i].getGains(proxies_[i].Kp_, proxies_[i].Ki_, proxies_[i].Kd_, proxies_[i].Ficl_, _);
232  }
233  else {
234  proxies_enabled_[i] = false;
235  }
236  }
237 
238  // Trajectory and goal tolerances
239  default_trajectory_tolerance_.resize(joints_.size());
240  default_goal_tolerance_.resize(joints_.size());
241  node_.param("joint_trajectory_action_node/constraints/goal_time", default_goal_time_constraint_, 0.0);
242  double stopped_velocity_tolerance;
243  node_.param("joint_trajectory_action_node/constraints/stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
244  for (size_t i = 0; i < default_goal_tolerance_.size(); ++i)
245  default_goal_tolerance_[i].velocity = stopped_velocity_tolerance;
246  for (size_t i = 0; i < joints_.size(); ++i)
247  {
248  std::string ns = std::string("joint_trajectory_action_node/constraints") + joints_[i]->joint_->name;
249  node_.param(ns + "/goal", default_goal_tolerance_[i].position, 0.0);
250  node_.param(ns + "/trajectory", default_trajectory_tolerance_[i].position, 0.0);
251  }
252 
253  // Output filters
254  output_filters_.resize(joints_.size());
255  for (size_t i = 0; i < joints_.size(); ++i)
256  {
257  std::string p = "output_filters/" + joints_[i]->joint_->name;
258  if (node_.hasParam(p))
259  {
260  output_filters_[i].reset(new filters::FilterChain<double>("double"));
261  if (!output_filters_[i]->configure(node_.resolveName(p)))
262  output_filters_[i].reset();
263  }
264  }
265 
266  // Creates a dummy trajectory
268  SpecifiedTrajectory &traj = *traj_ptr;
269  traj[0].start_time = robot_->getTime().toSec();
270  traj[0].duration = 0.0;
271  traj[0].splines.resize(joints_.size());
272  for (size_t j = 0; j < joints_.size(); ++j)
273  traj[0].splines[j].coef[0] = 0.0;
274  current_trajectory_box_.set(traj_ptr);
275 
279 
280  q.resize(joints_.size());
281  qd.resize(joints_.size());
282  qdd.resize(joints_.size());
283 
286  (node_, "state", 1));
288  for (size_t j = 0; j < joints_.size(); ++j)
289  controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
290  controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
291  controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
292  controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
293  controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
294  controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
295  controller_state_publisher_->msg_.error.positions.resize(joints_.size());
296  controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
297  controller_state_publisher_->unlock();
298 
299  action_server_.reset(new JTAS(node_, "joint_trajectory_action",
300  boost::bind(&JointTrajectoryActionController::goalCB, this, _1),
301  boost::bind(&JointTrajectoryActionController::cancelCB, this, _1),
302  false));
303  action_server_follow_.reset(new FJTAS(node_, "follow_joint_trajectory",
304  boost::bind(&JointTrajectoryActionController::goalCBFollow, this, _1),
305  boost::bind(&JointTrajectoryActionController::cancelCBFollow, this, _1),
306  false));
307  action_server_->start();
308  action_server_follow_->start();
309 
310  return true;
311 }
312 
314 {
316 
317  for (size_t i = 0; i < pids_.size(); ++i) {
318  pids_[i].reset();
319  proxies_[i].reset(joints_[i]->position_, joints_[i]->velocity_);
320  }
321 
322  // Creates a "hold current position" trajectory.
324  SpecifiedTrajectory &hold = *hold_ptr;
325  hold[0].start_time = last_time_.toSec() - 0.001;
326  hold[0].duration = 0.0;
327  hold[0].splines.resize(joints_.size());
328  for (size_t j = 0; j < joints_.size(); ++j)
329  hold[0].splines[j].coef[0] = joints_[j]->position_;
330 
331  current_trajectory_box_.set(hold_ptr);
332 }
333 
335 {
336  ros::Time time = robot_->getTime();
337  ros::Duration dt = time - last_time_;
338  last_time_ = time;
339 
342 
344  current_trajectory_box_.get(traj_ptr);
345  if (!traj_ptr)
346  ROS_FATAL("The current trajectory can never be null");
347 
348  // Only because this is what the code originally looked like.
349  const SpecifiedTrajectory &traj = *traj_ptr;
350 
351  // ------ Finds the current segment
352 
353  // Determines which segment of the trajectory to use. (Not particularly realtime friendly).
354  int seg = -1;
355  while (seg + 1 < (int)traj.size() &&
356  traj[seg+1].start_time < time.toSec())
357  {
358  ++seg;
359  }
360 
361  if (seg == -1)
362  {
363  if (traj.size() == 0)
364  ROS_ERROR("No segments in the trajectory");
365  else
366  ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
367  return;
368  }
369 
370  // ------ Trajectory Sampling
371 
372  for (size_t i = 0; i < q.size(); ++i)
373  {
374  sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
375  time.toSec() - traj[seg].start_time,
376  q[i], qd[i], qdd[i]);
377  }
378 
379  // ------ Trajectory Following
380 
381  std::vector<double> error(joints_.size());
382  std::vector<double> v_error(joints_.size());
383  for (size_t i = 0; i < joints_.size(); ++i)
384  {
385  double effort;
386 
387  // Compute the errors with respect to the desired trajectory
388  // (whether or not using the proxy controller). They are also
389  // used later to determine reaching the goal.
390  error[i] = q[i] - joints_[i]->position_;
391  v_error[i] = qd[i] - joints_[i]->velocity_;
392 
393  // Use the proxy controller (if enabled)
394  if (proxies_enabled_[i]) {
395  effort = proxies_[i].update(q[i], qd[i], qdd[i],
396  joints_[i]->position_, joints_[i]->velocity_,
397  dt.toSec());
398  }
399  else {
400  effort = pids_[i].computeCommand(error[i], v_error[i], dt);
401 
402  double effort_unfiltered = effort;
403  if (output_filters_[i])
404  output_filters_[i]->update(effort_unfiltered, effort);
405  }
406 
407  // Apply the effort. WHY IS THIS ADDITIVE?
408  joints_[i]->commanded_effort_ = effort;
409  }
410 
411  // ------ Determines if the goal has failed or succeeded
412 
413  if ((traj[seg].gh && traj[seg].gh == current_active_goal) ||
414  (traj[seg].gh_follow && traj[seg].gh_follow == current_active_goal_follow))
415  {
416  ros::Time end_time(traj[seg].start_time + traj[seg].duration);
417  if (time <= end_time)
418  {
419  // Verifies trajectory tolerances
420  for (size_t j = 0; j < joints_.size(); ++j)
421  {
422  if (traj[seg].trajectory_tolerance[j].violated(error[j], v_error[j]))
423  {
424  if (traj[seg].gh)
425  traj[seg].gh->setAborted();
426  else if (traj[seg].gh_follow) {
427  traj[seg].gh_follow->preallocated_result_->error_code =
428  control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
429  traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
430  }
431  break;
432  }
433  }
434  }
435  else if (seg == (int)traj.size() - 1)
436  {
437  // Checks that we have ended inside the goal tolerances
438  bool inside_goal_constraints = true;
439  for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
440  {
441  if (traj[seg].goal_tolerance[i].violated(error[i], v_error[i]))
442  inside_goal_constraints = false;
443  }
444 
445  if (inside_goal_constraints)
446  {
447  rt_active_goal_.reset();
448  rt_active_goal_follow_.reset();
449  if (traj[seg].gh)
450  traj[seg].gh->setSucceeded();
451  else if (traj[seg].gh_follow) {
452  traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
453  traj[seg].gh_follow->setSucceeded(traj[seg].gh_follow->preallocated_result_);
454  }
455  //ROS_ERROR("Success! (%s)", traj[seg].gh->gh_.getGoalID().id.c_str());
456  }
457  else if (time < end_time + ros::Duration(traj[seg].goal_time_tolerance))
458  {
459  // Still have some time left to make it.
460  }
461  else
462  {
463  //ROS_WARN("Aborting because we wound up outside the goal constraints");
464  rt_active_goal_.reset();
465  rt_active_goal_follow_.reset();
466  if (traj[seg].gh)
467  traj[seg].gh->setAborted();
468  else if (traj[seg].gh_follow) {
469  traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
470  traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
471  }
472  }
473  }
474  }
475 
476  // ------ State publishing
477 
478  if (loop_count_ % 10 == 0)
479  {
481  {
482  controller_state_publisher_->msg_.header.stamp = time;
483  for (size_t j = 0; j < joints_.size(); ++j)
484  {
485  controller_state_publisher_->msg_.desired.positions[j] = q[j];
486  controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
487  controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
488  controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
489  controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
490  controller_state_publisher_->msg_.error.positions[j] = error[j];
491  controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
492  }
493  controller_state_publisher_->unlockAndPublish();
494  }
495  if (current_active_goal_follow)
496  {
497  current_active_goal_follow->preallocated_feedback_->header.stamp = time;
498  current_active_goal_follow->preallocated_feedback_->joint_names.resize(joints_.size());
499  current_active_goal_follow->preallocated_feedback_->desired.positions.resize(joints_.size());
500  current_active_goal_follow->preallocated_feedback_->desired.velocities.resize(joints_.size());
501  current_active_goal_follow->preallocated_feedback_->desired.accelerations.resize(joints_.size());
502  current_active_goal_follow->preallocated_feedback_->actual.positions.resize(joints_.size());
503  current_active_goal_follow->preallocated_feedback_->actual.velocities.resize(joints_.size());
504  current_active_goal_follow->preallocated_feedback_->error.positions.resize(joints_.size());
505  current_active_goal_follow->preallocated_feedback_->error.velocities.resize(joints_.size());
506  for (size_t j = 0; j < joints_.size(); ++j)
507  {
508  current_active_goal_follow->preallocated_feedback_->joint_names[j] = joints_[j]->joint_->name;
509  current_active_goal_follow->preallocated_feedback_->desired.positions[j] = q[j];
510  current_active_goal_follow->preallocated_feedback_->desired.velocities[j] = qd[j];
511  current_active_goal_follow->preallocated_feedback_->desired.accelerations[j] = qdd[j];
512  current_active_goal_follow->preallocated_feedback_->actual.positions[j] = joints_[j]->position_;
513  current_active_goal_follow->preallocated_feedback_->actual.velocities[j] = joints_[j]->velocity_;
514  current_active_goal_follow->preallocated_feedback_->error.positions[j] = error[j];
515  current_active_goal_follow->preallocated_feedback_->error.velocities[j] = joints_[j]->velocity_ - qd[j];
516  }
517  const actionlib_msgs::GoalID goalID = current_active_goal_follow->gh_.getGoalID();
518  ros::Duration time_from_start = time - goalID.stamp;
519  current_active_goal_follow->preallocated_feedback_->desired.time_from_start = time_from_start;
520  current_active_goal_follow->preallocated_feedback_->actual.time_from_start = time_from_start;
521  current_active_goal_follow->preallocated_feedback_->error.time_from_start = time_from_start;
522  current_active_goal_follow->gh_.publishFeedback(*current_active_goal_follow->preallocated_feedback_);
523 
524  }
525  }
526 
527  ++loop_count_;
528 }
529 
530 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
531 {
533  commandTrajectory(msg);
534 }
535 
536 void JointTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg,
539 {
540  assert(!gh || !gh_follow);
541  ros::Time time = last_time_ + ros::Duration(0.01);
542  ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
543  time.toSec(), msg->header.stamp.toSec());
544 
546  SpecifiedTrajectory &new_traj = *new_traj_ptr;
547 
548  // ------ If requested, performs a stop
549 
550  if (msg->points.empty())
551  {
552  starting();
553  return;
554  }
555 
556  // ------ Computes the tolerances for following the trajectory
557 
558  std::vector<JointTolerance> trajectory_tolerance = default_trajectory_tolerance_;
559  std::vector<JointTolerance> goal_tolerance = default_goal_tolerance_;
560  double goal_time_tolerance = default_goal_time_constraint_;
561 
562  if (gh_follow)
563  {
564  for (size_t j = 0; j < joints_.size(); ++j)
565  {
566  // Extracts the path tolerance from the command
567  for (size_t k = 0; k < gh_follow->gh_.getGoal()->path_tolerance.size(); ++k)
568  {
569  const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->path_tolerance[k];
570  if (joints_[j]->joint_->name == tol.name)
571  {
572  // If the commanded tolerances are positive, overwrite the
573  // existing tolerances. If they are -1, remove any existing
574  // tolerance. If they are 0, leave the default tolerance in
575  // place.
576 
577  if (tol.position > 0)
578  trajectory_tolerance[j].position = tol.position;
579  else if (tol.position < 0)
580  trajectory_tolerance[j].position = 0.0;
581 
582  if (tol.velocity > 0)
583  trajectory_tolerance[j].velocity = tol.velocity;
584  else if (tol.velocity < 0)
585  trajectory_tolerance[j].velocity = 0.0;
586 
587  if (tol.acceleration > 0)
588  trajectory_tolerance[j].acceleration = tol.acceleration;
589  else if (tol.acceleration < 0)
590  trajectory_tolerance[j].acceleration = 0.0;
591 
592  break;
593  }
594  }
595 
596  // Extracts the goal tolerance from the command
597  for (size_t k = 0; k < gh_follow->gh_.getGoal()->goal_tolerance.size(); ++k)
598  {
599  const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->goal_tolerance[k];
600  if (joints_[j]->joint_->name == tol.name)
601  {
602  // If the commanded tolerances are positive, overwrite the
603  // existing tolerances. If they are -1, remove any existing
604  // tolerance. If they are 0, leave the default tolerance in
605  // place.
606 
607  if (tol.position > 0)
608  goal_tolerance[j].position = tol.position;
609  else if (tol.position < 0)
610  goal_tolerance[j].position = 0.0;
611 
612  if (tol.velocity > 0)
613  goal_tolerance[j].velocity = tol.velocity;
614  else if (tol.velocity < 0)
615  goal_tolerance[j].velocity = 0.0;
616 
617  if (tol.acceleration > 0)
618  goal_tolerance[j].acceleration = tol.acceleration;
619  else if (tol.acceleration < 0)
620  goal_tolerance[j].acceleration = 0.0;
621 
622  break;
623  }
624  }
625  }
626 
627  const ros::Duration &tol = gh_follow->gh_.getGoal()->goal_time_tolerance;
628  if (tol < ros::Duration(0.0))
629  goal_time_tolerance = 0.0;
630  else if (tol > ros::Duration(0.0))
631  goal_time_tolerance = tol.toSec();
632  }
633 
634  // ------ Correlates the joints we're commanding to the joints in the message
635 
636  std::vector<int> lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg
637  for (size_t j = 0; j < joints_.size(); ++j)
638  {
639  for (size_t k = 0; k < msg->joint_names.size(); ++k)
640  {
641  if (msg->joint_names[k] == joints_[j]->joint_->name)
642  {
643  lookup[j] = k;
644  break;
645  }
646  }
647 
648  if (lookup[j] == -1)
649  {
650  ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
651  if (gh)
652  gh->setAborted();
653  else if (gh_follow) {
654  gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
655  gh_follow->setAborted(gh_follow->preallocated_result_);
656  }
657  return;
658  }
659  }
660 
661  // ------ Grabs the trajectory that we're currently following.
662 
664  current_trajectory_box_.get(prev_traj_ptr);
665  if (!prev_traj_ptr)
666  {
667  ROS_FATAL("The current trajectory can never be null");
668  return;
669  }
670  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
671 
672  // ------ Copies over the segments from the previous trajectory that are still useful.
673 
674  // Useful segments are still relevant after the current time.
675  int first_useful = -1;
676  while (first_useful + 1 < (int)prev_traj.size() &&
677  prev_traj[first_useful + 1].start_time <= time.toSec())
678  {
679  ++first_useful;
680  }
681 
682  // Useful segments are not going to be completely overwritten by the message's splines.
683  int last_useful = -1;
684  double msg_start_time;
685  if (msg->header.stamp == ros::Time(0.0))
686  msg_start_time = time.toSec();
687  else
688  msg_start_time = msg->header.stamp.toSec();
689  /*
690  if (msg->points.size() > 0)
691  msg_start_time += msg->points[0].time_from_start.toSec();
692  */
693 
694  while (last_useful + 1 < (int)prev_traj.size() &&
695  prev_traj[last_useful + 1].start_time < msg_start_time)
696  {
697  ++last_useful;
698  }
699 
700  if (last_useful < first_useful)
701  first_useful = last_useful;
702 
703  // Copies over the old segments that were determined to be useful.
704  for (int i = std::max(first_useful,0); i <= last_useful; ++i)
705  {
706  new_traj.push_back(prev_traj[i]);
707  }
708 
709  // We always save the last segment so that we know where to stop if
710  // there are no new segments.
711  if (new_traj.size() == 0)
712  new_traj.push_back(prev_traj[prev_traj.size() - 1]);
713 
714  // ------ Determines when and where the new segments start
715 
716  // Finds the end conditions of the final segment
717  Segment &last = new_traj[new_traj.size() - 1];
718  std::vector<double> prev_positions(joints_.size());
719  std::vector<double> prev_velocities(joints_.size());
720  std::vector<double> prev_accelerations(joints_.size());
721 
722  double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec())
723  - last.start_time;
724  ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t);
725  for (size_t i = 0; i < joints_.size(); ++i)
726  {
727  sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
728  t,
729  prev_positions[i], prev_velocities[i], prev_accelerations[i]);
730  ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
731  prev_accelerations[i], joints_[i]->joint_->name.c_str());
732  }
733 
734  // ------ Tacks on the new segments
735 
736  std::vector<double> positions;
737  std::vector<double> velocities;
738  std::vector<double> accelerations;
739 
740  std::vector<double> durations(msg->points.size());
741  if (msg->points.size() > 0)
742  durations[0] = msg->points[0].time_from_start.toSec();
743  for (size_t i = 1; i < msg->points.size(); ++i)
744  durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
745 
746  // Checks if we should wrap
747  std::vector<double> wrap(joints_.size(), 0.0);
748  assert(!msg->points[0].positions.empty());
749  for (size_t j = 0; j < joints_.size(); ++j)
750  {
751  if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
752  {
753  double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[lookup[j]]);
754  wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[lookup[j]];
755  }
756  }
757 
758  for (size_t i = 0; i < msg->points.size(); ++i)
759  {
760  Segment seg;
761 
762  if (msg->header.stamp == ros::Time(0.0))
763  seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
764  else
765  seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
766  seg.duration = durations[i];
767  seg.gh = gh;
768  seg.gh_follow = gh_follow;
769  seg.splines.resize(joints_.size());
770 
771  // Checks that the incoming segment has the right number of elements.
772 
773  if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
774  {
775  ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
776  return;
777  }
778  if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
779  {
780  ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
781  return;
782  }
783  if (msg->points[i].positions.size() != joints_.size())
784  {
785  ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
786  return;
787  }
788 
789  // Re-orders the joints in the command to match the internal joint order.
790 
791  accelerations.resize(msg->points[i].accelerations.size());
792  velocities.resize(msg->points[i].velocities.size());
793  positions.resize(msg->points[i].positions.size());
794  for (size_t j = 0; j < joints_.size(); ++j)
795  {
796  if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
797  if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
798  if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
799  }
800 
801  // Converts the boundary conditions to splines.
802 
803  for (size_t j = 0; j < joints_.size(); ++j)
804  {
805  if (prev_accelerations.size() > 0 && accelerations.size() > 0)
806  {
808  prev_positions[j], prev_velocities[j], prev_accelerations[j],
809  positions[j], velocities[j], accelerations[j],
810  durations[i],
811  seg.splines[j].coef);
812  }
813  else if (prev_velocities.size() > 0 && velocities.size() > 0)
814  {
816  prev_positions[j], prev_velocities[j],
817  positions[j], velocities[j],
818  durations[i],
819  seg.splines[j].coef);
820  seg.splines[j].coef.resize(6, 0.0);
821  }
822  else
823  {
824  seg.splines[j].coef[0] = prev_positions[j];
825  if (durations[i] == 0.0)
826  seg.splines[j].coef[1] = 0.0;
827  else
828  seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
829  seg.splines[j].coef[2] = 0.0;
830  seg.splines[j].coef[3] = 0.0;
831  seg.splines[j].coef[4] = 0.0;
832  seg.splines[j].coef[5] = 0.0;
833  }
834  }
835 
836  // Writes the tolerances into the segment
837  seg.trajectory_tolerance = trajectory_tolerance;
838  seg.goal_tolerance = goal_tolerance;
839  seg.goal_time_tolerance = goal_time_tolerance;
840 
841  // Pushes the splines onto the end of the new trajectory.
842 
843  new_traj.push_back(seg);
844 
845  // Computes the starting conditions for the next segment
846 
847  prev_positions = positions;
848  prev_velocities = velocities;
849  prev_accelerations = accelerations;
850  }
851 
852  //ROS_ERROR("Last segment goal id: %s", new_traj[new_traj.size()-1].gh->gh_.getGoalID().id.c_str());
853 
854  // ------ Commits the new trajectory
855 
856  if (!new_traj_ptr)
857  {
858  ROS_ERROR("The new trajectory was null!");
859  return;
860  }
861 
862  current_trajectory_box_.set(new_traj_ptr);
863  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
864 #if 0
865  for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
866  {
867  ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
868  for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
869  {
870  ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)",
871  new_traj[i].splines[j].coef[0],
872  new_traj[i].splines[j].coef[1],
873  new_traj[i].splines[j].coef[2],
874  new_traj[i].splines[j].coef[3],
875  new_traj[i].splines[j].coef[4],
876  new_traj[i].splines[j].coef[5],
877  joints_[j]->joint_->name_.c_str());
878  }
879  }
880 #endif
881 }
882 
884  pr2_controllers_msgs::QueryTrajectoryState::Request &req,
885  pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
886 {
888  current_trajectory_box_.get(traj_ptr);
889  if (!traj_ptr)
890  {
891  ROS_FATAL("The current trajectory can never be null");
892  return false;
893  }
894  const SpecifiedTrajectory &traj = *traj_ptr;
895 
896  // Determines which segment of the trajectory to use
897  int seg = -1;
898  while (seg + 1 < (int)traj.size() &&
899  traj[seg+1].start_time < req.time.toSec())
900  {
901  ++seg;
902  }
903  if (seg == -1)
904  return false;
905 
906  for (size_t i = 0; i < q.size(); ++i)
907  {
908  }
909 
910 
911  resp.name.resize(joints_.size());
912  resp.position.resize(joints_.size());
913  resp.velocity.resize(joints_.size());
914  resp.acceleration.resize(joints_.size());
915  for (size_t j = 0; j < joints_.size(); ++j)
916  {
917  resp.name[j] = joints_[j]->joint_->name;
918  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
919  req.time.toSec() - traj[seg].start_time,
920  resp.position[j], resp.velocity[j], resp.acceleration[j]);
921  }
922 
923  return true;
924 }
925 
927  const std::vector<double>& coefficients, double duration, double time,
928  double& position, double& velocity, double& acceleration)
929 {
930  if (time < 0)
931  {
932  double _;
933  sampleQuinticSpline(coefficients, 0.0, position, _, _);
934  velocity = 0;
935  acceleration = 0;
936  }
937  else if (time > duration)
938  {
939  double _;
940  sampleQuinticSpline(coefficients, duration, position, _, _);
941  velocity = 0;
942  acceleration = 0;
943  }
944  else
945  {
946  sampleQuinticSpline(coefficients, time,
947  position, velocity, acceleration);
948  }
949 }
950 
951 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
952 {
953  if (a.size() != b.size())
954  return false;
955 
956  for (size_t i = 0; i < a.size(); ++i)
957  {
958  if (count(b.begin(), b.end(), a[i]) != 1)
959  return false;
960  }
961  for (size_t i = 0; i < b.size(); ++i)
962  {
963  if (count(a.begin(), a.end(), b[i]) != 1)
964  return false;
965  }
966 
967  return true;
968 }
969 
971 {
974 
975  // Cancels the currently active goal.
976  if (current_active_goal)
977  {
978  // Marks the current goal as canceled.
979  rt_active_goal_.reset();
980  current_active_goal->gh_.setCanceled();
981  }
982  if (current_active_goal_follow)
983  {
984  rt_active_goal_follow_.reset();
985  current_active_goal_follow->gh_.setCanceled();
986  }
987 }
988 
989 
990 template <class Enclosure, class Member>
992 {
994  boost::shared_ptr<Member> p(&member, d);
995  return p;
996 }
997 
999 {
1000  std::vector<std::string> joint_names(joints_.size());
1001  for (size_t j = 0; j < joints_.size(); ++j)
1002  joint_names[j] = joints_[j]->joint_->name;
1003 
1004  // Ensures that the joints in the goal match the joints we are commanding.
1005  if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
1006  {
1007  ROS_ERROR("Joints on incoming goal don't match our joints");
1008  gh.setRejected();
1009  return;
1010  }
1011 
1013 
1014  gh.setAccepted();
1016 
1017  // Sends the trajectory along to the controller
1019  commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory), rt_gh);
1020  rt_active_goal_ = rt_gh;
1022 }
1023 
1025 {
1026  std::vector<std::string> joint_names(joints_.size());
1027  for (size_t j = 0; j < joints_.size(); ++j)
1028  joint_names[j] = joints_[j]->joint_->name;
1029 
1030  // Ensures that the joints in the goal match the joints we are commanding.
1031  if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
1032  {
1033  ROS_ERROR("Joints on incoming goal don't match our joints");
1034  control_msgs::FollowJointTrajectoryResult result;
1035  result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
1036  gh.setRejected(result);
1037  return;
1038  }
1039 
1041 
1042  gh.setAccepted();
1044 
1045  // Sends the trajectory along to the controller
1047  commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory),
1049  rt_gh);
1050  rt_active_goal_follow_ = rt_gh;
1052 }
1053 
1055 {
1056  boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
1057  if (current_active_goal && current_active_goal->gh_ == gh)
1058  {
1059  rt_active_goal_.reset();
1060 
1061  trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
1062  empty->joint_names.resize(joints_.size());
1063  for (size_t j = 0; j < joints_.size(); ++j)
1064  empty->joint_names[j] = joints_[j]->joint_->name;
1065  commandTrajectory(empty);
1066 
1067  // Marks the current goal as canceled.
1068  current_active_goal->gh_.setCanceled();
1069  }
1070 }
1071 
1073 {
1075  if (current_active_goal && current_active_goal->gh_ == gh)
1076  {
1077  rt_active_goal_follow_.reset();
1078 
1079  trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
1080  empty->joint_names.resize(joints_.size());
1081  for (size_t j = 0; j < joints_.size(); ++j)
1082  empty->joint_names[j] = joints_[j]->joint_->name;
1083  commandTrajectory(empty);
1084 
1085  // Marks the current goal as canceled.
1086  current_active_goal->gh_.setCanceled();
1087  }
1088 }
1089 }
d
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
RTServerGoalHandle< pr2_controllers_msgs::JointTrajectoryAction > RTGoalHandle
#define ROS_FATAL(...)
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
static void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
static bool setsEqual(const std::vector< std::string > &a, const std::vector< std::string > &b)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
int size() const
std::string resolveName(const std::string &name, bool remap=true) const
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
static void sampleQuinticSpline(const std::vector< double > &coefficients, double time, double &position, double &velocity, double &acceleration)
Samples a quintic spline segment at a particular time.
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
void start()
static boost::shared_ptr< Member > share_member(boost::shared_ptr< Enclosure > enclosure, Member &member)
boost::shared_ptr< const Goal > getGoal() const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc, double end_pos, double end_vel, double end_acc, double time, std::vector< double > &coefficients)
Type const & getType() const
void setAccepted(const std::string &text=std::string(""))
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
def error(args, kwargs)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
static void generatePowers(int n, double x, double *powers)
const std::string & getNamespace() const
void set(const T &value)
JointState * getJointState(const std::string &name)
boost::shared_ptr< RTGoalHandleFollow > rt_active_goal_follow_
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > FJTAS
actionlib::ActionServer< pr2_controllers_msgs::JointTrajectoryAction > JTAS
bool getParam(const std::string &key, std::string &s) const
RTServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RTGoalHandleFollow
void commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &traj, boost::shared_ptr< RTGoalHandle > gh=boost::shared_ptr< RTGoalHandle >((RTGoalHandle *) NULL), boost::shared_ptr< RTGoalHandleFollow > gh_follow=boost::shared_ptr< RTGoalHandleFollow >((RTGoalHandleFollow *) NULL))
std::vector< control_toolbox::LimitedProxy > proxies_
std::vector< boost::shared_ptr< filters::FilterChain< double > > > output_filters_
bool hasParam(const std::string &key) const
bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
Definition: effort.py:1
#define ROS_ERROR(...)
if((endptr==val_str)||(endptr< (val_str+strlen(val_str))))
def shortest_angular_distance(from_angle, to_angle)
std::vector< pr2_mechanism_model::JointState * > joints_
#define ROS_DEBUG(...)


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Wed Jun 5 2019 19:34:03