joint_trajectory_action_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * joint_trajectory_action_controller.cpp
00020  *
00021  *  Created on: 07.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  *
00024  * based on joint_trajectory_action_controller.cpp by Stuart Glaser,
00025  * from the package robot_mechanism_controllers
00026  */
00027 
00028 #include "katana/joint_trajectory_action_controller.h"
00029 #include <fstream>
00030 #include <iostream>
00031 #include <cstdio>
00032 
00033 namespace katana
00034 {
00035 
00036 JointTrajectoryActionController::JointTrajectoryActionController(boost::shared_ptr<AbstractKatana> katana) :
00037   katana_(katana), action_server_(ros::NodeHandle(), "katana_arm_controller/joint_trajectory_action",
00038                                   boost::bind(&JointTrajectoryActionController::executeCB, this, _1), false),
00039   action_server_follow_(ros::NodeHandle(), "katana_arm_controller/follow_joint_trajectory",
00040                         boost::bind(&JointTrajectoryActionController::executeCBFollow, this, _1), false)
00041 {
00042   ros::NodeHandle node_;
00043 
00044   joints_ = katana_->getJointNames();
00045 
00046 
00047   // Trajectory and goal constraints
00048   //  node_.param("joint_trajectory_action_node/constraints/goal_time", goal_time_constraint_, 0.0);
00049   node_.param("joint_trajectory_action_node/constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 1e-6);
00050   goal_constraints_.resize(joints_.size());
00051   //  trajectory_constraints_.resize(joints_.size());
00052   for (size_t i = 0; i < joints_.size(); ++i)
00053   {
00054     std::string ns = std::string("joint_trajectory_action_node/constraints") + joints_[i];
00055     node_.param(ns + "/goal", goal_constraints_[i], 0.02);
00056     //    node_.param(ns + "/trajectory", trajectory_constraints_[i], -1.0);
00057   }
00058 
00059   // Subscriptions, publishers, services
00060   action_server_.start();
00061   action_server_follow_.start();
00062   sub_command_ = node_.subscribe("katana_arm_controller/command", 1, &JointTrajectoryActionController::commandCB, this);
00063   serve_query_state_ = node_.advertiseService("katana_arm_controller/query_state", &JointTrajectoryActionController::queryStateService, this);
00064   controller_state_publisher_ = node_.advertise<control_msgs::JointTrajectoryControllerState> ("katana_arm_controller/state", 1);
00065 
00066   // NOTE: current_trajectory_ is not initialized here, because that will happen in reset_trajectory_and_stop()
00067 
00068   reset_trajectory_and_stop();
00069 }
00070 
00071 JointTrajectoryActionController::~JointTrajectoryActionController()
00072 {
00073   sub_command_.shutdown();
00074   serve_query_state_.shutdown();
00075 }
00076 
00080 void JointTrajectoryActionController::reset_trajectory_and_stop()
00081 {
00082   katana_->freezeRobot();
00083 
00084   ros::Time time = ros::Time::now();
00085 
00086   // Creates a "hold current position" trajectory.
00087   // It's important that this trajectory is always there, because it will be used as a starting point for any new trajectory.
00088   boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00089   SpecifiedTrajectory &hold = *hold_ptr;
00090   hold[0].start_time = time.toSec() - 0.001;
00091   hold[0].duration = 0.0;
00092   hold[0].splines.resize(joints_.size());
00093   for (size_t j = 0; j < joints_.size(); ++j)
00094     hold[0].splines[j].coef[0] = katana_->getMotorAngles()[j];
00095 
00096   current_trajectory_ = hold_ptr;
00097 }
00098 
00099 void JointTrajectoryActionController::update()
00100 {
00101   ros::Time time = ros::Time::now();
00102 
00103   std::vector<double> q(joints_.size()), qd(joints_.size()), qdd(joints_.size());
00104 
00105   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr = current_trajectory_;
00106   if (!traj_ptr)
00107     ROS_FATAL("The current trajectory can never be null");
00108 
00109   // Only because this is what the code originally looked like.
00110   const SpecifiedTrajectory &traj = *traj_ptr;
00111 
00112   if (traj.size() == 0)
00113   {
00114     ROS_ERROR("No segments in the trajectory");
00115     return;
00116   }
00117 
00118   // ------ Finds the current segment
00119 
00120   // Determines which segment of the trajectory to use.
00121   int seg = -1;
00122   while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= time.toSec())
00123   {
00124     ++seg;
00125   }
00126 
00127   if (seg == -1)
00128   {
00129     // ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00130     // return;
00131     seg = 0;
00132   }
00133 
00134   // ------ Trajectory Sampling
00135 
00136   for (size_t i = 0; i < q.size(); ++i)
00137   {
00138     sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, time.toSec() - traj[seg].start_time,
00139                                q[i], qd[i], qdd[i]);
00140   }
00141 
00142   // ------ Calculate error
00143 
00144   std::vector<double> error(joints_.size());
00145   for (size_t i = 0; i < joints_.size(); ++i)
00146   {
00147     error[i] = katana_->getMotorAngles()[i] - q[i];
00148   }
00149 
00150   // ------ State publishing
00151 
00152   control_msgs::JointTrajectoryControllerState msg;
00153 
00154   // Message containing current state for all controlled joints
00155   for (size_t j = 0; j < joints_.size(); ++j)
00156     msg.joint_names.push_back(joints_[j]);
00157   msg.desired.positions.resize(joints_.size());
00158   msg.desired.velocities.resize(joints_.size());
00159   msg.desired.accelerations.resize(joints_.size());
00160   msg.actual.positions.resize(joints_.size());
00161   msg.actual.velocities.resize(joints_.size());
00162   // ignoring accelerations
00163   msg.error.positions.resize(joints_.size());
00164   msg.error.velocities.resize(joints_.size());
00165   // ignoring accelerations
00166 
00167   msg.header.stamp = time;
00168   for (size_t j = 0; j < joints_.size(); ++j)
00169   {
00170     msg.desired.positions[j] = q[j];
00171     msg.desired.velocities[j] = qd[j];
00172     msg.desired.accelerations[j] = qdd[j];
00173     msg.actual.positions[j] = katana_->getMotorAngles()[j];
00174     msg.actual.velocities[j] = katana_->getMotorVelocities()[j];
00175     // ignoring accelerations
00176     msg.error.positions[j] = error[j];
00177     msg.error.velocities[j] = katana_->getMotorVelocities()[j] - qd[j];
00178     // ignoring accelerations
00179   }
00180 
00181   controller_state_publisher_.publish(msg);
00182   // TODO: here we could publish feedback for the FollowJointTrajectory action; however,
00183   // this seems to be optional (the PR2's joint_trajectory_action_controller doesn't do it either)
00184 }
00185 
00191 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
00192 {
00193   ROS_WARN("commandCB() called, this is not tested yet");
00194   // just creates an action from the message and sends it on to the action server
00195 
00196   // create an action client spinning its own thread
00197   JTAC action_client("katana_arm_controller/joint_trajectory_action", true);
00198   action_client.waitForServer();
00199 
00200   JTAS::Goal goal;
00201   goal.trajectory = *(msg.get());
00202 
00203   // fire and forget
00204   action_client.sendGoal(goal);
00205 }
00206 
00207 boost::shared_ptr<SpecifiedTrajectory> JointTrajectoryActionController::calculateTrajectory(
00208                                                                                             const trajectory_msgs::JointTrajectory &msg)
00209 {
00210   boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr;
00211 
00212   bool allPointsHaveVelocities = true;
00213 
00214   // ------ Checks that the incoming segments have the right number of elements,
00215   //        determines which spline algorithm to use
00216   for (size_t i = 0; i < msg.points.size(); i++)
00217   {
00218     if (msg.points[i].accelerations.size() != 0 && msg.points[i].accelerations.size() != joints_.size())
00219     {
00220       ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg.points[i].accelerations.size());
00221       return new_traj_ptr; // return null pointer to signal error
00222     }
00223     if (msg.points[i].velocities.size() == 0)
00224     {
00225       // getCubicSplineCoefficients only works when the desired velocities are already given.
00226       allPointsHaveVelocities = false;
00227     }
00228     else if (msg.points[i].velocities.size() != joints_.size())
00229     {
00230       ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg.points[i].velocities.size());
00231       return new_traj_ptr; // return null pointer to signal error
00232     }
00233     if (msg.points[i].positions.size() != joints_.size())
00234     {
00235       ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg.points[i].positions.size());
00236       return new_traj_ptr; // return null pointer to signal error
00237     }
00238   }
00239 
00240   // ------ Correlates the joints we're commanding to the joints in the message
00241   std::vector<int> lookup = makeJointsLookup(msg);
00242   if (lookup.size() == 0)
00243     return new_traj_ptr; // return null pointer to signal error
00244 
00245 
00246   // ------ convert the boundary conditions to splines
00247   new_traj_ptr.reset(new SpecifiedTrajectory);
00248   SpecifiedTrajectory &new_traj = *new_traj_ptr;
00249 
00250   size_t steps = msg.points.size() - 1;
00251 
00252   ROS_DEBUG("steps: %zu", steps);
00253   assert(steps > 0); // this is checked before
00254 
00255   for (size_t i = 0; i < steps; i++)
00256   {
00257     Segment seg;
00258     seg.splines.resize(joints_.size());
00259     new_traj.push_back(seg);
00260   }
00261 
00262   for (size_t j = 0; j < joints_.size(); j++)
00263   {
00264     double times[steps + 1], positions[steps + 1], velocities[steps + 1], durations[steps], coeff0[steps],
00265            coeff1[steps], coeff2[steps], coeff3[steps];
00266 
00267     for (size_t i = 0; i < steps + 1; i++)
00268     {
00269       times[i] = msg.header.stamp.toSec() + msg.points[i].time_from_start.toSec();
00270       positions[i] = msg.points[i].positions[lookup[j]];
00271       if (allPointsHaveVelocities)
00272         velocities[i] = msg.points[i].velocities[lookup[j]];
00273       ROS_DEBUG("position %zu for joint %zu in message (= our joint %d): %f", i, j, lookup[j], positions[i]);
00274     }
00275 
00276     for (size_t i = 0; i < steps; i++)
00277     {
00278       durations[i] = times[i + 1] - times[i];
00279     }
00280 
00281     // calculate and store the coefficients
00282     if (allPointsHaveVelocities)
00283     {
00284       ROS_DEBUG("Using getCubicSplineCoefficients()");
00285       for (size_t i = 0; i < steps; ++i)
00286       {
00287         std::vector<double> coeff;
00288         getCubicSplineCoefficients(positions[i], velocities[i], positions[i + 1], velocities[i + 1], durations[i],
00289                                    coeff);
00290         coeff0[i] = coeff[0];
00291         coeff1[i] = coeff[1];
00292         coeff2[i] = coeff[2];
00293         coeff3[i] = coeff[3];
00294       }
00295     }
00296     else
00297     {
00298       ROS_DEBUG("Using splineCoefficients()");
00299       splineCoefficients(steps, times, positions, coeff0, coeff1, coeff2, coeff3);
00300     }
00301 
00302     for (size_t i = 0; i < steps; ++i)
00303     {
00304       new_traj[i].duration = durations[i];
00305       new_traj[i].start_time = times[i];
00306       new_traj[i].splines[j].target_position = positions[i + 1];
00307       new_traj[i].splines[j].coef[0] = coeff0[i];
00308       new_traj[i].splines[j].coef[1] = coeff1[i];
00309       new_traj[i].splines[j].coef[2] = coeff2[i];
00310       new_traj[i].splines[j].coef[3] = coeff3[i];
00311     }
00312   }
00313 
00314   ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
00315   for (size_t i = 0; i < std::min((size_t)20, new_traj.size()); i++)
00316   {
00317     ROS_DEBUG("Segment %2zu - start_time: %.3lf   duration: %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
00318     for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
00319     {
00320       ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf (%s)",
00321           new_traj[i].splines[j].coef[0],
00322           new_traj[i].splines[j].coef[1],
00323           new_traj[i].splines[j].coef[2],
00324           new_traj[i].splines[j].coef[3],
00325           joints_[j].c_str());
00326     }
00327   }
00328 
00329   // -------- sample trajectory and write to file
00330   for (size_t j = 0; j < NUM_JOINTS; j++)
00331   {
00332     char filename[25];
00333     sprintf(filename, "/tmp/trajectory-%zu.dat", j);
00334     std::ofstream trajectory_file(filename, std::ios_base::out);
00335     trajectory_file.precision(8);
00336     for (double t = new_traj[0].start_time; t < new_traj.back().start_time + new_traj.back().duration; t += 0.01)
00337     {
00338       // Determines which segment of the trajectory to use
00339       int seg = -1;
00340       while (seg + 1 < (int)new_traj.size() && new_traj[seg + 1].start_time <= t)
00341       {
00342         ++seg;
00343       }
00344 
00345       assert(seg >= 0);
00346 
00347       double pos_t, vel_t, acc_t;
00348       sampleSplineWithTimeBounds(new_traj[seg].splines[j].coef, new_traj[seg].duration, t - new_traj[seg].start_time,
00349                                  pos_t, vel_t, acc_t);
00350 
00351       trajectory_file << t - new_traj[0].start_time << " " << pos_t << " " << vel_t << " " << acc_t << std::endl;
00352     }
00353 
00354     trajectory_file.close();
00355   }
00356 
00357   return new_traj_ptr;
00358 }
00359 
00363 bool JointTrajectoryActionController::queryStateService(control_msgs::QueryTrajectoryState::Request &req,
00364                                                         control_msgs::QueryTrajectoryState::Response &resp)
00365 {
00366   ROS_WARN("queryStateService() called, this is not tested yet");
00367 
00368   boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00369   traj_ptr = current_trajectory_;
00370   if (!traj_ptr)
00371   {
00372     ROS_FATAL("The current trajectory can never be null");
00373     return false;
00374   }
00375   const SpecifiedTrajectory &traj = *traj_ptr;
00376 
00377   // Determines which segment of the trajectory to use
00378   int seg = -1;
00379   while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= req.time.toSec())
00380   {
00381     ++seg;
00382   }
00383   if (seg == -1)
00384     return false;
00385 
00386   resp.name.resize(joints_.size());
00387   resp.position.resize(joints_.size());
00388   resp.velocity.resize(joints_.size());
00389   resp.acceleration.resize(joints_.size());
00390   for (size_t j = 0; j < joints_.size(); ++j)
00391   {
00392     resp.name[j] = joints_[j];
00393     sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, req.time.toSec() - traj[seg].start_time,
00394                                resp.position[j], resp.velocity[j], resp.acceleration[j]);
00395   }
00396 
00397   return true;
00398 }
00399 
00403 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
00404 {
00405   if (a.size() != b.size())
00406     return false;
00407 
00408   for (size_t i = 0; i < a.size(); ++i)
00409   {
00410     if (count(b.begin(), b.end(), a[i]) != 1)
00411       return false;
00412   }
00413   for (size_t i = 0; i < b.size(); ++i)
00414   {
00415     if (count(a.begin(), a.end(), b[i]) != 1)
00416       return false;
00417   }
00418 
00419   return true;
00420 }
00421 
00422 void JointTrajectoryActionController::executeCB(const JTAS::GoalConstPtr &goal)
00423 {
00424   // note: the SimpleActionServer guarantees that we enter this function only when
00425   // there is no other active goal. in other words, only one instance of executeCB()
00426   // is ever running at the same time.
00427 
00428   //----- cancel other action server
00429   if (action_server_follow_.isActive())
00430   {
00431     ROS_WARN("joint_trajectory_action called while follow_joint_trajectory was active, canceling follow_joint_trajectory");
00432     action_server_follow_.setPreempted();
00433   }
00434 
00435   int error_code = executeCommon(goal->trajectory, boost::bind(&JTAS::isPreemptRequested, boost::ref(action_server_)));
00436 
00437   if (error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
00438     action_server_.setSucceeded();
00439   else if (error_code == PREEMPT_REQUESTED)
00440     action_server_.setPreempted();
00441   else
00442     action_server_.setAborted();
00443 }
00444 
00445 void JointTrajectoryActionController::executeCBFollow(const FJTAS::GoalConstPtr &goal)
00446 {
00447   //----- cancel other action server
00448   if (action_server_.isActive())
00449   {
00450     ROS_WARN("follow_joint_trajectory called while joint_trajectory_action was active, canceling joint_trajectory_action");
00451     action_server_.setPreempted();
00452   }
00453 
00454   // TODO: check tolerances from action goal
00455   int error_code = executeCommon(goal->trajectory,
00456                                  boost::bind(&FJTAS::isPreemptRequested, boost::ref(action_server_follow_)));
00457   FJTAS::Result result;
00458   result.error_code = error_code;
00459 
00460   if (error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
00461     action_server_follow_.setSucceeded(result);
00462   else if (error_code == PREEMPT_REQUESTED)
00463     action_server_follow_.setPreempted(); // don't return result here, PREEMPT_REQUESTED is not a valid error_code
00464   else
00465     action_server_follow_.setAborted(result);
00466 }
00467 
00471 int JointTrajectoryActionController::executeCommon(const trajectory_msgs::JointTrajectory &trajectory,
00472                                                    boost::function<bool()> isPreemptRequested)
00473 {
00474   if (!setsEqual(joints_, trajectory.joint_names))
00475   {
00476     ROS_ERROR("Joints on incoming goal don't match our joints");
00477     for (size_t i = 0; i < trajectory.joint_names.size(); i++)
00478     {
00479       ROS_INFO("  incoming joint %d: %s", (int)i, trajectory.joint_names[i].c_str());
00480     }
00481     for (size_t i = 0; i < joints_.size(); i++)
00482     {
00483       ROS_INFO("  our joint      %d: %s", (int)i, joints_[i].c_str());
00484     }
00485     return control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00486   }
00487 
00488   if (isPreemptRequested())
00489   {
00490     ROS_WARN("New action goal already seems to have been canceled!");
00491     return PREEMPT_REQUESTED;
00492   }
00493 
00494   // make sure the katana is stopped
00495   reset_trajectory_and_stop();
00496 
00497   // ------ If requested, performs a stop
00498   if (trajectory.points.empty())
00499   {
00500     // reset_trajectory_and_stop();
00501     return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00502   }
00503 
00504   // calculate new trajectory
00505   boost::shared_ptr<SpecifiedTrajectory> new_traj = calculateTrajectory(trajectory);
00506   if (!new_traj)
00507   {
00508     ROS_ERROR("Could not calculate new trajectory, aborting");
00509     return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00510   }
00511   if (!validTrajectory(*new_traj))
00512   {
00513     ROS_ERROR("Computed trajectory did not fulfill all constraints!");
00514     return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00515   }
00516   current_trajectory_ = new_traj;
00517 
00518   // sleep until 0.5 seconds before scheduled start
00519   ROS_DEBUG_COND(
00520       trajectory.header.stamp > ros::Time::now(),
00521       "Sleeping for %f seconds before start of trajectory", (trajectory.header.stamp - ros::Time::now()).toSec());
00522   ros::Rate rate(10);
00523   while ((trajectory.header.stamp - ros::Time::now()).toSec() > 0.5)
00524   {
00525     if (isPreemptRequested() || !ros::ok())
00526     {
00527       ROS_WARN("Goal canceled by client while waiting until scheduled start, aborting!");
00528       return PREEMPT_REQUESTED;
00529     }
00530     rate.sleep();
00531   }
00532 
00533   ROS_INFO("Sending trajectory to Katana arm...");
00534   bool success = katana_->executeTrajectory(new_traj, isPreemptRequested);
00535   if (!success)
00536   {
00537     ROS_ERROR("Problem while transferring trajectory to Katana arm, aborting");
00538     return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00539   }
00540 
00541   ROS_INFO("Waiting until goal reached...");
00542   ros::Rate goalWait(10);
00543   while (ros::ok())
00544   {
00545     // always have to call this before calling someMotorCrashed() or allJointsReady()
00546     katana_->refreshMotorStatus();
00547 
00548     if (katana_->someMotorCrashed())
00549     {
00550       ROS_ERROR("Some motor has crashed! Aborting trajectory...");
00551       return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00552     }
00553 
00554     // all joints are idle
00555     if (katana_->allJointsReady() && allJointsStopped())
00556     {
00557       // // make sure the joint positions are updated before checking for goalReached()
00558       // --> this isn't necessary because refreshEncoders() is periodically called
00559       //     by KatanaNode. Leaving it out saves us some Katana bandwidth.
00560       // katana_->refreshEncoders();
00561 
00562       if (goalReached())
00563       {
00564         // joints are idle and we are inside goal constraints. yippie!
00565         ROS_INFO("Goal reached.");
00566         return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00567       }
00568       else
00569       {
00570         ROS_ERROR("Joints are idle and motors are not crashed, but we did not reach the goal position! WTF?");
00571         return control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
00572       }
00573     }
00574 
00575     if (isPreemptRequested())
00576     {
00577       ROS_WARN("Goal canceled by client while waiting for trajectory to finish, aborting!");
00578       return PREEMPT_REQUESTED;
00579     }
00580 
00581     goalWait.sleep();
00582   }
00583 
00584   // this part is only reached when node is shut down
00585   return PREEMPT_REQUESTED;
00586 }
00587 
00591 bool JointTrajectoryActionController::goalReached()
00592 {
00593   for (size_t i = 0; i < joints_.size(); i++)
00594   {
00595     double error = current_trajectory_->back().splines[i].target_position - katana_->getMotorAngles()[i];
00596     if (goal_constraints_[i] > 0 && fabs(error) > goal_constraints_[i])
00597     {
00598       ROS_WARN_STREAM("Joint " << i << " did not reach its goal. target position: "
00599           << current_trajectory_->back().splines[i].target_position << " actual position: "
00600           << katana_->getMotorAngles()[i] << std::endl);
00601       return false;
00602     }
00603   }
00604   return true;
00605 }
00606 
00610 bool JointTrajectoryActionController::allJointsStopped()
00611 {
00612   for (size_t i = 0; i < joints_.size(); i++)
00613   {
00614     // It's important to be stopped if that's desired.
00615     if (fabs(katana_->getMotorVelocities()[i]) > stopped_velocity_tolerance_)
00616       return false;
00617   }
00618   return true;
00619 }
00620 
00621 std::vector<int> JointTrajectoryActionController::makeJointsLookup(const trajectory_msgs::JointTrajectory &msg)
00622 {
00623   std::vector<int> lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg
00624   for (size_t j = 0; j < joints_.size(); ++j)
00625   {
00626     for (size_t k = 0; k < msg.joint_names.size(); ++k)
00627     {
00628       if (msg.joint_names[k] == joints_[j])
00629       {
00630         lookup[j] = k;
00631         break;
00632       }
00633     }
00634 
00635     if (lookup[j] == -1)
00636     {
00637       ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j].c_str());
00638       return std::vector<int>(); // return empty vector to signal error
00639     }
00640   }
00641 
00642   return lookup;
00643 }
00644 
00651 bool JointTrajectoryActionController::validTrajectory(const SpecifiedTrajectory &traj)
00652 {
00653   const double MAX_SPEED = 2.21; // rad/s; TODO: should be same value as URDF
00654   const double MIN_TIME = 0.01; // seconds; the KNI calculates time in 10ms units, so this is the minimum duration of a spline
00655   const double EPSILON = 0.0001;
00656   const double POSITION_TOLERANCE = 0.1; // rad
00657 
00658   if (traj.size() > MOVE_BUFFER_SIZE)
00659     ROS_WARN("new trajectory has %zu segments (move buffer size: %zu)", traj.size(), MOVE_BUFFER_SIZE);
00660 
00661   // ------- check times
00662   for (size_t seg = 0; seg < traj.size() - 1; seg++)
00663   {
00664     if (std::abs(traj[seg].start_time + traj[seg].duration - traj[seg + 1].start_time) > EPSILON)
00665     {
00666       ROS_ERROR("start time and duration of segment %zu do not match next segment", seg);
00667       return false;
00668     }
00669   }
00670   for (size_t seg = 0; seg < traj.size(); seg++)
00671   {
00672     if (traj[seg].duration < MIN_TIME)
00673     {
00674       ROS_WARN("duration of segment %zu is too small: %f", seg, traj[seg].duration);
00675       // return false;
00676     }
00677   }
00678 
00679   // ------- check start position
00680   for (size_t j = 0; j < traj[0].splines.size(); j++)
00681   {
00682     if (std::abs(traj[0].splines[j].coef[0] - katana_->getMotorAngles()[j]) > POSITION_TOLERANCE)
00683     {
00684       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);
00685       return false;
00686     }
00687   }
00688 
00689   // ------- check conditions at t = 0 and t = N
00690   for (size_t j = 0; j < traj[0].splines.size(); j++)
00691   {
00692     if (std::abs(traj[0].splines[j].coef[1]) > EPSILON)
00693     {
00694       ROS_ERROR("Velocity at t = 0 is not 0: %f (joint %zu)", traj[0].splines[j].coef[1], j);
00695       return false;
00696     }
00697   }
00698 
00699   for (size_t j = 0; j < traj[traj.size() - 1].splines.size(); j++)
00700   {
00701     size_t seg = traj.size() - 1;
00702     double vel_t, _;
00703     sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, _, vel_t, _);
00704     if (std::abs(vel_t) > EPSILON)
00705     {
00706       ROS_ERROR("Velocity at t = N is not 0 (joint %zu)", j);
00707       return false;
00708     }
00709   }
00710 
00711   // ------- check for discontinuities between segments
00712   for (size_t seg = 0; seg < traj.size() - 1; seg++)
00713   {
00714     for (size_t j = 0; j < traj[seg].splines.size(); j++)
00715     {
00716       double pos_t, vel_t, acc_t;
00717       sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, traj[seg].duration, pos_t, vel_t, acc_t);
00718 
00719       if (std::abs(traj[seg + 1].splines[j].coef[0] - pos_t) > EPSILON)
00720       {
00721         ROS_ERROR("Position discontinuity at end of segment %zu (joint %zu)", seg, j);
00722         return false;
00723       }
00724       if (std::abs(traj[seg + 1].splines[j].coef[1] - vel_t) > EPSILON)
00725       {
00726         ROS_ERROR("Velocity discontinuity at end of segment %zu (joint %zu)", seg, j);
00727         return false;
00728       }
00729     }
00730   }
00731 
00732   // ------- check position, speed, acceleration limits
00733   for (double t = traj[0].start_time; t < traj.back().start_time + traj.back().duration; t += 0.01)
00734   {
00735     // Determines which segment of the trajectory to use
00736     int seg = -1;
00737     while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= t)
00738     {
00739       ++seg;
00740     }
00741 
00742     assert(seg >= 0);
00743 
00744     for (size_t j = 0; j < traj[seg].splines.size(); j++)
00745     {
00746       double pos_t, vel_t, acc_t;
00747       sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, t - traj[seg].start_time, pos_t, vel_t,
00748                                  acc_t);
00749 
00750       // TODO later: check position limits (min/max encoders)
00751 
00752       if (std::abs(vel_t) > MAX_SPEED)
00753       {
00754         ROS_ERROR("Velocity %f too high at time %f (joint %zu)", vel_t, t, j);
00755         return false;
00756       }
00757 
00758       // TODO later: check acceleration limits
00759     }
00760   }
00761   return true;
00762 }
00763 
00764 }


katana
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:43:33