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