trajectory_generation.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************       
00003  * Software License Agreement (BSD License)                                  
00004  *                                                                           
00005  *  Copyright (c) 2008, Willow Garage, Inc.                                  
00006  *  All rights reserved.                                                     
00007  *                                                                           
00008  *  Redistribution and use in source and binary forms, with or without       
00009  *  modification, are permitted provided that the following conditions       
00010  *  are met:                                                                 
00011  *                                                                           
00012  *   * Redistributions of source code must retain the above copyright        
00013  *     notice, this list of conditions and the following disclaimer.         
00014  *   * Redistributions in binary form must reproduce the above                
00015  *     copyright notice, this list of conditions and the following           
00016  *     disclaimer in the documentation and/or other materials provided       
00017  *     with the distribution.                                                
00018  *   * Neither the name of Willow Garage nor the names of its                
00019  *     contributors may be used to endorse or promote products derived       
00020  *     from this software without specific prior written permission.         
00021  *                                                                           
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS        
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE           
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,      
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,     
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;         
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER         
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT       
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN        
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE          
00033  *  POSSIBILITY OF SUCH DAMAGE.                                              
00034  *
00035  * Author: Wim Meeussen
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 #include <joint_trajectory_generator/trajectory_generation.h>
00040 #include <kdl/velocityprofile_trap.hpp>
00041 
00042 
00043 namespace trajectory{
00044 
00045   TrajectoryGenerator::TrajectoryGenerator(double max_vel, double max_acc, unsigned int size)
00046     : generators_(size)
00047   {
00048     for (unsigned int i=0; i<size; i++)
00049       generators_[i] = new KDL::VelocityProfile_Trap(max_vel, max_acc);
00050   }
00051 
00052 
00053   TrajectoryGenerator::~TrajectoryGenerator()
00054   {
00055     for (unsigned int i=0; i<generators_.size(); i++)
00056       delete generators_[i];
00057   }
00058 
00059   void TrajectoryGenerator::generate(const trajectory_msgs::JointTrajectory& traj_in, trajectory_msgs::JointTrajectory& traj_out)
00060   {
00061     ROS_DEBUG("Generating trajectory for list of points of size %d", (int) traj_in.points.size());
00062 
00063     // check trajectory message
00064     if (traj_in.points.size() < 2){
00065       ROS_WARN("Trajectory message should contain at least two points, but it contains %d points. Returning original trajectory", (int)traj_in.points.size());
00066       traj_out = traj_in;
00067       throw ros::Exception("Trajectory contains fewer than two points.");
00068     }
00069 
00070     // default result
00071     trajectory_msgs::JointTrajectory traj_res = traj_in;
00072     traj_res.points.clear();
00073     trajectory_msgs::JointTrajectoryPoint points_tmp; 
00074     points_tmp.positions.resize(generators_.size());
00075     points_tmp.velocities.resize(generators_.size());
00076     points_tmp.accelerations.resize(generators_.size());
00077 
00078     double initial_time = traj_in.points[0].time_from_start.toSec();
00079 
00080     for (unsigned int pnt=0; pnt<traj_in.points.size()-1; pnt++){
00081       // check
00082       if (traj_in.points[pnt].positions.size() != generators_.size() ||
00083           traj_in.points[pnt+1].positions.size() != generators_.size()){
00084         ROS_ERROR("The point lists in the trajectory do not have the same size as the generators");
00085         return;
00086       }
00087 
00088       // generate initial profiles
00089       for (unsigned int i=0; i<generators_.size(); i++)
00090         generators_[i]->SetProfile(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i]);
00091 
00092       // find profile that takes most time
00093       double max_time = (traj_in.points[pnt+1].time_from_start - traj_in.points[pnt].time_from_start).toSec();
00094       for (unsigned int i=0; i<generators_.size(); i++)
00095         if (generators_[i]->Duration() > max_time)
00096           max_time = generators_[i]->Duration();
00097 
00098       // generate profiles with max time
00099       for (unsigned int i=0; i<generators_.size(); i++)
00100         generators_[i]->SetProfileDuration(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i], max_time);
00101 
00102       // copy results in trajectory message
00103       unsigned int steps = fmax(10, (unsigned int)(max_time / 0.1));
00104       double time = 0;
00105       for (unsigned int s=0; s<=steps; s++){
00106         for (unsigned int i=0; i<generators_.size(); i++){
00107           points_tmp.positions[i] = generators_[i]->Pos(time);
00108           points_tmp.velocities[i] = generators_[i]->Vel(time);
00109           points_tmp.accelerations[i] = generators_[i]->Acc(time);
00110         }
00111         points_tmp.time_from_start = ros::Duration(initial_time) + ros::Duration(time);
00112         traj_res.points.push_back(points_tmp);
00113         time += max_time/(double)steps;
00114       }
00115       initial_time += max_time;
00116     }
00117     traj_out = traj_res;
00118   }
00119 
00120 
00121 
00122 }


joint_trajectory_generator
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Wed Sep 16 2015 10:39:25