trajectory_tests.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2015, Fetch Robotics Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Fetch Robotics Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
21  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27  */
28 
29 // Author: Michael Ferguson
30 
31 #include <gtest/gtest.h>
32 #include <robot_controllers/trajectory.h>
33 
34 TEST(TrajectoryTests, test_unwind)
35 {
36  robot_controllers::Trajectory t;
37  t.points.resize(3);
38  t.points[0].q.resize(3);
39  t.points[0].q[0] = 3.0;
40  t.points[0].q[1] = -3.0;
41  t.points[0].q[2] = -3.0;
42  t.points[1].q.resize(3);
43  t.points[1].q[0] = -3.0;
44  t.points[1].q[1] = 3.0;
45  t.points[1].q[2] = 3.0;
46  t.points[2].q.resize(3);
47  t.points[2].q[0] = 3.0;
48  t.points[2].q[1] = -3.0;
49  t.points[2].q[2] = -3.0;
50 
51  std::vector<bool> continuous(3, false);
52  continuous[0] = true;
53  continuous[1] = true;
54 
55  // Test windup
56  EXPECT_TRUE(windupTrajectory(continuous, t));
57  // First joint should be wound up
58  EXPECT_EQ(3.0, t.points[0].q[0]);
59  EXPECT_EQ(3.2831853071795862, t.points[1].q[0]);
60  EXPECT_EQ(3.0, t.points[2].q[0]);
61  // Second joint should be wound up
62  EXPECT_EQ(-3.0, t.points[0].q[1]);
63  EXPECT_EQ(-3.2831853071795862, t.points[1].q[1]);
64  EXPECT_EQ(-3.0, t.points[2].q[1]);
65  // Third joint should be unmodified
66  EXPECT_EQ(-3.0, t.points[0].q[2]);
67  EXPECT_EQ(3.0, t.points[1].q[2]);
68  EXPECT_EQ(-3.0, t.points[2].q[2]);
69 
70  // Test unwind
71  EXPECT_TRUE(unwindTrajectoryPoint(continuous, t.points[0]));
72  EXPECT_EQ(3.0, t.points[0].q[0]);
73  EXPECT_EQ(-3.0, t.points[0].q[1]);
74  EXPECT_EQ(-3.0, t.points[0].q[2]);
75  EXPECT_TRUE(unwindTrajectoryPoint(continuous, t.points[1]));
76  EXPECT_EQ(-3.0, t.points[1].q[0]);
77  EXPECT_EQ(3.0, t.points[1].q[1]);
78  EXPECT_EQ(3.0, t.points[1].q[2]);
79  EXPECT_TRUE(unwindTrajectoryPoint(continuous, t.points[2]));
80  EXPECT_EQ(3.0, t.points[2].q[0]);
81  EXPECT_EQ(-3.0, t.points[2].q[1]);
82  EXPECT_EQ(-3.0, t.points[2].q[2]);
83 
84 
85  // Make sure we catch mis-sized vectors
86  t.points[1].q.resize(2);
87  EXPECT_FALSE(windupTrajectory(continuous, t));
88 
89  continuous.resize(4);
90  t.points[1].q.resize(3);
91  EXPECT_FALSE(windupTrajectory(continuous, t));
92 }
93 
94 TEST(TrajectoryTests, test_multiple_unwinds)
95 {
96  robot_controllers::Trajectory t;
97  t.points.resize(10);
98  t.points[0].q.push_back(-3.0);
99  t.points[1].q.push_back(3.0);
100  t.points[2].q.push_back(1.0);
101  t.points[3].q.push_back(-1.0);
102  t.points[4].q.push_back(-3.0);
103  t.points[5].q.push_back(3.0);
104  t.points[6].q.push_back(1.0);
105  t.points[7].q.push_back(-1.0);
106  t.points[8].q.push_back(-3.0);
107  t.points[9].q.push_back(3.0);
108 
109  std::vector<bool> continuous(1, true);
110 
111  // Test unwind
112  EXPECT_TRUE(windupTrajectory(continuous, t));
113  // First joint should be unwound
114  EXPECT_EQ(-3.0, t.points[0].q[0]);
115  EXPECT_EQ(-3.2831853071795862, t.points[1].q[0]);
116  EXPECT_EQ(-5.2831853071795862, t.points[2].q[0]);
117  EXPECT_EQ(-7.2831853071795862, t.points[3].q[0]);
118  EXPECT_EQ(-9.2831853071795862, t.points[4].q[0]);
119  EXPECT_EQ(-9.566370614359172, t.points[5].q[0]);
120  EXPECT_EQ(-11.566370614359172, t.points[6].q[0]);
121  EXPECT_EQ(-13.566370614359172, t.points[7].q[0]);
122  EXPECT_EQ(-15.566370614359172, t.points[8].q[0]);
123  EXPECT_EQ(-15.849555921538759, t.points[9].q[0]);
124 }
125 
126 // Run all the tests that were declared with TEST()
127 int main(int argc, char **argv)
128 {
129  testing::InitGoogleTest(&argc, argv);
130  return RUN_ALL_TESTS();
131 }
robot_controllers::Trajectory
Definition: trajectory.h:59
robot_controllers::Trajectory::points
std::vector< TrajectoryPoint > points
Definition: trajectory.h:61
robot_controllers::windupTrajectory
bool windupTrajectory(std::vector< bool > continuous, Trajectory &trajectory)
Windup the trajectory so that continuous joints do not wrap.
Definition: trajectory.h:253
TEST
TEST(TrajectoryTests, test_unwind)
Definition: trajectory_tests.cpp:34
robot_controllers::unwindTrajectoryPoint
bool unwindTrajectoryPoint(std::vector< bool > continuous, TrajectoryPoint &p)
Definition: trajectory.h:285
main
int main(int argc, char **argv)
Definition: trajectory_tests.cpp:127

robot_controllers
Author(s): Michael Ferguson
autogenerated on Sun Sep 27 2020 03:22:39