test_time_parameterization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Ken Anderson
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 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: Ken Anderson */
36 
37 #include <gtest/gtest.h>
38 #include <fstream>
45 
46 // Static variables used in all tests
47 moveit::core::RobotModelConstPtr RMODEL = moveit::core::loadTestingRobotModel("pr2");
49 
50 // Initialize one-joint, 3 points exactly the same.
52 {
53  const int num = 3;
54  unsigned i;
55 
56  const moveit::core::JointModelGroup* group = trajectory.getGroup();
57  if (!group)
58  {
59  ROS_ERROR_NAMED("trajectory_processing", "Need to set the group");
60  return -1;
61  }
62  // leave initial velocity/acceleration unset
63  const std::vector<int>& idx = group->getVariableIndexList();
64  moveit::core::RobotState state(trajectory.getRobotModel());
65 
66  trajectory.clear();
67  for (i = 0; i < num; i++)
68  {
69  state.setVariablePosition(idx[0], 1.0);
70  trajectory.addSuffixWayPoint(state, 0.0);
71  }
72 
73  return 0;
74 }
75 
76 // Initialize one-joint, straight-line trajectory
78 {
79  const int num = 10;
80  const double max = 2.0;
81  unsigned i;
82 
83  const moveit::core::JointModelGroup* group = trajectory.getGroup();
84  if (!group)
85  {
86  ROS_ERROR_NAMED("trajectory_processing", "Need to set the group");
87  return -1;
88  }
89  // leave initial velocity/acceleration unset
90  const std::vector<int>& idx = group->getVariableIndexList();
91  moveit::core::RobotState state(trajectory.getRobotModel());
92 
93  trajectory.clear();
94  for (i = 0; i < num; i++)
95  {
96  state.setVariablePosition(idx[0], i * max / num);
97  trajectory.addSuffixWayPoint(state, 0.0);
98  }
99 
100  // leave final velocity/acceleration unset
101  state.setVariablePosition(idx[0], max);
102  trajectory.addSuffixWayPoint(state, 0.0);
103 
104  return 0;
105 }
106 
108 {
109  const moveit::core::JointModelGroup* group = trajectory.getGroup();
110  const std::vector<int>& idx = group->getVariableIndexList();
111  unsigned int count = trajectory.getWayPointCount();
112 
113  std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."
114  << std::endl;
115  std::cout << " Trajectory Points" << std::endl;
116  for (unsigned i = 0; i < count; i++)
117  {
118  moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i);
119  printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i),
120  point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]),
121  point->getVariableAcceleration(idx[0]));
122  if (i > 0)
123  {
124  moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
125  printf("jrk %6.2f",
126  (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
127  (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
128  }
129  printf("\n");
130  }
131 }
132 
133 TEST(TestTimeParameterization, TestIterativeParabolic)
134 {
137 
139  EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY));
140  std::cout << "IterativeParabolicTimeParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
143 }
144 
145 TEST(TestTimeParameterization, TestIterativeSpline)
146 {
147  trajectory_processing::IterativeSplineParameterization time_parameterization(false);
149 
151  EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY));
152  std::cout << "IterativeSplineParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
155 }
156 
157 TEST(TestTimeParameterization, TestIterativeSplineAddPoints)
158 {
159  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
161 
163  EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY));
164  std::cout << "IterativeSplineParameterization with added points took " << (ros::WallTime::now() - wt).toSec()
165  << std::endl;
168 }
169 
170 TEST(TestTimeParameterization, TestRepeatedPoint)
171 {
172  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
174 
175  EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY));
178 }
179 
180 int main(int argc, char** argv)
181 {
182  testing::InitGoogleTest(&argc, argv);
183  return RUN_ALL_TESTS();
184 }
TRAJECTORY
robot_trajectory::RobotTrajectory TRAJECTORY(RMODEL, "right_arm")
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:131
robot_trajectory::RobotTrajectory::getGroup
const moveit::core::JointModelGroup * getGroup() const
Definition: robot_trajectory.h:118
RMODEL
moveit::core::RobotModelConstPtr RMODEL
Definition: test_time_parameterization.cpp:47
moveit::core::JointModelGroup::getVariableIndexList
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
Definition: joint_model_group.h:351
robot_trajectory::RobotTrajectory::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: robot_trajectory.h:113
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
trajectory_processing::IterativeParabolicTimeParameterization
Definition: iterative_time_parameterization.h:79
robot_trajectory.h
robot_trajectory::RobotTrajectory::addSuffixWayPoint
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
Definition: robot_trajectory.h:205
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
robot_trajectory::RobotTrajectory::getWayPointDurationFromStart
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
Definition: robot_trajectory.cpp:505
iterative_time_parameterization.h
initStraightTrajectory
int initStraightTrajectory(robot_trajectory::RobotTrajectory &trajectory)
Definition: test_time_parameterization.cpp:77
robot_model.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
initRepeatedPointTrajectory
int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory &trajectory)
Definition: test_time_parameterization.cpp:51
moveit::core::RobotState::setVariablePosition
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known.
Definition: robot_state.h:254
ros::WallTime::now
static WallTime now()
robot_trajectory::RobotTrajectory::clear
RobotTrajectory & clear()
Definition: robot_trajectory.h:264
prev
EndPoint * prev[3]
ros::WallTime
iterative_spline_parameterization.h
main
int main(int argc, char **argv)
Definition: test_time_parameterization.cpp:180
point
std::chrono::system_clock::time_point point
printTrajectory
void printTrajectory(robot_trajectory::RobotTrajectory &trajectory)
Definition: test_time_parameterization.cpp:107
robot_model_test_utils.h
trajectory_processing::IterativeSplineParameterization::computeTimeStamps
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
Definition: iterative_spline_parameterization.cpp:75
trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
Definition: iterative_time_parameterization.cpp:490
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:151
robot_state.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
trajectory_processing::IterativeSplineParameterization
This class sets the timestamps of a trajectory to enforce velocity, acceleration constraints....
Definition: iterative_spline_parameterization.h:104
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
TEST
TEST(TestTimeParameterization, TestIterativeParabolic)
Definition: test_time_parameterization.cpp:133


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