test_robot_trajectory.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 #include <gtest/gtest.h>
42 
43 class RobotTrajectoryTestFixture : public testing::Test
44 {
45 protected:
46  moveit::core::RobotModelConstPtr robot_model_;
47  moveit::core::RobotStatePtr robot_state_;
48  const std::string robot_model_name_ = "panda";
49  const std::string arm_jmg_name_ = "panda_arm";
50 
51 protected:
52  void SetUp() override
53  {
55  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
56  robot_state_->setToDefaultValues();
57  robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0);
58  robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1);
59  robot_state_->update();
60  }
61 
62  void TearDown() override
63  {
64  }
65 
66  void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
67  {
68  // Init a traj
69  ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_))
70  << "Robot model does not have group: " << arm_jmg_name_;
71 
72  trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, arm_jmg_name_);
73 
74  EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match";
75  EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty";
76 
77  double duration_from_previous = 0.1;
78  std::size_t waypoint_count = 5;
79  for (std::size_t ix = 0; ix < waypoint_count; ++ix)
80  trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous);
81  // Quick check that getDuration is working correctly
82  EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
83  << "Generated trajectory duration incorrect";
84  EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size())
85  << "Generated trajectory has the wrong number of waypoints";
86  }
87 
88  void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory,
89  robot_trajectory::RobotTrajectoryPtr& trajectory_copy, bool deepcopy)
90  {
91  // Copy the trajectory
92  trajectory_copy = std::make_shared<robot_trajectory::RobotTrajectory>(*trajectory, deepcopy);
93  // Quick check that the getDuration values match
94  EXPECT_EQ(trajectory_copy->getDuration(), trajectory->getDuration());
95  EXPECT_EQ(trajectory_copy->getWayPointDurations().size(), trajectory->getWayPointDurations().size());
96  }
97 
98  void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
99  {
101  // Get the first waypoint by POINTER, modify it, and check that the value WAS updated in trajectory
103  // Get the first waypoint by shared pointer
104  moveit::core::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0);
105  // Get the first waypoint joint values
106  std::vector<double> trajectory_first_state;
107  trajectory_first_waypoint->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state);
108 
109  // Modify the first waypoint joint values
110  trajectory_first_state[0] += 0.01;
111  trajectory_first_waypoint->setJointGroupPositions(arm_jmg_name_, trajectory_first_state);
112 
113  // Check that the trajectory's first waypoint was updated
114  moveit::core::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0);
115  std::vector<double> trajectory_first_state_after_update;
116  trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
117  EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]);
118 
119  // Modify the first waypoint duration
120  double trajectory_first_duration_before_update = trajectory->getWayPointDurationFromPrevious(0);
121  double new_duration = trajectory_first_duration_before_update + 0.1;
122  trajectory->setWayPointDurationFromPrevious(0, new_duration);
123 
124  // Check that the trajectory's first duration was updated
125  EXPECT_EQ(trajectory->getWayPointDurationFromPrevious(0), new_duration);
126  }
127 
128  void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
129  {
131  // Get the first waypoint by VALUE, modify it, and check that the value WAS NOT updated in trajectory
133  // Get the first waypoint by shared pointer
134  moveit::core::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0);
135  // Get the first waypoint joint values
136  std::vector<double> trajectory_first_state;
137  trajectory_first_waypoint.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state);
138 
139  // Modify the first waypoint joint values
140  trajectory_first_state[0] += 0.01;
141  trajectory_first_waypoint.setJointGroupPositions(arm_jmg_name_, trajectory_first_state);
142 
143  // Check that the trajectory's first waypoint was updated
144  moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
145  std::vector<double> trajectory_first_state_after_update;
146  trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
147  EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]);
148  }
149 };
150 
151 TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr)
152 {
153  robot_trajectory::RobotTrajectoryPtr trajectory;
154  initTestTrajectory(trajectory);
155  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
156 }
157 
158 TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByValue)
159 {
160  robot_trajectory::RobotTrajectoryPtr trajectory;
161  initTestTrajectory(trajectory);
162  modifyFirstWaypointAndCheckTrajectory(trajectory);
163 }
164 
166 {
167  robot_trajectory::RobotTrajectoryPtr trajectory;
168  initTestTrajectory(trajectory);
169  moveit_msgs::RobotTrajectory initial_trajectory_msg;
170  trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);
171 
172  trajectory->reverse().reverse();
173 
174  moveit_msgs::RobotTrajectory edited_trajectory_msg;
175  trajectory->getRobotTrajectoryMsg(edited_trajectory_msg);
176 
177  EXPECT_EQ(initial_trajectory_msg, edited_trajectory_msg);
178 }
179 
181 {
182  robot_trajectory::RobotTrajectoryPtr initial_trajectory;
183  initTestTrajectory(initial_trajectory);
184  moveit_msgs::RobotTrajectory initial_trajectory_msg;
185  initial_trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);
186 
187  robot_trajectory::RobotTrajectory trajectory(robot_model_);
188  trajectory.setGroupName(arm_jmg_name_)
189  .clear()
190  .setRobotTrajectoryMsg(*robot_state_, initial_trajectory_msg)
191  .reverse()
192  .addSuffixWayPoint(*robot_state_, 0.1)
193  .addPrefixWayPoint(*robot_state_, 0.1)
194  .insertWayPoint(1, *robot_state_, 0.1)
195  .append(*initial_trajectory, 0.1);
196 
197  EXPECT_EQ(trajectory.getGroupName(), arm_jmg_name_);
198  EXPECT_EQ(trajectory.getWayPointCount(), initial_trajectory->getWayPointCount() * 2 + 3);
199 }
200 
201 TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy)
202 {
203  bool deepcopy = false;
204 
205  robot_trajectory::RobotTrajectoryPtr trajectory;
206  robot_trajectory::RobotTrajectoryPtr trajectory_copy;
207 
208  initTestTrajectory(trajectory);
209  copyTrajectory(trajectory, trajectory_copy, deepcopy);
210  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
211 
212  // Check that modifying the waypoint also modified the trajectory
213  moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
214  std::vector<double> trajectory_first_state_after_update;
215  trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
216 
217  // Get the first waypoint in the modified trajectory_copy
218  moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0);
219  std::vector<double> trajectory_copy_first_state_after_update;
220  trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
221  trajectory_copy_first_state_after_update);
222 
223  // Check that we updated the joint position correctly in the trajectory
224  EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
225 }
226 
227 TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy)
228 {
229  bool deepcopy = true;
230 
231  robot_trajectory::RobotTrajectoryPtr trajectory;
232  robot_trajectory::RobotTrajectoryPtr trajectory_copy;
233 
234  initTestTrajectory(trajectory);
235  copyTrajectory(trajectory, trajectory_copy, deepcopy);
236  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
237 
238  // Check that modifying the waypoint also modified the trajectory
239  moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
240  std::vector<double> trajectory_first_state_after_update;
241  trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
242 
243  // Get the first waypoint in the modified trajectory_copy
244  moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0);
245  std::vector<double> trajectory_copy_first_state_after_update;
246  trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
247  trajectory_copy_first_state_after_update);
248 
249  // Check that joint positions changed in the original trajectory but not the deep copy
250  EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
251  // Check that the first waypoint duration changed in the original trajectory but not the deep copy
252  EXPECT_NE(trajectory->getWayPointDurationFromPrevious(0), trajectory_copy->getWayPointDurationFromPrevious(0));
253 }
254 
255 int main(int argc, char** argv)
256 {
257  testing::InitGoogleTest(&argc, argv);
258  return RUN_ALL_TESTS();
259 }
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:131
RobotTrajectoryTestFixture::arm_jmg_name_
const std::string arm_jmg_name_
Definition: test_robot_trajectory.cpp:81
RobotTrajectoryTestFixture::TearDown
void TearDown() override
Definition: test_robot_trajectory.cpp:94
RobotTrajectoryTestFixture::copyTrajectory
void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr &trajectory, robot_trajectory::RobotTrajectoryPtr &trajectory_copy, bool deepcopy)
Definition: test_robot_trajectory.cpp:120
robot_trajectory::RobotTrajectory::reverse
RobotTrajectory & reverse()
Definition: robot_trajectory.cpp:152
robot_trajectory.h
RobotTrajectoryTestFixture
Definition: test_robot_trajectory.cpp:43
RobotTrajectoryTestFixture::initTestTrajectory
void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
Definition: test_robot_trajectory.cpp:98
robot_trajectory::RobotTrajectory::addSuffixWayPoint
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
Definition: robot_trajectory.h:205
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
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...
Definition: robot_state.h:757
RobotTrajectoryTestFixture::SetUp
void SetUp() override
Definition: test_robot_trajectory.cpp:84
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
Definition: robot_trajectory.cpp:389
robot_model.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
RobotTrajectoryTestFixture::modifyFirstWaypointAndCheckTrajectory
void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
Definition: test_robot_trajectory.cpp:160
robot_trajectory::RobotTrajectory::setGroupName
RobotTrajectory & setGroupName(const std::string &group_name)
Definition: robot_trajectory.h:125
robot_trajectory::RobotTrajectory::clear
RobotTrajectory & clear()
Definition: robot_trajectory.h:264
main
int main(int argc, char **argv)
Definition: test_robot_trajectory.cpp:255
RobotTrajectoryTestFixture::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: test_robot_trajectory.cpp:78
RobotTrajectoryTestFixture::modifyFirstWaypointPtrAndCheckTrajectory
void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
Definition: test_robot_trajectory.cpp:130
RobotTrajectoryTestFixture::robot_model_name_
const std::string robot_model_name_
Definition: test_robot_trajectory.cpp:80
robot_model_test_utils.h
RobotTrajectoryTestFixture::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: test_robot_trajectory.cpp:79
TEST_F
TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr)
Definition: test_robot_trajectory.cpp:151
robot_trajectory::RobotTrajectory::getGroupName
const std::string & getGroupName() const
Definition: robot_trajectory.cpp:106
moveit::core::RobotState::setJointGroupPositions
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...
Definition: robot_state.h:671
robot_trajectory::RobotTrajectory::addPrefixWayPoint
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:223
robot_trajectory::RobotTrajectory::append
RobotTrajectory & append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
Definition: robot_trajectory.cpp:135
robot_state.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:117
robot_trajectory::RobotTrajectory::insertWayPoint
RobotTrajectory & insertWayPoint(std::size_t index, const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:236


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jul 21 2024 02:24:29