00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <gtest/gtest.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 TEST(TestCubicParameterizedTrajectory, TestCubicParameterizedTrajectoryWithWrapAround)
00049 {
00050 spline_smoother::CubicParameterizedTrajectory traj;
00051
00052
00053 int length = 2;
00054 int joints = 1;
00055
00056 arm_navigation_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
00066
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
00108 int length = 4;
00109 int joints = 2;
00110
00111 arm_navigation_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
00181 int length = 2;
00182 int joints = 1;
00183
00184 arm_navigation_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) );
00226
00227
00228 int length = 2;
00229 int joints = 1;
00230
00231 arm_navigation_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
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
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
00275
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
00298
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 }