test_limit_cartesian_speed.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Ken Anderson
5  * Copyright (c) 2020, Benjamin Scholz
6  * Copyright (c) 2021, Thies Oelerich
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the authors nor the names of other
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Authors: Benjamin Scholz, Thies Oelerich, based off test_time_parameterization.cpp by Ken Anderson */
38 
39 #include <gtest/gtest.h>
40 #include <fstream>
48 
49 // Static variables used in all tests
50 moveit::core::RobotModelConstPtr RMODEL = moveit::core::loadTestingRobotModel("panda");
52 
53 // Vector of distances between waypoints
54 std::vector<double> WAYPOINT_DISTANCES;
55 
56 // Name of logger
57 const char* LOGGER_NAME = "trajectory_processing";
58 
59 // Initialize one-joint, straight-line trajectory
61 {
62  const robot_model::JointModelGroup* group = trajectory.getGroup();
63  if (!group)
64  {
65  ROS_ERROR_NAMED(LOGGER_NAME, "Need to set the group");
66  return false;
67  }
68  // Get state of the robot
69  moveit::core::RobotState state(trajectory.getRobotModel());
70 
71  trajectory.clear();
72  // Initial waypoint
73  // Cartesian Position (in panda_link0 frame):
74  // [X: 0.30701957005161057, Y: 0, Z: 0.5902504197143968]
75  state.setVariablePosition("panda_joint1", 0);
76  state.setVariablePosition("panda_joint2", -0.785);
77  state.setVariablePosition("panda_joint3", 0);
78  state.setVariablePosition("panda_joint4", -2.356);
79  state.setVariablePosition("panda_joint5", 0);
80  state.setVariablePosition("panda_joint6", 1.571);
81  state.setVariablePosition("panda_joint7", 0.785);
82  trajectory.addSuffixWayPoint(state, 0.0);
83 
84  // First waypoint (+0.3 m in X direction)
85  // Cartesian Position (in panda_link0 frame):
86  // [X: 0.6070218670533757, Y: 0, Z: 0.5902504197143968]
87  state.setVariablePosition("panda_joint1", 0.00011058924053135735);
88  state.setVariablePosition("panda_joint2", 0.15980591412916012);
89  state.setVariablePosition("panda_joint3", -0.000269206763021151);
90  state.setVariablePosition("panda_joint4", -1.4550637907602342);
91  state.setVariablePosition("panda_joint5", 0.0006907805230834268);
92  state.setVariablePosition("panda_joint6", 1.61442119426705);
93  state.setVariablePosition("panda_joint7", 0.7845267455355481);
94  trajectory.addSuffixWayPoint(state, 0.0);
95  WAYPOINT_DISTANCES.push_back(0.3);
96 
97  // Second waypoint (+0.3 m in Y direction)
98  // Cartesian Position (in panda_link0 frame):
99  // [X: 0.6070218670533757, Y: 0.3, Z: 0.5902504197143968]
100  state.setVariablePosition("panda_joint1", 0.32516555661705315);
101  state.setVariablePosition("panda_joint2", 0.4668669802969372);
102  state.setVariablePosition("panda_joint3", 0.20650832887601522);
103  state.setVariablePosition("panda_joint4", -1.0166745094262262);
104  state.setVariablePosition("panda_joint5", -0.0931020003693296);
105  state.setVariablePosition("panda_joint6", 1.4764599578787032);
106  state.setVariablePosition("panda_joint7", 1.2855361917975465);
107  trajectory.addSuffixWayPoint(state, 0.0);
108  WAYPOINT_DISTANCES.push_back(0.3);
109 
110  // Third waypoint (-0.3 m in Z direction)
111  // Cartesian Position (in panda_link0 frame):
112  // [X: 0.6070218670533757, Y: 0.3, Z: 0.29026011026061577]
113  state.setVariablePosition("panda_joint1", 0.1928958411545848);
114  state.setVariablePosition("panda_joint2", 0.5600654280773957);
115  state.setVariablePosition("panda_joint3", 0.31117191776899084);
116  state.setVariablePosition("panda_joint4", -1.6747509079656924);
117  state.setVariablePosition("panda_joint5", -0.20206061876786893);
118  state.setVariablePosition("panda_joint6", 2.2024820844782385);
119  state.setVariablePosition("panda_joint7", 1.3635216856419043);
120  trajectory.addSuffixWayPoint(state, 0.0);
121  WAYPOINT_DISTANCES.push_back(0.3);
122 
123  return true;
124 }
125 
127 {
128  const robot_model::JointModelGroup* group = trajectory.getGroup();
129  const std::vector<int>& idx = group->getVariableIndexList();
130  unsigned int count = trajectory.getWayPointCount();
131 
133  "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds.");
134  for (unsigned i = 0; i < count; i++)
135  {
136  robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i);
137  ROS_INFO_STREAM_NAMED(LOGGER_NAME, "Waypoint " << i << " time " << trajectory.getWayPointDurationFromStart(i)
138  << " pos " << point->getVariablePosition(idx[0]) << " vel "
139  << point->getVariableVelocity(idx[0]) << " acc "
140  << point->getVariableAcceleration(idx[0]));
141 
142  if (i > 0)
143  {
144  robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
146  LOGGER_NAME,
147  "jrk " << (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
148  (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
149  }
150  }
151 }
152 
153 TEST(TestCartesianSpeed, TestCartesianEndEffectorSpeed)
154 {
157  const char* end_effector_link = "panda_link8";
158 
159  EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY));
162  size_t num_waypoints = TRAJECTORY.getWayPointCount();
163  robot_state::RobotStatePtr kinematic_state = TRAJECTORY.getFirstWayPointPtr();
164  Eigen::Isometry3d current_end_effector_state = kinematic_state->getGlobalLinkTransform(end_effector_link);
165  Eigen::Isometry3d next_end_effector_state;
166  double euclidean_distance = 0.0;
167 
168  // Check average speed of the total trajectory by exact calculations
169  for (size_t i = 0; i < num_waypoints - 1; i++)
170  {
171  kinematic_state = TRAJECTORY.getWayPointPtr(i + 1);
172  next_end_effector_state = kinematic_state->getGlobalLinkTransform(end_effector_link);
173  euclidean_distance += (next_end_effector_state.translation() - current_end_effector_state.translation()).norm();
174  current_end_effector_state = next_end_effector_state;
175  }
176  double avg_speed = euclidean_distance / TRAJECTORY.getWayPointDurationFromStart(num_waypoints);
177  ASSERT_NEAR(0.01, avg_speed, 2e-4);
178 
179  // Check average speed between waypoints using the information about the hand
180  // designed waypoints
181  for (size_t i = 1; i < num_waypoints; i++)
182  {
183  double current_avg_speed = WAYPOINT_DISTANCES[i - 1] / TRAJECTORY.getWayPointDurationFromPrevious(i);
184  ASSERT_NEAR(0.01, current_avg_speed, 2e-4);
185  }
186 }
187 
188 int main(int argc, char** argv)
189 {
190  testing::InitGoogleTest(&argc, argv);
191  return RUN_ALL_TESTS();
192 }
TRAJECTORY
robot_trajectory::RobotTrajectory TRAJECTORY(RMODEL, "panda_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
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
robot_trajectory::RobotTrajectory::getFirstWayPointPtr
moveit::core::RobotStatePtr & getFirstWayPointPtr()
Definition: robot_trajectory.h:161
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
robot_model.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
WAYPOINT_DISTANCES
std::vector< double > WAYPOINT_DISTANCES
Definition: test_limit_cartesian_speed.cpp:54
trajectory_processing::limitMaxCartesianLinkSpeed
bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory &trajectory, const double speed, const moveit::core::LinkModel *link_model)
Definition: limit_cartesian_speed.cpp:83
printTrajectory
void printTrajectory(robot_trajectory::RobotTrajectory &trajectory)
Definition: test_limit_cartesian_speed.cpp:126
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
main
int main(int argc, char **argv)
Definition: test_limit_cartesian_speed.cpp:188
robot_trajectory::RobotTrajectory::clear
RobotTrajectory & clear()
Definition: robot_trajectory.h:264
cartesian_interpolator.h
TEST
TEST(TestCartesianSpeed, TestCartesianEndEffectorSpeed)
Definition: test_limit_cartesian_speed.cpp:153
prev
EndPoint * prev[3]
robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious
double getWayPointDurationFromPrevious(std::size_t index) const
Definition: robot_trajectory.h:179
point
std::chrono::system_clock::time_point point
robot_model_test_utils.h
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
limit_cartesian_speed.h
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
RMODEL
moveit::core::RobotModelConstPtr RMODEL
Definition: test_limit_cartesian_speed.cpp:50
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:151
LOGGER_NAME
const char * LOGGER_NAME
Definition: test_limit_cartesian_speed.cpp:57
robot_state.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
initStraightTrajectory
bool initStraightTrajectory(robot_trajectory::RobotTrajectory &trajectory)
Definition: test_limit_cartesian_speed.cpp:60
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


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