cubic_spline_shortcutter.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #ifndef CUBIC_SPLINE_SHORT_CUTTER_H_
00038 #define CUBIC_SPLINE_SHORT_CUTTER_H_
00039 
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <spline_smoother/spline_smoother.h>
00043 #include <spline_smoother/cubic_trajectory.h>
00044 #include <planning_environment/models/collision_models_interface.h>
00045 #include <planning_environment/models/model_utils.h>
00046 #include <arm_navigation_msgs/RobotState.h>
00047 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00048 #include <trajectory_msgs/JointTrajectoryPoint.h>
00049 
00050 namespace constraint_aware_spline_smoother
00051 {
00052 
00056 template <typename T>
00057 class CubicSplineShortCutter: public spline_smoother::SplineSmoother<T>
00058 {
00059 public:
00063   CubicSplineShortCutter();
00064   virtual ~CubicSplineShortCutter();
00065   virtual bool smooth(const T& trajectory_in, 
00066                       T& trajectory_out) const;
00069   virtual bool configure();
00070 private:
00071   bool active_;
00072   double discretization_;
00073   bool setupCollisionEnvironment();
00074   planning_environment::CollisionModelsInterface *collision_models_interface_;
00075   //  ros::NodeHandle node_handle_;
00076   int getRandomInt(int min,int max) const;
00077   double getRandomTimeStamp(double min,double max) const;
00078   void discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00079                             const double &discretization,
00080                             trajectory_msgs::JointTrajectory &joint_trajectory) const;
00081   bool trimTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00082                       const double &segment_start_time, 
00083                       const double &segment_end_time) const;
00084   bool findTrajectoryPointsInInterval(const trajectory_msgs::JointTrajectory &trajectory,
00085                                       const double &segment_start_time, 
00086                                       const double &segment_end_time,
00087                                       int &index_1,
00088                                       int &index_2) const;
00089   bool getWaypoints(const spline_smoother::SplineTrajectory &spline, 
00090                     trajectory_msgs::JointTrajectory &joint_trajectory) const;
00091   bool addToTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00092                        const trajectory_msgs::JointTrajectoryPoint &trajectory_point,
00093                        const ros::Duration& delta_time) const;
00094 
00095   void printTrajectory(const trajectory_msgs::JointTrajectory &joint_trajectory) const;
00096 
00097   void discretizeAndAppendSegment(const spline_smoother::SplineTrajectorySegment &spline_segment,
00098                                   const double &discretization,
00099                                   trajectory_msgs::JointTrajectory &joint_trajectory,
00100                                   const ros::Duration &segment_start_time,
00101                                   const bool &include_segment_end) const;
00102 
00103   double maxLInfDistance(const trajectory_msgs::JointTrajectoryPoint &start, 
00104                          const trajectory_msgs::JointTrajectoryPoint &end) const;
00105 
00106   void refineTrajectory(T &trajectory) const;
00107 
00108 };
00109 
00110 template <typename T>
00111 bool CubicSplineShortCutter<T>::configure()
00112 {
00113   if (!spline_smoother::SplineSmoother<T>::getParam("discretization", discretization_))
00114   {
00115     ROS_ERROR("Spline smoother, \"%s\", params has no attribute discretization.", spline_smoother::SplineSmoother<T>::getName().c_str());
00116     return false;
00117   }
00118   else
00119   {
00120     ROS_DEBUG("Using a discretization value of %f",discretization_);
00121     return true;
00122   }
00123 };
00124 
00125 template <typename T>
00126 CubicSplineShortCutter<T>::CubicSplineShortCutter()
00127 {
00128   if(!setupCollisionEnvironment())
00129     active_ = false;
00130   else
00131     active_ = true;
00132 }
00133 
00134 template <typename T>
00135 CubicSplineShortCutter<T>::~CubicSplineShortCutter()
00136 {
00137 }
00138 
00139 template <typename T>
00140 int CubicSplineShortCutter<T>::getRandomInt(int min_index,int max_index)const
00141 {
00142   int rand_num = rand()%100+1;
00143   double result = min_index + (double)((max_index-min_index)*rand_num)/101.0;
00144   return (int) result;
00145 }
00146 
00147 template <typename T>
00148 double CubicSplineShortCutter<T>::getRandomTimeStamp(double min,double max)const
00149 {
00150   int rand_num = rand()%100+1;
00151   double result = min + (double)((max-min)*rand_num)/101.0;
00152   return result;
00153 }
00154 
00155 template <typename T>
00156 bool CubicSplineShortCutter<T>::smooth(const T& trajectory_in, 
00157                                        T& trajectory_out) const
00158 {
00159   double discretization = discretization_;
00160   srand(time(NULL)); // initialize random seed: 
00161   if(!active_)
00162   {
00163     ROS_ERROR("Smoother is not active");
00164     return false;
00165   }
00166 
00167   ROS_INFO("Got trajectory with %d points",(int)trajectory_in.request.trajectory.points.size());
00168 
00169   if(!collision_models_interface_->isPlanningSceneSet()) {
00170     ROS_INFO("Planning scene not set, can't do anything");
00171     return false;
00172   }
00173 
00174   arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00175   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00176   arm_navigation_msgs::RobotState robot_state;
00177   spline_smoother::CubicTrajectory trajectory_solver;
00178   spline_smoother::SplineTrajectory spline, shortcut_spline;
00179   arm_navigation_msgs::JointTrajectoryWithLimits shortcut, discretized_trajectory;
00180 
00181   trajectory_out.request = trajectory_in.request;
00182 
00183   collision_models_interface_->disableCollisionsForNonUpdatedLinks(trajectory_in.request.group_name);
00184 
00185   planning_environment::setRobotStateAndComputeTransforms(trajectory_in.request.start_state,
00186                                                           *collision_models_interface_->getPlanningSceneState());
00187 
00188   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00189                                                           trajectory_out.request.trajectory,
00190                                                           trajectory_in.request.goal_constraints,
00191                                                           trajectory_in.request.path_constraints,
00192                                                           trajectory_out.response.error_code,
00193                                                           trajectory_error_codes,
00194                                                           false)) {
00195     ROS_INFO_STREAM("Original trajectory invalid with error code " << trajectory_out.response.error_code.val);
00196     return false;
00197   }
00198 
00199 
00200   if (!spline_smoother::checkTrajectoryConsistency(trajectory_out))
00201     return false;
00202 
00203   shortcut.limits = trajectory_in.request.limits;
00204   shortcut.trajectory.joint_names = trajectory_in.request.trajectory.joint_names;
00205 
00206   discretized_trajectory.limits = trajectory_in.request.limits;
00207   discretized_trajectory.trajectory.joint_names = trajectory_in.request.trajectory.joint_names;
00208 
00209   ros::Time start_time = ros::Time::now();
00210   ros::Duration timeout = trajectory_in.request.allowed_time;
00211 
00212   bool success = trajectory_solver.parameterize(trajectory_out.request.trajectory,trajectory_in.request.limits,spline);      
00213   getWaypoints(spline,trajectory_out.request.trajectory);
00214   printTrajectory(trajectory_out.request.trajectory);
00215   
00216   for(unsigned int i = 0; i < trajectory_in.request.limits.size(); i++) {
00217     ROS_DEBUG_STREAM("Joint " << trajectory_in.request.limits[i].joint_name 
00218                      << " has " << (bool) trajectory_in.request.limits[i].has_position_limits
00219                      << " low " << trajectory_in.request.limits[i].min_position
00220                      << " high " << trajectory_in.request.limits[i].max_position);
00221   }
00222 
00223   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00224                                                           trajectory_out.request.trajectory,
00225                                                           trajectory_in.request.goal_constraints,
00226                                                           trajectory_in.request.path_constraints,
00227                                                           trajectory_out.response.error_code,
00228                                                           trajectory_error_codes,
00229                                                           false)) {
00230     ROS_INFO_STREAM("Original sampled trajectory invalid with error code " << trajectory_out.response.error_code.val);
00231     return false;
00232   } else {
00233     ROS_DEBUG_STREAM("Originally sampled trajectory ok");
00234   }
00235   
00236   T test_traj = trajectory_out;
00237 
00238   discretizeTrajectory(spline,discretization,test_traj.request.trajectory);
00239 
00240   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00241                                                           test_traj.request.trajectory,
00242                                                           trajectory_in.request.goal_constraints,
00243                                                           trajectory_in.request.path_constraints,
00244                                                           trajectory_out.response.error_code,
00245                                                           trajectory_error_codes,
00246                                                           false)) {
00247     ROS_WARN_STREAM("Original discretized trajectory invalid with error code " << trajectory_out.response.error_code.val);
00248     trajectory_out.request = test_traj.request;
00249     return false;
00250   } else {
00251     ROS_DEBUG_STREAM("Originally discretized trajectory ok");
00252   }
00253   
00254 
00255   std::vector<double> sample_times;
00256   sample_times.resize(2);
00257   bool first_try = true;
00258   while(ros::Time::now() - start_time < timeout)
00259   {
00260     double total_time = trajectory_out.request.trajectory.points.back().time_from_start.toSec();
00261     double segment_start_time = getRandomTimeStamp(0.0,total_time);
00262     double segment_end_time = getRandomTimeStamp(segment_start_time,total_time);
00263     if(segment_start_time == segment_end_time)
00264       continue;
00265     if(first_try)
00266     {
00267       segment_start_time = 0.0;
00268       segment_end_time = total_time;
00269       first_try = false;
00270     }
00271     sample_times[0] = segment_start_time;
00272     sample_times[1] = segment_end_time;
00273 
00274     spline_smoother::sampleSplineTrajectory(spline,sample_times,shortcut.trajectory);
00275     ROS_DEBUG("Start time: %f, %f",segment_start_time,shortcut.trajectory.points[0].positions[0]);
00276     ROS_DEBUG("End time  : %f, %f",segment_end_time,shortcut.trajectory.points[1].positions[0]);
00277     shortcut.trajectory.points[0].time_from_start = ros::Duration(0.0);
00278     shortcut.trajectory.points[1].time_from_start = ros::Duration(0.0);
00279     
00280     if(!trajectory_solver.parameterize(shortcut.trajectory,trajectory_in.request.limits,shortcut_spline))
00281       return false;
00282     discretizeTrajectory(shortcut_spline,discretization,discretized_trajectory.trajectory);
00283     ROS_DEBUG("Succeeded in sampling trajectory with size: %d",(int)discretized_trajectory.trajectory.points.size());
00284 
00285     arm_navigation_msgs::Constraints empty_goal_constraints;
00286 
00287     if(collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00288                                                            discretized_trajectory.trajectory,
00289                                                            empty_goal_constraints,
00290                                                            trajectory_in.request.path_constraints,
00291                                                            error_code,
00292                                                            trajectory_error_codes,
00293                                                            false))
00294     {
00295       ros::Duration shortcut_duration = discretized_trajectory.trajectory.points.back().time_from_start - discretized_trajectory.trajectory.points.front().time_from_start;
00296       if(segment_end_time-segment_start_time <= shortcut_duration.toSec())
00297         continue;
00298       if(!trimTrajectory(trajectory_out.request.trajectory,segment_start_time,segment_end_time))
00299         continue;
00300       ROS_DEBUG_STREAM("Trimmed trajectory has " << trajectory_out.request.trajectory.points.size() << " points");
00301 
00302       ROS_DEBUG("Shortcut reduced duration from: %f to %f",
00303                 segment_end_time-segment_start_time,
00304                 shortcut_duration.toSec());
00305       shortcut.trajectory.points[0].time_from_start = ros::Duration(segment_start_time);
00306       shortcut.trajectory.points[1].time_from_start = ros::Duration(segment_start_time) + shortcut_duration;
00307       addToTrajectory(trajectory_out.request.trajectory,
00308                       shortcut.trajectory.points[0],
00309                       ros::Duration(0.0));
00310       addToTrajectory(trajectory_out.request.trajectory,
00311                       shortcut.trajectory.points[1],
00312                       shortcut_duration-ros::Duration(segment_end_time-segment_start_time));
00313       spline.segments.clear();
00314       if(!trajectory_solver.parameterize(trajectory_out.request.trajectory,trajectory_in.request.limits,spline)) {
00315         trajectory_out.response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_TRAJECTORY;
00316         return false;
00317       }
00318       if(!getWaypoints(spline,trajectory_out.request.trajectory)) {
00319         trajectory_out.response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_TRAJECTORY;
00320         return false;
00321       }
00322       printTrajectory(trajectory_out.request.trajectory);
00323       if(trajectory_out.request.trajectory.points.size() < 3)
00324         break;
00325     }
00326     else 
00327     {
00328       ROS_DEBUG("Traj segment rejected with error code: %d",error_code.val);
00329       continue;
00330     }
00331   }
00332   ROS_INFO("Trajectory filter took %f seconds",(ros::Time::now() - start_time).toSec());
00333   for(unsigned int i=0; i < trajectory_out.request.trajectory.points.size(); i++)
00334   {
00335     trajectory_out.request.trajectory.points[i].accelerations.clear();
00336   }
00337   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00338                                                           trajectory_out.request.trajectory,
00339                                                           trajectory_in.request.goal_constraints,
00340                                                           trajectory_in.request.path_constraints,
00341                                                           trajectory_out.response.error_code,
00342                                                           trajectory_error_codes,
00343                                                           false)) {
00344     ROS_INFO_STREAM("Before refine trajectory invalid with error code " << trajectory_out.response.error_code.val);
00345   } else {
00346     ROS_DEBUG_STREAM("Before refine trajectory ok");
00347   }
00348 
00349   printTrajectory(trajectory_out.request.trajectory);
00350   refineTrajectory(trajectory_out);
00351   
00352   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00353                                                           trajectory_out.request.trajectory,
00354                                                           trajectory_in.request.goal_constraints,
00355                                                           trajectory_in.request.path_constraints,
00356                                                           trajectory_out.response.error_code,
00357                                                           trajectory_error_codes,
00358                                                           false)) {
00359     ROS_INFO_STREAM("Before waypoints trajectory invalid with error code " << trajectory_out.response.error_code.val);
00360   } else {
00361     ROS_DEBUG_STREAM("Before waypoints trajectory ok");
00362   }
00363 
00364   if(!trajectory_solver.parameterize(trajectory_out.request.trajectory,trajectory_in.request.limits,spline))
00365     return false;
00366   if(!getWaypoints(spline,trajectory_out.request.trajectory))
00367     return false;
00368   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00369                                                           trajectory_out.request.trajectory,
00370                                                           trajectory_in.request.goal_constraints,
00371                                                           trajectory_in.request.path_constraints,
00372                                                           error_code,
00373                                                           trajectory_error_codes,
00374                                                           false)) {
00375     ROS_INFO_STREAM("Before discretize trajectory invalid with error code " << error_code.val);
00376   } else {
00377     ROS_DEBUG_STREAM("Before discretize trajectory ok");
00378   }
00379   discretizeTrajectory(spline,discretization,trajectory_out.request.trajectory);
00380 
00381   trajectory_out.request.limits = trajectory_in.request.limits;
00382 
00383   printTrajectory(trajectory_out.request.trajectory);
00384         
00385   ROS_DEBUG("Final trajectory has %d points and %f total time",(int)trajectory_out.request.trajectory.points.size(),
00386             trajectory_out.request.trajectory.points.back().time_from_start.toSec());
00387   
00388   if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00389                                                           trajectory_out.request.trajectory,
00390                                                           trajectory_in.request.goal_constraints,
00391                                                           trajectory_in.request.path_constraints,
00392                                                           trajectory_out.response.error_code,
00393                                                           trajectory_error_codes,
00394                                                           false)) {
00395     ROS_INFO_STREAM("Final trajectory invalid with error code " << error_code.val);
00396     ROS_INFO_STREAM("Trajectory error codes size is " << trajectory_error_codes.size());
00397     return false;
00398   } else {
00399     ROS_DEBUG_STREAM("Final trajectory ok");
00400   }
00401 
00402   return success;
00403 }
00404 
00405 template <typename T>
00406 void CubicSplineShortCutter<T>::refineTrajectory(T &trajectory) const
00407 {
00408   if(trajectory.request.trajectory.points.size() < 3)
00409     return;
00410 
00411   for(unsigned int i=1; i < trajectory.request.trajectory.points.size()-1; i++)
00412   {
00413     for(unsigned int j=0; j < trajectory.request.trajectory.points[i].positions.size(); j++)
00414     {
00415       double dq_first = trajectory.request.trajectory.points[i].positions[j] - trajectory.request.trajectory.points[i-1].positions[j];
00416       double dq_second = trajectory.request.trajectory.points[i+1].positions[j] - trajectory.request.trajectory.points[i].positions[j];
00417       //double dq_dot = trajectory.request.trajectory.points[i].velocities[j];
00418       double dt_first = (trajectory.request.trajectory.points[i].time_from_start - trajectory.request.trajectory.points[i-1].time_from_start).toSec();
00419       double dt_second = (trajectory.request.trajectory.points[i+1].time_from_start - trajectory.request.trajectory.points[i].time_from_start).toSec();
00420       if( (dq_first > 0 && dq_second > 0) || (dq_first < 0 && dq_second < 0)) 
00421       {       
00422         if(trajectory.request.trajectory.points[i].velocities[j] == 0.0)
00423         {
00424           trajectory.request.trajectory.points[i].velocities[j] = 0.5*(dq_first/dt_first + dq_second/dt_second);
00425           trajectory.request.trajectory.points[i].velocities[j] = std::max(std::min(trajectory.request.trajectory.points[i].velocities[j],
00426                                                                                     trajectory.request.limits[j].max_velocity),
00427                                                                            -trajectory.request.limits[j].max_velocity);
00428         }
00429       }
00430     }
00431   }
00432 }
00433 
00434 template <typename T>
00435 void CubicSplineShortCutter<T>::printTrajectory(const trajectory_msgs::JointTrajectory &trajectory) const
00436 {
00437   for(unsigned int i = 0; i < trajectory.points.size(); i++)
00438   {
00439     ROS_DEBUG("%f: %f %f %f %f %f %f %f, %f %f %f %f %f %f %f, %f %f %f %f %f %f %f",
00440              trajectory.points[i].time_from_start.toSec(),
00441              trajectory.points[i].positions[0],
00442              trajectory.points[i].positions[1],
00443              trajectory.points[i].positions[2],
00444              trajectory.points[i].positions[3],
00445              trajectory.points[i].positions[4],
00446              trajectory.points[i].positions[5],
00447              trajectory.points[i].positions[6],
00448              trajectory.points[i].velocities[0],
00449              trajectory.points[i].velocities[1],
00450              trajectory.points[i].velocities[2],
00451              trajectory.points[i].velocities[3],
00452              trajectory.points[i].velocities[4],
00453              trajectory.points[i].velocities[5],
00454              trajectory.points[i].velocities[6],
00455              trajectory.points[i].accelerations[0],
00456              trajectory.points[i].accelerations[1],
00457              trajectory.points[i].accelerations[2],
00458              trajectory.points[i].accelerations[3],
00459              trajectory.points[i].accelerations[4],
00460              trajectory.points[i].accelerations[5],
00461              trajectory.points[i].accelerations[6]);
00462   }
00463   ROS_DEBUG(" ");
00464 }
00465 
00466 /*template <typename T>
00467 void CubicSplineShortCutter<T>::discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00468                                                      const double &discretization,
00469                                                      trajectory_msgs::JointTrajectory &joint_trajectory) const
00470 {
00471   double total_time;
00472   spline_smoother::getTotalTime(spline,total_time);
00473   std::vector<double> times;
00474   double time_index = 0.0;
00475   while(time_index < total_time)
00476   {
00477     times.push_back(time_index);
00478     time_index += discretization;
00479   }
00480   times.push_back(total_time);  
00481   spline_smoother::sampleSplineTrajectory(spline,times,joint_trajectory);
00482 }
00483 */
00484 
00485 template <typename T>
00486 void CubicSplineShortCutter<T>::discretizeTrajectory(const spline_smoother::SplineTrajectory &spline, 
00487                                                      const double &discretization,
00488                                                      trajectory_msgs::JointTrajectory &joint_trajectory) const
00489 {
00490   if(spline.segments.empty())
00491     return;
00492   joint_trajectory.points.clear();
00493   ros::Duration segment_start_time(0.0);
00494   for(unsigned int i=0; i < spline.segments.size(); i++)
00495   {
00496     if(i == spline.segments.size()-1)
00497       discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,true);
00498     else
00499       discretizeAndAppendSegment(spline.segments[i],discretization,joint_trajectory,segment_start_time,false);
00500     segment_start_time += spline.segments[i].duration;
00501     ROS_DEBUG("Discretizing and appending segment %d",i);
00502   }
00503 }
00504 
00505 template <typename T>
00506 void CubicSplineShortCutter<T>::discretizeAndAppendSegment(const spline_smoother::SplineTrajectorySegment &spline_segment,
00507                                                            const double &discretization,
00508                                                            trajectory_msgs::JointTrajectory &joint_trajectory,
00509                                                            const ros::Duration &segment_start_time,
00510                                                            const bool &include_segment_end) const
00511 {
00512   ros::Duration time_from_start = segment_start_time;
00513   double total_time = spline_segment.duration.toSec();
00514   double sample_time = 0.0;
00515   trajectory_msgs::JointTrajectoryPoint start,end;
00516   spline_smoother::sampleSplineTrajectory(spline_segment,0.0,start);
00517   if(joint_trajectory.points.empty())
00518   {
00519     start.time_from_start = ros::Duration(0.0);
00520     joint_trajectory.points.push_back(start);
00521     sample_time += 0.01;
00522   }
00523   start = joint_trajectory.points.back();
00524   while(sample_time < total_time)
00525   {
00526      ROS_DEBUG("Sample time is %f",sample_time);
00527      spline_smoother::sampleSplineTrajectory(spline_segment,sample_time,end);
00528      double max_diff = maxLInfDistance(start,end);
00529      if(sample_time > 0 && max_diff < discretization)
00530      {
00531        ROS_DEBUG("Max diff is %f. Skipping",max_diff);
00532        sample_time += 0.01;
00533        continue;
00534      }
00535      end.time_from_start = time_from_start + ros::Duration(sample_time);
00536      joint_trajectory.points.push_back(end);
00537      ROS_DEBUG("Pushing back point with time: %f",end.time_from_start.toSec());
00538      sample_time += 0.01;
00539      start = end;
00540   }
00541   if(include_segment_end)
00542   {
00543     spline_smoother::sampleSplineTrajectory(spline_segment,total_time,end);
00544     end.time_from_start = time_from_start + ros::Duration(total_time);
00545     joint_trajectory.points.push_back(end);
00546   }    
00547 }
00548 
00549 template <typename T>
00550 double CubicSplineShortCutter<T>::maxLInfDistance(const trajectory_msgs::JointTrajectoryPoint &start, 
00551                                                   const trajectory_msgs::JointTrajectoryPoint &end) const
00552 {
00553   double max_diff = 0.0;
00554   for(unsigned int i=0; i< start.positions.size(); i++)
00555   {
00556     double diff = fabs(end.positions[i]-start.positions[i]);
00557     if(diff > max_diff)
00558       max_diff = diff;
00559   }
00560   return max_diff;
00561 }
00562       
00563 
00564 
00565 template <typename T>
00566 bool CubicSplineShortCutter<T>::trimTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00567                                                const double &segment_start_time, 
00568                                                const double &segment_end_time) const
00569 {
00570   int index1;
00571   int index2;
00572   if(findTrajectoryPointsInInterval(trajectory_out,segment_start_time,segment_end_time,index1,index2))
00573   {
00574     ROS_DEBUG("Trimming trajectory between segments: %d and %d",index1,index2);
00575     std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_start = trajectory_out.points.begin() + index1;
00576     std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator remove_end;
00577     if((unsigned int) index2 >= trajectory_out.points.size())
00578       remove_end = trajectory_out.points.end();
00579     else
00580       remove_end = trajectory_out.points.begin()+index2;
00581       
00582 
00583     if(remove_start != remove_end)
00584       trajectory_out.points.erase(remove_start,remove_end);
00585     else
00586       trajectory_out.points.erase(remove_start);
00587   }
00588   else
00589     return false;
00590   return true;
00591 }
00592 
00593 template <typename T>
00594 bool CubicSplineShortCutter<T>::findTrajectoryPointsInInterval(const trajectory_msgs::JointTrajectory &trajectory,
00595                                                                const double &segment_start_time, 
00596                                                                const double &segment_end_time,
00597                                                                int &index_1,
00598                                                                int &index_2) const
00599 {
00600   index_1 = -1;
00601   index_2 = -1;
00602   if(segment_start_time > segment_end_time)
00603     return false;
00604   for(unsigned int i=0; i < trajectory.points.size(); i++)
00605     if(trajectory.points[i].time_from_start.toSec() >= segment_start_time)
00606     {
00607       index_1 = i;
00608       break;
00609     }
00610   ROS_DEBUG("First trim index: %d",index_1);
00611   if(index_1>=0)
00612     for(unsigned int i=index_1; i < trajectory.points.size(); i++)
00613     {
00614       if(trajectory.points[i].time_from_start.toSec() > segment_end_time)
00615       {
00616         index_2 = i;
00617         break;
00618       }
00619       if(trajectory.points[i].time_from_start.toSec() == segment_end_time)
00620       {
00621         index_2 = i+1;
00622         break;
00623       }
00624     }
00625   ROS_DEBUG("Second trim index: %d",index_2);
00626   if(index_1 >= index_2 || index_1 < 0 || index_2 < 0)
00627     return false;
00628   return true;
00629 }
00630 
00631 template <typename T>
00632 bool CubicSplineShortCutter<T>::getWaypoints(const spline_smoother::SplineTrajectory &spline, 
00633                                              trajectory_msgs::JointTrajectory &joint_trajectory) const
00634 {
00635   std::vector<double> waypoint_times_vector;
00636   double waypoint_time = 0.0;
00637   waypoint_times_vector.push_back(waypoint_time);
00638   for(unsigned int i=0; i < spline.segments.size(); i++)
00639   {
00640     waypoint_time = waypoint_time + spline.segments[i].duration.toSec();
00641     waypoint_times_vector.push_back(waypoint_time);
00642     ROS_DEBUG("Spline segment time: %f",spline.segments[i].duration.toSec());
00643   }
00644   if(!spline_smoother::sampleSplineTrajectory(spline,waypoint_times_vector,joint_trajectory))
00645     return false;
00646   return true;
00647 }
00648 
00649 template <typename T>
00650 bool CubicSplineShortCutter<T>::addToTrajectory(trajectory_msgs::JointTrajectory &trajectory_out, 
00651                                                 const trajectory_msgs::JointTrajectoryPoint &trajectory_point,
00652                                                 const ros::Duration &delta_time) const
00653 {
00654 
00655   ROS_DEBUG("Inserting point at time: %f",trajectory_point.time_from_start.toSec());
00656   ROS_DEBUG("Old trajectory has %u points",(unsigned int)trajectory_out.points.size());
00657 
00658   if(trajectory_out.points.empty())
00659   {
00660     trajectory_out.points.push_back(trajectory_point);
00661     return true;
00662   }
00663 
00664   unsigned int counter = 0;
00665   unsigned int old_size = trajectory_out.points.size();
00666   for(std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator iter = trajectory_out.points.begin(); 
00667       iter != trajectory_out.points.end() ; iter++)
00668   {   
00669     if(iter->time_from_start >= trajectory_point.time_from_start)
00670     {
00671       trajectory_out.points.insert(iter,trajectory_point);
00672       break;
00673     }
00674     counter++;
00675   }
00676 
00677   if(delta_time == ros::Duration(0.0))
00678     return true;
00679 
00680   if(counter == old_size)
00681     trajectory_out.points.push_back(trajectory_point);
00682   else
00683     if(counter+1 < trajectory_out.points.size())
00684       for(unsigned int i= counter+1; i < trajectory_out.points.size(); i++)
00685       {
00686         trajectory_out.points[i].time_from_start += delta_time;
00687       } 
00688   return true;
00689 }
00690 
00691 template <typename T>
00692 bool CubicSplineShortCutter<T>::setupCollisionEnvironment()
00693 {
00694   bool use_collision_map;
00695   ros::NodeHandle node_handle("~");
00696   node_handle.param<bool>("use_collision_map", use_collision_map, true);
00697 
00698   // monitor robot
00699   collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00700   if(!collision_models_interface_->loadedModels())
00701     return false;
00702 
00703   return true;
00704 }
00705 }
00706 
00707 #endif /* CUBIC_SPLINE_SMOOTHER_H_ */


constraint_aware_spline_smoother
Author(s): Sachin Chitta
autogenerated on Mon Dec 2 2013 12:37:23