4 tf2::Quaternion x_rot(0,0,0,1);
5 tf2::Quaternion y_rot(0,0,0.707,0.707);
6 tf2::Quaternion z_rot(0,-0.707,0,0.707);
7 std::vector<tf2::Quaternion> basic_rot{x_rot, x_rot.inverse(), y_rot, y_rot.inverse(), z_rot.inverse()};
8 ROS_INFO(
"=> 5 Basic grasp roatations (x, -x, y, -y, -z) ");
10 for(
auto&od_q: s_od_q.second){
11 od_q.second = basic_rot;
17 std::vector<tf2::Transform> trans;
18 for(
long unsigned int i = 0; i < map.size(); i++) {
19 for(
auto&s_od_q: task_space){
20 for(
auto&od_q: s_od_q.second){
21 for(
auto&q: od_q.second){
22 if (q.angle(map[i].getRotation()) < 0.349066f){
23 inv_map.push_back(tf2::Transform(q, map[i].getOrigin()).inverse());
31 ROS_INFO(
"caculated [inv_map] contains %li entrys...", inv_map.size());