measure_ik_call_cost.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Rice University
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 Rice University 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: Mark Moll */
36 
37 #include <cstring>
38 #include <chrono>
39 #include <ros/ros.h>
40 #include <boost/program_options.hpp>
43 
44 namespace po = boost::program_options;
45 
46 int main(int argc, char* argv[])
47 {
48  std::string group;
49  std::string tip;
50  unsigned int num;
51  bool reset_to_default;
52  po::options_description desc("Options");
53  desc.add_options()("help", "show help message")("group", po::value<std::string>(&group)->default_value("all"),
54  "name of planning group")(
55  "tip", po::value<std::string>(&tip)->default_value("default"), "name of the end effector in the planning group")(
56  "num", po::value<unsigned int>(&num)->default_value(100000),
57  "number of IK solutions to compute")("reset_to_default", po::value<bool>(&reset_to_default)->default_value(true),
58  "wether to reset IK seed to default state. If set to false, the seed is the "
59  "correct IK solution (to accelerate filling the cache).");
60 
61  po::variables_map vm;
62  po::store(po::parse_command_line(argc, argv, desc), vm);
63  po::notify(vm);
64 
65  if (vm.count("help") != 0u)
66  {
67  std::cout << desc << "\n";
68  return 1;
69  }
70 
71  ros::init(argc, argv, "measure_ik_call_cost");
73  spinner.start();
74 
76  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
78  robot_state::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst();
79  collision_detection::CollisionRequest collision_request;
80  collision_detection::CollisionResult collision_result;
81  std::vector<double> joint_values;
82  std::chrono::duration<double> ik_time;
83  std::chrono::time_point<std::chrono::system_clock> start, end;
84  std::vector<robot_state::JointModelGroup*> groups;
85  std::vector<std::string> end_effectors;
86 
87  if (group == "all")
88  groups = kinematic_model->getJointModelGroups();
89  else
90  groups.push_back(kinematic_model->getJointModelGroup(group));
91 
92  for (const auto& group : groups)
93  {
94  // skip group if there's no IK solver
95  if (group->getSolverInstance() == nullptr)
96  continue;
97 
98  if (tip == "default")
99  group->getEndEffectorTips(end_effectors);
100  else
101  end_effectors = std::vector<std::string>(1, tip);
102 
103  // perform first IK call to load the cache, so that the time for loading is not included in
104  // average IK call time
105  kinematic_state.setToDefaultValues();
106  EigenSTL::vector_Affine3d default_eef_states;
107  for (const auto& end_effector : end_effectors)
108  default_eef_states.push_back(kinematic_state.getGlobalLinkTransform(end_effector));
109  if (end_effectors.size() == 1)
110  kinematic_state.setFromIK(group, default_eef_states[0], end_effectors[0], 1, 0.1);
111  else
112  kinematic_state.setFromIK(group, default_eef_states, end_effectors, 1, 0.1);
113 
114  bool found_ik;
115  unsigned int num_failed_calls = 0, num_self_collisions = 0;
116  EigenSTL::vector_Affine3d end_effector_states(end_effectors.size());
117  unsigned int i = 0;
118  while (i < num)
119  {
120  kinematic_state.setToRandomPositions(group);
121  collision_result.clear();
122  planning_scene.checkSelfCollision(collision_request, collision_result);
123  if (collision_result.collision)
124  {
125  ++num_self_collisions;
126  continue;
127  }
128  for (unsigned j = 0; j < end_effectors.size(); ++j)
129  end_effector_states[j] = kinematic_state.getGlobalLinkTransform(end_effectors[j]);
130  if (reset_to_default)
131  kinematic_state.setToDefaultValues();
132  start = std::chrono::system_clock::now();
133  if (end_effectors.size() == 1)
134  found_ik = kinematic_state.setFromIK(group, end_effector_states[0], end_effectors[0], 10, 0.1);
135  else
136  found_ik = kinematic_state.setFromIK(group, end_effector_states, end_effectors, 10, 0.1);
137  ik_time += std::chrono::system_clock::now() - start;
138  if (!found_ik)
139  num_failed_calls++;
140  ++i;
141  if (i % 100 == 0)
142  ROS_INFO_NAMED("cached_ik.measure_ik_call_cost",
143  "Avg. time per IK solver call is %g after %d calls. %g%% of calls failed to return a solution. "
144  "%g%% of random joint configurations were ignored due to self-collisions.",
145  ik_time.count() / (double)i, i, 100. * num_failed_calls / i,
146  100. * num_self_collisions / (num_self_collisions + i));
147  }
148  ROS_INFO_NAMED("cached_ik.measure_ik_call_cost", "Summary for group %s: %g %g %g", group->getName().c_str(),
149  ik_time.count() / (double)i, 100. * num_failed_calls / i,
150  100. * num_self_collisions / (num_self_collisions + i));
151  }
152 
153  ros::shutdown();
154  return 0;
155 }
robot_state::RobotState & getCurrentStateNonConst()
#define ROS_INFO_NAMED(name,...)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void spinner()
int main(int argc, char *argv[])
ROSCPP_DECL void shutdown()
const robot_model::RobotModelPtr & getModel() const


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:41