test_cubic_parameterized.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 <ros/ros.h>
00038 #include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
00039 #include <spline_smoother/cubic_parameterized_trajectory.h>
00040 
00041 double gen_rand(double min, double max)
00042 {
00043   int rand_num = rand()%100+1;
00044   double result = min + (double)((max-min)*rand_num)/101.0;
00045   return result;
00046 }
00047 
00048 
00049 //TEST(TestCubicParameterizedTrajectory, TestWithAccelerationLimits2)
00050 void run_test(int num_times)
00051 {
00052   spline_smoother::CubicParameterizedTrajectory traj;
00053   srand ( time(NULL) ); // initialize random seed: 
00054 
00055   bool success, ss;
00056   double total_time;
00057 
00058   // create the input:
00059   int length = 2;
00060   int joints = 1;
00061 
00062   arm_navigation_msgs::JointTrajectoryWithLimits wpt;
00063   wpt.trajectory.points.resize(length);
00064   wpt.trajectory.joint_names.resize(joints);
00065   wpt.trajectory.joint_names[0] = std::string("test0");
00066 
00067   wpt.limits.resize(joints);
00068   wpt.limits[0].has_velocity_limits = 1;
00069   wpt.limits[0].has_acceleration_limits = 1;
00070   wpt.limits[0].max_velocity = 1.0;
00071   wpt.limits[0].max_acceleration = 0.5;
00072 
00073   for (int i=0; i<length; i++)
00074   {
00075     wpt.trajectory.points[i].positions.resize(joints);
00076     wpt.trajectory.points[i].velocities.resize(joints);
00077     wpt.trajectory.points[i].accelerations.resize(joints);
00078     wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00079   }
00080   spline_smoother::SplineTrajectory spline;
00081   double eps = 1e-1;
00082   for(int i=0; i < num_times; i++)
00083   {
00084     //24.752475, -66.336634, velocities: 0.000000, 0.000000
00085     //[ INFO] [WallTime: 1285288331.834028213]: Limits: 14.851485, 6.930693
00086     wpt.trajectory.points[0].positions[0] = -24.752475;
00087     wpt.trajectory.points[1].positions[0] = 66.336634;
00088     wpt.limits[0].max_velocity = 14.851485;
00089     wpt.limits[0].max_acceleration =  6.930693;
00090     wpt.trajectory.points[0].velocities[0] = 0.0;
00091     wpt.trajectory.points[1].velocities[0] = 0.0;
00092     if(wpt.trajectory.points[0].positions[0] == wpt.trajectory.points[1].positions[0])
00093       continue;
00094     success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00095   
00096     ss = spline_smoother::getTotalTime(spline,total_time);
00097     std::vector<double> test_times;
00098     double dT = 0.0;
00099     while(dT < total_time)
00100     {
00101       test_times.push_back(dT);
00102       dT += 0.01;
00103     }
00104     test_times.push_back(total_time);
00105     trajectory_msgs::JointTrajectory test_trajectory;
00106     spline_smoother::sampleSplineTrajectory(spline,test_times,test_trajectory);
00107     
00108     for(unsigned int i=0; i < test_trajectory.points.size(); i++)
00109     {
00110       double vel_error = fabs(test_trajectory.points[i].velocities[0]) - wpt.limits[0].max_velocity;
00111       double acc_error = fabs(test_trajectory.points[i].accelerations[0]) - wpt.limits[0].max_acceleration;
00112       if(!(vel_error <= eps) || isnan(vel_error))
00113       {
00114         ROS_INFO("Time: %f",test_trajectory.points[i].time_from_start.toSec());
00115         ROS_INFO("Actual: %f %f",test_trajectory.points[i].velocities[0],test_trajectory.points[i].accelerations[0]);
00116         ROS_INFO("error: %f %f",vel_error,acc_error);      
00117         ROS_INFO("positions: %f, %f, velocities: %f, %f",wpt.trajectory.points[0].positions[0],
00118                  wpt.trajectory.points[1].positions[0],
00119                  wpt.trajectory.points[0].velocities[0],
00120                  wpt.trajectory.points[1].velocities[0]);
00121         ROS_INFO("Limits: %f, %f",wpt.limits[0].max_velocity,wpt.limits[0].max_acceleration);
00122         ROS_INFO(" ");
00123         ROS_INFO(" ");
00124         ROS_INFO(" ");
00125         break;
00126       }
00127       if(!(acc_error <= eps))
00128       {
00129         ROS_INFO("Time: %f",test_trajectory.points[i].time_from_start.toSec());
00130         ROS_INFO("error: %f %f",vel_error,acc_error);      
00131         ROS_INFO(" ");
00132         ROS_INFO(" ");
00133         ROS_INFO(" ");
00134         break;
00135       }
00136     }
00137   }
00138   
00139 }
00140 
00141 
00142 int main(int argc, char** argv)
00143 {
00144   int num_times = 10;
00145   if(argc > 1)
00146     num_times = atoi(argv[1]);
00147   run_test(num_times);
00148   //  testing::InitGoogleTest(&argc, argv);
00149   //  return RUN_ALL_TESTS();
00150 }


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