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>
39 #include <boost/filesystem/path.hpp>
40 #include <urdf_parser/urdf_parser.h>
41 #include <moveit_resources/config.h>
47 
48 // Function declarations
49 moveit::core::RobotModelConstPtr loadModel();
50 
51 // Static variables used in all tests
52 moveit::core::RobotModelConstPtr rmodel = loadModel();
54 
55 // Load pr2. Take a look at test/ in planning_scene, robot_mode,
56 // and robot_state for inspiration.
57 moveit::core::RobotModelConstPtr loadModel()
58 {
59  moveit::core::RobotModelConstPtr robot_model;
60  urdf::ModelInterfaceSharedPtr urdf_model;
61  srdf::ModelSharedPtr srdf_model;
62  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
63  std::string xml_string;
64  std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
65  EXPECT_TRUE(xml_file.is_open());
66  while (xml_file.good())
67  {
68  std::string line;
69  std::getline(xml_file, line);
70  xml_string += (line + "\n");
71  }
72  xml_file.close();
73  urdf_model = urdf::parseURDF(xml_string);
74  srdf_model.reset(new srdf::Model());
75  srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string());
76  robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
77  return robot_model;
78 }
79 
80 // Initialize one-joint, 3 points exactly the same.
82 {
83  const int num = 3;
84  unsigned i;
85 
86  const robot_model::JointModelGroup* group = trajectory.getGroup();
87  if (!group)
88  {
89  ROS_ERROR_NAMED("trajectory_processing", "Need to set the group");
90  return -1;
91  }
92  // leave initial velocity/acceleration unset
93  const std::vector<int>& idx = group->getVariableIndexList();
94  moveit::core::RobotState state(trajectory.getRobotModel());
95 
96  trajectory.clear();
97  for (i = 0; i < num; i++)
98  {
99  state.setVariablePosition(idx[0], 1.0);
100  trajectory.addSuffixWayPoint(state, 0.0);
101  }
102 
103  return 0;
104 }
105 
106 // Initialize one-joint, straight-line trajectory
107 // Can specify init/final velocity/acceleration,
108 // but not all time parameterization methods may accept it.
109 int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double vel_i = 0.0, double vel_f = 0.0,
110  double acc_i = 0.0, double acc_f = 0.0)
111 {
112  const int num = 10;
113  const double max = 2.0;
114  unsigned i;
115 
116  const robot_model::JointModelGroup* group = trajectory.getGroup();
117  if (!group)
118  {
119  ROS_ERROR_NAMED("trajectory_processing", "Need to set the group");
120  return -1;
121  }
122  // leave initial velocity/acceleration unset
123  const std::vector<int>& idx = group->getVariableIndexList();
124  moveit::core::RobotState state(trajectory.getRobotModel());
125 
126  trajectory.clear();
127  for (i = 0; i < num; i++)
128  {
129  state.setVariablePosition(idx[0], i * max / num);
130  trajectory.addSuffixWayPoint(state, 0.0);
131  }
132 
133  // leave final velocity/acceleration unset
134  state.setVariablePosition(idx[0], max);
135  trajectory.addSuffixWayPoint(state, 0.0);
136 
137  return 0;
138 }
139 
141 {
142  const robot_model::JointModelGroup* group = trajectory.getGroup();
143  const std::vector<int>& idx = group->getVariableIndexList();
144  unsigned int count = trajectory.getWayPointCount();
145 
146  std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."
147  << std::endl;
148  std::cout << " Trajectory Points" << std::endl;
149  for (unsigned i = 0; i < count; i++)
150  {
151  robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i);
152  printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i),
153  point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]),
154  point->getVariableAcceleration(idx[0]));
155  if (i > 0)
156  {
157  robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
158  printf("jrk %6.2f",
159  (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
160  (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
161  }
162  printf("\n");
163  }
164 }
165 
166 TEST(TestTimeParameterization, TestIterativeParabolic)
167 {
170 
172  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
173  std::cout << "IterativeParabolicTimeParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
176 }
177 
178 TEST(TestTimeParameterization, TestIterativeSpline)
179 {
180  trajectory_processing::IterativeSplineParameterization time_parameterization(false);
182 
184  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
185  std::cout << "IterativeSplineParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
188 }
189 
190 TEST(TestTimeParameterization, TestIterativeSplineAddPoints)
191 {
192  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
194 
196  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
197  std::cout << "IterativeSplineParameterization with added points took " << (ros::WallTime::now() - wt).toSec()
198  << std::endl;
201 }
202 
203 TEST(TestTimeParameterization, TestRepeatedPoint)
204 {
205  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
207 
208  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
211 }
212 
213 int main(int argc, char** argv)
214 {
215  testing::InitGoogleTest(&argc, argv);
216  return RUN_ALL_TESTS();
217 }
Core components of MoveIt!
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
void printTrajectory(robot_trajectory::RobotTrajectory &trajectory)
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
int main(int argc, char **argv)
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
TEST(TestTimeParameterization, TestIterativeParabolic)
#define EXPECT_EQ(a, b)
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
const robot_model::JointModelGroup * getGroup() const
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
Maintain a sequence of waypoints and the time durations between these waypoints.
def xml_string(rootXml, addHeader=True)
This class modifies the timestamps of a trajectory to respect velocity and acceleration constraints...
std::size_t getWayPointCount() const
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
static WallTime now()
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory &trajectory)
const robot_model::RobotModelConstPtr & getRobotModel() const
int initStraightTrajectory(robot_trajectory::RobotTrajectory &trajectory, double vel_i=0.0, double vel_f=0.0, double acc_i=0.0, double acc_f=0.0)
moveit::core::RobotModelConstPtr loadModel()
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
Add a point to the trajectory.
double max(double a, double b)
#define EXPECT_TRUE(args)
moveit::core::RobotModelConstPtr rmodel
This class sets the timestamps of a trajectory to enforce velocity, acceleration constraints. Initial/final velocities and accelerations may be specified in the trajectory. Velocity and acceleration limits are specified in the model.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56