regress_lspb.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 <gtest/gtest.h>
00038 #include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
00039 #include <spline_smoother/lspb_trajectory.h>
00040 
00041 
00042 double gen_rand(double min, double max)
00043 {
00044   int rand_num = rand()%100+1;
00045   double result = min + (double)((max-min)*rand_num)/101.0;
00046   return result;
00047 }
00048 
00049 TEST(TestLSPBTrajectory, TestLSPBTrajectory)
00050 {
00051   srand(time(NULL)); // initialize random seed: 
00052   spline_smoother::LSPBTrajectory traj;
00053 
00054   // create the input:
00055   int length = 4;
00056   int joints = 2;
00057 
00058   arm_navigation_msgs::JointTrajectoryWithLimits wpt;
00059   wpt.trajectory.points.resize(length);
00060   wpt.trajectory.joint_names.resize(joints);
00061   wpt.trajectory.joint_names[0] = std::string("test0");
00062   wpt.trajectory.joint_names[1] = std::string("test1");
00063 
00064   wpt.limits.resize(joints);
00065   wpt.limits[0].max_velocity = 0.5;
00066   wpt.limits[0].has_velocity_limits = 1;
00067 
00068   wpt.limits[1].max_velocity = 0.25;
00069   wpt.limits[1].has_velocity_limits = 1;
00070 
00071   wpt.limits[0].max_acceleration = 0.25;
00072   wpt.limits[0].has_acceleration_limits = 1;
00073 
00074   wpt.limits[1].max_acceleration = 0.5;
00075   wpt.limits[1].has_acceleration_limits = 1;
00076 
00077   for (int i=0; i<length; i++)
00078   {
00079     wpt.trajectory.points[i].positions.resize(joints);
00080     wpt.trajectory.points[i].velocities.resize(joints);
00081     wpt.trajectory.points[i].accelerations.resize(joints);
00082     for(int j=0; j<joints; j++)
00083     {
00084       wpt.trajectory.points[i].positions[j] = i+j+gen_rand(0.0,1.0);
00085       wpt.trajectory.points[i].velocities[j] = 0.0;
00086       wpt.trajectory.points[i].accelerations[j] = 0.0;
00087     }
00088     wpt.trajectory.points[i].time_from_start = ros::Duration(i);
00089   }
00090 
00091    wpt.trajectory.points[0].positions[0] = 1.693069;
00092    wpt.trajectory.points[0].positions[1] = 2.910891;
00093 
00094    wpt.trajectory.points[1].positions[0] = 2.782178;
00095    wpt.trajectory.points[1].positions[1] = 3.594059;
00096 
00097   FILE* f = fopen("test_lspb_original.txt","w");
00098   for(int i=0; i<length; i++)
00099   {
00100     fprintf(f,"%f ",wpt.trajectory.points[i].time_from_start.toSec());
00101     for(int j=0; j<joints; j++)
00102     {
00103       fprintf(f,"%f ",wpt.trajectory.points[i].positions[j]);
00104     }
00105     for(int j=0; j<joints; j++)
00106     {
00107       fprintf(f,"%f ",wpt.trajectory.points[i].velocities[j]);
00108     }
00109     fprintf(f,"\n");
00110   }
00111   fclose(f);
00112 
00113   spline_smoother::LSPBTrajectoryMsg spline;
00114   bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00115   EXPECT_TRUE(success);
00116   // traj->writeSpline(spline,"test_lspb_spline.txt");
00117 
00118   trajectory_msgs::JointTrajectory wpt_out;
00119   int num_seg = spline.segments.size();
00120   std::vector<double> knot_times;
00121   knot_times.resize(num_seg+1);
00122   knot_times[0] = 0.0;
00123   for(int i=1; i < (int) knot_times.size(); i++)
00124   {
00125     knot_times[i] = knot_times[i-1]+spline.segments[i-1].duration.toSec();
00126   }
00127   bool ss = spline_smoother::sampleSplineTrajectory(spline,knot_times,wpt_out);
00128 
00129   EXPECT_TRUE(ss);
00130 
00131   EXPECT_NEAR(wpt.trajectory.points[0].positions[0],wpt_out.points[0].positions[0],1e-5);
00132   EXPECT_NEAR(wpt.trajectory.points[0].positions[1],wpt_out.points[0].positions[1],1e-5);
00133   EXPECT_NEAR(wpt.trajectory.points[1].positions[0],wpt_out.points[1].positions[0],1e-5);
00134   EXPECT_NEAR(wpt.trajectory.points[1].positions[1],wpt_out.points[1].positions[1],1e-5);
00135 
00136 
00137   double total_time;
00138   bool st = spline_smoother::getTotalTime(spline,total_time);
00139   EXPECT_TRUE(st);
00140   double dT = 0.01;
00141   int sample_length = (int) (total_time/dT);
00142   std::vector<double> times;
00143   times.resize(sample_length+1);
00144   for (int i=0; i<sample_length; i++)
00145   {
00146     times[i] = dT*i;
00147   }
00148   times[sample_length] = total_time;
00149   bool sw = spline_smoother::write(spline,times,"test_lspb.txt");
00150   EXPECT_TRUE(sw);
00151 
00152 }
00153 
00154 int main(int argc, char** argv)
00155 {
00156   testing::InitGoogleTest(&argc, argv);
00157   return RUN_ALL_TESTS();
00158 }


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