joint_trajectory_action_controller.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2010 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * joint_trajectory_action_controller.cpp
20  *
21  * Created on: 07.12.2010
22  * Author: Martin Günther <mguenthe@uos.de>
23  *
24  * based on joint_trajectory_action_controller.cpp by Stuart Glaser,
25  * from the package robot_mechanism_controllers
26  */
27 
29 #include <fstream>
30 #include <iostream>
31 #include <cstdio>
32 
33 namespace katana
34 {
35 
37  katana_(katana), action_server_(ros::NodeHandle(), "katana_arm_controller/joint_trajectory_action",
38  boost::bind(&JointTrajectoryActionController::executeCB, this, _1), false),
39  action_server_follow_(ros::NodeHandle(), "katana_arm_controller/follow_joint_trajectory",
40  boost::bind(&JointTrajectoryActionController::executeCBFollow, this, _1), false)
41 {
42  ros::NodeHandle node_;
43 
44  joints_ = katana_->getJointNames();
45 
46 
47  // Trajectory and goal constraints
48  // node_.param("joint_trajectory_action_node/constraints/goal_time", goal_time_constraint_, 0.0);
49  node_.param("joint_trajectory_action_node/constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 1e-6);
50  goal_constraints_.resize(joints_.size());
51  // trajectory_constraints_.resize(joints_.size());
52  for (size_t i = 0; i < joints_.size(); ++i)
53  {
54  std::string ns = std::string("joint_trajectory_action_node/constraints") + joints_[i];
55  node_.param(ns + "/goal", goal_constraints_[i], 0.02);
56  // node_.param(ns + "/trajectory", trajectory_constraints_[i], -1.0);
57  }
58 
59  // Subscriptions, publishers, services
62  sub_command_ = node_.subscribe("katana_arm_controller/command", 1, &JointTrajectoryActionController::commandCB, this);
63  serve_query_state_ = node_.advertiseService("katana_arm_controller/query_state", &JointTrajectoryActionController::queryStateService, this);
64  controller_state_publisher_ = node_.advertise<control_msgs::JointTrajectoryControllerState> ("katana_arm_controller/state", 1);
65 
66  // NOTE: current_trajectory_ is not initialized here, because that will happen in reset_trajectory_and_stop()
67 
69 }
70 
72 {
75 }
76 
81 {
82  katana_->freezeRobot();
83 
84  ros::Time time = ros::Time::now();
85 
86  // Creates a "hold current position" trajectory.
87  // It's important that this trajectory is always there, because it will be used as a starting point for any new trajectory.
89  SpecifiedTrajectory &hold = *hold_ptr;
90  hold[0].start_time = time.toSec() - 0.001;
91  hold[0].duration = 0.0;
92  hold[0].splines.resize(joints_.size());
93  for (size_t j = 0; j < joints_.size(); ++j)
94  hold[0].splines[j].coef[0] = katana_->getMotorAngles()[j];
95 
96  current_trajectory_ = hold_ptr;
97 }
98 
100 {
101  ros::Time time = ros::Time::now();
102 
103  std::vector<double> q(joints_.size()), qd(joints_.size()), qdd(joints_.size());
104 
106  if (!traj_ptr)
107  ROS_FATAL("The current trajectory can never be null");
108 
109  // Only because this is what the code originally looked like.
110  const SpecifiedTrajectory &traj = *traj_ptr;
111 
112  if (traj.size() == 0)
113  {
114  ROS_ERROR("No segments in the trajectory");
115  return;
116  }
117 
118  // ------ Finds the current segment
119 
120  // Determines which segment of the trajectory to use.
121  int seg = -1;
122  while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= time.toSec())
123  {
124  ++seg;
125  }
126 
127  if (seg == -1)
128  {
129  // ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
130  // return;
131  seg = 0;
132  }
133 
134  // ------ Trajectory Sampling
135 
136  for (size_t i = 0; i < q.size(); ++i)
137  {
138  sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, time.toSec() - traj[seg].start_time,
139  q[i], qd[i], qdd[i]);
140  }
141 
142  // ------ Calculate error
143 
144  std::vector<double> error(joints_.size());
145  for (size_t i = 0; i < joints_.size(); ++i)
146  {
147  error[i] = katana_->getMotorAngles()[i] - q[i];
148  }
149 
150  // ------ State publishing
151 
152  control_msgs::JointTrajectoryControllerState msg;
153 
154  // Message containing current state for all controlled joints
155  for (size_t j = 0; j < joints_.size(); ++j)
156  msg.joint_names.push_back(joints_[j]);
157  msg.desired.positions.resize(joints_.size());
158  msg.desired.velocities.resize(joints_.size());
159  msg.desired.accelerations.resize(joints_.size());
160  msg.actual.positions.resize(joints_.size());
161  msg.actual.velocities.resize(joints_.size());
162  // ignoring accelerations
163  msg.error.positions.resize(joints_.size());
164  msg.error.velocities.resize(joints_.size());
165  // ignoring accelerations
166 
167  msg.header.stamp = time;
168  for (size_t j = 0; j < joints_.size(); ++j)
169  {
170  msg.desired.positions[j] = q[j];
171  msg.desired.velocities[j] = qd[j];
172  msg.desired.accelerations[j] = qdd[j];
173  msg.actual.positions[j] = katana_->getMotorAngles()[j];
174  msg.actual.velocities[j] = katana_->getMotorVelocities()[j];
175  // ignoring accelerations
176  msg.error.positions[j] = error[j];
177  msg.error.velocities[j] = katana_->getMotorVelocities()[j] - qd[j];
178  // ignoring accelerations
179  }
180 
182  // TODO: here we could publish feedback for the FollowJointTrajectory action; however,
183  // this seems to be optional (the PR2's joint_trajectory_action_controller doesn't do it either)
184 }
185 
191 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
192 {
193  ROS_WARN("commandCB() called, this is not tested yet");
194  // just creates an action from the message and sends it on to the action server
195 
196  // create an action client spinning its own thread
197  JTAC action_client("katana_arm_controller/joint_trajectory_action", true);
198  action_client.waitForServer();
199 
200  JTAS::Goal goal;
201  goal.trajectory = *(msg.get());
202 
203  // fire and forget
204  action_client.sendGoal(goal);
205 }
206 
208  const trajectory_msgs::JointTrajectory &msg)
209 {
211 
212  bool allPointsHaveVelocities = true;
213 
214  // ------ Checks that the incoming segments have the right number of elements,
215  // determines which spline algorithm to use
216  for (size_t i = 0; i < msg.points.size(); i++)
217  {
218  if (msg.points[i].accelerations.size() != 0 && msg.points[i].accelerations.size() != joints_.size())
219  {
220  ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg.points[i].accelerations.size());
221  return new_traj_ptr; // return null pointer to signal error
222  }
223  if (msg.points[i].velocities.size() == 0)
224  {
225  // getCubicSplineCoefficients only works when the desired velocities are already given.
226  allPointsHaveVelocities = false;
227  }
228  else if (msg.points[i].velocities.size() != joints_.size())
229  {
230  ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg.points[i].velocities.size());
231  return new_traj_ptr; // return null pointer to signal error
232  }
233  if (msg.points[i].positions.size() != joints_.size())
234  {
235  ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg.points[i].positions.size());
236  return new_traj_ptr; // return null pointer to signal error
237  }
238  }
239 
240  // ------ Correlates the joints we're commanding to the joints in the message
241  std::vector<int> lookup = makeJointsLookup(msg);
242  if (lookup.size() == 0)
243  return new_traj_ptr; // return null pointer to signal error
244 
245 
246  // ------ convert the boundary conditions to splines
247  new_traj_ptr.reset(new SpecifiedTrajectory);
248  SpecifiedTrajectory &new_traj = *new_traj_ptr;
249 
250  size_t steps = msg.points.size() - 1;
251 
252  ROS_DEBUG("steps: %zu", steps);
253  assert(steps > 0); // this is checked before
254 
255  for (size_t i = 0; i < steps; i++)
256  {
257  Segment seg;
258  seg.splines.resize(joints_.size());
259  new_traj.push_back(seg);
260  }
261 
262  for (size_t j = 0; j < joints_.size(); j++)
263  {
264  double times[steps + 1], positions[steps + 1], velocities[steps + 1], durations[steps], coeff0[steps],
265  coeff1[steps], coeff2[steps], coeff3[steps];
266 
267  for (size_t i = 0; i < steps + 1; i++)
268  {
269  times[i] = msg.header.stamp.toSec() + msg.points[i].time_from_start.toSec();
270  positions[i] = msg.points[i].positions[lookup[j]];
271  if (allPointsHaveVelocities)
272  velocities[i] = msg.points[i].velocities[lookup[j]];
273  ROS_DEBUG("position %zu for joint %zu in message (= our joint %d): %f", i, j, lookup[j], positions[i]);
274  }
275 
276  for (size_t i = 0; i < steps; i++)
277  {
278  durations[i] = times[i + 1] - times[i];
279  }
280 
281  // calculate and store the coefficients
282  if (allPointsHaveVelocities)
283  {
284  ROS_DEBUG("Using getCubicSplineCoefficients()");
285  for (size_t i = 0; i < steps; ++i)
286  {
287  std::vector<double> coeff;
288  getCubicSplineCoefficients(positions[i], velocities[i], positions[i + 1], velocities[i + 1], durations[i],
289  coeff);
290  coeff0[i] = coeff[0];
291  coeff1[i] = coeff[1];
292  coeff2[i] = coeff[2];
293  coeff3[i] = coeff[3];
294  }
295  }
296  else
297  {
298  ROS_DEBUG("Using splineCoefficients()");
299  splineCoefficients(steps, times, positions, coeff0, coeff1, coeff2, coeff3);
300  }
301 
302  for (size_t i = 0; i < steps; ++i)
303  {
304  new_traj[i].duration = durations[i];
305  new_traj[i].start_time = times[i];
306  new_traj[i].splines[j].target_position = positions[i + 1];
307  new_traj[i].splines[j].coef[0] = coeff0[i];
308  new_traj[i].splines[j].coef[1] = coeff1[i];
309  new_traj[i].splines[j].coef[2] = coeff2[i];
310  new_traj[i].splines[j].coef[3] = coeff3[i];
311  }
312  }
313 
314  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
315  for (size_t i = 0; i < std::min((size_t)20, new_traj.size()); i++)
316  {
317  ROS_DEBUG("Segment %2zu - start_time: %.3lf duration: %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
318  for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
319  {
320  ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf (%s)",
321  new_traj[i].splines[j].coef[0],
322  new_traj[i].splines[j].coef[1],
323  new_traj[i].splines[j].coef[2],
324  new_traj[i].splines[j].coef[3],
325  joints_[j].c_str());
326  }
327  }
328 
329  // -------- sample trajectory and write to file
330  for (size_t j = 0; j < NUM_JOINTS; j++)
331  {
332  char filename[25];
333  sprintf(filename, "/tmp/trajectory-%zu.dat", j);
334  std::ofstream trajectory_file(filename, std::ios_base::out);
335  trajectory_file.precision(8);
336  for (double t = new_traj[0].start_time; t < new_traj.back().start_time + new_traj.back().duration; t += 0.01)
337  {
338  // Determines which segment of the trajectory to use
339  int seg = -1;
340  while (seg + 1 < (int)new_traj.size() && new_traj[seg + 1].start_time <= t)
341  {
342  ++seg;
343  }
344 
345  assert(seg >= 0);
346 
347  double pos_t, vel_t, acc_t;
348  sampleSplineWithTimeBounds(new_traj[seg].splines[j].coef, new_traj[seg].duration, t - new_traj[seg].start_time,
349  pos_t, vel_t, acc_t);
350 
351  trajectory_file << t - new_traj[0].start_time << " " << pos_t << " " << vel_t << " " << acc_t << std::endl;
352  }
353 
354  trajectory_file.close();
355  }
356 
357  return new_traj_ptr;
358 }
359 
363 bool JointTrajectoryActionController::queryStateService(control_msgs::QueryTrajectoryState::Request &req,
364  control_msgs::QueryTrajectoryState::Response &resp)
365 {
366  ROS_WARN("queryStateService() called, this is not tested yet");
367 
369  traj_ptr = current_trajectory_;
370  if (!traj_ptr)
371  {
372  ROS_FATAL("The current trajectory can never be null");
373  return false;
374  }
375  const SpecifiedTrajectory &traj = *traj_ptr;
376 
377  // Determines which segment of the trajectory to use
378  int seg = -1;
379  while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= req.time.toSec())
380  {
381  ++seg;
382  }
383  if (seg == -1)
384  return false;
385 
386  resp.name.resize(joints_.size());
387  resp.position.resize(joints_.size());
388  resp.velocity.resize(joints_.size());
389  resp.acceleration.resize(joints_.size());
390  for (size_t j = 0; j < joints_.size(); ++j)
391  {
392  resp.name[j] = joints_[j];
393  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, req.time.toSec() - traj[seg].start_time,
394  resp.position[j], resp.velocity[j], resp.acceleration[j]);
395  }
396 
397  return true;
398 }
399 
403 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
404 {
405  if (a.size() != b.size())
406  return false;
407 
408  for (size_t i = 0; i < a.size(); ++i)
409  {
410  if (count(b.begin(), b.end(), a[i]) != 1)
411  return false;
412  }
413  for (size_t i = 0; i < b.size(); ++i)
414  {
415  if (count(a.begin(), a.end(), b[i]) != 1)
416  return false;
417  }
418 
419  return true;
420 }
421 
422 void JointTrajectoryActionController::executeCB(const JTAS::GoalConstPtr &goal)
423 {
424  // note: the SimpleActionServer guarantees that we enter this function only when
425  // there is no other active goal. in other words, only one instance of executeCB()
426  // is ever running at the same time.
427 
428  //----- cancel other action server
430  {
431  ROS_WARN("joint_trajectory_action called while follow_joint_trajectory was active, canceling follow_joint_trajectory");
433  }
434 
435  int error_code = executeCommon(goal->trajectory, boost::bind(&JTAS::isPreemptRequested, boost::ref(action_server_)));
436 
437  if (error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
439  else if (error_code == PREEMPT_REQUESTED)
441  else
443 }
444 
445 void JointTrajectoryActionController::executeCBFollow(const FJTAS::GoalConstPtr &goal)
446 {
447  //----- cancel other action server
448  if (action_server_.isActive())
449  {
450  ROS_WARN("follow_joint_trajectory called while joint_trajectory_action was active, canceling joint_trajectory_action");
452  }
453 
454  // TODO: check tolerances from action goal
455  int error_code = executeCommon(goal->trajectory,
456  boost::bind(&FJTAS::isPreemptRequested, boost::ref(action_server_follow_)));
457  FJTAS::Result result;
458  result.error_code = error_code;
459 
460  if (error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
462  else if (error_code == PREEMPT_REQUESTED)
463  action_server_follow_.setPreempted(); // don't return result here, PREEMPT_REQUESTED is not a valid error_code
464  else
466 }
467 
471 int JointTrajectoryActionController::executeCommon(const trajectory_msgs::JointTrajectory &trajectory,
472  boost::function<bool()> isPreemptRequested)
473 {
474  if (!setsEqual(joints_, trajectory.joint_names))
475  {
476  ROS_ERROR("Joints on incoming goal don't match our joints");
477  for (size_t i = 0; i < trajectory.joint_names.size(); i++)
478  {
479  ROS_INFO(" incoming joint %d: %s", (int)i, trajectory.joint_names[i].c_str());
480  }
481  for (size_t i = 0; i < joints_.size(); i++)
482  {
483  ROS_INFO(" our joint %d: %s", (int)i, joints_[i].c_str());
484  }
485  return control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
486  }
487 
488  if (isPreemptRequested())
489  {
490  ROS_WARN("New action goal already seems to have been canceled!");
491  return PREEMPT_REQUESTED;
492  }
493 
494  // make sure the katana is stopped
496 
497  // ------ If requested, performs a stop
498  if (trajectory.points.empty())
499  {
500  // reset_trajectory_and_stop();
501  return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
502  }
503 
504  // calculate new trajectory
506  if (!new_traj)
507  {
508  ROS_ERROR("Could not calculate new trajectory, aborting");
509  return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
510  }
511  if (!validTrajectory(*new_traj))
512  {
513  ROS_ERROR("Computed trajectory did not fulfill all constraints!");
514  return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
515  }
516  current_trajectory_ = new_traj;
517 
518  // sleep until 0.5 seconds before scheduled start
520  trajectory.header.stamp > ros::Time::now(),
521  "Sleeping for %f seconds before start of trajectory", (trajectory.header.stamp - ros::Time::now()).toSec());
522  ros::Rate rate(10);
523  while ((trajectory.header.stamp - ros::Time::now()).toSec() > 0.5)
524  {
525  if (isPreemptRequested() || !ros::ok())
526  {
527  ROS_WARN("Goal canceled by client while waiting until scheduled start, aborting!");
528  return PREEMPT_REQUESTED;
529  }
530  rate.sleep();
531  }
532 
533  ROS_INFO("Sending trajectory to Katana arm...");
534  bool success = katana_->executeTrajectory(new_traj, isPreemptRequested);
535  if (!success)
536  {
537  ROS_ERROR("Problem while transferring trajectory to Katana arm, aborting");
538  return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
539  }
540 
541  ROS_INFO("Waiting until goal reached...");
542  ros::Rate goalWait(10);
543  while (ros::ok())
544  {
545  // always have to call this before calling someMotorCrashed() or allJointsReady()
546  katana_->refreshMotorStatus();
547 
548  if (katana_->someMotorCrashed())
549  {
550  ROS_ERROR("Some motor has crashed! Aborting trajectory...");
551  return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
552  }
553 
554  // all joints are idle
555  if (katana_->allJointsReady() && allJointsStopped())
556  {
557  // // make sure the joint positions are updated before checking for goalReached()
558  // --> this isn't necessary because refreshEncoders() is periodically called
559  // by KatanaNode. Leaving it out saves us some Katana bandwidth.
560  // katana_->refreshEncoders();
561 
562  if (goalReached())
563  {
564  // joints are idle and we are inside goal constraints. yippie!
565  ROS_INFO("Goal reached.");
566  return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
567  }
568  else
569  {
570  ROS_ERROR("Joints are idle and motors are not crashed, but we did not reach the goal position! WTF?");
571  return control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
572  }
573  }
574 
575  if (isPreemptRequested())
576  {
577  ROS_WARN("Goal canceled by client while waiting for trajectory to finish, aborting!");
578  return PREEMPT_REQUESTED;
579  }
580 
581  goalWait.sleep();
582  }
583 
584  // this part is only reached when node is shut down
585  return PREEMPT_REQUESTED;
586 }
587 
592 {
593  for (size_t i = 0; i < joints_.size(); i++)
594  {
595  double error = current_trajectory_->back().splines[i].target_position - katana_->getMotorAngles()[i];
596  if (goal_constraints_[i] > 0 && fabs(error) > goal_constraints_[i])
597  {
598  ROS_WARN_STREAM("Joint " << i << " did not reach its goal. target position: "
599  << current_trajectory_->back().splines[i].target_position << " actual position: "
600  << katana_->getMotorAngles()[i] << std::endl);
601  return false;
602  }
603  }
604  return true;
605 }
606 
611 {
612  for (size_t i = 0; i < joints_.size(); i++)
613  {
614  // It's important to be stopped if that's desired.
615  if (fabs(katana_->getMotorVelocities()[i]) > stopped_velocity_tolerance_)
616  return false;
617  }
618  return true;
619 }
620 
621 std::vector<int> JointTrajectoryActionController::makeJointsLookup(const trajectory_msgs::JointTrajectory &msg)
622 {
623  std::vector<int> lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg
624  for (size_t j = 0; j < joints_.size(); ++j)
625  {
626  for (size_t k = 0; k < msg.joint_names.size(); ++k)
627  {
628  if (msg.joint_names[k] == joints_[j])
629  {
630  lookup[j] = k;
631  break;
632  }
633  }
634 
635  if (lookup[j] == -1)
636  {
637  ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j].c_str());
638  return std::vector<int>(); // return empty vector to signal error
639  }
640  }
641 
642  return lookup;
643 }
644 
652 {
653  const double MAX_SPEED = 2.21; // rad/s; TODO: should be same value as URDF
654  const double MIN_TIME = 0.01; // seconds; the KNI calculates time in 10ms units, so this is the minimum duration of a spline
655  const double EPSILON = 0.0001;
656  const double POSITION_TOLERANCE = 0.1; // rad
657 
658  if (traj.size() > MOVE_BUFFER_SIZE)
659  ROS_WARN("new trajectory has %zu segments (move buffer size: %zu)", traj.size(), MOVE_BUFFER_SIZE);
660 
661  // ------- check times
662  for (size_t seg = 0; seg < traj.size() - 1; seg++)
663  {
664  if (std::abs(traj[seg].start_time + traj[seg].duration - traj[seg + 1].start_time) > EPSILON)
665  {
666  ROS_ERROR("start time and duration of segment %zu do not match next segment", seg);
667  return false;
668  }
669  }
670  for (size_t seg = 0; seg < traj.size(); seg++)
671  {
672  if (traj[seg].duration < MIN_TIME)
673  {
674  ROS_WARN("duration of segment %zu is too small: %f", seg, traj[seg].duration);
675  // return false;
676  }
677  }
678 
679  // ------- check start position
680  for (size_t j = 0; j < traj[0].splines.size(); j++)
681  {
682  if (std::abs(traj[0].splines[j].coef[0] - katana_->getMotorAngles()[j]) > POSITION_TOLERANCE)
683  {
684  ROS_ERROR("Initial joint angle of trajectory (%f) does not match current joint angle (%f) (joint %zu)", traj[0].splines[j].coef[0], katana_->getMotorAngles()[j], j);
685  return false;
686  }
687  }
688 
689  // ------- check conditions at t = 0 and t = N
690  for (size_t j = 0; j < traj[0].splines.size(); j++)
691  {
692  if (std::abs(traj[0].splines[j].coef[1]) > EPSILON)
693  {
694  ROS_ERROR("Velocity at t = 0 is not 0: %f (joint %zu)", traj[0].splines[j].coef[1], j);
695  return false;
696  }
697  }
698 
699  for (size_t j = 0; j < traj[traj.size() - 1].splines.size(); j++)
700  {
701  size_t seg = traj.size() - 1;
702  double vel_t, _;
703  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, _, vel_t, _);
704  if (std::abs(vel_t) > EPSILON)
705  {
706  ROS_ERROR("Velocity at t = N is not 0 (joint %zu)", j);
707  return false;
708  }
709  }
710 
711  // ------- check for discontinuities between segments
712  for (size_t seg = 0; seg < traj.size() - 1; seg++)
713  {
714  for (size_t j = 0; j < traj[seg].splines.size(); j++)
715  {
716  double pos_t, vel_t, acc_t;
717  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, pos_t, vel_t, acc_t);
718 
719  if (std::abs(traj[seg + 1].splines[j].coef[0] - pos_t) > EPSILON)
720  {
721  ROS_ERROR("Position discontinuity at end of segment %zu (joint %zu)", seg, j);
722  return false;
723  }
724  if (std::abs(traj[seg + 1].splines[j].coef[1] - vel_t) > EPSILON)
725  {
726  ROS_ERROR("Velocity discontinuity at end of segment %zu (joint %zu)", seg, j);
727  return false;
728  }
729  }
730  }
731 
732  // ------- check position, speed, acceleration limits
733  for (double t = traj[0].start_time; t < traj.back().start_time + traj.back().duration; t += 0.01)
734  {
735  // Determines which segment of the trajectory to use
736  int seg = -1;
737  while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= t)
738  {
739  ++seg;
740  }
741 
742  assert(seg >= 0);
743 
744  for (size_t j = 0; j < traj[seg].splines.size(); j++)
745  {
746  double pos_t, vel_t, acc_t;
747  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, t - traj[seg].start_time, pos_t, vel_t,
748  acc_t);
749 
750  // TODO later: check position limits (min/max encoders)
751 
752  if (std::abs(vel_t) > MAX_SPEED)
753  {
754  ROS_ERROR("Velocity %f too high at time %f (joint %zu)", vel_t, t, j);
755  return false;
756  }
757 
758  // TODO later: check acceleration limits
759  }
760  }
761  return true;
762 }
763 
764 }
filename
#define ROS_FATAL(...)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
int executeCommon(const trajectory_msgs::JointTrajectory &trajectory, boost::function< bool()> isPreemptRequested)
void publish(const boost::shared_ptr< M > &message) const
bool queryStateService(control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
boost::shared_ptr< SpecifiedTrajectory > calculateTrajectory(const trajectory_msgs::JointTrajectory &msg)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ROS_WARN(...)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients)
std::vector< int > makeJointsLookup(const trajectory_msgs::JointTrajectory &msg)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< Segment > SpecifiedTrajectory
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
static const size_t MOVE_BUFFER_SIZE
#define EPSILON
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
JointTrajectoryActionController(boost::shared_ptr< AbstractKatana > katana)
void splineCoefficients(int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4)
#define ROS_WARN_STREAM(args)
bool sleep()
#define ROS_DEBUG_COND(cond,...)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
static Time now()
const size_t NUM_JOINTS
The number of joints in the katana (= only the 5 "real" joints)
static bool setsEqual(const std::vector< std::string > &a, const std::vector< std::string > &b)
std::vector< Spline > splines
boost::shared_ptr< SpecifiedTrajectory > current_trajectory_
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25