From eca64ec192037d2c419bb08ffc56b82661c87191 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Sat, 5 Dec 2015 14:52:45 +0100
Subject: [PATCH] forcetorque visu enhancements
---
.../components/units/ForceTorqueObserver.cpp | 87 +++++++++++++------
.../components/units/ForceTorqueObserver.h | 3 +
2 files changed, 65 insertions(+), 25 deletions(-)
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 16d028f07..550183449 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -82,42 +82,77 @@ void ForceTorqueObserver::onInitObserver()
void ForceTorqueObserver::onConnectObserver()
{
- assignProxy(robot, "RobotStateComponent");
+
debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 50, false, "visualizerFunction");
- localRobot = RemoteRobot::createLocalClone(robot);
- visualizerTask->start();
+ if (getProperty<bool>("VisualizeForce").getValue())
+ {
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, getProperty<bool>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+ visualizerTask->start();
+ }
}
void ForceTorqueObserver::visualizerFunction()
{
- RemoteRobot::synchronizeLocalClone(localRobot, robot);
- auto channels = getAvailableChannels(false);
- for(auto& channel : channels)
+ if (!robot)
{
- if(localRobot->hasRobotNode(channel.first))
+ try
{
- DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
- FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
- auto force = value->toGlobal(localRobot);
-// ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
-
- float forceMag = force->toEigen().norm()/100;
- forceMag = std::min(1.0f, forceMag);
- debugDrawer->setArrowVisu("Forces",
- "Force" + channel.first,
- new Vector3(localRobot->getRobotNode(channel.first)->getGlobalPose()),
- force,
- DrawColor{forceMag, 1.0f-forceMag, 0.0f, 0.5f},
- 50,
- 5);
+ assignProxy(robot, "RobotStateComponent");
+ }
+ catch (...)
+ {
+
}
- else
+
+ if (!robot)
{
-// ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+ return;
}
}
+
+ try
+ {
+ if (!localRobot)
+ {
+ localRobot = RemoteRobot::createLocalClone(robot);
+ }
+ if (!localRobot)
+ {
+ return;
+ }
+
+ RemoteRobot::synchronizeLocalClone(localRobot, robot);
+ auto channels = getAvailableChannels(false);
+ for (auto& channel : channels)
+ {
+ if (localRobot->hasRobotNode(channel.first))
+ {
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
+ FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
+ auto force = value->toGlobal(localRobot);
+ // ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
+
+ float forceMag = force->toEigen().norm() * getProperty<float>("ForceVisualizerFactor").getValue();
+ forceMag = std::min(1.0f, forceMag);
+ debugDrawer->setArrowVisu("Forces",
+ "Force" + channel.first,
+ new Vector3(localRobot->getRobotNode(channel.first)->getGlobalPose()),
+ force,
+ DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
+ std::max(100 * forceMag, 10.f),
+ 3);
+ }
+ else
+ {
+ // ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+ }
+ }
+ }
+ catch (...)
+ {
+
+ }
}
@@ -273,6 +308,8 @@ DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::stri
void ForceTorqueObserver::onExitObserver()
{
- if(visualizerTask)
+ if (visualizerTask)
+ {
visualizerTask->stop();
+ }
}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index ca2f27cc6..051d96ba4 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -42,6 +42,9 @@ namespace armarx
ComponentPropertyDefinitions(prefix)
{
defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
+ defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
+ defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 20, "Frequency with which the force is visualized");
+ defineOptionalProperty<float>("ForceVisualizerFactor", 0.01, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
}
};
--
GitLab