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 <pr2_arm_move_ik/trajectory_generation.h>
00039 #include <kdl/velocityprofile_trap.hpp>
00040 
00041 
00042 namespace trajectory{
00043 
00044   TrajectoryGenerator::TrajectoryGenerator(double max_vel, double max_acc, unsigned int size)
00045     : generators_(size)
00046   {
00047     for (unsigned int i=0; i<size; i++)
00048       generators_[i] = new KDL::VelocityProfile_Trap(max_vel, max_acc);
00049   }
00050 
00051 
00052   TrajectoryGenerator::~TrajectoryGenerator()
00053   {
00054     for (unsigned int i=0; i<generators_.size(); i++)
00055       delete generators_[i];
00056   }
00057 
00058   void TrajectoryGenerator::generate(const trajectory_msgs::JointTrajectory& traj_in, trajectory_msgs::JointTrajectory& traj_out)
00059   {
00060     ROS_INFO("Generating trajectory for list of points of size %d", (int) traj_in.points.size());
00061 
00062     // check trajectory message
00063     if (traj_in.points.size() < 2){
00064       ROS_WARN("Trajectory message should contain at least two points, but it contains %d joints. Returning original trajectory", (int)traj_in.points.size());
00065       traj_out = traj_in;
00066       return;
00067     }
00068 
00069     // default result
00070     trajectory_msgs::JointTrajectory traj_res = traj_in;
00071     traj_res.points.clear();
00072     trajectory_msgs::JointTrajectoryPoint points_tmp; 
00073     points_tmp.positions.resize(generators_.size());
00074     points_tmp.velocities.resize(generators_.size());
00075     points_tmp.accelerations.resize(generators_.size());
00076 
00077     double initial_time = traj_in.points[0].time_from_start.toSec();
00078 
00079     for (unsigned int pnt=0; pnt<traj_in.points.size()-1; pnt++){
00080       // check
00081       if (traj_in.points[pnt].positions.size() != generators_.size() ||
00082           traj_in.points[pnt+1].positions.size() != generators_.size()){
00083         ROS_ERROR("The point lists in the trajectory do not have the same size as the generators");
00084         return;
00085       }
00086 
00087       // generate initial profiles
00088       for (unsigned int i=0; i<generators_.size(); i++)
00089         generators_[i]->SetProfile(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i]);
00090 
00091       // find profile that takes most time
00092       double max_time = (traj_in.points[pnt+1].time_from_start - traj_in.points[pnt].time_from_start).toSec();
00093       for (unsigned int i=0; i<generators_.size(); i++)
00094         if (generators_[i]->Duration() > max_time)
00095           max_time = generators_[i]->Duration();
00096 
00097       // generate profiles with max time
00098       for (unsigned int i=0; i<generators_.size(); i++)
00099         generators_[i]->SetProfileDuration(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i], max_time);
00100 
00101       // copy results in trajectory message
00102       unsigned int steps = fmax(10, (unsigned int)(max_time / 0.1));
00103       double time = 0;
00104       for (unsigned int s=0; s<=steps; s++){
00105         for (unsigned int i=0; i<generators_.size(); i++){
00106           points_tmp.positions[i] = generators_[i]->Pos(time);
00107           points_tmp.velocities[i] = generators_[i]->Vel(time);
00108           points_tmp.accelerations[i] = generators_[i]->Acc(time);
00109         }
00110         points_tmp.time_from_start = ros::Duration(initial_time) + ros::Duration(time);
00111         traj_res.points.push_back(points_tmp);
00112         time += max_time/(double)steps;
00113       }
00114       initial_time += max_time;
00115     }
00116     traj_out = traj_res;
00117   }
00118 
00119 
00120 
00121 }


pr2_arm_move_ik
Author(s): Melonee Wise, Wim Meeussen
autogenerated on Tue Apr 22 2014 19:29:55