40 #include <boost/program_options.hpp> 44 namespace po = boost::program_options;
46 int main(
int argc,
char* argv[])
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).");
62 po::store(po::parse_command_line(argc, argv, desc), vm);
65 if (vm.count(
"help") != 0u)
67 std::cout << desc <<
"\n";
71 ros::init(argc, argv,
"measure_ik_call_cost");
76 robot_model::RobotModelPtr kinematic_model = robot_model_loader.
getModel();
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;
88 groups = kinematic_model->getJointModelGroups();
90 groups.push_back(kinematic_model->getJointModelGroup(group));
92 for (
const auto& group : groups)
95 if (group->getSolverInstance() ==
nullptr)
99 group->getEndEffectorTips(end_effectors);
101 end_effectors = std::vector<std::string>(1, tip);
105 kinematic_state.setToDefaultValues();
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);
112 kinematic_state.setFromIK(group, default_eef_states, end_effectors, 1, 0.1);
115 unsigned int num_failed_calls = 0, num_self_collisions = 0;
120 kinematic_state.setToRandomPositions(group);
121 collision_result.
clear();
125 ++num_self_collisions;
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);
136 found_ik = kinematic_state.setFromIK(group, end_effector_states, end_effectors, 10, 0.1);
137 ik_time += std::chrono::system_clock::now() - start;
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));
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));
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()
int main(int argc, char *argv[])
ROSCPP_DECL void shutdown()
const robot_model::RobotModelPtr & getModel() const