moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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>
42
46
47namespace
48{
49// The URDF used in moveit::core::loadTestingRobotModel() does not contain acceleration limits,
50// so add them here.
51// TODO(andyz): Function won't be needed once this issue has been addressed:
52// https://github.com/ros/urdfdom/issues/177
53void setAccelerationLimits(const moveit::core::RobotModelPtr& robot_model)
54{
55 const std::vector<moveit::core::JointModel*> joint_models = robot_model->getActiveJointModels();
56 for (auto& joint_model : joint_models)
57 {
58 std::vector<moveit_msgs::msg::JointLimits> joint_bounds_msg(joint_model->getVariableBoundsMsg());
59 // TODO: update individual bounds
60 for (auto& joint_bound : joint_bounds_msg)
61 {
62 joint_bound.has_acceleration_limits = true;
63 joint_bound.max_acceleration = 1.0;
64 }
65 joint_model->setVariableBounds(joint_bounds_msg);
66 }
67}
68} // namespace
69
70TEST(time_optimal_trajectory_generation, test1)
71{
72 Eigen::VectorXd waypoint(4);
73 std::vector<Eigen::VectorXd> waypoints;
74
75 waypoint << 1424.0, 984.999694824219, 2126.0, 0.0;
76 waypoints.push_back(waypoint);
77 waypoint << 1423.0, 985.000244140625, 2126.0, 0.0;
78 waypoints.push_back(waypoint);
79
80 Eigen::VectorXd max_velocities(4);
81 max_velocities << 1.3, 0.67, 0.67, 0.5;
82 Eigen::VectorXd max_accelerations(4);
83 max_accelerations << 0.00249, 0.00249, 0.00249, 0.00249;
84
85 auto path_maybe = Path::create(waypoints, 100.0);
86 ASSERT_TRUE(path_maybe.has_value());
87
88 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 10.0);
89 ASSERT_TRUE(trajectory_maybe.has_value());
90 const Trajectory& trajectory = trajectory_maybe.value();
91
92 EXPECT_DOUBLE_EQ(40.080256821829849, trajectory.getDuration());
93
94 // Test start matches
95 EXPECT_DOUBLE_EQ(1424.0, trajectory.getPosition(0.0)[0]);
96 EXPECT_DOUBLE_EQ(984.999694824219, trajectory.getPosition(0.0)[1]);
97 EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(0.0)[2]);
98 EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(0.0)[3]);
99
100 // Test end matches
101 EXPECT_DOUBLE_EQ(1423.0, trajectory.getPosition(trajectory.getDuration())[0]);
102 EXPECT_DOUBLE_EQ(985.000244140625, trajectory.getPosition(trajectory.getDuration())[1]);
103 EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(trajectory.getDuration())[2]);
104 EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(trajectory.getDuration())[3]);
105
106 // Start at rest and end at rest
107 const double traj_duration = trajectory.getDuration();
108 EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
109 EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
110}
111
112TEST(time_optimal_trajectory_generation, test2)
113{
114 Eigen::VectorXd waypoint(4);
115 std::vector<Eigen::VectorXd> waypoints;
116
117 waypoint << 1427.0, 368.0, 690.0, 90.0;
118 waypoints.push_back(waypoint);
119 waypoint << 1427.0, 368.0, 790.0, 90.0;
120 waypoints.push_back(waypoint);
121 waypoint << 952.499938964844, 433.0, 1051.0, 90.0;
122 waypoints.push_back(waypoint);
123 waypoint << 452.5, 533.0, 1051.0, 90.0;
124 waypoints.push_back(waypoint);
125 waypoint << 452.5, 533.0, 951.0, 90.0;
126 waypoints.push_back(waypoint);
127
128 Eigen::VectorXd max_velocities(4);
129 max_velocities << 1.3, 0.67, 0.67, 0.5;
130 Eigen::VectorXd max_accelerations(4);
131 max_accelerations << 0.002, 0.002, 0.002, 0.002;
132
133 auto path_maybe = Path::create(waypoints, 100.0);
134 ASSERT_TRUE(path_maybe.has_value());
135
136 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 10.0);
137 ASSERT_TRUE(trajectory_maybe.has_value());
138 const Trajectory& trajectory = trajectory_maybe.value();
139
140 EXPECT_DOUBLE_EQ(1922.1418427445944, trajectory.getDuration());
141
142 // Test start matches
143 EXPECT_DOUBLE_EQ(1427.0, trajectory.getPosition(0.0)[0]);
144 EXPECT_DOUBLE_EQ(368.0, trajectory.getPosition(0.0)[1]);
145 EXPECT_DOUBLE_EQ(690.0, trajectory.getPosition(0.0)[2]);
146 EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(0.0)[3]);
147
148 // Test end matches
149 EXPECT_DOUBLE_EQ(452.5, trajectory.getPosition(trajectory.getDuration())[0]);
150 EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
151 EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
152 EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
153
154 // Start at rest and end at rest
155 const double traj_duration = trajectory.getDuration();
156 EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
157 EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
158}
159
160TEST(time_optimal_trajectory_generation, test3)
161{
162 Eigen::VectorXd waypoint(4);
163 std::vector<Eigen::VectorXd> waypoints;
164
165 waypoint << 1427.0, 368.0, 690.0, 90.0;
166 waypoints.push_back(waypoint);
167 waypoint << 1427.0, 368.0, 790.0, 90.0;
168 waypoints.push_back(waypoint);
169 waypoint << 952.499938964844, 433.0, 1051.0, 90.0;
170 waypoints.push_back(waypoint);
171 waypoint << 452.5, 533.0, 1051.0, 90.0;
172 waypoints.push_back(waypoint);
173 waypoint << 452.5, 533.0, 951.0, 90.0;
174 waypoints.push_back(waypoint);
175
176 Eigen::VectorXd max_velocities(4);
177 max_velocities << 1.3, 0.67, 0.67, 0.5;
178 Eigen::VectorXd max_accelerations(4);
179 max_accelerations << 0.002, 0.002, 0.002, 0.002;
180
181 auto path_maybe = Path::create(waypoints, 100.0);
182 ASSERT_TRUE(path_maybe.has_value());
183
184 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations);
185 ASSERT_TRUE(trajectory_maybe.has_value());
186 const Trajectory& trajectory = trajectory_maybe.value();
187
188 EXPECT_DOUBLE_EQ(1919.5597888812974, trajectory.getDuration());
189
190 // Test start matches
191 EXPECT_DOUBLE_EQ(1427.0, trajectory.getPosition(0.0)[0]);
192 EXPECT_DOUBLE_EQ(368.0, trajectory.getPosition(0.0)[1]);
193 EXPECT_DOUBLE_EQ(690.0, trajectory.getPosition(0.0)[2]);
194 EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(0.0)[3]);
195
196 // Test end matches
197 EXPECT_DOUBLE_EQ(452.5, trajectory.getPosition(trajectory.getDuration())[0]);
198 EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
199 EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
200 EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
201
202 // Start at rest and end at rest
203 const double traj_duration = trajectory.getDuration();
204 EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
205 EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
206}
207
208// Test the version of computeTimeStamps that takes custom velocity/acceleration limits
209TEST(time_optimal_trajectory_generation, testCustomLimits)
210{
211 constexpr auto robot_name{ "panda" };
212 constexpr auto group_name{ "panda_arm" };
213
214 auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
215 ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
216 setAccelerationLimits(robot_model);
217 auto group = robot_model->getJointModelGroup(group_name);
218 ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
219 moveit::core::RobotState waypoint_state(robot_model);
220 waypoint_state.setToDefaultValues();
221
222 const double delta_t = 0.1;
223 robot_trajectory::RobotTrajectory trajectory(robot_model, group);
224 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
225 trajectory.addSuffixWayPoint(waypoint_state, delta_t);
226 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 });
227 trajectory.addSuffixWayPoint(waypoint_state, delta_t);
228
230 // Custom velocity & acceleration limits for some joints
231 std::unordered_map<std::string, double> vel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 },
232 { "panda_joint3", 3.3 }, { "panda_joint4", 4.3 },
233 { "panda_joint5", 5.3 }, { "panda_joint6", 6.3 },
234 { "panda_joint7", 7.3 } };
235 std::unordered_map<std::string, double> accel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 },
236 { "panda_joint3", 3.3 }, { "panda_joint4", 4.3 },
237 { "panda_joint5", 5.3 }, { "panda_joint6", 6.3 },
238 { "panda_joint7", 7.3 } };
239 ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps";
240}
241
242// Test that totg algorithm doesn't give large acceleration
243TEST(time_optimal_trajectory_generation, testLargeAccel)
244{
245 double path_tolerance = 0.1;
246 double resample_dt = 0.1;
247 Eigen::VectorXd waypoint(6);
248 std::vector<Eigen::VectorXd> waypoints;
249 Eigen::VectorXd max_velocities(6);
250 Eigen::VectorXd max_accelerations(6);
251
252 // Waypoints
253 // clang-format off
254 waypoint << 1.6113056281076339,
255 -0.21400163389235427,
256 -1.974502599739185,
257 9.9653618690354051e-12,
258 -1.3810916877429624,
259 1.5293902838041467;
260 waypoints.push_back(waypoint);
261
262 waypoint << 1.6088016187976597,
263 -0.21792862470933924,
264 -1.9758628799742952,
265 0.00010424017303217738,
266 -1.3835690515335755,
267 1.5279972853269816;
268 waypoints.push_back(waypoint);
269
270 waypoint << 1.5887695443178671,
271 -0.24934455124521923,
272 -1.9867451218551782,
273 0.00093816147756670078,
274 -1.4033879618584812,
275 1.5168532975096607;
276 waypoints.push_back(waypoint);
277
278 waypoint << 1.1647412393815282,
279 -0.91434018564402375,
280 -2.2170946337498498,
281 0.018590164397622583,
282 -1.8229041212673529,
283 1.2809632867583278;
284 waypoints.push_back(waypoint);
285
286 // Max velocities
287 max_velocities << 0.89535390627300004,
288 0.89535390627300004,
289 0.79587013890930003,
290 0.92022484811399996,
291 0.82074108075029995,
292 1.3927727430915;
293 // Max accelerations
294 max_accelerations << 0.82673490883799994,
295 0.78539816339699997,
296 0.60883578557700002,
297 3.2074759432319997,
298 1.4398966328939999,
299 4.7292792634680003;
300 // clang-format on
301
302 auto path_maybe = Path::create(waypoints, path_tolerance);
303 ASSERT_TRUE(path_maybe.has_value());
304
305 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001);
306 ASSERT_TRUE(trajectory_maybe.has_value());
307 const Trajectory& parameterized = trajectory_maybe.value();
308
309 size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt);
310 for (size_t sample = 0; sample <= sample_count; ++sample)
311 {
312 // always sample the end of the trajectory as well
313 double t = std::min(parameterized.getDuration(), sample * resample_dt);
314 Eigen::VectorXd acceleration = parameterized.getAcceleration(t);
315
316 ASSERT_EQ(acceleration.size(), 6);
317 for (std::size_t i = 0; i < 6; ++i)
318 EXPECT_NEAR(acceleration(i), 0.0, 100.0) << "Invalid acceleration at position " << sample_count << '\n';
319 }
320}
321
322// This test differentiates the trajectory velocities and verifies that the acceleration limit is actually respected.
323// A path tolerance of 0.0 makes this test fail. Output trajectory accelerations are all within the limits, but there
324// are jumps in velocity, i.e. the derivative of the velocity is not consistent with the acceleration.
325// The test is here mostly to prevent us from making a path tolerance of 0.0 a valid default again in the future.
326TEST(time_optimal_trajectory_generation, AccelerationLimitIsRespected)
327{
328 double path_tolerance = 0.001;
329 double resample_dt = 0.01;
330
331 // Waypoints.
332 std::vector<Eigen::VectorXd> waypoints;
333 waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
334 waypoints.push_back(Eigen::Vector3d(1.0, 0.0, 0.0));
335 waypoints.push_back(Eigen::Vector3d(1.0, 1.0, 0.0));
336
337 Eigen::VectorXd max_velocities = Eigen::VectorXd::Constant(3, 0.1);
338 Eigen::VectorXd max_accelerations = Eigen::VectorXd::Constant(3, 0.5);
339
340 auto path_maybe = Path::create(waypoints, path_tolerance);
341 ASSERT_TRUE(path_maybe.has_value());
342
343 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001);
344 ASSERT_TRUE(trajectory_maybe.has_value());
345 const Trajectory& parameterized = trajectory_maybe.value();
346
347 size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt);
348 Eigen::Vector3d previous_velocity = Eigen::Vector3d::Zero();
349 for (size_t sample = 0; sample <= sample_count; ++sample)
350 {
351 double t = std::min(parameterized.getDuration(), sample * resample_dt);
352 Eigen::Vector3d velocity = parameterized.getVelocity(t);
353
354 double acceleration = (velocity - previous_velocity).norm() / resample_dt;
355 EXPECT_LT(acceleration, max_accelerations.norm() + 1e-3);
356 previous_velocity = velocity;
357 }
358}
359
360// A path that requires a full 180 degree turn at any point is not supported by the current implementation.
361// Path::create() should fail.
362TEST(time_optimal_trajectory_generation, PathMakes180DegreeTurn)
363{
364 // Waypoints.
365 std::vector<Eigen::VectorXd> waypoints;
366 waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
367 waypoints.push_back(Eigen::Vector3d(1.0, 0.0, 0.0));
368 waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
369
370 EXPECT_FALSE(Path::create(waypoints, /*path_tolerance=*/0.01));
371}
372
373// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end
374// waypoint
375TEST(time_optimal_trajectory_generation, testLastWaypoint)
376{
377 constexpr auto robot_name{ "panda" };
378 constexpr auto group_name{ "panda_arm" };
379
380 auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
381 ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
382 setAccelerationLimits(robot_model);
383 auto group = robot_model->getJointModelGroup(group_name);
384 ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
385 moveit::core::RobotState waypoint_state(robot_model);
386 waypoint_state.setToDefaultValues();
387
388 robot_trajectory::RobotTrajectory trajectory(robot_model, group);
389 auto add_waypoint = [&](const std::vector<double>& waypoint) {
390 waypoint_state.setJointGroupPositions(group, waypoint);
391 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
392 };
393 add_waypoint({ 0.000000000, 0.000000000, 0, 0, 0, 0, 0 });
394 add_waypoint({ 0.009521808, 0.009521808, 0, 0, 0, 0, 0 });
395 add_waypoint({ 0.011902261, 0.011902261, 0, 0, 0, 0, 0 });
396 add_waypoint({ 0.016663165, 0.016663165, 0, 0, 0, 0, 0 });
397 add_waypoint({ 0.026184973, 0.026184973, 0, 0, 0, 0, 0 });
398 add_waypoint({ 0.034516555, 0.034516555, 0, 0, 0, 0, 0 });
399
400 const std::vector<double> expected_last_waypoint = { 0.034516555, 0.034516555, 0, 0, 0, 0, 0 };
401 add_waypoint(expected_last_waypoint);
402
404 ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps";
405 const auto robot_state = trajectory.getLastWayPoint();
406 std::vector<double> actual_last_waypoint;
407 robot_state.copyJointGroupPositions(group, actual_last_waypoint);
408 EXPECT_TRUE(std::equal(actual_last_waypoint.cbegin(), actual_last_waypoint.cend(), expected_last_waypoint.cbegin(),
409 expected_last_waypoint.cend(), [](const double rhs, const double lhs) {
410 return std::abs(rhs - lhs) < std::numeric_limits<double>::epsilon();
411 }));
412}
413
414TEST(time_optimal_trajectory_generation, testPluginAPI)
415{
416 constexpr auto robot_name{ "panda" };
417 constexpr auto group_name{ "panda_arm" };
418
419 auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
420 ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
421 setAccelerationLimits(robot_model);
422 auto group = robot_model->getJointModelGroup(group_name);
423 ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
424 moveit::core::RobotState waypoint_state(robot_model);
425 waypoint_state.setToDefaultValues();
426
427 robot_trajectory::RobotTrajectory trajectory(robot_model, group);
428 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
429 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
430 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
431 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
432 waypoint_state.setJointGroupPositions(group, std::vector<double>{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 });
433 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
434 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.00, 1.35, -2.51, -0.88, 0.63, 0.0 });
435 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
436 waypoint_state.setJointGroupPositions(group, std::vector<double>{ 0.0, -3.5, 1.4, -1.2, -1.5, -0.2, 0.0 });
437 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
438 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
439 trajectory.addSuffixWayPoint(waypoint_state, 0.1);
440
441 // Number TOTG iterations
442 constexpr size_t totg_iterations = 10;
443
444 // Test computing the dynamics repeatedly with the same totg instance
445 moveit_msgs::msg::RobotTrajectory first_trajectory_msg_start, first_trajectory_msg_end;
446 {
447 robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */);
448
449 // Test if the trajectory was copied correctly
450 ASSERT_EQ(test_trajectory.getDuration(), trajectory.getDuration());
451 moveit::core::JointBoundsVector test_bounds = test_trajectory.getRobotModel()->getActiveJointModelsBounds();
452 moveit::core::JointBoundsVector original_bounds = trajectory.getRobotModel()->getActiveJointModelsBounds();
453 ASSERT_EQ(test_bounds.size(), original_bounds.size());
454 for (size_t bound_idx = 0; bound_idx < test_bounds.at(0)->size(); ++bound_idx)
455 {
456 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_velocity_, original_bounds.at(0)->at(bound_idx).max_velocity_);
457 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_velocity_, original_bounds.at(0)->at(bound_idx).min_velocity_);
458 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).velocity_bounded_,
459 original_bounds.at(0)->at(bound_idx).velocity_bounded_);
460
461 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_acceleration_,
462 original_bounds.at(0)->at(bound_idx).max_acceleration_);
463 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_acceleration_,
464 original_bounds.at(0)->at(bound_idx).min_acceleration_);
465 ASSERT_EQ(test_bounds.at(0)->at(bound_idx).acceleration_bounded_,
466 original_bounds.at(0)->at(bound_idx).acceleration_bounded_);
467 }
468 ASSERT_EQ(test_trajectory.getWayPointDurationFromPrevious(1), trajectory.getWayPointDurationFromPrevious(1));
469
471 ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps";
472
473 test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_start);
474
475 // Iteratively recompute timestamps with same totg instance
476 for (size_t i = 0; i < totg_iterations; ++i)
477 {
478 bool totg_success = totg.computeTimeStamps(test_trajectory);
479 EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a same TOTG instance in iteration " << i;
480 }
481
482 test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_end);
483 }
484
485 // Test computing the dynamics repeatedly with one TOTG instance per call
486 moveit_msgs::msg::RobotTrajectory second_trajectory_msg_start, second_trajectory_msg_end;
487 {
488 robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */);
489
490 {
492 ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps";
493 }
494
495 test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_start);
496
497 // Iteratively recompute timestamps with new totg instances
498 for (size_t i = 0; i < totg_iterations; ++i)
499 {
501 bool totg_success = totg.computeTimeStamps(test_trajectory);
502 EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a new TOTG instance in iteration " << i;
503 }
504
505 test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_end);
506 }
507
508 // Make sure trajectories produce equal waypoints independent of TOTG instances
509 ASSERT_EQ(first_trajectory_msg_start, second_trajectory_msg_start);
510 ASSERT_EQ(first_trajectory_msg_end, second_trajectory_msg_end);
511
512 // Iterate on the original trajectory again
513 moveit_msgs::msg::RobotTrajectory third_trajectory_msg_end;
514
515 {
517 ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps";
518 }
519
520 for (size_t i = 0; i < totg_iterations; ++i)
521 {
523 bool totg_success = totg.computeTimeStamps(trajectory);
524 ASSERT_TRUE(totg_success) << "Failed to compute timestamps on a new TOTG instance in iteration " << i;
525 }
526
527 // Compare with previous work
528 trajectory.getRobotTrajectoryMsg(third_trajectory_msg_end);
529
530 // Make sure trajectories produce equal waypoints independent of TOTG instances
531 ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end);
532}
533
534TEST(time_optimal_trajectory_generation, testFixedNumWaypoints)
535{
536 // Test the version of computeTimeStamps() that gives a fixed num waypoints
537 constexpr size_t desired_num_waypoints = 42;
538
539 constexpr auto robot_name{ "panda" };
540 constexpr auto group_name{ "panda_arm" };
541
542 auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
543 ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name;
544 setAccelerationLimits(robot_model);
545 auto group = robot_model->getJointModelGroup(group_name);
546 ASSERT_TRUE(group) << "Failed to load joint model group " << group_name;
547 moveit::core::RobotState waypoint_state(robot_model);
548 waypoint_state.setToDefaultValues();
549
550 const double delta_t = 0.1;
551 robot_trajectory::RobotTrajectory trajectory(robot_model, group);
552 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
553 trajectory.addSuffixWayPoint(waypoint_state, delta_t);
554 waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 });
555 trajectory.addSuffixWayPoint(waypoint_state, delta_t);
556
557 ASSERT_TRUE(trajectory_processing::totgComputeTimeStamps(desired_num_waypoints, trajectory)) << "Failed to compute "
558 "time stamps";
559 // Allow +/-1 waypoint due to floating point error
560 EXPECT_NEAR(trajectory.getWayPointCount(), desired_num_waypoints, 1);
561}
562
563TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
564{
565 // Test a (prior) specific failure case
566 Eigen::VectorXd waypoint(1);
567 std::vector<Eigen::VectorXd> waypoints;
568
569 const double start_position = 1.881943;
570 waypoint << start_position;
571 waypoints.push_back(waypoint);
572 waypoint << 2.600542;
573 waypoints.push_back(waypoint);
574
575 Eigen::VectorXd max_velocities(1);
576 max_velocities << 4.54;
577 Eigen::VectorXd max_accelerations(1);
578 max_accelerations << 28.0;
579
580 auto path_maybe = Path::create(waypoints, 0.1 /* path tolerance */);
581 ASSERT_TRUE(path_maybe.has_value());
582
583 auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001 /* timestep */);
584 ASSERT_TRUE(trajectory_maybe.has_value());
585 const Trajectory& trajectory = trajectory_maybe.value();
586
587 EXPECT_GT(trajectory.getDuration(), 0.0);
588 const double traj_duration = trajectory.getDuration();
589 EXPECT_NEAR(0.320681, traj_duration, 1e-3);
590
591 // Start matches
592 EXPECT_DOUBLE_EQ(start_position, trajectory.getPosition(0.0)[0]);
593 // Start at rest and end at rest
594 EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
595 EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
596
597 // Check vels and accels at all points
598 for (double time = 0; time < traj_duration; time += 0.01)
599 {
600 // This trajectory has a single switching point
601 double t_switch = 0.1603407;
602 if (time < t_switch)
603 {
604 EXPECT_NEAR(trajectory.getAcceleration(time)[0], max_accelerations[0], 1e-3) << "Time: " << time;
605 }
606 else if (time > t_switch)
607 {
608 EXPECT_NEAR(trajectory.getAcceleration(time)[0], -max_accelerations[0], 1e-3) << "Time: " << time;
609 }
610 }
611}
612
613TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory)
614{
615 const Eigen::Vector2d max_velocity(1, 1);
616 const Path path = *Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) });
617
618 EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1)));
619 EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0)));
620 EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0)));
621}
622
623TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory)
624{
625 const Eigen::Vector2d max_velocity(1, 1);
626
627 EXPECT_TRUE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity,
628 /*max_acceleration=*/Eigen::Vector2d(0, 1)));
629 EXPECT_TRUE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity,
630 /*max_acceleration=*/Eigen::Vector2d(1, 0)));
631}
632
633TEST(time_optimal_trajectory_generation, testTimeStepZeroMakesTrajectoryInvalid)
634{
635 EXPECT_FALSE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
636 /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1),
637 /*time_step=*/0));
638}
639
640int main(int argc, char** argv)
641{
642 testing::InitGoogleTest(&argc, argv);
643 return RUN_ALL_TESTS();
644}
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
const moveit::core::RobotState & getLastWayPoint() const
const moveit::core::RobotModelConstPtr & getRobotModel() const
void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
double getWayPointDurationFromPrevious(std::size_t index) const
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
Compute a trajectory with waypoints spaced equally in time (according to resample_dt_)....
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.
double getDuration() const
Returns the optimal duration of the trajectory.
Eigen::VectorXd getVelocity(double time) const
Return the velocity vector for a given point in time.
static std::optional< Trajectory > create(const Path &path, const Eigen::VectorXd &max_velocity, const Eigen::VectorXd &max_acceleration, double time_step=0.001)
Generates a time-optimal trajectory.
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
std::vector< const JointModel::Bounds * > JointBoundsVector
bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0)
Compute a trajectory with the desired number of waypoints. Resampling the trajectory doesn't change t...
int main(int argc, char **argv)
TEST(time_optimal_trajectory_generation, test1)