parabolic_linear_blend.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 <constraint_aware_spline_smoother/KunzStilman/Trajectory.h>
00038 #include <constraint_aware_spline_smoother/parabolic_linear_blend.h>
00039 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00040 #include <arm_navigation_msgs/JointLimits.h>
00041 #include <Eigen/Core>
00042 
00043 using namespace constraint_aware_spline_smoother;
00044 
00045 const double    DEFAULT_VEL_MAX=1.0;
00046 const double    DEFAULT_ACCEL_MAX=1.0;
00047 const double    ROUNDING_THRESHOLD = 0.01;
00048 const int                       NUM_OUTPUT_POINTS = 100;        // TODO - make this a parameter
00049 
00050 
00051 template <typename T>
00052 bool ParabolicLinearBlendSmoother<T>::smooth(const T& trajectory_in,
00053                                    T& trajectory_out) const
00054 {
00055   std::list<Eigen::VectorXd> path;
00056   Eigen::VectorXd vmax; // velocity
00057   Eigen::VectorXd amax; // acceleration
00058   double smin = 0.03;           // minimum waypoint separation
00059 
00060   // Convert to expected form
00061   vmax.resize(trajectory_in.request.limits.size());
00062   amax.resize(trajectory_in.request.limits.size());
00063 
00064   // limits
00065   for(unsigned int i=0; i<trajectory_in.request.limits.size(); i++)
00066   {
00067     if( trajectory_in.request.limits[i].has_velocity_limits )
00068     {
00069       vmax[i] = trajectory_in.request.limits[i].max_velocity;
00070     }
00071     else
00072     {
00073       vmax[i] = DEFAULT_VEL_MAX;
00074     }
00075     if( trajectory_in.request.limits[i].has_acceleration_limits )
00076     {
00077       amax[i] = trajectory_in.request.limits[i].max_acceleration;
00078     }
00079     else
00080     {
00081       amax[i] = DEFAULT_ACCEL_MAX;
00082     }
00083   }
00084 
00085 
00086   // trajectory points
00087   for(unsigned int i=0; i<trajectory_in.request.trajectory.points.size(); i++)
00088   {
00089     Eigen::VectorXd point;
00090     point.resize(trajectory_in.request.trajectory.points[i].positions.size());
00091     for(unsigned int j=0; j<trajectory_in.request.trajectory.points[i].positions.size(); j++)
00092     {
00093       point[j]=trajectory_in.request.trajectory.points[i].positions[j];
00094     }
00095     std::vector<double> positions=trajectory_in.request.trajectory.points[i].positions;
00096     path.push_back(point);
00097   }
00098 
00099   ParabolicBlend::Trajectory blend_trajectory(path,vmax,amax,smin);
00100 
00101   // Convert back
00102   trajectory_out = trajectory_in;
00103   unsigned int num_joints = trajectory_in.request.trajectory.joint_names.size();
00104   unsigned int num_points = NUM_OUTPUT_POINTS;
00105   double duration = blend_trajectory.getDuration();
00106   double discretization = duration / num_points;
00107 
00108   // add on the new trjectory points
00109   trajectory_out.request.trajectory.points.clear();
00110   for (unsigned int i=0; i<=num_points; ++i)
00111   {
00112    double time_from_start = discretization * i;
00113     if(i==0) time_from_start=0.0;       //safety check.
00114 
00115     Eigen::VectorXd positions = blend_trajectory.getPosition( time_from_start );
00116     Eigen::VectorXd velocities = blend_trajectory.getVelocity( time_from_start );
00117 
00118     trajectory_msgs::JointTrajectoryPoint point;
00119     point.time_from_start = ros::Duration(time_from_start);
00120     point.positions.resize(num_joints);
00121     point.velocities.resize(num_joints);
00122     for (unsigned int j=0; j<num_joints; ++j)
00123     {
00124       point.positions[j] = positions[j];
00125       point.velocities[j] = velocities[j];
00126     }
00127 
00128     trajectory_out.request.trajectory.points.push_back(point);  // TODO - can make this faster by initializing the size
00129   }
00130 
00131   return true;
00132 }
00133 
00134 
00135 PLUGINLIB_DECLARE_CLASS(constraint_aware_spline_smoother,
00136                         ParabolicLinearBlendFilterJointTrajectoryWithConstraints,
00137                         constraint_aware_spline_smoother::ParabolicLinearBlendSmoother<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
00138                         filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>)


constraint_aware_spline_smoother
Author(s): Sachin Chitta
autogenerated on Mon Dec 2 2013 12:37:23