Skip to content
Snippets Groups Projects
Commit 1c48df0e authored by Rainer Kartmann's avatar Rainer Kartmann Committed by Rainer Kartmann
Browse files

Fix visualization of force torque values

parent 2971714e
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 @ 1c48df0e
......@@ -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 @ 1c48df0e
......@@ -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