26 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
27 #include <VirtualRobot/Robot.h>
28 #include <VirtualRobot/SceneObjectSet.h>
44 const VirtualRobot::SceneObjectSetPtr& obstacles,
46 const std::string& robotCollisonModelName) :
49 parameters(parameters),
50 robotCollisionModelName(robotCollisonModelName)
54 ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName));
65 Costmap costmap(grid, parameters, sceneBounds);
68 << costmap.
getGrid().cols() <<
")";
69 fillGridCosts(costmap);
72 initializeMask(costmap);
79 CostmapBuilder::initializeMask(
Costmap& costmap)
81 costmap.mask = costmap.grid.array() > 0.F;
83 ARMARX_VERBOSE <<
"Initializing mask: Fraction of valid elements: "
84 << costmap.mask->cast<
float>().sum() / costmap.mask->size();
96 size_t c_x = (sceneBounds.
max.x() - sceneBounds.
min.x()) / parameters.
cellSize + 1;
97 size_t c_y = (sceneBounds.
max.y() - sceneBounds.
min.y()) / parameters.
cellSize + 1;
102 Eigen::MatrixXf grid(c_x, c_y);
112 const VirtualRobot::CollisionModelPtr& robotCollisionModel)
122 collisionRobot->setGlobalPose(globalPose.matrix());
125 VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
126 robotCollisionModel, obstacles);
177 CostmapBuilder::fillGridCosts(Costmap& costmap)
181 VirtualRobot::CollisionModelPtr robotCollisionModel;
183 const std::size_t c_x = costmap.grid.rows();
184 const std::size_t c_y = costmap.grid.cols();
186 robot->setUpdateVisualization(
false);
188 #pragma omp parallel for schedule(static) private(collisionRobot, \
189 robotCollisionModel) default(shared)
190 for (
unsigned int x = 0; x < c_x; x++)
193 if (collisionRobot ==
nullptr)
200 robot->clone(
"collision_robot_" +
std::to_string(omp_get_thread_num()));
203 collisionRobot->setPrimitiveApproximationModel({
"navigation"},
false);
205 collisionRobot->setUpdateVisualization(
false);
210 ARMARX_CHECK(collisionRobot->hasRobotNode(robotCollisionModelName));
213 const auto collisionRobotNode =
214 collisionRobot->getRobotNode(robotCollisionModelName);
217 robotCollisionModel = collisionRobotNode->getCollisionModel();
218 ARMARX_CHECK(robotCollisionModel) <<
"Collision model not available. "
219 "Make sure that you load the robot correctly!";
225 for (
unsigned int y = 0; y < c_y; y++)
230 costmap.grid(x, y) = computeCost(position, collisionRobot, robotCollisionModel);