spline_smoother: regress_cubic_parameterized.cpp Source 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 <gtest/gtest.h>
00038 #include <motion_planning_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 TEST(TestCubicParameterizedTrajectory, TestCubicParameterizedTrajectoryWithWrapAround)
00049 {
00050   spline_smoother::CubicParameterizedTrajectory traj;
00051 
00052   // create the input:
00053   int length = 2;
00054   int joints = 1;
00055 
00056   motion_planning_msgs::JointTrajectoryWithLimits wpt;
00057   wpt.trajectory.points.resize(length);
00058   wpt.trajectory.joint_names.resize(joints);
00059   wpt.trajectory.joint_names[0] = std::string("test0");
00060 
00061   wpt.limits.resize(joints);
00062   wpt.limits[0].max_velocity = 0.5;
00063   wpt.limits[0].has_velocity_limits = 1;
00064 
00065   //  wpt.limits[1].max_velocity = 0.25;
00066   //  wpt.limits[1].has_velocity_limits = 1;
00067 
00068   for (int i=0; i<length; i++)
00069   {
00070     wpt.trajectory.points[i].positions.resize(joints);
00071     wpt.trajectory.points[i].velocities.resize(joints);
00072     wpt.trajectory.points[i].accelerations.resize(joints);
00073     for(int j=0; j<joints; j++)
00074     {
00075       wpt.trajectory.points[i].positions[j] = 0.0;
00076       wpt.trajectory.points[i].velocities[j] = 0.0;
00077       wpt.trajectory.points[i].accelerations[j] = 0.0;
00078     }
00079     wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00080   }
00081 
00082   wpt.trajectory.points[0].positions[0] = M_PI;
00083   wpt.trajectory.points[1].positions[0] = -M_PI;
00084 
00085   spline_smoother::SplineTrajectory spline;
00086   bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00087 
00088   double total_time;
00089   bool ss = spline_smoother::getTotalTime(spline,total_time);
00090   EXPECT_TRUE(ss);
00091 
00092   trajectory_msgs::JointTrajectory wpt_out;
00093   std::vector<double> times_out;
00094   times_out.resize(length);
00095   times_out[0] = 0.0;
00096   times_out[1] = total_time;
00097   spline_smoother::sampleSplineTrajectory(spline,times_out,wpt_out);
00098   EXPECT_NEAR(wpt_out.points[0].positions[0], M_PI, 1e-5);
00099   EXPECT_NEAR(wpt_out.points[1].positions[0], -M_PI, 1e-5);
00100 }
00101 
00102 
00103 TEST(TestCubicParameterizedTrajectory, TestCubicParameterizedTrajectory)
00104 {
00105   spline_smoother::CubicParameterizedTrajectory traj;
00106 
00107   // create the input:
00108   int length = 4;
00109   int joints = 2;
00110 
00111   motion_planning_msgs::JointTrajectoryWithLimits wpt;
00112   wpt.trajectory.points.resize(length);
00113   wpt.trajectory.joint_names.resize(joints);
00114   wpt.trajectory.joint_names[0] = std::string("test0");
00115   wpt.trajectory.joint_names[1] = std::string("test1");
00116 
00117   wpt.limits.resize(joints);
00118   wpt.limits[0].max_velocity = 0.5;
00119   wpt.limits[0].has_velocity_limits = 1;
00120 
00121   wpt.limits[1].max_velocity = 0.25;
00122   wpt.limits[1].has_velocity_limits = 1;
00123 
00124   for (int i=0; i<length; i++)
00125   {
00126     wpt.trajectory.points[i].positions.resize(joints);
00127     wpt.trajectory.points[i].velocities.resize(joints);
00128     wpt.trajectory.points[i].accelerations.resize(joints);
00129     for(int j=0; j<joints; j++)
00130     {
00131       wpt.trajectory.points[i].positions[j] = i+j;
00132       wpt.trajectory.points[i].velocities[j] = 0.0;
00133       wpt.trajectory.points[i].accelerations[j] = 0.0;
00134     }
00135     wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00136   }
00137 
00138   spline_smoother::SplineTrajectory spline;
00139   bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00140 
00141   double total_time;
00142   bool ss = spline_smoother::getTotalTime(spline,total_time);
00143   EXPECT_TRUE(ss);
00144 
00145   double dT = 0.01;
00146   int sample_length = (int) (total_time/dT);
00147 
00148   std::vector<double> times;
00149   times.resize(sample_length);
00150   for (int i=0; i<sample_length; i++)
00151   {
00152     times[i] = dT*i;
00153   }
00154 
00155   EXPECT_TRUE(success);
00156 
00157   trajectory_msgs::JointTrajectory wpt_out;
00158   std::vector<double> times_out;
00159   times_out.resize(length);
00160   for (int i=0; i<length; i++)
00161   {
00162     times_out[i] = i;
00163   }
00164   spline_smoother::sampleSplineTrajectory(spline,times_out,wpt_out);
00165   EXPECT_NEAR(wpt_out.points[0].positions[0], 0.0, 1e-5);
00166   EXPECT_NEAR(wpt_out.points[1].positions[0], 0.0740741, 1e-5);
00167   EXPECT_NEAR(wpt_out.points[2].positions[0], 0.259259, 1e-5);
00168   EXPECT_NEAR(wpt_out.points[3].positions[0], 0.5, 1e-5);
00169 
00170   EXPECT_NEAR(wpt_out.points[0].positions[1], 1.0, 1e-5);
00171   EXPECT_NEAR(wpt_out.points[1].positions[1], 1.0740741, 1e-5);
00172   EXPECT_NEAR(wpt_out.points[2].positions[1], 1.259259, 1e-5);
00173   EXPECT_NEAR(wpt_out.points[3].positions[1], 1.5, 1e-5);
00174 }
00175 
00176 TEST(TestCubicParameterizedTrajectory, TestWithAccelerationLimits1)
00177 {
00178   spline_smoother::CubicParameterizedTrajectory traj;
00179 
00180   // create the input:
00181   int length = 2;
00182   int joints = 1;
00183 
00184   motion_planning_msgs::JointTrajectoryWithLimits wpt;
00185   wpt.trajectory.points.resize(length);
00186   wpt.trajectory.joint_names.resize(joints);
00187   wpt.trajectory.joint_names[0] = std::string("test0");
00188 
00189   wpt.limits.resize(joints);
00190   wpt.limits[0].max_velocity = 0.2;
00191   wpt.limits[0].has_velocity_limits = 1;
00192 
00193   wpt.limits[0].max_acceleration = 0.1;
00194   wpt.limits[0].has_acceleration_limits = 1;
00195 
00196   for (int i=0; i<length; i++)
00197   {
00198     wpt.trajectory.points[i].positions.resize(joints);
00199     wpt.trajectory.points[i].velocities.resize(joints);
00200     wpt.trajectory.points[i].accelerations.resize(joints);
00201     wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00202   }
00203   wpt.trajectory.points[1].positions[0] = 1.0;
00204   spline_smoother::SplineTrajectory spline;
00205   bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00206   EXPECT_TRUE(success);
00207 
00208   double total_time;
00209   bool ss = spline_smoother::getTotalTime(spline,total_time);
00210   EXPECT_TRUE(ss);
00211   EXPECT_NEAR(total_time,7.74597,1e-5);
00212 
00213   trajectory_msgs::JointTrajectory wpt_out;
00214   std::vector<double> times_out;
00215   times_out.push_back(total_time);
00216   spline_smoother::sampleSplineTrajectory(spline,times_out,wpt_out);
00217 
00218   EXPECT_NEAR(wpt.trajectory.points[1].positions[0],wpt_out.points[0].positions[0],1e-5);
00219   EXPECT_NEAR(wpt.trajectory.points[1].velocities[0],wpt_out.points[0].velocities[0],1e-5);
00220 }
00221 
00222 TEST(TestCubicParameterizedTrajectory, TestWithAccelerationLimits2)
00223 {
00224   spline_smoother::CubicParameterizedTrajectory traj;
00225   srand ( time(NULL) ); // initialize random seed: 
00226 
00227   // create the input:
00228   int length = 2;
00229   int joints = 1;
00230 
00231   motion_planning_msgs::JointTrajectoryWithLimits wpt;
00232   wpt.trajectory.points.resize(length);
00233   wpt.trajectory.joint_names.resize(joints);
00234   wpt.trajectory.joint_names[0] = std::string("test0");
00235 
00236   wpt.limits.resize(joints);
00237   wpt.limits[0].max_velocity = 0.2;
00238   wpt.limits[0].has_velocity_limits = 1;
00239 
00240   wpt.limits[0].max_acceleration = 0.1;
00241   wpt.limits[0].has_acceleration_limits = 1;
00242 
00243   for (int i=0; i<length; i++)
00244   {
00245     wpt.trajectory.points[i].positions.resize(joints);
00246     wpt.trajectory.points[i].velocities.resize(joints);
00247     wpt.trajectory.points[i].accelerations.resize(joints);
00248     wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00249   }
00250   wpt.trajectory.points[1].positions[0] = 1.0;
00251   wpt.trajectory.points[1].velocities[0] = 0.0;
00252   spline_smoother::SplineTrajectory spline;
00253   bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00254   EXPECT_TRUE(success);
00255 
00256   double total_time;
00257   bool ss = spline_smoother::getTotalTime(spline,total_time);
00258   EXPECT_TRUE(ss);
00259   //  EXPECT_NEAR(total_time,12.717798,1e-5);
00260 
00261   ROS_INFO("Next test");
00262   wpt.trajectory.points[1].velocities[0] = 0.0;
00263   success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00264   EXPECT_TRUE(success);
00265   ss = spline_smoother::getTotalTime(spline,total_time);
00266   EXPECT_TRUE(ss);
00267   //  EXPECT_NEAR(total_time,7.5,1e-5);
00268 
00269   trajectory_msgs::JointTrajectory wpt_out;
00270   std::vector<double> times_out;
00271   times_out.push_back(total_time);
00272   spline_smoother::sampleSplineTrajectory(spline,times_out,wpt_out);
00273 
00274   //  EXPECT_NEAR(wpt.trajectory.points[1].positions[0],wpt_out.points[0].positions[0],1e-5);
00275   //  EXPECT_NEAR(wpt.trajectory.points[1].velocities[0],wpt_out.points[0].velocities[0],1e-5);
00276 
00277   wpt.trajectory.points[0].positions[0] = -0.000720;
00278   wpt.trajectory.points[1].positions[0] = -0.0000080;
00279   wpt.trajectory.points[0].velocities[0] = .000028;
00280   wpt.trajectory.points[1].velocities[0] = 0.000034;
00281   wpt.limits[0].max_velocity = 1.0;
00282   wpt.limits[0].max_acceleration = 0.5;
00283 
00284   success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00285   EXPECT_TRUE(success);
00286   ss = spline_smoother::getTotalTime(spline,total_time);
00287   EXPECT_TRUE(ss);
00288   EXPECT_NEAR(total_time,0.092254,1e-3);
00289 
00290     double eps = 1e-2;
00291   for(unsigned int i=0; i < 2000; i++)
00292   {
00293     wpt.trajectory.points[0].positions[0] = gen_rand(-100.0,100.0);
00294     wpt.trajectory.points[1].positions[0] = gen_rand(-100.0,100.0);
00295     wpt.limits[0].max_velocity = fabs(gen_rand(-100.0,100.0));
00296     wpt.limits[0].max_acceleration = fabs(gen_rand(-100.0,100.0));
00297     //    wpt.trajectory.points[0].velocities[0] = gen_rand(-wpt.limits[0].max_velocity,wpt.limits[0].max_velocity);
00298     //    wpt.trajectory.points[1].velocities[0] = gen_rand(-wpt.limits[0].max_velocity,wpt.limits[0].max_velocity);
00299     wpt.trajectory.points[0].velocities[0] = 0.0;
00300     wpt.trajectory.points[1].velocities[0] = 0.0;
00301     if(wpt.trajectory.points[0].positions[0] == wpt.trajectory.points[1].positions[0])
00302       continue;
00303     success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00304     EXPECT_TRUE(success);
00305     ss = spline_smoother::getTotalTime(spline,total_time);
00306     std::vector<double> test_times;
00307     double dT = 0.0;
00308     while(dT < total_time)
00309     {
00310       test_times.push_back(dT);
00311       dT += 0.01;
00312     }
00313     test_times.push_back(total_time);
00314     trajectory_msgs::JointTrajectory test_trajectory;
00315     spline_smoother::sampleSplineTrajectory(spline,test_times,test_trajectory);
00316     for(unsigned int i=0; i < test_trajectory.points.size(); i++)
00317     {
00318       double vel_error = fabs(test_trajectory.points[i].velocities[0]) - wpt.limits[0].max_velocity;
00319       double acc_error = fabs(test_trajectory.points[i].accelerations[0]) - wpt.limits[0].max_acceleration;
00320       if(!(vel_error <= eps) || isnan(vel_error))
00321       {
00322         ROS_INFO("error: %f %f",vel_error,acc_error);      
00323         ROS_INFO("positions: %f, %f, velocities: %f, %f",wpt.trajectory.points[0].positions[0],
00324                  wpt.trajectory.points[1].positions[0],
00325                  wpt.trajectory.points[0].velocities[0],
00326                  wpt.trajectory.points[1].velocities[0]);
00327         ROS_INFO("Limits: %f, %f",wpt.limits[0].max_velocity,wpt.limits[0].max_acceleration);
00328         ROS_INFO(" ");
00329         ROS_INFO(" ");
00330         ROS_INFO(" ");
00331       }
00332       if(!(acc_error <= eps))
00333         ROS_INFO("error: %f %f",vel_error,acc_error);      
00334 
00335       EXPECT_TRUE(vel_error <= eps);
00336       EXPECT_TRUE(acc_error <= eps);      
00337     }
00338   }
00339   
00340 }
00341 
00342 
00343 int main(int argc, char** argv)
00344 {
00345   testing::InitGoogleTest(&argc, argv);
00346   return RUN_ALL_TESTS();
00347 }