lspb_trajectory.cpp
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 #include <spline_smoother/lspb_trajectory.h>
00038 
00039 namespace spline_smoother
00040 {
00041   LSPBTrajectory::LSPBTrajectory()
00042   {
00043     apply_limits_ = true;
00044   }
00045 
00046   double LSPBTrajectory::calculateMinimumTime(const trajectory_msgs::JointTrajectoryPoint &start, 
00047                                               const trajectory_msgs::JointTrajectoryPoint &end, 
00048                                               const std::vector<arm_navigation_msgs::JointLimits> &limits)
00049   {
00050     double minJointTime(MAX_ALLOWABLE_TIME);
00051     double segmentTime(0);
00052     int num_joints = (int) start.positions.size();
00053 
00054     for(int i = 0; i < num_joints; i++)
00055     {
00056       minJointTime = minSegmentTime(start.positions[i],end.positions[i],start.velocities[i],end.velocities[i],limits[i]);
00057       if(segmentTime < minJointTime)
00058         segmentTime = minJointTime;
00059     }
00060     return segmentTime;
00061   }
00062 
00063 /*  double LSPBTrajectory::minSegmentTime(double q0, double q1, double v0, double v1, manipulation_msgs::Limits limit)
00064   {
00065     double vmax = limit.max_velocity;
00066     double amax = limit.max_acceleration;
00067     double diff = jointDiff(q0,q1,limit);
00068 
00069     double tb = std::max(fabs(vmax/amax),sqrt(fabs(diff)/amax));
00070 
00071     double acc(0);
00072     if(diff>0)
00073       acc = amax;
00074     else
00075       acc = -amax;
00076     double dist_tb = acc*tb*tb;
00077     double ts = (diff - dist_tb)/(acc*tb);
00078     if(ts < 0)
00079       ts = 0;
00080 
00081     return (2*tb+ts);
00082   }
00083 */
00084 
00085   double LSPBTrajectory::minSegmentTime(const double &q0, 
00086                                         const double &q1, 
00087                                         const double &v0, 
00088                                         const double &v1, 
00089                                         const arm_navigation_msgs::JointLimits &limit)
00090   {
00091     double vmax = limit.max_velocity;
00092     double amax = limit.max_acceleration;
00093     double diff = jointDiff(q0,q1,limit);
00094 
00095     double tb = fabs(vmax/amax);
00096 
00097     double acc(0);
00098     if(diff>0)
00099       acc = amax;
00100     else
00101       acc = -amax;
00102     double dist_tb = acc*tb*tb;
00103     double ts = (diff - dist_tb)/(acc*tb);
00104     if(ts < 0)
00105     {
00106       ts = 0.0;
00107     }
00108     return (2*tb+ts);
00109   }
00110 
00111 
00112   double LSPBTrajectory::blendTime(const double &aa,
00113                                    const double &bb,
00114                                    const double &cc)
00115   {
00116     double disc = (pow(bb,2) - 4*aa*cc);
00117     if(disc < 0)
00118     {   
00119       ROS_DEBUG("Blend time quadratic coeff: %f %f %f",aa,bb,cc);
00120       return 0.0;
00121     }
00122     double tb1 = (-bb + sqrt(disc))/(2*aa);
00123     double tb2 = (-bb - sqrt(disc))/(2*aa);
00124     if(std::isnan(tb1))
00125       tb1 = 0.0;
00126     if(std::isnan(tb2))
00127       tb2 = 0.0;
00128     return std::min(tb1,tb2);
00129   }
00130 
00131 /*  double LSPBTrajectory::blendTime(double diff,double a,double tf)
00132   {
00133     return fabs(diff/(a*tf));
00134   }
00135 */
00136   bool LSPBTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in,
00137                                     const std::vector<arm_navigation_msgs::JointLimits>& limits,
00138                                     spline_smoother::LSPBTrajectoryMsg& spline)
00139   {
00140     int num_traj = trajectory_in.points.size();
00141     int num_joints = trajectory_in.joint_names.size();
00142     spline.names = trajectory_in.joint_names;
00143     spline.segments.resize(num_traj-1);
00144 
00145     for(int i=0; i<num_joints; i++)
00146     {
00147       if(!limits[i].has_velocity_limits)
00148       {
00149         ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00150         return false;
00151       }
00152       if(!limits[i].has_acceleration_limits)
00153       {
00154         ROS_ERROR("Trying to apply acceleration limits without supplying them. Set acceleration_limits in the message and set has_acceleration_limits flag to true.");
00155         return false;
00156       }
00157     }
00158     for (int i=1; i< num_traj; ++i)
00159     {
00160       spline.segments[i-1].joints.resize(num_joints);
00161       for(unsigned int j=0; j < spline.segments[i-1].joints.size(); j++)
00162         spline.segments[i-1].joints[j].coefficients.resize(3);
00163 
00164       double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00165       if(apply_limits_)
00166       {
00167         double dTMin = calculateMinimumTime(trajectory_in.points[i-1],trajectory_in.points[i],limits);
00168         if(dTMin > dT) // if minimum time required to satisfy limits is greater than time available, stretch this segment
00169           dT = dTMin;      
00170       }
00171       spline.segments[i-1].duration = ros::Duration(dT);
00172       for(int j=0; j<num_joints; j++)
00173       {
00174         double diff = jointDiff(trajectory_in.points[i-1].positions[j],trajectory_in.points[i].positions[j],limits[j]);
00175         double acc = 0.0;
00176         if(diff > 0)
00177           acc = limits[j].max_acceleration;
00178         else
00179           acc = - limits[j].max_acceleration;
00180         double tb = blendTime(acc,-acc*dT,diff);
00181 //        double tb = blendTime(diff,acc,dT);
00182 
00183         spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j];
00184         spline.segments[i-1].joints[j].coefficients[1] = 0.0;
00185         spline.segments[i-1].joints[j].coefficients[2] = 0.5*acc;
00186         spline.segments[i-1].joints[j].quadratic_segment_duration = tb;
00187         spline.segments[i-1].joints[j].linear_segment_duration = std::max(dT-2*tb,0.0);
00188       }
00189     }
00190     return true;
00191   }
00192 }


spline_smoother
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:09:50