35 #include <bio_ik/goal_types.h> 37 #include <geometric_shapes/bodies.h> 38 #include <geometric_shapes/shapes.h> 45 #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) 46 void TouchGoal::describe(GoalContext& context)
const 48 LinkGoalBase::describe(context);
49 auto* robot_model = &context.getRobotModel();
51 static std::map<const moveit::core::RobotModel*, CollisionModel*> collision_cache;
52 if(collision_cache.find(robot_model) == collision_cache.end()) collision_cache[&context.getRobotModel()] =
new CollisionModel();
53 collision_model = collision_cache[robot_model];
54 collision_model->collision_links.resize(robot_model->getLinkModelCount());
56 link_model = robot_model->getLinkModel(this->getLinkName());
57 size_t link_index = link_model->getLinkIndex();
58 auto touch_goal_normal = normal.normalized();
60 auto& collision_link = collision_model->collision_links[link_index];
61 if(!collision_link.initialized)
63 collision_link.initialized =
true;
64 collision_link.shapes.resize(link_model->getShapes().size());
65 for(
size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++)
67 collision_link.shapes[shape_index] = std::make_shared<CollisionShape>();
68 auto& s = *collision_link.shapes[shape_index];
69 s.frame = Frame(link_model->getCollisionOriginTransforms()[shape_index]);
70 auto* shape = link_model->getShapes()[shape_index].get();
72 if(
auto* mesh = dynamic_cast<const shapes::Mesh*>(shape))
74 struct : bodies::ConvexMesh
76 std::vector<fcl::Vec3f> points;
77 std::vector<int> polygons;
78 std::vector<fcl::Vec3f> plane_normals;
79 std::vector<double> plane_dis;
80 void init(
const shapes::Shape* shape)
83 scaled_vertices_ =
nullptr;
85 static std::mutex mutex;
86 std::lock_guard<std::mutex> lock(mutex);
89 for(
const auto& v : getVertices())
90 points.emplace_back(v.x(), v.y(), v.z());
92 const auto& triangles = getTriangles();
93 for(
size_t triangle_index = 0; triangle_index < triangles.size() / 3; triangle_index++)
95 polygons.push_back(3);
96 polygons.push_back(triangles[triangle_index * 3 + 0]);
97 polygons.push_back(triangles[triangle_index * 3 + 1]);
98 polygons.push_back(triangles[triangle_index * 3 + 2]);
101 for(
const auto& plane : getPlanes())
104 plane_normals.emplace_back(plane.x(), plane.y(), plane.z());
105 plane_dis.push_back(plane.w());
110 s.points = convex.points;
111 s.polygons = convex.polygons;
112 s.plane_normals = convex.plane_normals;
113 s.plane_dis = convex.plane_dis;
118 auto* fcl = (fcl::Convex*)::
operator new(
sizeof(fcl::Convex));
119 fcl->num_points = s.points.size();
120 fcl =
new(fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data());
122 s.geometry = decltype(s.geometry)(
new collision_detection::FCLGeometry(fcl, link_model, shape_index));
123 s.edges.resize(s.points.size());
124 std::vector<std::unordered_set<size_t>> edge_sets(s.points.size());
125 for(
size_t edge_index = 0; edge_index < fcl->num_edges; edge_index++)
127 auto edge = fcl->edges[edge_index];
128 if(edge_sets[edge.first].find(edge.second) == edge_sets[edge.first].end())
130 edge_sets[edge.first].insert(edge.second);
131 s.edges[edge.first].push_back(edge.second);
133 if(edge_sets[edge.second].find(edge.first) == edge_sets[edge.second].end())
135 edge_sets[edge.second].insert(edge.first);
136 s.edges[edge.second].push_back(edge.first);
139 for(
auto& p : s.points)
140 s.vertices.emplace_back(p[0], p[1], p[2]);
144 s.geometry = collision_detection::createCollisionGeometry(link_model->getShapes()[shape_index], link_model, shape_index);
152 double TouchGoal::evaluate(
const GoalContext& context)
const 154 double dmin = DBL_MAX;
155 context.getTempVector().resize(1);
156 auto& last_collision_vertex = context.getTempVector()[0];
157 auto& fb = context.getLinkFrame();
158 size_t link_index = link_model->getLinkIndex();
159 auto& collision_link = collision_model->collision_links[link_index];
160 for(
size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++)
162 if(!collision_link.shapes[shape_index]->geometry)
continue;
163 auto* shape = link_model->getShapes()[shape_index].get();
165 if(
auto* mesh = dynamic_cast<const shapes::Mesh*>(shape))
167 auto& s = collision_link.shapes[shape_index];
169 auto goal_normal = normal;
170 quat_mul_vec(fb.rot.inverse(), goal_normal, goal_normal);
171 quat_mul_vec(s->frame.rot.inverse(), goal_normal, goal_normal);
183 if(mesh->vertex_count > 0)
185 size_t vertex_index = last_collision_vertex;
186 double vertex_dot_normal = goal_normal.dot(s->vertices[vertex_index]);
191 for(
auto vertex_index_2 : s->edges[vertex_index])
193 auto vertex_dot_normal_2 = goal_normal.dot(s->vertices[vertex_index_2]);
194 if(vertex_dot_normal_2 < vertex_dot_normal)
196 vertex_index = vertex_index_2;
197 vertex_dot_normal = vertex_dot_normal_2;
206 d = vertex_dot_normal;
207 last_collision_vertex = vertex_index;
209 d -= normal.dot(position - fb.pos);
211 if(d < dmin) dmin = d;
215 double offset = 10000;
216 static fcl::Sphere shape1(offset);
217 fcl::DistanceRequest request;
218 fcl::DistanceResult result;
219 auto pos1 = position - normal * offset * 2;
220 auto* shape2 = collision_link.shapes[shape_index]->geometry->collision_geometry_.get();
221 auto frame2 = Frame(fb.pos, fb.rot.normalized()) * collision_link.shapes[shape_index]->frame;
222 double d = fcl::distance(&shape1, fcl::Transform3f(fcl::Vec3f(pos1.x(), pos1.y(), pos1.z())), shape2, fcl::Transform3f(fcl::Quaternion3f(frame2.rot.w(), frame2.rot.x(), frame2.rot.y(), frame2.rot.z()), fcl::Vec3f(frame2.pos.x(), frame2.pos.y(), frame2.pos.z())), request, result);
224 if(d < dmin) dmin = d;
231 void BalanceGoal::describe(GoalContext& context)
const 233 Goal::describe(context);
234 balance_infos.clear();
236 for(
auto& link_name : context.getRobotModel().getLinkModelNames())
238 auto link_urdf = context.getRobotModel().getURDF()->getLink(link_name);
239 if(!link_urdf)
continue;
240 if(!link_urdf->inertial)
continue;
241 const auto& center_urdf = link_urdf->inertial->origin.position;
242 tf2::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z);
243 double mass = link_urdf->inertial->mass;
244 if(!(mass > 0))
continue;
245 balance_infos.emplace_back();
246 balance_infos.back().center = center;
247 balance_infos.back().weight = mass;
249 context.addLink(link_name);
251 for(
auto& b : balance_infos)
257 double BalanceGoal::evaluate(
const GoalContext& context)
const 259 tf2::Vector3 center(0, 0, 0);
260 for(
size_t i = 0; i < balance_infos.size(); i++)
262 auto& info = balance_infos[i];
263 auto& frame = context.getLinkFrame(i);
264 auto c = info.center;
265 quat_mul_vec(frame.rot, c, c);
267 center += c * info.weight;
270 center -= axis_ * axis_.dot(center);
271 return center.length2();