test_time_optimal_trajectory_generation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Georgia Tech Research Corporation
3  * All rights reserved.
4  *
5  * Author: Tobias Kunz <tobias@gatech.edu>
6  * Date: 05/2012
7  *
8  * Humanoid Robotics Lab Georgia Institute of Technology
9  * Director: Mike Stilman http://www.golems.org
10  *
11  * Algorithm details and publications:
12  * http://www.golems.org/node/1570
13  *
14  * This file is provided under the following "BSD-style" License:
15  * Redistribution and use in source and binary forms, with or
16  * without modification, are permitted provided that the following
17  * conditions are met:
18  * * Redistributions of source code must retain the above copyright
19  * notice, this list of conditions and the following disclaimer.
20  * * Redistributions in binary form must reproduce the above
21  * copyright notice, this list of conditions and the following
22  * disclaimer in the documentation and/or other materials provided
23  * with the distribution.
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
25  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
26  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
27  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
29  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
30  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
31  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
32  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
33  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  */
38 
39 #include <gtest/gtest.h>
41 
44 
45 TEST(time_optimal_trajectory_generation, test1)
46 {
47  Eigen::VectorXd waypoint(4);
48  std::list<Eigen::VectorXd> waypoints;
49 
50  waypoint << 1424.0, 984.999694824219, 2126.0, 0.0;
51  waypoints.push_back(waypoint);
52  waypoint << 1423.0, 985.000244140625, 2126.0, 0.0;
53  waypoints.push_back(waypoint);
54 
55  Eigen::VectorXd max_velocities(4);
56  max_velocities << 1.3, 0.67, 0.67, 0.5;
57  Eigen::VectorXd max_accelerations(4);
58  max_accelerations << 0.00249, 0.00249, 0.00249, 0.00249;
59 
60  Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0);
61  EXPECT_TRUE(trajectory.isValid());
62  EXPECT_DOUBLE_EQ(40.080256821829849, trajectory.getDuration());
63 
64  // Test start matches
65  EXPECT_DOUBLE_EQ(1424.0, trajectory.getPosition(0.0)[0]);
66  EXPECT_DOUBLE_EQ(984.999694824219, trajectory.getPosition(0.0)[1]);
67  EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(0.0)[2]);
68  EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(0.0)[3]);
69 
70  // Test end matches
71  EXPECT_DOUBLE_EQ(1423.0, trajectory.getPosition(trajectory.getDuration())[0]);
72  EXPECT_DOUBLE_EQ(985.000244140625, trajectory.getPosition(trajectory.getDuration())[1]);
73  EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(trajectory.getDuration())[2]);
74  EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(trajectory.getDuration())[3]);
75 }
76 
77 TEST(time_optimal_trajectory_generation, test2)
78 {
79  Eigen::VectorXd waypoint(4);
80  std::list<Eigen::VectorXd> waypoints;
81 
82  waypoint << 1427.0, 368.0, 690.0, 90.0;
83  waypoints.push_back(waypoint);
84  waypoint << 1427.0, 368.0, 790.0, 90.0;
85  waypoints.push_back(waypoint);
86  waypoint << 952.499938964844, 433.0, 1051.0, 90.0;
87  waypoints.push_back(waypoint);
88  waypoint << 452.5, 533.0, 1051.0, 90.0;
89  waypoints.push_back(waypoint);
90  waypoint << 452.5, 533.0, 951.0, 90.0;
91  waypoints.push_back(waypoint);
92 
93  Eigen::VectorXd max_velocities(4);
94  max_velocities << 1.3, 0.67, 0.67, 0.5;
95  Eigen::VectorXd max_accelerations(4);
96  max_accelerations << 0.002, 0.002, 0.002, 0.002;
97 
98  Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0);
99  EXPECT_TRUE(trajectory.isValid());
100  EXPECT_DOUBLE_EQ(1922.1418427445944, trajectory.getDuration());
101 
102  // Test start matches
103  EXPECT_DOUBLE_EQ(1427.0, trajectory.getPosition(0.0)[0]);
104  EXPECT_DOUBLE_EQ(368.0, trajectory.getPosition(0.0)[1]);
105  EXPECT_DOUBLE_EQ(690.0, trajectory.getPosition(0.0)[2]);
106  EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(0.0)[3]);
107 
108  // Test end matches
109  EXPECT_DOUBLE_EQ(452.5, trajectory.getPosition(trajectory.getDuration())[0]);
110  EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
111  EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
112  EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
113 }
114 
115 TEST(time_optimal_trajectory_generation, test3)
116 {
117  Eigen::VectorXd waypoint(4);
118  std::list<Eigen::VectorXd> waypoints;
119 
120  waypoint << 1427.0, 368.0, 690.0, 90.0;
121  waypoints.push_back(waypoint);
122  waypoint << 1427.0, 368.0, 790.0, 90.0;
123  waypoints.push_back(waypoint);
124  waypoint << 952.499938964844, 433.0, 1051.0, 90.0;
125  waypoints.push_back(waypoint);
126  waypoint << 452.5, 533.0, 1051.0, 90.0;
127  waypoints.push_back(waypoint);
128  waypoint << 452.5, 533.0, 951.0, 90.0;
129  waypoints.push_back(waypoint);
130 
131  Eigen::VectorXd max_velocities(4);
132  max_velocities << 1.3, 0.67, 0.67, 0.5;
133  Eigen::VectorXd max_accelerations(4);
134  max_accelerations << 0.002, 0.002, 0.002, 0.002;
135 
136  Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations);
137  EXPECT_TRUE(trajectory.isValid());
138  EXPECT_DOUBLE_EQ(1919.5597888812974, trajectory.getDuration());
139 
140  // Test start matches
141  EXPECT_DOUBLE_EQ(1427.0, trajectory.getPosition(0.0)[0]);
142  EXPECT_DOUBLE_EQ(368.0, trajectory.getPosition(0.0)[1]);
143  EXPECT_DOUBLE_EQ(690.0, trajectory.getPosition(0.0)[2]);
144  EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(0.0)[3]);
145 
146  // Test end matches
147  EXPECT_DOUBLE_EQ(452.5, trajectory.getPosition(trajectory.getDuration())[0]);
148  EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
149  EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
150  EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
151 }
152 
153 // Test that totg algorithm doesn't give large acceleration
154 TEST(time_optimal_trajectory_generation, testLargeAccel)
155 {
156  double path_tolerance = 0.1;
157  double resample_dt = 0.1;
158  Eigen::VectorXd waypoint(6);
159  std::list<Eigen::VectorXd> waypoints;
160  Eigen::VectorXd max_velocities(6);
161  Eigen::VectorXd max_accelerations(6);
162 
163  // Waypoints
164  // clang-format off
165  waypoint << 1.6113056281076339,
166  -0.21400163389235427,
167  -1.974502599739185,
168  9.9653618690354051e-12,
169  -1.3810916877429624,
170  1.5293902838041467;
171  waypoints.push_back(waypoint);
172 
173  waypoint << 1.6088016187976597,
174  -0.21792862470933924,
175  -1.9758628799742952,
176  0.00010424017303217738,
177  -1.3835690515335755,
178  1.5279972853269816;
179  waypoints.push_back(waypoint);
180 
181  waypoint << 1.5887695443178671,
182  -0.24934455124521923,
183  -1.9867451218551782,
184  0.00093816147756670078,
185  -1.4033879618584812,
186  1.5168532975096607;
187  waypoints.push_back(waypoint);
188 
189  waypoint << 1.1647412393815282,
190  -0.91434018564402375,
191  -2.2170946337498498,
192  0.018590164397622583,
193  -1.8229041212673529,
194  1.2809632867583278;
195  waypoints.push_back(waypoint);
196 
197  // Max velocities
198  max_velocities << 0.89535390627300004,
199  0.89535390627300004,
200  0.79587013890930003,
201  0.92022484811399996,
202  0.82074108075029995,
203  1.3927727430915;
204  // Max accelerations
205  max_accelerations << 0.82673490883799994,
206  0.78539816339699997,
207  0.60883578557700002,
208  3.2074759432319997,
209  1.4398966328939999,
210  4.7292792634680003;
211  // clang-format on
212 
213  Trajectory parameterized(Path(waypoints, path_tolerance), max_velocities, max_accelerations, 0.001);
214 
215  ASSERT_TRUE(parameterized.isValid());
216 
217  size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt);
218  for (size_t sample = 0; sample <= sample_count; ++sample)
219  {
220  // always sample the end of the trajectory as well
221  double t = std::min(parameterized.getDuration(), sample * resample_dt);
222  Eigen::VectorXd acceleration = parameterized.getAcceleration(t);
223 
224  ASSERT_EQ(acceleration.size(), 6);
225  for (std::size_t i = 0; i < 6; ++i)
226  EXPECT_NEAR(acceleration(i), 0.0, 100.0) << "Invalid acceleration at position " << sample_count << "\n";
227  }
228 }
229 
230 int main(int argc, char** argv)
231 {
232  testing::InitGoogleTest(&argc, argv);
233  return RUN_ALL_TESTS();
234 }
double getDuration() const
Returns the optimal duration of the trajectory.
int main(int argc, char **argv)
#define EXPECT_NEAR(a, b, prec)
geometry_msgs::TransformStamped t
Eigen::VectorXd getAcceleration(double time) const
Return the acceleration vector for a given point in time.
Eigen::VectorXd getPosition(double time) const
Return the position/configuration vector for a given point in time.
#define EXPECT_TRUE(args)
bool isValid() const
Call this method after constructing the object to make sure the trajectory generation succeeded witho...
TEST(time_optimal_trajectory_generation, test1)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Aug 14 2020 03:57:26