constraint_aware_spline_smoother: parabolic_blend

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/monitors/planning_monitor.h>
00045 #include <motion_planning_msgs/RobotState.h>
00046 #include <motion_planning_msgs/ArmNavigationErrorCodes.h>
00047 #include <motion_planning_msgs/LinkPadding.h>
00048 
00049 #include <trajectory_msgs/JointTrajectoryPoint.h>
00050 
00051 #include <constraint_aware_spline_smoother/Math.h>
00052 #include <constraint_aware_spline_smoother/DynamicPath.h>
00053 
00054 namespace constraint_aware_spline_smoother
00055 {
00056 static const double MIN_DELTA = 0.01;
00057 
00058 class FeasibilityChecker : public FeasibilityCheckerBase
00059 {
00060 public: 
00061   FeasibilityChecker();
00062   virtual bool ConfigFeasible(const Vector& x);
00063   virtual bool SegmentFeasible(const Vector& a,const Vector& b);
00064   bool setInitial(const trajectory_msgs::JointTrajectory &trajectory,
00065                   const motion_planning_msgs::OrderedCollisionOperations &ordered_collision_operations,
00066                   const std::vector<motion_planning_msgs::AllowedContactSpecification> &allowed_contact_regions,
00067                   const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
00068                   const motion_planning_msgs::Constraints &path_constraints);
00069   void resetRequest();
00070   bool isActive();
00071   void initialize();
00072 private:
00073   std::vector<std::string> joint_names_;
00074   bool active_;
00075   double discretization_;
00076   tf::TransformListener tf_;
00077   ros::NodeHandle node_handle_;
00078   bool setupCollisionEnvironment();
00079   planning_environment::CollisionModels *collision_models_;
00080   planning_environment::PlanningMonitor *planning_monitor_;    
00081   void discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00082                             trajectory_msgs::JointTrajectory &trajectory_out);
00083 };
00084 
00085 FeasibilityChecker::FeasibilityChecker() : FeasibilityCheckerBase(), node_handle_("~")
00086 {
00087   initialize();
00088 }
00089 
00090 bool FeasibilityChecker::isActive()
00091 {
00092   return active_;
00093 }
00094 
00095 void FeasibilityChecker::initialize()
00096 {
00097   if(!setupCollisionEnvironment())
00098   {
00099     ROS_ERROR("Could not setup collision environment");
00100     active_ = false;
00101   }
00102   else
00103   {
00104     ROS_INFO("Setup collision environment");
00105     active_ = true;
00106   }
00107 }
00108 
00109 bool FeasibilityChecker::setInitial(const trajectory_msgs::JointTrajectory &trajectory,
00110                                     const motion_planning_msgs::OrderedCollisionOperations &ordered_collision_operations,
00111                                     const std::vector<motion_planning_msgs::AllowedContactSpecification> &allowed_contact_regions,
00112                                     const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
00113                                     const motion_planning_msgs::Constraints &path_constraints)
00114 {
00115   std::vector<std::string> child_links;
00116   motion_planning_msgs::ArmNavigationErrorCodes error_code;
00117   motion_planning_msgs::OrderedCollisionOperations operations;
00118 
00119   joint_names_ = trajectory.joint_names;
00120 
00121   motion_planning_msgs::Constraints emp;
00122   
00123   planning_monitor_->prepareForValidityChecks(joint_names_,
00124                                               ordered_collision_operations,
00125                                               allowed_contact_regions,
00126                                               path_constraints,
00127                                               emp,
00128                                               link_padding, 
00129                                               error_code); 
00130   if(error_code.val != error_code.SUCCESS)
00131   {
00132     ROS_ERROR("Could not set path constraints");
00133     return false;
00134   }
00135   return true;
00136 }
00137 
00138 void FeasibilityChecker::resetRequest()
00139 {
00140   planning_monitor_->revertToDefaultState();
00141 }
00142 
00143 bool FeasibilityChecker::setupCollisionEnvironment()
00144 {
00145   bool use_collision_map;
00146   node_handle_.param<bool>("use_collision_map", use_collision_map, true);
00147   
00148   // monitor robot
00149   collision_models_ = new planning_environment::CollisionModels("robot_description");
00150   planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_);
00151   planning_monitor_->setUseCollisionMap(use_collision_map);
00152   if(!collision_models_->loadedModels())
00153     return false;
00154   if (planning_monitor_->getExpectedJointStateUpdateInterval() > 1e-3)
00155     planning_monitor_->waitForState();
00156 
00157   planning_monitor_->startEnvironmentMonitor();
00158 
00159   if (planning_monitor_->getExpectedMapUpdateInterval() > 1e-3 && use_collision_map)
00160     planning_monitor_->waitForMap();
00161   return true;
00162 }
00163 
00164 void FeasibilityChecker::discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00165                                               trajectory_msgs::JointTrajectory &trajectory_out)
00166 {    
00167   trajectory_out.joint_names = trajectory.joint_names;
00168   for(unsigned int i=1; i < trajectory.points.size(); i++)
00169   {
00170     double diff = 0.0;      
00171     for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00172     {
00173       double start = trajectory.points[i-1].positions[j];
00174       double end   = trajectory.points[i].positions[j];
00175       if(fabs(end-start) > diff)
00176         diff = fabs(end-start);        
00177     }
00178     int num_intervals =(int) (diff/MIN_DELTA+1.0);
00179       
00180     for(unsigned int k=0; k < (unsigned int) num_intervals; k++)
00181     {
00182       trajectory_msgs::JointTrajectoryPoint point;
00183       for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00184       {
00185         double start = trajectory.points[i-1].positions[j];
00186         double end   = trajectory.points[i].positions[j];
00187         point.positions.push_back(start + (end-start)*k/num_intervals);
00188       }
00189       point.time_from_start = ros::Duration(trajectory.points[i].time_from_start.toSec() + k* (trajectory.points[i].time_from_start - trajectory.points[i-1].time_from_start).toSec()/num_intervals);
00190       trajectory_out.points.push_back(point);
00191     }
00192   }
00193   trajectory_out.points.push_back(trajectory.points.back());
00194 }
00195 
00196 bool FeasibilityChecker::ConfigFeasible(const Vector& x)
00197 {
00198   motion_planning_msgs::ArmNavigationErrorCodes error_code;
00199   std::vector<motion_planning_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00200   motion_planning_msgs::RobotState robot_state;
00201   planning_monitor_->getCurrentRobotState(robot_state);
00202 
00203   trajectory_msgs::JointTrajectory joint_traj;
00204   joint_traj.joint_names = joint_names_;
00205   joint_traj.header.stamp = ros::Time::now();
00206   joint_traj.points.resize(1);
00207   joint_traj.points[0].positions = x;
00208   return planning_monitor_->isTrajectoryValid(joint_traj,robot_state,0,joint_traj.points.size(),
00209                                               planning_environment::PlanningMonitor::COLLISION_TEST | planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST,
00210                                               false,error_code,trajectory_error_codes);    
00211 }
00212 
00213 bool FeasibilityChecker::SegmentFeasible(const Vector& a,const Vector& b)
00214 {
00215   motion_planning_msgs::ArmNavigationErrorCodes error_code;
00216   std::vector<motion_planning_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00217   motion_planning_msgs::RobotState robot_state;
00218   //empty state is ok
00219 
00220   trajectory_msgs::JointTrajectory joint_traj_in, joint_traj;
00221   joint_traj_in.joint_names = joint_names_;
00222   joint_traj.header.stamp = ros::Time::now();
00223   joint_traj_in.points.resize(2);
00224   joint_traj_in.points[0].positions = a;
00225   joint_traj_in.points[1].positions = b;
00226   joint_traj.joint_names = joint_traj_in.joint_names;
00227   discretizeTrajectory(joint_traj_in,joint_traj);
00228   return planning_monitor_->isTrajectoryValid(joint_traj,robot_state,0,joint_traj.points.size(),
00229                                               planning_environment::PlanningMonitor::COLLISION_TEST | planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST,
00230                                               false,error_code,trajectory_error_codes);
00231 }
00232 
00236 template <typename T>
00237 class ParabolicBlendShortCutter: public spline_smoother::SplineSmoother<T>
00238 {
00239 public:
00243   ParabolicBlendShortCutter();
00244   virtual ~ParabolicBlendShortCutter();
00245   virtual bool smooth(const T& trajectory_in, 
00246                       T& trajectory_out) const;
00249   virtual bool configure();
00250 private:
00251   int num_iterations_;
00252   double discretization_;
00253   bool active_;
00254   boost::shared_ptr<FeasibilityChecker> feasibility_checker_;
00255 };
00256 
00257 template <typename T>
00258 bool ParabolicBlendShortCutter<T>::configure()
00259 {
00260   if (!spline_smoother::SplineSmoother<T>::getParam("discretization", discretization_))
00261   {
00262     ROS_ERROR("Spline smoother, \"%s\", params has no attribute discretization.", spline_smoother::SplineSmoother<T>::getName().c_str());
00263     return false;
00264   }
00265   if (!spline_smoother::SplineSmoother<T>::getParam("num_iterations", num_iterations_))
00266   {
00267     ROS_ERROR("Spline smoother, \"%s\", params has no attribute num_iterations.", spline_smoother::SplineSmoother<T>::getName().c_str());
00268     return false;
00269   }
00270   ROS_INFO("Configuring parabolic blend short cutter");
00271   ROS_INFO("Using a discretization value of %f",discretization_);
00272   ROS_INFO("Using num_iterations value of %d",(int)num_iterations_);
00273   feasibility_checker_.reset(new constraint_aware_spline_smoother::FeasibilityChecker());
00274   return true;
00275 };
00276 
00277 template <typename T>
00278 ParabolicBlendShortCutter<T>::ParabolicBlendShortCutter()
00279 {
00280   ROS_INFO("Setting up parabolic blend short cutter");
00281 }
00282 
00283 template <typename T>
00284 ParabolicBlendShortCutter<T>::~ParabolicBlendShortCutter()
00285 {
00286 }
00287 
00288 template <typename T>
00289 bool ParabolicBlendShortCutter<T>::smooth(const T& trajectory_in, 
00290                                           T& trajectory_out) const
00291 {
00292   srand(time(NULL)); // initialize random seed: 
00293   if(!feasibility_checker_->isActive())
00294   {
00295     ROS_ERROR("Smoother is not active");
00296     return false;
00297   }
00298   
00299   feasibility_checker_->setInitial(trajectory_in.trajectory,
00300                                    trajectory_in.ordered_collision_operations,
00301                                    trajectory_in.allowed_contacts,
00302                                    trajectory_in.link_padding,
00303                                    trajectory_in.path_constraints);
00304   std::vector<Vector> path;        //the sequence of milestones
00305   Vector vmax,amax;           //velocity and acceleration bounds, respectively
00306   Real tol=1e-4;              //if a point is feasible, any point within tol is considered acceptable
00307   //TODO: compute milestones, velocity and acceleration bounds
00308 
00309   vmax.resize(trajectory_in.limits.size());
00310   amax.resize(trajectory_in.limits.size());
00311 
00312   for(unsigned int i=0; i < trajectory_in.limits.size(); i++)
00313   {
00314     vmax[i] = trajectory_in.limits[i].max_velocity;
00315     amax[i] = trajectory_in.limits[i].max_acceleration;
00316   }
00317 
00318   for(unsigned int i=0; i<trajectory_in.trajectory.points.size(); i++)
00319   {
00320     path.push_back(trajectory_in.trajectory.points[i].positions);
00321   }                   
00322 
00323   DynamicPath traj;
00324   traj.Init(vmax,amax);
00325   traj.SetMilestones(path);   //now the trajectory starts and stops at every milestone
00326   ROS_DEBUG("Initial path duration: %g\n",(double)traj.GetTotalTime());
00327   int res=traj.Shortcut(num_iterations_,feasibility_checker_.get(),tol);
00328   ROS_DEBUG("After shortcutting: duration %g\n",(double)traj.GetTotalTime());
00329   unsigned int num_points = (unsigned int)(traj.GetTotalTime()/discretization_+0.5) + 1;
00330   double totalTime = (double) traj.GetTotalTime();
00331   for(unsigned int i=0; i < num_points; i++)
00332   {
00333     Vector x, dx;
00334     double t = i*totalTime/(num_points-1);
00335     traj.Evaluate(t,x);
00336     traj.Derivative(t,dx);
00337     trajectory_msgs::JointTrajectoryPoint point;
00338     point.positions = x;
00339     point.velocities = dx;
00340     point.time_from_start = ros::Duration(t);
00341     trajectory_out.trajectory.points.push_back(point);
00342   }
00343   trajectory_out.trajectory.joint_names = trajectory_in.trajectory.joint_names;
00344   feasibility_checker_->resetRequest();
00345   return res;
00346 }
00347 
00348 }
00349 
00350 #endif /* CUBIC_SPLINE_SMOOTHER_H_ */
parabolic_blend_shortcutter.h
