7 #include <Eigen/Geometry>
9 #include <SimoxUtility/algorithm/get_map_keys_values.h>
10 #include <SimoxUtility/math/pose.h>
11 #include <SimoxUtility/math/rescale.h>
12 #include <VirtualRobot/XML/RobotIO.h>
17 #include <ArmarXCore/interface/core/PackagePath.h>
34 descriptionSegment(descriptionSegment),
35 proprioceptionSegment(proprioceptionSegment),
36 localizationSegment(localizationSegment)
45 p.enabled, prefix +
"enabled",
"Enable or disable visualization of objects.");
46 defs->optional(p.frequencyHz, prefix +
"frequenzyHz",
"Frequency of visualization.");
47 defs->optional(p.framesEnabled,
48 prefix +
"framesEnabled",
49 "Enable or disable visualization of frames.");
50 defs->optional(p.forceTorque.enabled,
51 prefix +
"forceTorque.enabled",
52 "Enable or disable visualization of force torque sensors.");
53 defs->optional(p.forceTorque.forceScale,
54 prefix +
"forceTorque.forceScale",
55 "Scaling of force arrows.");
69 bool batchMode =
true;
88 const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
92 .file(xmlPath.package, xmlPath.package +
"/" + xmlPath.path)
93 .joints(robot.config.jointMap)
94 .pose(robot.config.globalPose);
104 Visu::visualizeFrames(
106 const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
108 for (
const auto& [robotName, robotFrames] : frames)
113 for (
const auto& frame : robotFrames)
115 Eigen::Affine3f from = pose;
116 Eigen::Affine3f to = pose * frame;
121 .fromTo(from.translation(), to.translation()));
127 Visu::visualizeFramesIndividual(
129 const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
132 std::vector<std::string> FRAME_NAMES{
"map",
"odom",
"robot"};
134 for (
const auto& [robotName, robotFrames] : frames)
137 for (
const auto& frame : robotFrames)
148 CycleUtil cycle(
static_cast<int>(1000 / p.frequencyHz));
149 while (updateTask and not updateTask->isStopped())
158 visualizeOnce(timestamp);
160 catch (
const std::out_of_range& e)
162 ARMARX_WARNING <<
"Caught std::out_of_range while visualizing robots: \n"
165 catch (
const std::exception& e)
167 ARMARX_WARNING <<
"Caught exception while visualizing robots: \n" << e.what();
171 ARMARX_WARNING <<
"Caught unknown exception while visualizing robots.";
174 if (debugObserver.has_value())
176 debugObserver->sendDebugObserverBatch();
179 cycle.waitForCycleDuration();
184 Visu::visualizeOnce(
const Time& timestamp)
209 debugObserver ? &*debugObserver :
nullptr);
224 <<
"\n- " << robotDescriptions.size() <<
" descriptions"
225 <<
"\n- " << globalPoses.size() <<
" global poses"
226 <<
"\n- " << sensorValues.size() <<
" joint positions";
229 combine(robotDescriptions, globalPoses, sensorValues, timestamp);
232 std::vector<viz::Layer> layers;
236 visualizeRobots(layer,
robots);
242 viz::Layer& layer = layers.emplace_back(arviz.
layer(
"Localization Frames"));
243 visualizeFrames(layer, frames);
249 viz::Layer& layer = layers.emplace_back(arviz.
layer(
"Localization Frames Individual"));
250 visualizeFramesIndividual(layer, frames);
253 if (p.forceTorque.enabled)
255 viz::Layer& layerForceTorque = layers.emplace_back(arviz.
layer(
"Force Torque"));
256 std::stringstream log;
260 const std::string& robotName = robot.description.name;
262 auto itSensorValues = sensorValues.find(robotName);
263 if (itSensorValues == sensorValues.end())
265 log <<
"\nNo robot '" << robotName <<
"' in sensor values. Available are: ";
266 log << simox::alg::join(simox::alg::get_keys(sensorValues),
", ");
270 visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
273 if (not log.str().empty())
275 ARMARX_INFO <<
"While visualizing robot force torques: " << log.str();
289 if (debugObserver.has_value())
291 const std::string p =
"Visu | ";
292 debugObserver->setDebugObserverDatafield(p +
"t Total (ms)",
293 tVisuTotal.toMilliSecondsDouble());
294 debugObserver->setDebugObserverDatafield(p +
"t 1 Get Data (ms)",
295 tVisuGetData.toMilliSecondsDouble());
296 debugObserver->setDebugObserverDatafield(p +
"t 1.1 Descriptions (ms)",
297 tRobotDescriptions.toMilliSecondsDouble());
298 debugObserver->setDebugObserverDatafield(p +
"t 1.2 Global Poses (ms)",
299 tGlobalPoses.toMilliSecondsDouble());
300 debugObserver->setDebugObserverDatafield(p +
"t 1.3 Frames (ms)",
301 tRobotFramePoses.toMilliSecondsDouble());
302 debugObserver->setDebugObserverDatafield(p +
"t 1.4 Sensor Values (ms)",
303 tSensorValues.toMilliSecondsDouble());
304 debugObserver->setDebugObserverDatafield(p +
"t 2 Build Layers (ms)",
305 tVisuBuildLayers.toMilliSecondsDouble());
306 debugObserver->setDebugObserverDatafield(p +
"t 3 Commit (ms)",
307 tVisuCommit.toMilliSecondsDouble());
314 const proprioception::SensorValues& sensorValues)
316 const std::string& robotName = robot.description.name;
318 RobotModel* robotModel =
nullptr;
319 if (
auto it = models.find(robotName); it != models.end())
321 robotModel = &it->second;
325 const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
326 ARMARX_INFO <<
"Loading robot model for '" << robotName <<
"' from " << robotPath
327 <<
" for visualization.";
328 auto [it2, _] = models.emplace(robotName, robotPath);
329 robotModel = &it2->second;
332 robotModel->model->setJointValues(robot.config.jointMap);
333 robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
336 sensorValues.forceTorqueValuesMap;
338 for (
const auto& [side, ft] : forceTorques)
344 const std::string forceTorqueSensorName =
345 robotModel->nameHelper->getArm(side).getForceTorqueSensor();
348 robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
350 const std::string xyz =
"XYZ";
352 const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
353 for (
int i = 0; i < 3; ++i)
360 const float length = p.forceTorque.forceScale * ft.force(i);
364 const Eigen::Vector3f to =
365 from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
367 std::stringstream key;
369 key << side <<
" Force " << xyz.at(i);
377 Visu::RobotModel::RobotModel(
const std::filesystem::path& robotPath) :
379 VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),