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 robot_model::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
77 // Can specify init/final velocity/acceleration,
78 // but not all time parameterization methods may accept it.
79 int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double vel_i = 0.0, double vel_f = 0.0,
80  double acc_i = 0.0, double acc_f = 0.0)
81 {
82  const int num = 10;
83  const double max = 2.0;
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], i * max / num);
100  trajectory.addSuffixWayPoint(state, 0.0);
101  }
102 
103  // leave final velocity/acceleration unset
104  state.setVariablePosition(idx[0], max);
105  trajectory.addSuffixWayPoint(state, 0.0);
106 
107  return 0;
108 }
109 
111 {
112  const robot_model::JointModelGroup* group = trajectory.getGroup();
113  const std::vector<int>& idx = group->getVariableIndexList();
114  unsigned int count = trajectory.getWayPointCount();
115 
116  std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."
117  << std::endl;
118  std::cout << " Trajectory Points" << std::endl;
119  for (unsigned i = 0; i < count; i++)
120  {
121  robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i);
122  printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i),
123  point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]),
124  point->getVariableAcceleration(idx[0]));
125  if (i > 0)
126  {
127  robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
128  printf("jrk %6.2f",
129  (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
130  (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
131  }
132  printf("\n");
133  }
134 }
135 
136 TEST(TestTimeParameterization, TestIterativeParabolic)
137 {
140 
142  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
143  std::cout << "IterativeParabolicTimeParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
146 }
147 
148 TEST(TestTimeParameterization, TestIterativeSpline)
149 {
150  trajectory_processing::IterativeSplineParameterization time_parameterization(false);
152 
154  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
155  std::cout << "IterativeSplineParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl;
158 }
159 
160 TEST(TestTimeParameterization, TestIterativeSplineAddPoints)
161 {
162  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
164 
166  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
167  std::cout << "IterativeSplineParameterization with added points took " << (ros::WallTime::now() - wt).toSec()
168  << std::endl;
171 }
172 
173 TEST(TestTimeParameterization, TestRepeatedPoint)
174 {
175  trajectory_processing::IterativeSplineParameterization time_parameterization(true);
177 
178  EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory));
181 }
182 
183 int main(int argc, char** argv)
184 {
185  testing::InitGoogleTest(&argc, argv);
186  return RUN_ALL_TESTS();
187 }
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
void printTrajectory(robot_trajectory::RobotTrajectory &trajectory)
int main(int argc, char **argv)
std::size_t getWayPointCount() const
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
TEST(TestTimeParameterization, TestIterativeParabolic)
#define EXPECT_EQ(a, b)
const robot_model::JointModelGroup * getGroup() const
Maintain a sequence of waypoints and the time durations between these waypoints.
This class modifies the timestamps of a trajectory to respect velocity and acceleration constraints...
const robot_model::RobotModelConstPtr & getRobotModel() const
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
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory &trajectory)
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)
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
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
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 Mon Dec 2 2019 03:56:59