Skip to content
Snippets Groups Projects
Commit 6884e491 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Merge branch 'robot-state-visu' into 'master' 

Fix visualization of force torque values

See merge request sw/armarx/robot-api!409
parents cb4010a0 1c48df0e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+ 163
− 107
View file @ 6884e491
......@@ -9,7 +9,6 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/logging/Logging.h>
......@@ -17,10 +16,8 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/core/PackagePath.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
......@@ -28,40 +25,43 @@
#include "combine.h"
namespace armarx::armem::server::robot_state
{
Visu::Visu(
const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
const localization::Segment& localizationSegment)
: descriptionSegment(descriptionSegment),
proprioceptionSegment(proprioceptionSegment),
localizationSegment(localizationSegment)
Visu::Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
const localization::Segment& localizationSegment) :
descriptionSegment(descriptionSegment),
proprioceptionSegment(proprioceptionSegment),
localizationSegment(localizationSegment)
{
Logging::setTag("Visu");
}
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
void
Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(
p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
defs->optional(p.framesEnabled, prefix + "framesEnabled", "Enable or disable visualization of frames.");
defs->optional(p.forceTorque.enabled, prefix + "forceTorque.enabled",
defs->optional(p.framesEnabled,
prefix + "framesEnabled",
"Enable or disable visualization of frames.");
defs->optional(p.forceTorque.enabled,
prefix + "forceTorque.enabled",
"Enable or disable visualization of force torque sensors.");
defs->optional(p.forceTorque.forceScale, prefix + "forceTorque.forceScale",
defs->optional(p.forceTorque.forceScale,
prefix + "forceTorque.forceScale",
"Scaling of force arrows.");
}
void Visu::init()
void
Visu::init()
{
}
void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
void
Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
this->arviz = arviz;
if (debugObserver)
......@@ -76,15 +76,12 @@ namespace armarx::armem::server::robot_state
updateTask->join();
updateTask = nullptr;
}
updateTask = new SimpleRunningTask<>([this]()
{
this->visualizeRun();
});
updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
updateTask->start();
}
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
void
Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
for (const robot::Robot& robot : robots)
{
......@@ -103,8 +100,8 @@ namespace armarx::armem::server::robot_state
}
}
void Visu::visualizeFrames(
void
Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
......@@ -120,12 +117,14 @@ namespace armarx::armem::server::robot_state
pose = to;
layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
layerFrames.add(viz::Arrow(robotName + std::to_string(++i))
.fromTo(from.translation(), to.translation()));
}
}
}
void Visu::visualizeFramesIndividual(
void
Visu::visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
......@@ -137,14 +136,14 @@ namespace armarx::armem::server::robot_state
int i = 0;
for (const auto& frame : robotFrames)
{
layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
layerFrames.add(
viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
}
}
}
void Visu::visualizeRun()
void
Visu::visualizeRun()
{
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask and not updateTask->isStopped())
......@@ -158,6 +157,11 @@ namespace armarx::armem::server::robot_state
{
visualizeOnce(timestamp);
}
catch (const std::out_of_range& e)
{
ARMARX_WARNING << "Caught std::out_of_range while visualizing robots: \n"
<< e.what();
}
catch (const std::exception& e)
{
ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
......@@ -176,8 +180,8 @@ namespace armarx::armem::server::robot_state
}
}
void Visu::visualizeOnce(const Time& timestamp)
void
Visu::visualizeOnce(const Time& timestamp)
{
TIMING_START(tVisuTotal);
......@@ -200,9 +204,9 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
TIMING_START(tSensorValues);
const auto sensorValues =
proprioceptionSegment.getSensorValuesLocking(
timestamp, debugObserver ? &*debugObserver : nullptr);
const proprioception::SensorValuesMap sensorValues =
proprioceptionSegment.getSensorValuesLocking(timestamp,
debugObserver ? &*debugObserver : nullptr);
TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
......@@ -225,83 +229,51 @@ namespace armarx::armem::server::robot_state
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
std::vector layers{layer};
std::vector<viz::Layer> layers;
{
viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
visualizeRobots(layer, robots);
}
ARMARX_DEBUG << "Visualize frames ...";
if(p.framesEnabled)
if (p.framesEnabled)
{
viz::Layer layerFrames = arviz.layer("Frames");
visualizeFrames(layerFrames, frames);
layers.push_back(layerFrames);
viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames"));
visualizeFrames(layer, frames);
}
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
{
viz::Layer layerFrames = arviz.layer("FramesIndividual");
visualizeFramesIndividual(layerFrames, frames);
layers.push_back(layerFrames);
viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames Individual"));
visualizeFramesIndividual(layer, frames);
}
if (p.forceTorque.enabled)
{
viz::Layer layerFrames = arviz.layer("ForceTorque");
viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
std::stringstream log;
for (const robot::Robot& robot : robots)
{
const std::string& name = robot.description.name;
if (robotNameHelper.find(name) == robotNameHelper.end())
{
const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
robotNameHelper[name] = RobotNameHelper::Create(robotPath);
robotModels[name]
= VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure);
}
auto model = robotModels.at(name);
model->setJointValues(robot.config.jointMap);
model->setGlobalPose(robot.config.globalPose.matrix());
const std::string& robotName = robot.description.name;
const proprioception::ForceTorqueValuesMap& forceTorques
= sensorValues.at(name).forceTorqueValuesMap;
for (const auto& [side, ft] : forceTorques)
auto itSensorValues = sensorValues.find(robotName);
if (itSensorValues == sensorValues.end())
{
ARMARX_CHECK(side == RobotNameHelper::LocationLeft or side == RobotNameHelper::LocationRight) << side;
const std::string forceTorqueSensorName =
robotNameHelper.at(name)->getArm(side).getForceTorqueSensor();
const Eigen::Matrix4f forceTorqueSensorPose
= model->getSensor(forceTorqueSensorName)->getGlobalPose();
const std::string xyz = "XYZ";
const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
for (int i = 0; i < 3; ++i)
{
simox::Color color = simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
color.a = 128;
// Force arrows.
const float length = p.forceTorque.forceScale * ft.force(i);
const float width = std::min(
simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F),
10.F);
const Eigen::Vector3f to = from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
std::stringstream key;
key << side << " Force " << xyz.at(i);
layerFrames.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
// Torque circle arrows.
}
log << "\nNo robot '" << robotName << "' in sensor values. Available are: ";
log << simox::alg::join(simox::alg::get_keys(sensorValues), ", ");
continue;
}
visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
}
if (not log.str().empty())
{
ARMARX_INFO << "While visualizing robot force torques: " << log.str();
}
layers.push_back(layerFrames);
}
......@@ -317,15 +289,99 @@ namespace armarx::armem::server::robot_state
if (debugObserver.has_value())
{
const std::string p = "Visu | ";
debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)", tSensorValues.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
tVisuTotal.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
tVisuGetData.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
tRobotDescriptions.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)",
tGlobalPoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)",
tRobotFramePoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)",
tSensorValues.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
tVisuBuildLayers.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
tVisuCommit.toMilliSecondsDouble());
}
}
void
Visu::visualizeForceTorque(viz::Layer& layer,
const robot::Robot& robot,
const proprioception::SensorValues& sensorValues)
{
const std::string& robotName = robot.description.name;
RobotModel* robotModel = nullptr;
if (auto it = models.find(robotName); it != models.end())
{
robotModel = &it->second;
}
else
{
const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
ARMARX_INFO << "Loading robot model for '" << robotName << "' from " << robotPath
<< " for visualization.";
auto [it2, _] = models.emplace(robotName, robotPath);
robotModel = &it2->second;
}
robotModel->model->setJointValues(robot.config.jointMap);
robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
const proprioception::ForceTorqueValuesMap& forceTorques =
sensorValues.forceTorqueValuesMap;
for (const auto& [side, ft] : forceTorques)
{
ARMARX_CHECK(side == RobotNameHelper::LocationLeft or
side == RobotNameHelper::LocationRight)
<< side;
const std::string forceTorqueSensorName =
robotModel->nameHelper->getArm(side).getForceTorqueSensor();
const Eigen::Matrix4f forceTorqueSensorPose =
robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
const std::string xyz = "XYZ";
const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
for (int i = 0; i < 3; ++i)
{
simox::Color color =
simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
color.a = 128;
// Force arrows.
const float length = p.forceTorque.forceScale * ft.force(i);
const float width =
std::min(simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F), 10.F);
const Eigen::Vector3f to =
from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
std::stringstream key;
ARMARX_CHECK_FITS_SIZE(i, xyz.size());
key << side << " Force " << xyz.at(i);
layer.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
// TODO: Torque circle arrows.
}
}
}
Visu::RobotModel::RobotModel(const std::filesystem::path& robotPath) :
model{
VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),
},
nameHelper{
RobotNameHelper::Create(robotPath),
}
{
}
} // namespace armarx::armem::server::robot_state
source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+ 21
− 23
View file @ 6884e491
......@@ -28,14 +28,10 @@
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
namespace armarx::armem::server::robot_state
{
......@@ -46,41 +42,38 @@ namespace armarx::armem::server::robot_state
class Visu : public armarx::Logging
{
public:
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
const localization::Segment& localizationSegment);
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
void defineProperties(armarx::PropertyDefinitionsPtr defs,
const std::string& prefix = "visu.");
void init();
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
private:
void visualizeRun();
void visualizeOnce(const Time& timestamp);
static
void visualizeRobots(
viz::Layer& layer,
const robot::Robots& robots);
static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots);
static
void visualizeFrames(
static void visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
static
void visualizeFramesIndividual(
static void visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
private:
void visualizeForceTorque(viz::Layer& layer,
const robot::Robot& robot,
const proprioception::SensorValues& sensorValues);
private:
viz::Client arviz;
std::optional<DebugObserverHelper> debugObserver;
......@@ -88,9 +81,15 @@ namespace armarx::armem::server::robot_state
const proprioception::Segment& proprioceptionSegment;
const localization::Segment& localizationSegment;
std::map<std::string, RobotNameHelperPtr> robotNameHelper;
std::map<std::string, VirtualRobot::RobotPtr> robotModels;
struct RobotModel
{
RobotModel(const std::filesystem::path& robotPath);
VirtualRobot::RobotPtr model;
RobotNameHelperPtr nameHelper;
};
std::map<std::string, RobotModel> models;
struct Properties
{
......@@ -98,18 +97,17 @@ namespace armarx::armem::server::robot_state
float frequencyHz = 25.f;
bool framesEnabled = false;
struct ForceTorque
{
bool enabled = true;
float forceScale = 1.F;
};
ForceTorque forceTorque;
} p;
SimpleRunningTask<>::pointer_type updateTask;
};
} // namespace armarx::armem::server::robot_state
} // namespace armarx::armem::server::robot_state
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment