joint_spline_trajectory_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 JointSplineTrajectoryController::JointSplineTrajectoryController()
143  : loop_count_(0), robot_(NULL)
144 {
145 }
146 
148 {
151 }
152 
154 {
155  using namespace XmlRpc;
156  node_ = n;
157  robot_ = robot;
158 
159  // Gets all of the joints
160  XmlRpc::XmlRpcValue joint_names;
161  if (!node_.getParam("joints", joint_names))
162  {
163  ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
164  return false;
165  }
166  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
167  {
168  ROS_ERROR("Malformed joint specification. (namespace: %s)", node_.getNamespace().c_str());
169  return false;
170  }
171  for (int i = 0; i < joint_names.size(); ++i)
172  {
173  XmlRpcValue &name_value = joint_names[i];
174  if (name_value.getType() != XmlRpcValue::TypeString)
175  {
176  ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)",
177  node_.getNamespace().c_str());
178  return false;
179  }
180 
181  pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
182  if (!j) {
183  ROS_ERROR("Joint not found: %s. (namespace: %s)",
184  ((std::string)name_value).c_str(), node_.getNamespace().c_str());
185  return false;
186  }
187  joints_.push_back(j);
188  }
189 
190  // Ensures that all the joints are calibrated.
191  for (size_t i = 0; i < joints_.size(); ++i)
192  {
193  if (!joints_[i]->calibrated_)
194  {
195  ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
196  joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
197  return false;
198  }
199  }
200 
201  // Sets up pid controllers for all of the joints
202  std::string gains_ns;
203  if (!node_.getParam("gains", gains_ns))
204  gains_ns = node_.getNamespace() + "/gains";
205  pids_.resize(joints_.size());
206  for (size_t i = 0; i < joints_.size(); ++i)
207  if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
208  return false;
209 
210  // Creates a dummy trajectory
212  SpecifiedTrajectory &traj = *traj_ptr;
213  traj[0].start_time = robot_->getTime().toSec();
214  traj[0].duration = 0.0;
215  traj[0].splines.resize(joints_.size());
216  for (size_t j = 0; j < joints_.size(); ++j)
217  traj[0].splines[j].coef[0] = 0.0;
218  current_trajectory_box_.set(traj_ptr);
219 
223 
224  q.resize(joints_.size());
225  qd.resize(joints_.size());
226  qdd.resize(joints_.size());
227 
230  (node_, "state", 1));
232  for (size_t j = 0; j < joints_.size(); ++j)
233  controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
234  controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
235  controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
236  controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
237  controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
238  controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
239  controller_state_publisher_->msg_.error.positions.resize(joints_.size());
240  controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
241  controller_state_publisher_->unlock();
242 
243 
244  return true;
245 }
246 
248 {
250 
251  for (size_t i = 0; i < pids_.size(); ++i)
252  pids_[i].reset();
253 
254  // Creates a "hold current position" trajectory.
256  SpecifiedTrajectory &hold = *hold_ptr;
257  hold[0].start_time = last_time_.toSec() - 0.001;
258  hold[0].duration = 0.0;
259  hold[0].splines.resize(joints_.size());
260  for (size_t j = 0; j < joints_.size(); ++j)
261  hold[0].splines[j].coef[0] = joints_[j]->position_;
262 
263  current_trajectory_box_.set(hold_ptr);
264 }
265 
267 {
268  // Checks if all the joints are calibrated.
269 
270  ros::Time time = robot_->getTime();
271  ros::Duration dt = time - last_time_;
272  last_time_ = time;
273 
275  current_trajectory_box_.get(traj_ptr);
276  if (!traj_ptr)
277  ROS_FATAL("The current trajectory can never be null");
278 
279  // Only because this is what the code originally looked like.
280  const SpecifiedTrajectory &traj = *traj_ptr;
281 
282  // Determines which segment of the trajectory to use. (Not particularly realtime friendly).
283  int seg = -1;
284  while (seg + 1 < (int)traj.size() &&
285  traj[seg+1].start_time < time.toSec())
286  {
287  ++seg;
288  }
289 
290  if (seg == -1)
291  {
292  if (traj.size() == 0)
293  ROS_ERROR("No segments in the trajectory");
294  else
295  ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
296  return;
297  }
298 
299  // ------ Trajectory Sampling
300 
301  for (size_t i = 0; i < q.size(); ++i)
302  {
303  sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
304  time.toSec() - traj[seg].start_time,
305  q[i], qd[i], qdd[i]);
306  }
307 
308  // ------ Trajectory Following
309 
310  std::vector<double> error(joints_.size());
311  for (size_t i = 0; i < joints_.size(); ++i)
312  {
313  error[i] = q[i] - joints_[i]->position_;
314  joints_[i]->commanded_effort_ += pids_[i].computeCommand(error[i],
315  joints_[i]->velocity_ - qd[i], dt);
316  }
317 
318  // ------ State publishing
319 
320  if (loop_count_ % 10 == 0)
321  {
323  {
324  controller_state_publisher_->msg_.header.stamp = time;
325  for (size_t j = 0; j < joints_.size(); ++j)
326  {
327  controller_state_publisher_->msg_.desired.positions[j] = q[j];
328  controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
329  controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
330  controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
331  controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
332  controller_state_publisher_->msg_.error.positions[j] = error[j];
333  controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
334  }
335  controller_state_publisher_->unlockAndPublish();
336  }
337  }
338 
339  ++loop_count_;
340 }
341 
342 void JointSplineTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
343 {
344  ros::Time time = last_time_;
345  ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
346  time.toSec(), msg->header.stamp.toSec());
347 
349  SpecifiedTrajectory &new_traj = *new_traj_ptr;
350 
351  // ------ If requested, performs a stop
352 
353  if (msg->points.empty())
354  {
355  starting();
356  return;
357  }
358 
359  // ------ Correlates the joints we're commanding to the joints in the message
360 
361  std::vector<int> lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg
362  for (size_t j = 0; j < joints_.size(); ++j)
363  {
364  for (size_t k = 0; k < msg->joint_names.size(); ++k)
365  {
366  if (msg->joint_names[k] == joints_[j]->joint_->name)
367  {
368  lookup[j] = k;
369  break;
370  }
371  }
372 
373  if (lookup[j] == -1)
374  {
375  ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
376  return;
377  }
378  }
379 
380  // ------ Grabs the trajectory that we're currently following.
381 
383  current_trajectory_box_.get(prev_traj_ptr);
384  if (!prev_traj_ptr)
385  {
386  ROS_FATAL("The current trajectory can never be null");
387  return;
388  }
389  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
390 
391  // ------ Copies over the segments from the previous trajectory that are still useful.
392 
393  // Useful segments are still relevant after the current time.
394  int first_useful = -1;
395  while (first_useful + 1 < (int)prev_traj.size() &&
396  prev_traj[first_useful + 1].start_time <= time.toSec())
397  {
398  ++first_useful;
399  }
400 
401  // Useful segments are not going to be completely overwritten by the message's splines.
402  int last_useful = -1;
403  double msg_start_time;
404  if (msg->points.size() > 0)
405  msg_start_time = (msg->header.stamp + msg->points[0].time_from_start).toSec();
406  else
407  msg_start_time = std::max(time.toSec(), msg->header.stamp.toSec());
408 
409  while (last_useful + 1 < (int)prev_traj.size() &&
410  prev_traj[last_useful + 1].start_time < msg_start_time)
411  {
412  ++last_useful;
413  }
414 
415  if (last_useful < first_useful)
416  first_useful = last_useful;
417 
418  // Copies over the old segments that were determined to be useful.
419  for (int i = std::max(first_useful,0); i <= last_useful; ++i)
420  {
421  new_traj.push_back(prev_traj[i]);
422  }
423 
424  // We always save the last segment so that we know where to stop if
425  // there are no new segments.
426  if (new_traj.size() == 0)
427  new_traj.push_back(prev_traj[prev_traj.size() - 1]);
428 
429  // ------ Determines when and where the new segments start
430 
431  // Finds the end conditions of the final segment
432  Segment &last = new_traj[new_traj.size() - 1];
433  std::vector<double> prev_positions(joints_.size());
434  std::vector<double> prev_velocities(joints_.size());
435  std::vector<double> prev_accelerations(joints_.size());
436 
437  ROS_DEBUG("Initial conditions for new set of splines:");
438  for (size_t i = 0; i < joints_.size(); ++i)
439  {
440  sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
441  msg->header.stamp.toSec() - last.start_time,
442  prev_positions[i], prev_velocities[i], prev_accelerations[i]);
443  ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
444  prev_accelerations[i], joints_[i]->joint_->name.c_str());
445  }
446 
447  // ------ Tacks on the new segments
448 
449  std::vector<double> positions;
450  std::vector<double> velocities;
451  std::vector<double> accelerations;
452 
453  std::vector<double> durations(msg->points.size());
454  durations[0] = msg->points[0].time_from_start.toSec();
455  for (size_t i = 1; i < msg->points.size(); ++i)
456  durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
457 
458  // Checks if we should wrap
459  std::vector<double> wrap(joints_.size(), 0.0);
460  assert(!msg->points[0].positions.empty());
461  for (size_t j = 0; j < joints_.size(); ++j)
462  {
463  if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
464  {
465  double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]);
466  wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j];
467  }
468  }
469 
470  for (size_t i = 0; i < msg->points.size(); ++i)
471  {
472  Segment seg;
473 
474  seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
475  seg.duration = durations[i];
476  seg.splines.resize(joints_.size());
477 
478  // Checks that the incoming segment has the right number of elements.
479 
480  if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
481  {
482  ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
483  return;
484  }
485  if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
486  {
487  ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
488  return;
489  }
490  if (msg->points[i].positions.size() != joints_.size())
491  {
492  ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
493  return;
494  }
495 
496  // Re-orders the joints in the command to match the interal joint order.
497 
498  accelerations.resize(msg->points[i].accelerations.size());
499  velocities.resize(msg->points[i].velocities.size());
500  positions.resize(msg->points[i].positions.size());
501  for (size_t j = 0; j < joints_.size(); ++j)
502  {
503  if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
504  if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
505  if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
506  }
507 
508  // Converts the boundary conditions to splines.
509 
510  for (size_t j = 0; j < joints_.size(); ++j)
511  {
512  if (prev_accelerations.size() > 0 && accelerations.size() > 0)
513  {
515  prev_positions[j], prev_velocities[j], prev_accelerations[j],
516  positions[j], velocities[j], accelerations[j],
517  durations[i],
518  seg.splines[j].coef);
519  }
520  else if (prev_velocities.size() > 0 && velocities.size() > 0)
521  {
523  prev_positions[j], prev_velocities[j],
524  positions[j], velocities[j],
525  durations[i],
526  seg.splines[j].coef);
527  seg.splines[j].coef.resize(6, 0.0);
528  }
529  else
530  {
531  seg.splines[j].coef[0] = prev_positions[j];
532  if (durations[i] == 0.0)
533  seg.splines[j].coef[1] = 0.0;
534  else
535  seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
536  seg.splines[j].coef[2] = 0.0;
537  seg.splines[j].coef[3] = 0.0;
538  seg.splines[j].coef[4] = 0.0;
539  seg.splines[j].coef[5] = 0.0;
540  }
541  }
542 
543  // Pushes the splines onto the end of the new trajectory.
544 
545  new_traj.push_back(seg);
546 
547  // Computes the starting conditions for the next segment
548 
549  prev_positions = positions;
550  prev_velocities = velocities;
551  prev_accelerations = accelerations;
552  }
553 
554  // ------ Commits the new trajectory
555 
556  if (!new_traj_ptr)
557  {
558  ROS_ERROR("The new trajectory was null!");
559  return;
560  }
561 
562  current_trajectory_box_.set(new_traj_ptr);
563  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
564 #if 0
565  for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
566  {
567  ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
568  for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
569  {
570  ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)",
571  new_traj[i].splines[j].coef[0],
572  new_traj[i].splines[j].coef[1],
573  new_traj[i].splines[j].coef[2],
574  new_traj[i].splines[j].coef[3],
575  new_traj[i].splines[j].coef[4],
576  new_traj[i].splines[j].coef[5],
577  joints_[j]->joint_->name_.c_str());
578  }
579  }
580 #endif
581 }
582 
584  pr2_controllers_msgs::QueryTrajectoryState::Request &req,
585  pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
586 {
588  current_trajectory_box_.get(traj_ptr);
589  if (!traj_ptr)
590  {
591  ROS_FATAL("The current trajectory can never be null");
592  return false;
593  }
594  const SpecifiedTrajectory &traj = *traj_ptr;
595 
596  // Determines which segment of the trajectory to use
597  int seg = -1;
598  while (seg + 1 < (int)traj.size() &&
599  traj[seg+1].start_time < req.time.toSec())
600  {
601  ++seg;
602  }
603  if (seg == -1)
604  return false;
605 
606  for (size_t i = 0; i < q.size(); ++i)
607  {
608  }
609 
610 
611  resp.name.resize(joints_.size());
612  resp.position.resize(joints_.size());
613  resp.velocity.resize(joints_.size());
614  resp.acceleration.resize(joints_.size());
615  for (size_t j = 0; j < joints_.size(); ++j)
616  {
617  resp.name[j] = joints_[j]->joint_->name;
618  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
619  req.time.toSec() - traj[seg].start_time,
620  resp.position[j], resp.velocity[j], resp.acceleration[j]);
621  }
622 
623  return true;
624 }
625 
627  const std::vector<double>& coefficients, double duration, double time,
628  double& position, double& velocity, double& acceleration)
629 {
630  if (time < 0)
631  {
632  double _;
633  sampleQuinticSpline(coefficients, 0.0, position, _, _);
634  velocity = 0;
635  acceleration = 0;
636  }
637  else if (time > duration)
638  {
639  double _;
640  sampleQuinticSpline(coefficients, duration, position, _, _);
641  velocity = 0;
642  acceleration = 0;
643  }
644  else
645  {
646  sampleQuinticSpline(coefficients, time,
647  position, velocity, acceleration);
648  }
649 }
650 
651 }
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
#define ROS_FATAL(...)
static void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
std::vector< pr2_mechanism_model::JointState * > joints_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
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.
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
def error(args, kwargs)
static void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients)
static void generatePowers(int n, double x, double *powers)
const std::string & getNamespace() const
void set(const T &value)
bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
JointState * getJointState(const std::string &name)
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
bool getParam(const std::string &key, std::string &s) const
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
#define ROS_ERROR(...)
if((endptr==val_str)||(endptr< (val_str+strlen(val_str))))
def shortest_angular_distance(from_angle, to_angle)
#define ROS_DEBUG(...)


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