39 #include <boost/program_options.hpp>
40 #include <moveit/robot_model_loader/robot_model_loader.h>
43 namespace po = boost::program_options;
46 int main(
int argc,
char* argv[])
51 bool reset_to_default;
52 po::options_description desc(
"Options");
55 (
"help",
"show help message")
56 (
"group", po::value<std::string>(&group)->default_value(
"all"),
"name of planning group")
57 (
"tip", po::value<std::string>(&tip)->default_value(
"default"),
"name of the end effector in the planning group")
58 (
"num", po::value<unsigned int>(&num)->default_value(100000),
"number of IK solutions to compute")
59 (
"reset_to_default", po::value<bool>(&reset_to_default)->default_value(
true),
60 "whether to reset IK seed to default state. If set to false, the seed is the "
61 "correct IK solution (to accelerate filling the cache).");
65 po::store(po::parse_command_line(argc, argv, desc), vm);
68 if (vm.count(
"help") != 0u)
70 std::cout << desc <<
"\n";
78 robot_model_loader::RobotModelLoader robot_model_loader;
79 const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
84 std::chrono::duration<double> ik_time(0);
85 std::chrono::time_point<std::chrono::system_clock>
start;
86 std::vector<moveit::core::JointModelGroup*> groups;
87 std::vector<std::string> end_effectors;
90 groups = kinematic_model->getJointModelGroups();
92 groups.push_back(kinematic_model->getJointModelGroup(group));
94 for (
const auto& group : groups)
97 if (group->getSolverInstance() ==
nullptr)
100 if (tip ==
"default")
101 group->getEndEffectorTips(end_effectors);
103 end_effectors = std::vector<std::string>(1, tip);
109 for (
const auto& end_effector : end_effectors)
111 if (end_effectors.size() == 1)
112 kinematic_state.
setFromIK(group, default_eef_states[0], end_effectors[0], 0.1);
114 kinematic_state.
setFromIK(group, default_eef_states, end_effectors, 0.1);
117 unsigned int num_failed_calls = 0, num_self_collisions = 0;
123 collision_result.
clear();
124 planning_scene.checkSelfCollision(collision_request, collision_result);
127 ++num_self_collisions;
130 for (
unsigned j = 0; j < end_effectors.size(); ++j)
132 if (reset_to_default)
134 start = std::chrono::system_clock::now();
135 if (end_effectors.size() == 1)
136 found_ik = kinematic_state.
setFromIK(group, end_effector_states[0], end_effectors[0], 0.1);
138 found_ik = kinematic_state.
setFromIK(group, end_effector_states, end_effectors, 0.1);
139 ik_time += std::chrono::system_clock::now() -
start;
145 "Avg. time per IK solver call is %g after %d calls. %g%% of calls failed to return a solution. "
146 "%g%% of random joint configurations were ignored due to self-collisions.",
147 ik_time.count() / (
double)i, i, 100. * num_failed_calls / i,
148 100. * num_self_collisions / (num_self_collisions + i));
150 ROS_INFO_NAMED(
"cached_ik.measure_ik_call_cost",
"Summary for group %s: %g %g %g", group->getName().c_str(),
151 ik_time.count() / (
double)i, 100. * num_failed_calls / i,
152 100. * num_self_collisions / (num_self_collisions + i));