From db8948f8f42c27156bfececcf8eaf2d4181edf73 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Mon, 17 Feb 2020 17:46:50 +0100
Subject: [PATCH] Remove using namespace on global scope and format code
---
.../RobotControlUI/RobotControlUI.cpp | 77 +-
.../components/ArViz/ArVizStorage.cpp | 252 +-
.../RobotAPI/components/ArViz/Client/Client.h | 15 +-
.../ArViz/Coin/ElementVisualizer.cpp | 99 +-
.../components/ArViz/Coin/ElementVisualizer.h | 114 +-
.../ArViz/Coin/VisualizationObject.cpp | 12 +-
.../ArViz/Coin/VisualizationRobot.cpp | 12 +-
.../components/ArViz/Example/ArVizExample.cpp | 517 +-
.../components/ArViz/IceConversions.h | 45 +-
.../ArViz/test/Client/PointCloudTest.cpp | 24 +-
.../CyberGloveObserver/CyberGloveObserver.cpp | 142 +-
.../DebugDrawer/DebugDrawerHelper.cpp | 12 +-
.../DummyTextToSpeech/DummyTextToSpeech.cpp | 76 +-
.../FrameTracking/FrameTracking.cpp | 895 ++-
.../GamepadControlUnit/GamepadControlUnit.cpp | 77 +-
.../KITProstheticHandObserver.cpp | 37 +-
.../KITProstheticHandUnit.cpp | 323 +-
.../components/RobotHealth/RobotHealth.cpp | 400 +-
.../RobotHealth/RobotHealthDummy.cpp | 195 +-
.../RobotNameService/RobotNameService.cpp | 37 +-
.../TopicTimingTest/TopicTimingClient.h | 4 +-
.../TopicTimingTest/TopicTimingServer.cpp | 2 +-
.../ViewSelection/ViewSelection.cpp | 690 ++-
.../components/units/ATINetFTUnit.cpp | 388 +-
.../components/units/ForceTorqueObserver.cpp | 653 +--
.../components/units/ForceTorqueUnit.cpp | 41 +-
.../units/ForceTorqueUnitSimulation.cpp | 90 +-
.../components/units/GamepadUnitObserver.cpp | 119 +-
.../components/units/HandUnitSimulation.cpp | 125 +-
.../components/units/HapticObserver.cpp | 219 +-
.../RobotAPI/components/units/HapticUnit.cpp | 37 +-
.../units/InertialMeasurementUnit.cpp | 47 +-
.../units/InertialMeasurementUnitObserver.cpp | 191 +-
.../components/units/KinematicUnit.cpp | 225 +-
.../units/KinematicUnitObserver.cpp | 415 +-
.../components/units/KinematicUnitObserver.h | 23 +-
.../units/LaserScannerUnitObserver.cpp | 111 +-
.../components/units/MetaWearIMUObserver.cpp | 108 +-
.../units/OptoForceUnitObserver.cpp | 165 +-
.../OrientedTactileSensorUnitObserver.cpp | 110 +-
.../components/units/PlatformUnit.cpp | 64 +-
.../components/units/PlatformUnitObserver.cpp | 187 +-
.../components/units/PlatformUnitObserver.h | 17 +-
.../units/PlatformUnitSimulation.cpp | 300 +-
.../components/units/RobotPoseUnit.cpp | 64 +-
.../components/units/RobotUnit/CMakeLists.txt | 1 +
.../components/units/RobotUnit/Constants.h | 7 +-
.../components/units/RobotUnit/ControlModes.h | 65 +-
.../RobotUnit/DefaultWidgetDescriptions.cpp | 95 +-
.../RobotUnit/DefaultWidgetDescriptions.h | 97 +-
.../units/RobotUnit/Devices/DeviceBase.h | 4 -
.../JointControllers/JointController.cpp | 19 +-
...tformUnitVelocityPassThroughController.cpp | 93 +-
...ointKinematicUnitPassThroughController.cpp | 93 +-
.../components/units/RobotUnit/util.h | 2 +
.../units/RobotUnit/util/CtrlUtil.h | 269 +-
.../units/RobotUnit/util/DynamicsHelper.h | 11 +-
.../util/introspection/ClassMemberInfo.h | 279 +-
.../util/introspection/ClassMemberInfoEntry.h | 383 +-
.../util/introspection/DataFieldsInfo.cpp | 285 +-
.../util/introspection/DataFieldsInfo.h | 369 +-
.../drivers/XsensIMU/IMU/Xsens/Xsens.h | 25 +-
.../XsensIMU/IMU/Xsens/XsensMTiModule.cpp | 4891 ++++++++---------
.../XsensIMU/IMU/Xsens/XsensMTiModule.h | 331 +-
source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp | 186 +-
.../gui-plugins/ArViz/ArVizGuiPlugin.cpp | 13 +-
.../gui-plugins/ArViz/ArVizGuiPlugin.h | 3 +-
.../ArViz/ArVizWidgetController.cpp | 1299 ++---
.../DebugDrawerViewerGuiPlugin.cpp | 9 +-
.../DebugDrawerViewerWidgetController.cpp | 381 +-
.../GuiHealthClientGuiPlugin.cpp | 9 +-
.../GuiHealthClientWidgetController.cpp | 183 +-
.../HandUnitPlugin/HandUnitGuiPlugin.cpp | 387 +-
.../HapticUnitPlugin/HapticUnitGuiPlugin.cpp | 258 +-
.../MatrixDatafieldDisplayWidget.cpp | 199 +-
.../HapticUnitPlugin/MatrixDisplayWidget.cpp | 121 +-
.../HomogeneousMatrixCalculatorGuiPlugin.cpp | 9 +-
.../ViewSelection/ViewSelectionConfigDialog.h | 4 +-
.../KinematicUnitInterfaceStdOverloads.cpp | 50 +-
.../KinematicUnitInterfaceStdOverloads.h | 5 +-
.../DSControllers/DSRTBimanualController.cpp | 1407 ++---
.../DSControllers/DSRTController.cpp | 587 +-
.../libraries/DSControllers/Gaussians.cpp | 777 +--
.../libraries/DSControllers/Gaussians.h | 2 -
.../JointPWMPositionController.cpp | 496 +-
.../JointPWMVelocityController.cpp | 363 +-
.../JointZeroTorqueController.cpp | 339 +-
.../ParallelGripperPositionController.cpp | 67 +-
.../ParallelGripperVelocityController.cpp | 55 +-
.../KITGripperBasisBoardSlave.h | 2 +-
.../NJointAdaptiveWipingController.h | 14 -
...AnomalyDetectionAdaptiveWipingController.h | 14 -
.../NJointBimanualCCDMPController.h | 6 +-
.../NJointBimanualCCDMPVelocityController.h | 5 +-
.../NJointBimanualDMPForceController.h | 6 +-
.../NJointBimanualForceMPController.cpp | 2 +-
.../NJointBimanualForceMPController.h | 7 +-
.../DMPController/NJointCCDMPController.cpp | 16 +-
.../DMPController/NJointCCDMPController.h | 40 +-
.../NJointPeriodicTSDMPCompliantController.h | 14 -
.../NJointPeriodicTSDMPForwardVelController.h | 14 -
.../DMPController/NJointTSDMPController.h | 33 +-
.../NJointTaskSpaceImpedanceDMPController.h | 5 -
.../RobotAPIVariantWidget.cpp | 803 ++-
.../RobotAPIVariantWidget.h | 20 +-
.../ForceTorqueHelper.cpp | 39 +-
.../KinematicUnitHelper.cpp | 54 +-
.../PositionControllerHelper.cpp | 375 +-
.../RobotNameHelper.cpp | 316 +-
.../RobotStatechartHelpers.cpp | 5 -
.../VelocityControllerHelper.cpp | 98 +-
.../SimpleJsonLogger/SimpleJsonLogger.cpp | 60 +-
.../SimpleJsonLoggerEntry.cpp | 239 +-
.../libraries/SimpleTrajectory/exceptions.cpp | 94 +-
...CartesianFeedForwardPositionController.cpp | 127 +-
.../CartesianFeedForwardPositionController.h | 2 +-
.../core/CartesianPositionController.cpp | 209 +-
.../core/CartesianPositionController.h | 15 +-
.../core/CartesianVelocityController.cpp | 267 +-
.../CartesianVelocityControllerWithRamp.cpp | 97 +-
.../libraries/core/CartesianVelocityRamp.cpp | 94 +-
.../core/CartesianWaypointController.cpp | 2 +-
.../libraries/core/FramedOrientedPoint.cpp | 250 +-
.../libraries/core/FramedOrientedPoint.h | 12 +-
source/RobotAPI/libraries/core/FramedPose.h | 17 +-
.../libraries/core/JointVelocityRamp.cpp | 55 +-
source/RobotAPI/libraries/core/LinkedPose.h | 22 +-
.../RobotAPI/libraries/core/OrientedPoint.h | 12 +-
.../RobotAPI/libraries/core/PIDController.cpp | 239 +-
source/RobotAPI/libraries/core/Pose.cpp | 59 +-
source/RobotAPI/libraries/core/Pose.h | 25 +-
.../libraries/core/RobotAPIObjectFactories.h | 26 +-
.../RobotAPI/libraries/core/SimpleDiffIK.cpp | 111 +-
.../libraries/core/SimpleGridReachability.cpp | 60 +-
source/RobotAPI/libraries/core/Trajectory.cpp | 11 +-
source/RobotAPI/libraries/core/Trajectory.h | 14 +-
.../RobotAPI/libraries/core/math/ColorUtils.h | 264 +-
.../core/math/LinearizeAngularTrajectory.h | 36 +-
.../RobotAPI/libraries/core/math/MathUtils.h | 238 +-
.../libraries/core/math/MatrixHelpers.h | 54 +-
source/RobotAPI/libraries/core/math/SVD.h | 50 +-
.../core/math/SlidingWindowVectorMedian.h | 90 +-
.../RobotAPI/libraries/core/math/StatUtils.h | 66 +-
.../libraries/core/math/TimeSeriesUtils.h | 40 +-
.../libraries/core/math/Trigonometry.h | 50 +-
.../core/observerfilters/MatrixFilters.h | 442 +-
.../MedianDerivativeFilterV3.h | 68 +-
.../core/observerfilters/OffsetFilter.h | 204 +-
.../observerfilters/PoseAverageFilter.cpp | 139 +-
.../core/observerfilters/PoseAverageFilter.h | 55 +-
.../core/observerfilters/PoseMedianFilter.h | 186 +-
.../observerfilters/PoseMedianOffsetFilter.h | 64 +-
.../core/remoterobot/test/ArmarPoseTest.cpp | 1 +
.../core/test/DebugDrawerTopicTest.cpp | 50 +-
.../core/visualization/DebugDrawerTopic.cpp | 34 +-
.../widgets/DebugLayerControlWidget.cpp | 2 +-
.../widgets/DebugLayerControlWidget.h | 5 +-
.../ForceTorqueUtility/DetectForceFlank.h | 46 +-
.../ForceTorqueUtility/DetectForceSpike.h | 41 +-
.../ForceTorqueUtilityRemoteStateOfferer.h | 32 +-
...ntedTactileSensorGroupRemoteStateOfferer.h | 32 +-
.../OrientedTactileSensorTest.h | 42 +-
.../CyberGloveProsthesisControl.cpp | 42 +-
.../CyberGloveProsthesisControl.h | 64 +-
...esisKinestheticTeachInRemoteStateOfferer.h | 32 +-
...botNameHelperTestGroupRemoteStateOfferer.h | 32 +-
.../RobotNameHelperTestGroup/TestGetNames.h | 48 +-
...peechObserverTestGroupRemoteStateOfferer.h | 32 +-
.../TestTextToSpeech.h | 45 +-
.../StatechartExecution.h | 43 +-
...atechartExecutionGroupRemoteStateOfferer.h | 32 +-
.../Substates/Executor.h | 44 +-
.../Substates/PrepareNext.h | 43 +-
.../Substates/WaitForNext.h | 44 +-
.../Testing/TestStateForStatechartExecution.h | 43 +-
...chartProfilesTestGroupRemoteStateOfferer.h | 32 +-
.../StatechartProfilesTestGroup/TestState.h | 38 +-
.../TrajectoryExecutionCode/PlayTrajectory.h | 48 +-
...rajectoryExecutionCodeRemoteStateOfferer.h | 32 +-
.../WeissHapticGroupRemoteStateOfferer.h | 32 +-
.../WeissHapticGroup/WeissHapticSensorTest.h | 40 +-
181 files changed, 14556 insertions(+), 14832 deletions(-)
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
index a7665b076..e67717c86 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
@@ -28,49 +28,50 @@
#include <iostream>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-using namespace armarx;
-
-void RobotControlUI::onInitComponent()
+namespace armarx
{
- usingProxy("RobotControl");
- stateId = -1;
- controlTask = new RunningTask<RobotControlUI>(this, &RobotControlUI::run, "RobotControlUI");
-}
-
-void RobotControlUI::onConnectComponent()
-{
- robotProxy = getProxy<RobotControlInterfacePrx>("RobotControl");
- controlTask->start();
-}
-
-void RobotControlUI::onExitComponent()
-{
- controlTask->stop();
-}
-
-void RobotControlUI::run()
-{
- std::string eventstring;
- std::cout << "Please insert the event string: " << std::flush;
- // cin >> eventstring;
- eventstring = "EvLoadScenario";
+ void RobotControlUI::onInitComponent()
+ {
+ usingProxy("RobotControl");
+ stateId = -1;
+ controlTask = new RunningTask<RobotControlUI>(this, &RobotControlUI::run, "RobotControlUI");
+ }
- if (eventstring == "q")
+ void RobotControlUI::onConnectComponent()
{
- // shutdown();
+ robotProxy = getProxy<RobotControlInterfacePrx>("RobotControl");
+ controlTask->start();
}
- else
+
+ void RobotControlUI::onExitComponent()
{
- std::cout << "Please insert the state id of the state that should process the event: " << std::flush;
- int id;
- // cin >> id;
- id = 11;
- std::cout << "sending to id:" << id << std::endl;
- EventPtr evt = new Event("EVENTTOALL", eventstring);
- StateUtilFunctions::addToDictionary(evt, "proxyName", "RemoteStateOfferer");
- StateUtilFunctions::addToDictionary(evt, "stateName", "MoveArm");
- //robotProxy->issueEvent(id, evt);
+ controlTask->stop();
}
- // cin >> eventstring;
+ void RobotControlUI::run()
+ {
+ std::string eventstring;
+ std::cout << "Please insert the event string: " << std::flush;
+ // cin >> eventstring;
+ eventstring = "EvLoadScenario";
+
+ if (eventstring == "q")
+ {
+ // shutdown();
+ }
+ else
+ {
+ std::cout << "Please insert the state id of the state that should process the event: " << std::flush;
+ int id;
+ // cin >> id;
+ id = 11;
+ std::cout << "sending to id:" << id << std::endl;
+ EventPtr evt = new Event("EVENTTOALL", eventstring);
+ StateUtilFunctions::addToDictionary(evt, "proxyName", "RemoteStateOfferer");
+ StateUtilFunctions::addToDictionary(evt, "stateName", "MoveArm");
+ //robotProxy->issueEvent(id, evt);
+ }
+
+ // cin >> eventstring;
+ }
}
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index cc41ae605..84576c3f3 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -31,173 +31,174 @@
#include <iomanip>
-using namespace armarx;
-
-static std::filesystem::path getAbsolutePath(const std::filesystem::path& path)
+namespace armarx
{
- if (path.is_absolute())
- {
- return path;
- }
- else
+ static std::filesystem::path getAbsolutePath(const std::filesystem::path& path)
{
- std::string absolute;
- if (ArmarXDataPath::getAbsolutePath(path.string(), absolute))
+ if (path.is_absolute())
{
- return absolute;
+ return path;
}
else
{
- ARMARX_WARNING_S << "Could not resolve relative file as package data file: "
- << path;
- return path;
+ std::string absolute;
+ if (ArmarXDataPath::getAbsolutePath(path.string(), absolute))
+ {
+ return absolute;
+ }
+ else
+ {
+ ARMARX_WARNING_S << "Could not resolve relative file as package data file: "
+ << path;
+ return path;
+ }
}
}
-}
-void ArVizStorage::onInitComponent()
-{
- topicName = getProperty<std::string>("TopicName").getValue();
- maxHistorySize = getProperty<int>("MaxHistorySize").getValue();
- std::filesystem::path historyPathProp = getProperty<std::string>("HistoryPath").getValue();
- historyPath = getAbsolutePath(historyPathProp);
- if (!std::filesystem::exists(historyPath))
+ void ArVizStorage::onInitComponent()
{
- ARMARX_INFO << "Creating history path: " << historyPath;
- std::error_code error;
- std::filesystem::create_directory(historyPath, error);
- if (error)
+ topicName = getProperty<std::string>("TopicName").getValue();
+ maxHistorySize = getProperty<int>("MaxHistorySize").getValue();
+ std::filesystem::path historyPathProp = getProperty<std::string>("HistoryPath").getValue();
+ historyPath = getAbsolutePath(historyPathProp);
+ if (!std::filesystem::exists(historyPath))
{
- ARMARX_WARNING << "Could not create directory for history: \n"
- << error.message();
+ ARMARX_INFO << "Creating history path: " << historyPath;
+ std::error_code error;
+ std::filesystem::create_directory(historyPath, error);
+ if (error)
+ {
+ ARMARX_WARNING << "Could not create directory for history: \n"
+ << error.message();
+ }
}
- }
- usingTopic(topicName);
-}
+ usingTopic(topicName);
+ }
-void ArVizStorage::onConnectComponent()
-{
- revision = 0;
- currentState.clear();
- history.clear();
- recordingInitialState.clear();
+ void ArVizStorage::onConnectComponent()
+ {
+ revision = 0;
+ currentState.clear();
+ history.clear();
+ recordingInitialState.clear();
- recordingBuffer.clear();
- recordingMetaData.id = "";
-}
+ recordingBuffer.clear();
+ recordingMetaData.id = "";
+ }
-void ArVizStorage::onDisconnectComponent()
-{
- if (recordingTask)
+ void ArVizStorage::onDisconnectComponent()
{
- recordingTask->stop();
- recordingTask = nullptr;
+ if (recordingTask)
+ {
+ recordingTask->stop();
+ recordingTask = nullptr;
+ }
}
-}
-void ArVizStorage::onExitComponent()
-{
-
-}
-
-armarx::PropertyDefinitionsPtr ArVizStorage::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new ArVizPropertyDefinitions(
- getConfigIdentifier()));
-}
+ void ArVizStorage::onExitComponent()
+ {
-void ArVizStorage::updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&)
-{
- std::unique_lock<std::mutex> lock(historyMutex);
+ }
- revision += 1;
- IceUtil::Time now = TimeUtil::GetTime();
- long nowInMicroSeconds = now.toMicroSeconds();
+ armarx::PropertyDefinitionsPtr ArVizStorage::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new ArVizPropertyDefinitions(
+ getConfigIdentifier()));
+ }
- for (auto& update : updates)
+ void ArVizStorage::updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&)
{
- auto& historyEntry = history.emplace_back();
- historyEntry.revision = revision;
- historyEntry.timestampInMicroseconds = nowInMicroSeconds;
- historyEntry.update = update;
+ std::unique_lock<std::mutex> lock(historyMutex);
- // Insert or create the layer
- bool found = false;
- for (auto& layer : currentState)
+ revision += 1;
+ IceUtil::Time now = TimeUtil::GetTime();
+ long nowInMicroSeconds = now.toMicroSeconds();
+
+ for (auto& update : updates)
{
- if (layer.update.component == update.component
- && layer.update.name == update.name)
+ auto& historyEntry = history.emplace_back();
+ historyEntry.revision = revision;
+ historyEntry.timestampInMicroseconds = nowInMicroSeconds;
+ historyEntry.update = update;
+
+ // Insert or create the layer
+ bool found = false;
+ for (auto& layer : currentState)
{
- layer = historyEntry;
- found = true;
- continue;
+ if (layer.update.component == update.component
+ && layer.update.name == update.name)
+ {
+ layer = historyEntry;
+ found = true;
+ continue;
+ }
+ }
+ if (!found)
+ {
+ currentState.push_back(historyEntry);
}
}
- if (!found)
- {
- currentState.push_back(historyEntry);
- }
- }
- long currentHistorySize = history.size();
- if (currentHistorySize >= maxHistorySize)
- {
+ long currentHistorySize = history.size();
+ if (currentHistorySize >= maxHistorySize)
{
- std::unique_lock<std::mutex> lock(recordingMutex);
- if (recordingMetaData.id.size() > 0)
{
- auto& newBatch = recordingBuffer.emplace_back();
- newBatch.initialState = recordingInitialState;
- newBatch.updates = std::move(history);
- recordingInitialState = currentState;
-
- recordingCondition.notify_one();
+ std::unique_lock<std::mutex> lock(recordingMutex);
+ if (recordingMetaData.id.size() > 0)
+ {
+ auto& newBatch = recordingBuffer.emplace_back();
+ newBatch.initialState = recordingInitialState;
+ newBatch.updates = std::move(history);
+ recordingInitialState = currentState;
+
+ recordingCondition.notify_one();
+ }
}
+ history.clear();
}
- history.clear();
}
-}
-viz::data::LayerUpdates armarx::ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
-{
- viz::data::LayerUpdates result;
+ viz::data::LayerUpdates armarx::ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
+ {
+ viz::data::LayerUpdates result;
- std::unique_lock<std::mutex> lock(historyMutex);
+ std::unique_lock<std::mutex> lock(historyMutex);
- result.updates.reserve(currentState.size());
- for (auto& layer : currentState)
- {
- if (layer.revision > revision)
+ result.updates.reserve(currentState.size());
+ for (auto& layer : currentState)
{
- result.updates.push_back(layer.update);
+ if (layer.revision > revision)
+ {
+ result.updates.push_back(layer.update);
+ }
}
- }
- result.revision = this->revision;
+ result.revision = this->revision;
- return result;
-}
+ return result;
+ }
-void ArVizStorage::record()
-{
- while (!recordingTask->isStopped())
+ void ArVizStorage::record()
{
- std::unique_lock<std::mutex> lock(recordingMutex);
- while (!recordingTask->isStopped() && recordingBuffer.empty())
+ while (!recordingTask->isStopped())
{
- recordingCondition.wait_for(lock, std::chrono::milliseconds(10));
- }
- for (auto& batch : recordingBuffer)
- {
- recordBatch(batch);
+ std::unique_lock<std::mutex> lock(recordingMutex);
+ while (!recordingTask->isStopped() && recordingBuffer.empty())
+ {
+ recordingCondition.wait_for(lock, std::chrono::milliseconds(10));
+ }
+ for (auto& batch : recordingBuffer)
+ {
+ recordBatch(batch);
+ }
+ recordingBuffer.clear();
}
- recordingBuffer.clear();
}
}
@@ -276,9 +277,9 @@ static std::string readCompleteFile(std::filesystem::path const& path)
return result;
}
-static std::optional<viz::data::Recording> readRecordingInfo(std::filesystem::path const& recordingDirectory)
+static std::optional<armarx::viz::data::Recording> readRecordingInfo(std::filesystem::path const& recordingDirectory)
{
- std::optional<viz::data::Recording> result;
+ std::optional<::armarx::viz::data::Recording> result;
std::filesystem::path recordingFilePath = recordingDirectory / "recording.json";
if (!std::filesystem::exists(recordingFilePath))
@@ -292,7 +293,7 @@ static std::optional<viz::data::Recording> readRecordingInfo(std::filesystem::pa
std::string recordingString = readCompleteFile(recordingFilePath);
nlohmann::json recordingJson = nlohmann::json::parse(recordingString);
- viz::data::Recording recording;
+ ::armarx::viz::data::Recording recording;
recordingJson.get_to(recording);
result = std::move(recording);
@@ -306,12 +307,12 @@ static std::optional<viz::data::Recording> readRecordingInfo(std::filesystem::pa
}
}
-static std::string batchFileName(viz::data::RecordingBatchHeader const& batchHeader)
+static std::string batchFileName(armarx::viz::data::RecordingBatchHeader const& batchHeader)
{
return std::to_string(batchHeader.firstRevision) + ".bin";
}
-void ArVizStorage::recordBatch(viz::data::RecordingBatch& batch)
+void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
{
if (batch.updates.empty())
{
@@ -353,7 +354,7 @@ void ArVizStorage::recordBatch(viz::data::RecordingBatch& batch)
}
recordingMetaData.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
- viz::data::RecordingBatchHeader& newBatch = recordingMetaData.batchHeaders.emplace_back();
+ armarx::viz::data::RecordingBatchHeader& newBatch = recordingMetaData.batchHeaders.emplace_back();
newBatch.index = recordingMetaData.batchHeaders.size() - 1;
newBatch.firstRevision = first.revision;
newBatch.lastRevision = last.revision;
@@ -373,7 +374,6 @@ void ArVizStorage::recordBatch(viz::data::RecordingBatch& batch)
ARMARX_INFO << "Recorded ArViz batch to: " << filePath;
}
-
std::string armarx::ArVizStorage::startRecording(std::string const& newRecordingPrefix, const Ice::Current&)
{
{
@@ -447,7 +447,7 @@ void armarx::ArVizStorage::stopRecording(const Ice::Current&)
recordingMetaData.firstTimestampInMicroSeconds = -1;
}
-viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
+armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
{
viz::data::RecordingSeq result;
@@ -471,7 +471,7 @@ viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice::Curren
return result;
}
-viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
+armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
{
viz::data::RecordingBatch result;
result.header.index = -1;
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 8e5cc4088..1307eae6e 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -46,7 +46,10 @@ namespace armarx::viz
{
updates.clear();
std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto& layer) { return layer.data_; });
+ [](const auto & layer)
+ {
+ return layer.data_;
+ });
topic->updateLayers(updates);
}
@@ -59,7 +62,10 @@ namespace armarx::viz
{
updates.clear();
std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto& layerRef) { return layerRef->data_; });
+ [](const auto & layerRef)
+ {
+ return layerRef->data_;
+ });
topic->updateLayers(updates);
}
@@ -67,7 +73,10 @@ namespace armarx::viz
{
updates.clear();
std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto& layer) { return layer.data_; });
+ [](const auto & layer)
+ {
+ return layer.data_;
+ });
topic->updateLayers(updates);
}
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index 502bca5d0..2103961b7 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -4,59 +4,54 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-namespace armarx
+namespace armarx::viz::coin
{
- namespace viz
+ ElementVisualization::ElementVisualization()
{
- namespace coin
- {
- ElementVisualization::ElementVisualization()
- {
- // We do everything in millimeters...
- units = new SoUnits();
- units->units = SoUnits::MILLIMETERS;
-
- transform = new SoTransform;
- material = new SoMaterial;
-
- separator = new SoSeparator;
- separator->addChild(units);
- separator->addChild(transform);
- separator->addChild(material);
- }
-
- void ElementVisualization::updateBase(data::Element const& element)
- {
- auto& p = element.pose;
- transform->translation.setValue(p.x, p.y, p.z);
- transform->rotation.setValue(p.qx, p.qy, p.qz, p.qw);
- transform->scaleFactor.setValue(element.scale, element.scale, element.scale);
-
- auto color = element.color;
- const float conv = 1.0f / 255.0f;
- SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
- material->ambientColor.setValue(coinColor);
- material->diffuseColor.setValue(coinColor);
- material->transparency.setValue(1.0f - conv * color.a);
-
- // This enables textured meshes to be displayed with transparency
- bool overrideMaterial = (element.flags & data::ElementFlags::OVERRIDE_MATERIAL);
- material->setOverride(overrideMaterial);
- }
-
- std::unique_ptr<ElementVisualization> ElementVisualizer::create(const data::Element& element)
- {
- std::unique_ptr<ElementVisualization> result(createDerived());
- update(element, result.get());
-
- return result;
- }
-
- bool ElementVisualizer::update(data::Element const& element, ElementVisualization* visu)
- {
- visu->updateBase(element);
- return updateDerived(element, visu);
- }
- }
+ // We do everything in millimeters...
+ units = new SoUnits();
+ units->units = SoUnits::MILLIMETERS;
+
+ transform = new SoTransform;
+ material = new SoMaterial;
+
+ separator = new SoSeparator;
+ separator->addChild(units);
+ separator->addChild(transform);
+ separator->addChild(material);
+ }
+
+ void ElementVisualization::updateBase(data::Element const& element)
+ {
+ auto& p = element.pose;
+ transform->translation.setValue(p.x, p.y, p.z);
+ transform->rotation.setValue(p.qx, p.qy, p.qz, p.qw);
+ transform->scaleFactor.setValue(element.scale, element.scale, element.scale);
+
+ auto color = element.color;
+ const float conv = 1.0f / 255.0f;
+ SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
+ material->ambientColor.setValue(coinColor);
+ material->diffuseColor.setValue(coinColor);
+ material->transparency.setValue(1.0f - conv * color.a);
+
+ // This enables textured meshes to be displayed with transparency
+ bool overrideMaterial = (element.flags & data::ElementFlags::OVERRIDE_MATERIAL);
+ material->setOverride(overrideMaterial);
+ }
+
+ std::unique_ptr<ElementVisualization> ElementVisualizer::create(const data::Element& element)
+ {
+ std::unique_ptr<ElementVisualization> result(createDerived());
+ update(element, result.get());
+
+ return result;
+ }
+
+ bool ElementVisualizer::update(data::Element const& element, ElementVisualization* visu)
+ {
+ visu->updateBase(element);
+ return updateDerived(element, visu);
}
}
+
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index f84dfbbdd..8c3f524fb 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -7,82 +7,78 @@
#include <memory>
-namespace armarx::viz
+namespace armarx::viz::data
{
- namespace data
- {
- class Element;
- }
+ class Element;
+}
- namespace coin
+namespace armarx::viz::coin
+{
+ struct ElementVisualization
{
- struct ElementVisualization
- {
- ElementVisualization();
+ ElementVisualization();
- virtual ~ElementVisualization() = default;
+ virtual ~ElementVisualization() = default;
- void updateBase(data::Element const& element);
+ void updateBase(data::Element const& element);
- SoSeparator* separator;
- SoUnits* units;
- SoTransform* transform;
- SoMaterial* material;
- bool wasUpdated = true;
- };
+ SoSeparator* separator;
+ SoUnits* units;
+ SoTransform* transform;
+ SoMaterial* material;
+ bool wasUpdated = true;
+ };
- class ElementVisualizer
- {
- public:
- virtual ~ElementVisualizer() = default;
+ class ElementVisualizer
+ {
+ public:
+ virtual ~ElementVisualizer() = default;
- std::unique_ptr<ElementVisualization> create(data::Element const& element);
- bool update(data::Element const& element, ElementVisualization* visu);
+ std::unique_ptr<ElementVisualization> create(data::Element const& element);
+ bool update(data::Element const& element, ElementVisualization* visu);
- virtual ElementVisualization* createDerived() = 0;
- virtual bool updateDerived(data::Element const& element, ElementVisualization* data) = 0;
- };
+ virtual ElementVisualization* createDerived() = 0;
+ virtual bool updateDerived(data::Element const& element, ElementVisualization* data) = 0;
+ };
- template <typename NodeT>
- struct TypedElementVisualization : ElementVisualization
+ template <typename NodeT>
+ struct TypedElementVisualization : ElementVisualization
+ {
+ using NodeType = NodeT;
+
+ TypedElementVisualization()
{
- using NodeType = NodeT;
+ node = new NodeType;
+ }
- TypedElementVisualization()
- {
- node = new NodeType;
- }
+ NodeType* node;
+ };
- NodeType* node;
- };
+ template <typename DataT>
+ class TypedElementVisualizer : public ElementVisualizer
+ {
+ public:
+ using DataType = DataT;
+ using ElementType = typename DataType::ElementType;
- template <typename DataT>
- class TypedElementVisualizer : public ElementVisualizer
+ DataType* createDerived() final
{
- public:
- using DataType = DataT;
- using ElementType = typename DataType::ElementType;
-
- DataType* createDerived() final
- {
- DataType* result = new DataType;
- result->separator->addChild(result->node);
- return result;
- }
+ DataType* result = new DataType;
+ result->separator->addChild(result->node);
+ return result;
+ }
- bool updateDerived(data::Element const& element_, ElementVisualization* data_) final
+ bool updateDerived(data::Element const& element_, ElementVisualization* data_) final
+ {
+ auto const& element = static_cast<ElementType const&>(element_);
+ auto* data = dynamic_cast<DataType*>(data_);
+ if (data)
{
- auto const& element = static_cast<ElementType const&>(element_);
- auto* data = dynamic_cast<DataType*>(data_);
- if (data)
- {
- // We want to call update with the correctly downcasted arguments
- return data->update(element);
- }
- return false;
+ // We want to call update with the correctly downcasted arguments
+ return data->update(element);
}
- };
-
- }
+ return false;
+ }
+ };
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
index 7ade9a747..d1e10e97f 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
@@ -58,7 +58,7 @@ namespace armarx::viz::coin
return result;
}
- static std::vector<LoadedObject> cache;
+ static std::vector<LoadedObject> objectcache;
LoadedObject getObjectFromCache(std::string const& project, std::string const& filename)
{
@@ -66,7 +66,7 @@ namespace armarx::viz::coin
LoadedObject result;
- for (LoadedObject const& loaded : cache)
+ for (LoadedObject const& loaded : objectcache)
{
if (loaded.project == project && loaded.filename == filename)
{
@@ -80,7 +80,7 @@ namespace armarx::viz::coin
result.filename = filename;
result.object = loadObject(project, filename);
- cache.push_back(result);
+ objectcache.push_back(result);
return result;
}
@@ -175,10 +175,6 @@ namespace armarx::viz::coin
void clearObjectCache()
{
- cache.clear();
+ objectcache.clear();
}
-
-
-
-
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 7de0d53c8..7c466cdac 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -63,7 +63,7 @@ namespace armarx::viz::coin
return result;
}
- static std::vector<LoadedRobot> cache;
+ static std::vector<LoadedRobot> robotcache;
LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
{
@@ -71,7 +71,7 @@ namespace armarx::viz::coin
LoadedRobot result;
- for (LoadedRobot const& loaded : cache)
+ for (LoadedRobot const& loaded : robotcache)
{
if (loaded.project == project && loaded.filename == filename)
{
@@ -85,7 +85,7 @@ namespace armarx::viz::coin
result.filename = filename;
result.robot = loadRobot(project, filename);
- cache.push_back(result);
+ robotcache.push_back(result);
return result;
}
@@ -209,10 +209,6 @@ namespace armarx::viz::coin
void clearRobotCache()
{
- cache.clear();
+ robotcache.clear();
}
-
-
-
-
}
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index ef8365958..bf823730d 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -29,327 +29,328 @@
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-using namespace armarx;
-
-
-void ArVizExample::onInitComponent()
+namespace armarx
{
- offeringTopicFromProperty("ArVizTopicName");
-}
+ void ArVizExample::onInitComponent()
+ {
+ offeringTopicFromProperty("ArVizTopicName");
+ }
-void ArVizExample::onConnectComponent()
-{
- task = new RunningTask<ArVizExample>(this, &ArVizExample::update);
- task->start();
-}
+ void ArVizExample::onConnectComponent()
+ {
+ task = new RunningTask<ArVizExample>(this, &ArVizExample::update);
+ task->start();
+ }
-void ArVizExample::onDisconnectComponent()
-{
- task->stop();
- task = nullptr;
-}
+ void ArVizExample::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
-void fillTestLayer(viz::Layer& layer, double timeInSeconds)
-{
+ void fillTestLayer(viz::Layer& layer, double timeInSeconds)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 100.0f * std::sin(timeInSeconds);
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 100.0f * std::sin(timeInSeconds);
- viz::Box box = viz::Box("box")
- .position(pos)
- .color(viz::Color::red())
- .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
+ viz::Box box = viz::Box("box")
+ .position(pos)
+ .color(viz::Color::red())
+ .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
- layer.add(box);
- }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.y() = 100.0f * std::sin(timeInSeconds);
- pos.x() = 150.0f;
+ layer.add(box);
+ }
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.y() = 100.0f * std::sin(timeInSeconds);
+ pos.x() = 150.0f;
- viz::Cylinder cyl = viz::Cylinder("cylinder")
- .position(pos)
- .color(viz::Color::green())
- .radius(50.0f)
- .height(100.0f);
+ viz::Cylinder cyl = viz::Cylinder("cylinder")
+ .position(pos)
+ .color(viz::Color::green())
+ .radius(50.0f)
+ .height(100.0f);
- layer.add(cyl);
- }
+ layer.add(cyl);
+ }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = 100.0f * std::sin(timeInSeconds);
- pos.x() = -150.0f;
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = 100.0f * std::sin(timeInSeconds);
+ pos.x() = -150.0f;
- viz::Pose pose = viz::Pose("pose")
- .position(pos)
- .scale(1.0f);
+ viz::Pose pose = viz::Pose("pose")
+ .position(pos)
+ .scale(1.0f);
- layer.add(pose);
- }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
+ layer.add(pose);
+ }
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
- viz::Text text = viz::Text("text")
- .text("Test Text")
- .scale(4.0f)
- .position(pos)
- .color(viz::Color::black());
+ viz::Text text = viz::Text("text")
+ .text("Test Text")
+ .scale(4.0f)
+ .position(pos)
+ .color(viz::Color::black());
- layer.add(text);
- }
- {
- viz::Arrow arrow = viz::Arrow("arrow");
+ layer.add(text);
+ }
+ {
+ viz::Arrow arrow = viz::Arrow("arrow");
- float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
- arrow.length(200.0f + 100.0f * std::sin(modTime));
+ float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
+ arrow.length(200.0f + 100.0f * std::sin(modTime));
- Eigen::AngleAxisf dirRot(modTime, Eigen::Vector3f::UnitZ());
- Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
- arrow.direction(direction);
+ Eigen::AngleAxisf dirRot(modTime, Eigen::Vector3f::UnitZ());
+ Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
+ arrow.direction(direction);
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
- pos.x() = -500.0f;
- arrow.position(pos);
- arrow.color(viz::Color::blue());
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
+ pos.x() = -500.0f;
+ arrow.position(pos);
+ arrow.color(viz::Color::blue());
- layer.add(arrow);
+ layer.add(arrow);
+ }
}
-}
-void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
-{
+ void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
- viz::ArrowCircle circle = viz::ArrowCircle("circle")
- .position(pos)
- .radius(100.0f)
- .width(10.0f)
- .color(viz::Color::fromRGBA(255, 0, 255));
+ viz::ArrowCircle circle = viz::ArrowCircle("circle")
+ .position(pos)
+ .radius(100.0f)
+ .width(10.0f)
+ .color(viz::Color::fromRGBA(255, 0, 255));
- float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
- circle.completion(std::sin(modTime));
+ float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
+ circle.completion(std::sin(modTime));
- layer.add(circle);
- }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1000.0f;
-
- viz::Polygon poly = viz::Polygon("poly")
- .position(pos)
- .color(viz::Color::fromRGBA(0, 128, 255, 128))
- .lineColor(viz::Color::fromRGBA(0, 0, 255))
- .lineWidth(1.0f);
-
- float t = 1.0f + std::sin(timeInSeconds);
- float offset = 50.0f * t;
- poly.addPoint(Eigen::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f});
- poly.addPoint(Eigen::Vector3f{-200.0f, +200.0f, 0.0f});
- poly.addPoint(Eigen::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f});
- poly.addPoint(Eigen::Vector3f{+200.0f, -200.0f, 0.0f});
-
- layer.add(poly);
- }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1500.0f;
-
- viz::Polygon poly = viz::Polygon("poly2")
- .position(pos)
- .color(viz::Color::fromRGBA(255, 128, 0, 128))
- .lineWidth(0.0f);
-
- float t = 1.0f + std::sin(timeInSeconds);
- float offset = 20.0f * t;
- poly.addPoint(Eigen::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
- poly.addPoint(Eigen::Vector3f{-100.0f, +100.0f, 0.0f});
- poly.addPoint(Eigen::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
- poly.addPoint(Eigen::Vector3f{+100.0f, -100.0f, 0.0f});
-
- layer.add(poly);
- }
- {
- armarx::Vector3f vertices[] =
+ layer.add(circle);
+ }
{
- {-100.0f, -100.0f, 0.0f},
- {-100.0f, +100.0f, 0.0f},
- {+100.0f, +100.0f, 0.0f},
- {+100.0f, +100.0f, 200.0f},
- };
- std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
-
- armarx::viz::data::Color colors[] =
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1000.0f;
+
+ viz::Polygon poly = viz::Polygon("poly")
+ .position(pos)
+ .color(viz::Color::fromRGBA(0, 128, 255, 128))
+ .lineColor(viz::Color::fromRGBA(0, 0, 255))
+ .lineWidth(1.0f);
+
+ float t = 1.0f + std::sin(timeInSeconds);
+ float offset = 50.0f * t;
+ poly.addPoint(Eigen::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f});
+ poly.addPoint(Eigen::Vector3f{-200.0f, +200.0f, 0.0f});
+ poly.addPoint(Eigen::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f});
+ poly.addPoint(Eigen::Vector3f{+200.0f, -200.0f, 0.0f});
+
+ layer.add(poly);
+ }
{
- {255, 255, 0, 0},
- {255, 0, 255, 0},
- {255, 0, 0, 255},
- };
- std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
-
- viz::data::Face faces[] =
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1500.0f;
+
+ viz::Polygon poly = viz::Polygon("poly2")
+ .position(pos)
+ .color(viz::Color::fromRGBA(255, 128, 0, 128))
+ .lineWidth(0.0f);
+
+ float t = 1.0f + std::sin(timeInSeconds);
+ float offset = 20.0f * t;
+ poly.addPoint(Eigen::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
+ poly.addPoint(Eigen::Vector3f{-100.0f, +100.0f, 0.0f});
+ poly.addPoint(Eigen::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
+ poly.addPoint(Eigen::Vector3f{+100.0f, -100.0f, 0.0f});
+
+ layer.add(poly);
+ }
{
+ armarx::Vector3f vertices[] =
{
- 0, 1, 2,
- 0, 1, 2,
- },
+ {-100.0f, -100.0f, 0.0f},
+ {-100.0f, +100.0f, 0.0f},
+ {+100.0f, +100.0f, 0.0f},
+ {+100.0f, +100.0f, 200.0f},
+ };
+ std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
+
+ armarx::viz::data::Color colors[] =
{
- 1, 2, 3,
- 0, 1, 2,
- },
- };
- std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1000.0f;
- pos.x() = -500.0f;
-
- viz::Mesh mesh = viz::Mesh("mesh")
- .position(pos)
- .vertices(vertices, verticesSize)
- .colors(colors, colorsSize)
- .faces(faces, facesSize);
-
- layer.add(mesh);
- }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 500.0f;
+ {255, 255, 0, 0},
+ {255, 0, 255, 0},
+ {255, 0, 0, 255},
+ };
+ std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
- viz::Robot robot = viz::Robot("robot")
- .position(pos)
- .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ viz::data::Face faces[] =
+ {
+ {
+ 0, 1, 2,
+ 0, 1, 2,
+ },
+ {
+ 1, 2, 3,
+ 0, 1, 2,
+ },
+ };
+ std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1000.0f;
+ pos.x() = -500.0f;
+
+ viz::Mesh mesh = viz::Mesh("mesh")
+ .position(pos)
+ .vertices(vertices, verticesSize)
+ .colors(colors, colorsSize)
+ .faces(faces, facesSize);
- // Full model
- if (true)
- {
- robot.useFullModel();
+ layer.add(mesh);
}
- else
{
- robot.useCollisionModel()
- .overrideColor(viz::Color::fromRGBA(0, 255, 128, 128));
- }
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 500.0f;
- float value = 0.5f * (1.0f + std::sin(timeInSeconds));
- robot.joint("ArmR2_Sho1", value);
- robot.joint("ArmR3_Sho2", value);
+ viz::Robot robot = viz::Robot("robot")
+ .position(pos)
+ .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
- layer.add(robot);
- }
-}
+ // Full model
+ if (true)
+ {
+ robot.useFullModel();
+ }
+ else
+ {
+ robot.useCollisionModel()
+ .overrideColor(viz::Color::fromRGBA(0, 255, 128, 128));
+ }
-void fillPermanentLayer(viz::Layer& layer)
-{
- viz::Box box = viz::Box("permBox")
- .position(Eigen::Vector3f(2000.0f, 0.0f, 0.0f))
- .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
- .color(viz::Color::fromRGBA(255, 165, 0));
+ float value = 0.5f * (1.0f + std::sin(timeInSeconds));
+ robot.joint("ArmR2_Sho1", value);
+ robot.joint("ArmR3_Sho2", value);
- layer.add(box);
-}
+ layer.add(robot);
+ }
+ }
-void fillPointsLayer(viz::Layer& layer, double timeInSeconds)
-{
- viz::PointCloud pc = viz::PointCloud("points")
- .position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
- .transparency(0.0f);
+ void fillPermanentLayer(viz::Layer& layer)
+ {
+ viz::Box box = viz::Box("permBox")
+ .position(Eigen::Vector3f(2000.0f, 0.0f, 0.0f))
+ .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
+ .color(viz::Color::fromRGBA(255, 165, 0));
+
+ layer.add(box);
+ }
- viz::ColoredPoint p;
- p.color = viz::Color::fromRGBA(255, 255, 0, 255);
- for (int x = -200; x <= 200; ++x)
+ void fillPointsLayer(viz::Layer& layer, double timeInSeconds)
{
- p.x = 2.0f * x;
- double phase = timeInSeconds + x / 50.0f;
- double heightT = std::max(0.0, std::min(0.5 * (1.0 + std::sin(phase)), 1.0));
- for (int y = -200; y <= 200; ++y)
+ viz::PointCloud pc = viz::PointCloud("points")
+ .position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
+ .transparency(0.0f);
+
+ viz::ColoredPoint p;
+ p.color = viz::Color::fromRGBA(255, 255, 0, 255);
+ for (int x = -200; x <= 200; ++x)
{
- p.y = 2.0f * y;
- p.z = 100.0 * heightT;
+ p.x = 2.0f * x;
+ double phase = timeInSeconds + x / 50.0f;
+ double heightT = std::max(0.0, std::min(0.5 * (1.0 + std::sin(phase)), 1.0));
+ for (int y = -200; y <= 200; ++y)
+ {
+ p.y = 2.0f * y;
+ p.z = 100.0 * heightT;
- p.color.g = 255.0 * heightT;
- p.color.b = 255.0 * (1.0 - heightT);
- pc.addPoint(p);
+ p.color.g = 255.0 * heightT;
+ p.color.b = 255.0 * (1.0 - heightT);
+ pc.addPoint(p);
+ }
}
- }
- layer.add(pc);
-}
+ layer.add(pc);
+ }
-void fillObjectsLayer(viz::Layer& layer, double timeInSeconds)
-{
+ void fillObjectsLayer(viz::Layer& layer, double timeInSeconds)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 100.0f * std::sin(timeInSeconds);
- pos.y() = 1000.0f;
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 100.0f * std::sin(timeInSeconds);
+ pos.y() = 1000.0f;
- viz::Object sponge = viz::Object("sponge")
- .position(pos)
- .file("ArmarObjects", "ArmarObjects/sponge/sponge.xml");
+ viz::Object sponge = viz::Object("sponge")
+ .position(pos)
+ .file("ArmarObjects", "ArmarObjects/sponge/sponge.xml");
- layer.add(sponge);
- }
+ layer.add(sponge);
+ }
- {
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 300.0f + 100.0f * std::sin(timeInSeconds);
- pos.y() = 1000.0f;
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 300.0f + 100.0f * std::sin(timeInSeconds);
+ pos.y() = 1000.0f;
- viz::Object spraybottle = viz::Object("spraybottle")
- .position(pos)
- .file("ArmarObjects", "ArmarObjects/spraybottle/spraybottle.xml");
+ viz::Object spraybottle = viz::Object("spraybottle")
+ .position(pos)
+ .file("ArmarObjects", "ArmarObjects/spraybottle/spraybottle.xml");
- layer.add(spraybottle);
+ layer.add(spraybottle);
+ }
}
-}
-void ArVizExample::update()
-{
- viz::Client arviz(*this);
+ void ArVizExample::update()
+ {
+ viz::Client arviz(*this);
- viz::Layer testLayer = arviz.layer("Test");
- viz::Layer exampleLayer = arviz.layer("Example");
- viz::Layer pointsLayer = arviz.layer("Points");
- viz::Layer objectsLayer = arviz.layer("Objects");
+ viz::Layer testLayer = arviz.layer("Test");
+ viz::Layer exampleLayer = arviz.layer("Example");
+ viz::Layer pointsLayer = arviz.layer("Points");
+ viz::Layer objectsLayer = arviz.layer("Objects");
- // This layer is not updated in the loop
- viz::Layer permanentLayer = arviz.layer("Permanent");
- fillPermanentLayer(permanentLayer);
- arviz.commit(permanentLayer);
+ // This layer is not updated in the loop
+ viz::Layer permanentLayer = arviz.layer("Permanent");
+ fillPermanentLayer(permanentLayer);
+ arviz.commit(permanentLayer);
- CycleUtil c(20);
- while (!task->isStopped())
- {
- double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
+ CycleUtil c(20);
+ while (!task->isStopped())
+ {
+ double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
- testLayer.clear();
- fillTestLayer(testLayer, timeInSeconds);
- exampleLayer.clear();
- fillExampleLayer(exampleLayer, timeInSeconds);
- pointsLayer.clear();
- fillPointsLayer(pointsLayer, timeInSeconds);
- objectsLayer.clear();
- fillObjectsLayer(objectsLayer, timeInSeconds);
+ testLayer.clear();
+ fillTestLayer(testLayer, timeInSeconds);
+ exampleLayer.clear();
+ fillExampleLayer(exampleLayer, timeInSeconds);
+ pointsLayer.clear();
+ fillPointsLayer(pointsLayer, timeInSeconds);
+ objectsLayer.clear();
+ fillObjectsLayer(objectsLayer, timeInSeconds);
- arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer});
+ arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer});
- c.waitForCycleDuration();
+ c.waitForCycleDuration();
+ }
}
-}
-void ArVizExample::onExitComponent()
-{
+ void ArVizExample::onExitComponent()
+ {
-}
+ }
-armarx::PropertyDefinitionsPtr ArVizExample::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new ArVizExamplePropertyDefinitions(
- getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr ArVizExample::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new ArVizExamplePropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
+
diff --git a/source/RobotAPI/components/ArViz/IceConversions.h b/source/RobotAPI/components/ArViz/IceConversions.h
index 8a9eb2564..4c1dc54f4 100644
--- a/source/RobotAPI/components/ArViz/IceConversions.h
+++ b/source/RobotAPI/components/ArViz/IceConversions.h
@@ -5,32 +5,29 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-namespace armarx
+namespace armarx::viz
{
- namespace viz
- {
-
- inline Eigen::Matrix4f toEigen(data::GlobalPose const& pose)
- {
- Eigen::Matrix4f result;
- result.block<3, 3>(0, 0) = Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz).toRotationMatrix();
- result.block<3, 1>(0, 3) = Eigen::Vector3f(pose.x, pose.y, pose.z);
- result.block<1, 4>(3, 0) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);
- return result;
- }
- inline data::GlobalPose poseToIce(Eigen::Vector3f const& pos, Eigen::Quaternionf const& ori)
- {
- data::GlobalPose result;
- result.x = pos.x();
- result.y = pos.y();
- result.z = pos.z();
- result.qw = ori.w();
- result.qx = ori.x();
- result.qy = ori.y();
- result.qz = ori.z();
- return result;
- }
+ inline Eigen::Matrix4f toEigen(data::GlobalPose const& pose)
+ {
+ Eigen::Matrix4f result;
+ result.block<3, 3>(0, 0) = Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz).toRotationMatrix();
+ result.block<3, 1>(0, 3) = Eigen::Vector3f(pose.x, pose.y, pose.z);
+ result.block<1, 4>(3, 0) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);
+ return result;
+ }
+ inline data::GlobalPose poseToIce(Eigen::Vector3f const& pos, Eigen::Quaternionf const& ori)
+ {
+ data::GlobalPose result;
+ result.x = pos.x();
+ result.y = pos.y();
+ result.z = pos.z();
+ result.qw = ori.w();
+ result.qx = ori.x();
+ result.qy = ori.y();
+ result.qz = ori.z();
+ return result;
}
+
}
diff --git a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
index 25825973d..f420ee596 100644
--- a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
+++ b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
@@ -56,24 +56,24 @@ namespace
BOOST_AUTO_TEST_CASE(test_has_member_rgba)
{
- static_assert (!armarx::viz::detail::has_members_rgba<PointXYZ>::value,
- "has_members_rgba<PointXYZ>::value must be false.");
+ static_assert(!armarx::viz::detail::has_members_rgba<PointXYZ>::value,
+ "has_members_rgba<PointXYZ>::value must be false.");
- static_assert (armarx::viz::detail::has_members_rgba<PointXYZRGBA>::value,
- "has_members_rgba<PointXYZRGBA>::value must be true.");
+ static_assert(armarx::viz::detail::has_members_rgba<PointXYZRGBA>::value,
+ "has_members_rgba<PointXYZRGBA>::value must be true.");
- static_assert (armarx::viz::detail::has_members_rgba<PointXYZRGBL>::value,
- "has_members_rgba<PointXYZRGBL>::value must be true.");
+ static_assert(armarx::viz::detail::has_members_rgba<PointXYZRGBL>::value,
+ "has_members_rgba<PointXYZRGBL>::value must be true.");
- static_assert (!armarx::viz::detail::has_member_label<PointXYZ>::value,
- "has_member_label<PointXYZ>::value must be false.");
+ static_assert(!armarx::viz::detail::has_member_label<PointXYZ>::value,
+ "has_member_label<PointXYZ>::value must be false.");
- static_assert (!armarx::viz::detail::has_member_label<PointXYZRGBA>::value,
- "has_member_label<PointXYZRGBA>::value must be false.");
+ static_assert(!armarx::viz::detail::has_member_label<PointXYZRGBA>::value,
+ "has_member_label<PointXYZRGBA>::value must be false.");
- static_assert (armarx::viz::detail::has_member_label<PointXYZRGBL>::value,
- "has_member_label<PointXYZRGBL>::value must be true.");
+ static_assert(armarx::viz::detail::has_member_label<PointXYZRGBL>::value,
+ "has_member_label<PointXYZRGBL>::value must be true.");
BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
index a7697f8d5..a68eb49d8 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
@@ -23,91 +23,91 @@
#include "CyberGloveObserver.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-using namespace armarx;
-
-
-void CyberGloveObserver::onInitObserver()
-{
- usingTopic(getProperty<std::string>("CyberGloveTopicName").getValue());
-}
-
-
-void CyberGloveObserver::onConnectObserver()
+namespace armarx
{
- lastUpdate = TimeUtil::GetTime();
-}
-
-
-//void CyberGloveObserver::onDisconnectComponent()
-//{
-
-//}
-
+ void CyberGloveObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("CyberGloveTopicName").getValue());
+ }
-void CyberGloveObserver::onExitObserver()
-{
-}
-
-PropertyDefinitionsPtr CyberGloveObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new CyberGloveObserverPropertyDefinitions(
- getConfigIdentifier()));
-}
+ void CyberGloveObserver::onConnectObserver()
+ {
+ lastUpdate = TimeUtil::GetTime();
+ }
-void CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- IceUtil::Time now = TimeUtil::GetTime();
+ //void CyberGloveObserver::onDisconnectComponent()
+ //{
- long deltaUS = (now - lastUpdate).toMicroSeconds();
+ //}
- ARMARX_IMPORTANT << deltaUS << " " << gloveValues.time << " " << gloveValues.indexDIP;
- lastUpdate = now;
- latestValues = gloveValues;
+ void CyberGloveObserver::onExitObserver()
+ {
+ }
- std::string name = gloveValues.name;
- if (!existsChannel(name))
+ PropertyDefinitionsPtr CyberGloveObserver::createPropertyDefinitions()
{
- offerChannel(name, "CyberGlove motor data");
+ return PropertyDefinitionsPtr(new CyberGloveObserverPropertyDefinitions(
+ getConfigIdentifier()));
}
- offerOrUpdateDataField(name, "name", Variant(name), "Name of the prostesis");
-
- offerOrUpdateDataField(name, "thumbCMC", Variant(gloveValues.thumbCMC), "Value of thumbCMC");
- offerOrUpdateDataField(name, "thumbMCP", Variant(gloveValues.thumbMCP), "Value of thumbMCP");
- offerOrUpdateDataField(name, "thumbIP", Variant(gloveValues.thumbIP), "Value of thumbIP");
- offerOrUpdateDataField(name, "thumbAbd", Variant(gloveValues.thumbAbd), "Value of thumbAbd");
- offerOrUpdateDataField(name, "indexMCP", Variant(gloveValues.indexMCP), "Value of indexMCP");
- offerOrUpdateDataField(name, "indexPIP", Variant(gloveValues.indexPIP), "Value of indexPIP");
- offerOrUpdateDataField(name, "indexDIP", Variant(gloveValues.indexDIP), "Value of indexDIP");
- offerOrUpdateDataField(name, "middleMCP", Variant(gloveValues.middleMCP), "Value of middleMCP");
- offerOrUpdateDataField(name, "middlePIP", Variant(gloveValues.middlePIP), "Value of middlePIP");
- offerOrUpdateDataField(name, "middleDIP", Variant(gloveValues.middleDIP), "Value of middleDIP");
- offerOrUpdateDataField(name, "middleAbd", Variant(gloveValues.middleAbd), "Value of middleAbd");
- offerOrUpdateDataField(name, "ringMCP", Variant(gloveValues.ringMCP), "Value of ringMCP");
- offerOrUpdateDataField(name, "ringPIP", Variant(gloveValues.ringPIP), "Value of ringPIP");
- offerOrUpdateDataField(name, "ringDIP", Variant(gloveValues.ringDIP), "Value of ringDIP");
- offerOrUpdateDataField(name, "ringAbd", Variant(gloveValues.ringAbd), "Value of ringAbd");
- offerOrUpdateDataField(name, "littleMCP", Variant(gloveValues.littleMCP), "Value of littleMCP");
- offerOrUpdateDataField(name, "littlePIP", Variant(gloveValues.littlePIP), "Value of littlePIP");
- offerOrUpdateDataField(name, "littleDIP", Variant(gloveValues.littleDIP), "Value of littleDIP");
- offerOrUpdateDataField(name, "littleAbd", Variant(gloveValues.littleAbd), "Value of littleAbd");
- offerOrUpdateDataField(name, "palmArch", Variant(gloveValues.palmArch), "Value of palmArch");
- offerOrUpdateDataField(name, "wristFlex", Variant(gloveValues.wristFlex), "Value of wristFlex");
- offerOrUpdateDataField(name, "wristAbd", Variant(gloveValues.wristAbd), "Value of wristAbd");
-
-
- updateChannel(name);
-}
+ void CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
+ {
+ ScopedLock lock(dataMutex);
+
+ IceUtil::Time now = TimeUtil::GetTime();
+
+ long deltaUS = (now - lastUpdate).toMicroSeconds();
+
+ ARMARX_IMPORTANT << deltaUS << " " << gloveValues.time << " " << gloveValues.indexDIP;
+ lastUpdate = now;
+
+ latestValues = gloveValues;
+
+
+ std::string name = gloveValues.name;
+ if (!existsChannel(name))
+ {
+ offerChannel(name, "CyberGlove motor data");
+ }
+
+ offerOrUpdateDataField(name, "name", Variant(name), "Name of the prostesis");
+
+ offerOrUpdateDataField(name, "thumbCMC", Variant(gloveValues.thumbCMC), "Value of thumbCMC");
+ offerOrUpdateDataField(name, "thumbMCP", Variant(gloveValues.thumbMCP), "Value of thumbMCP");
+ offerOrUpdateDataField(name, "thumbIP", Variant(gloveValues.thumbIP), "Value of thumbIP");
+ offerOrUpdateDataField(name, "thumbAbd", Variant(gloveValues.thumbAbd), "Value of thumbAbd");
+ offerOrUpdateDataField(name, "indexMCP", Variant(gloveValues.indexMCP), "Value of indexMCP");
+ offerOrUpdateDataField(name, "indexPIP", Variant(gloveValues.indexPIP), "Value of indexPIP");
+ offerOrUpdateDataField(name, "indexDIP", Variant(gloveValues.indexDIP), "Value of indexDIP");
+ offerOrUpdateDataField(name, "middleMCP", Variant(gloveValues.middleMCP), "Value of middleMCP");
+ offerOrUpdateDataField(name, "middlePIP", Variant(gloveValues.middlePIP), "Value of middlePIP");
+ offerOrUpdateDataField(name, "middleDIP", Variant(gloveValues.middleDIP), "Value of middleDIP");
+ offerOrUpdateDataField(name, "middleAbd", Variant(gloveValues.middleAbd), "Value of middleAbd");
+ offerOrUpdateDataField(name, "ringMCP", Variant(gloveValues.ringMCP), "Value of ringMCP");
+ offerOrUpdateDataField(name, "ringPIP", Variant(gloveValues.ringPIP), "Value of ringPIP");
+ offerOrUpdateDataField(name, "ringDIP", Variant(gloveValues.ringDIP), "Value of ringDIP");
+ offerOrUpdateDataField(name, "ringAbd", Variant(gloveValues.ringAbd), "Value of ringAbd");
+ offerOrUpdateDataField(name, "littleMCP", Variant(gloveValues.littleMCP), "Value of littleMCP");
+ offerOrUpdateDataField(name, "littlePIP", Variant(gloveValues.littlePIP), "Value of littlePIP");
+ offerOrUpdateDataField(name, "littleDIP", Variant(gloveValues.littleDIP), "Value of littleDIP");
+ offerOrUpdateDataField(name, "littleAbd", Variant(gloveValues.littleAbd), "Value of littleAbd");
+ offerOrUpdateDataField(name, "palmArch", Variant(gloveValues.palmArch), "Value of palmArch");
+ offerOrUpdateDataField(name, "wristFlex", Variant(gloveValues.wristFlex), "Value of wristFlex");
+ offerOrUpdateDataField(name, "wristAbd", Variant(gloveValues.wristAbd), "Value of wristAbd");
+
+
+ updateChannel(name);
+ }
-CyberGloveValues armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- return latestValues;
+ CyberGloveValues armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
+ {
+ ScopedLock lock(dataMutex);
+ return latestValues;
+ }
}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index bad03e396..2edd13823 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -67,9 +67,9 @@ namespace armarx::detail::DebugDrawerHelper
ARMARX_TRACE;
if (_rn)
{
- return math::Helpers::TransformDirection(_rn->getGlobalPose(), direction);
+ return ::math::Helpers::TransformDirection(_rn->getGlobalPose(), direction);
}
- return math::Helpers::TransformDirection(_fallbackFrame, direction);
+ return ::math::Helpers::TransformDirection(_fallbackFrame, direction);
}
PosePtr FrameView::makeGlobal(const Eigen::Matrix4f& pose) const
{
@@ -435,10 +435,10 @@ namespace armarx
{
return DrawColor
{
- math::Helpers::Lerp(a.r, b.r, f),
- math::Helpers::Lerp(a.g, b.g, f),
- math::Helpers::Lerp(a.b, b.b, f),
- math::Helpers::Lerp(a.a, b.a, f)
+ ::math::Helpers::Lerp(a.r, b.r, f),
+ ::math::Helpers::Lerp(a.g, b.g, f),
+ ::math::Helpers::Lerp(a.b, b.b, f),
+ ::math::Helpers::Lerp(a.a, b.a, f)
};
}
}
diff --git a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
index c5a80a832..30cbeea21 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
+++ b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
@@ -24,58 +24,58 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-using namespace armarx;
-
-
-void DummyTextToSpeech::onInitComponent()
+namespace armarx
{
- usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
- offeringTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
-}
+ void DummyTextToSpeech::onInitComponent()
+ {
+ usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
+ offeringTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
+ }
-void DummyTextToSpeech::onConnectComponent()
-{
- textToSpeechStateTopicPrx = getTopic<TextToSpeechStateInterfacePrx>(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
- textToSpeechStateTopicPrx->reportState(eIdle);
-}
+ void DummyTextToSpeech::onConnectComponent()
+ {
+ textToSpeechStateTopicPrx = getTopic<TextToSpeechStateInterfacePrx>(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
+ textToSpeechStateTopicPrx->reportState(eIdle);
+ }
-void DummyTextToSpeech::onDisconnectComponent()
-{
+ void DummyTextToSpeech::onDisconnectComponent()
+ {
-}
+ }
-void DummyTextToSpeech::onExitComponent()
-{
+ void DummyTextToSpeech::onExitComponent()
+ {
-}
+ }
-armarx::PropertyDefinitionsPtr DummyTextToSpeech::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new DummyTextToSpeechPropertyDefinitions(
- getConfigIdentifier()));
-}
+ armarx::PropertyDefinitionsPtr DummyTextToSpeech::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new DummyTextToSpeechPropertyDefinitions(
+ getConfigIdentifier()));
+ }
-void armarx::DummyTextToSpeech::reportText(const std::string& text, const Ice::Current&)
-{
- //ARMARX_IMPORTANT << "reportState";
- textToSpeechStateTopicPrx->reportState(eStartedSpeaking);
+ void armarx::DummyTextToSpeech::reportText(const std::string& text, const Ice::Current&)
+ {
+ //ARMARX_IMPORTANT << "reportState";
+ textToSpeechStateTopicPrx->reportState(eStartedSpeaking);
- ARMARX_IMPORTANT << "DummyTTS StartedSpeaking: " << text;
+ ARMARX_IMPORTANT << "DummyTTS StartedSpeaking: " << text;
- //ARMARX_IMPORTANT << "sleep";
- sleep(1);
- TimeUtil::SleepMS(text.length() * 10);
- //ARMARX_IMPORTANT << "reportState";
- ARMARX_IMPORTANT << "DummyTTS FinishedSpeaking";
- textToSpeechStateTopicPrx->reportState(eFinishedSpeaking);
-}
+ //ARMARX_IMPORTANT << "sleep";
+ sleep(1);
+ TimeUtil::SleepMS(text.length() * 10);
+ //ARMARX_IMPORTANT << "reportState";
+ ARMARX_IMPORTANT << "DummyTTS FinishedSpeaking";
+ textToSpeechStateTopicPrx->reportState(eFinishedSpeaking);
+ }
-void armarx::DummyTextToSpeech::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
-{
- ARMARX_WARNING << "reportTextWithParams is not implemented";
+ void armarx::DummyTextToSpeech::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
+ {
+ ARMARX_WARNING << "reportTextWithParams is not implemented";
+ }
}
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index 57f83e7a6..0653de63c 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -32,551 +32,550 @@
#include <time.h>
-using namespace armarx;
-
-
-void FrameTracking::onInitComponent()
+namespace armarx
{
- usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
- usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
- usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
- if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ void FrameTracking::onInitComponent()
{
- usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
- }
-
- enabled = false;
- frameName = getProperty<std::string>("FrameOnStartup").getValue();
-
- maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
- yawAcceleration = getProperty<float>("YawAcceleration").getValue();
-
- maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
- pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
-}
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
+ usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ }
+ enabled = false;
+ frameName = getProperty<std::string>("FrameOnStartup").getValue();
-void FrameTracking::onConnectComponent()
-{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(getProperty<std::string>("KinematicUnitObserverName").getValue());
+ maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
+ yawAcceleration = getProperty<float>("YawAcceleration").getValue();
- localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
- headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
- if (!headYawJoint || !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
- {
- ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue() << " is not a valid joint.";
- getArmarXManager()->asyncShutdown();
- }
- headPitchJoint = localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
- if (!headPitchJoint || !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
- {
- ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue() << " is not a valid joint.";
- getArmarXManager()->asyncShutdown();
- }
- cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
- if (!cameraNode)
- {
- ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue() << " is not a valid node.";
- getArmarXManager()->asyncShutdown();
+ maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
+ pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
}
- processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
- _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
- if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ void FrameTracking::onConnectComponent()
{
- _remoteGui = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
- RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Tracking: "))
- .addChild(RemoteGui::makeTextLabel("Enabled"))
- .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
- .addChild(RemoteGui::makeTextLabel("Frame"))
- .addChild(
- RemoteGui::makeComboBox("tracking_frame")
- .options(localRobot->getRobotNodeNames())
- .addOptions({""})
- .value(frameName)
- )
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at frame: "))
- .addChild(
- RemoteGui::makeComboBox("frame_look")
- .options(localRobot->getRobotNodeNames())
- .value(frameName)
- )
- .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at global point: "))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_global_point").label("look at"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_robot_point").label("look at"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("yaw from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("yaw to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("pitch "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(static_cast<int>((headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeButton("button_scan_in_configuration_space").label("scan"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeTextLabel("acceleration "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(static_cast<int>(8 / 0.1)).value(4.0f))
- .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan"))
- );
-
- rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
-
- _guiTask = new SimplePeriodicTask<>([this]()
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(getProperty<std::string>("KinematicUnitObserverName").getValue());
+
+ localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
+ headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
+ if (!headYawJoint || !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
{
- bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
- std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
+ ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue() << " is not a valid joint.";
+ getArmarXManager()->asyncShutdown();
+ }
+ headPitchJoint = localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
+ if (!headPitchJoint || !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
+ {
+ ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue() << " is not a valid joint.";
+ getArmarXManager()->asyncShutdown();
+ }
+ cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
+ if (!cameraNode)
+ {
+ ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue() << " is not a valid node.";
+ getArmarXManager()->asyncShutdown();
+ }
- _guiTab.receiveUpdates();
+ processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
+ _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
- if (oldEnabledGui == enabled)
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ _remoteGui = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Tracking: "))
+ .addChild(RemoteGui::makeTextLabel("Enabled"))
+ .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
+ .addChild(RemoteGui::makeTextLabel("Frame"))
+ .addChild(
+ RemoteGui::makeComboBox("tracking_frame")
+ .options(localRobot->getRobotNodeNames())
+ .addOptions({""})
+ .value(frameName)
+ )
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at frame: "))
+ .addChild(
+ RemoteGui::makeComboBox("frame_look")
+ .options(localRobot->getRobotNodeNames())
+ .value(frameName)
+ )
+ .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at global point: "))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeButton("button_look_at_global_point").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeButton("button_look_at_robot_point").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("yaw from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("yaw to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("pitch "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(static_cast<int>((headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
+ .addChild(RemoteGui::makeButton("button_scan_in_configuration_space").label("scan"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeTextLabel("to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
+ .addChild(RemoteGui::makeTextLabel("acceleration "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(static_cast<int>(8 / 0.1)).value(4.0f))
+ .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan"))
+ );
+
+ rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
+
+ _guiTask = new SimplePeriodicTask<>([this]()
{
- // only apply changes of gui if not already changed by ice
- _enableTracking(_guiTab.getValue<bool>("enabled").get());
- }
- _guiTab.getValue<bool>("enabled").set(enabled);
+ bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
+ std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
- if (oldFrameGui == frameName && oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
- {
- // only apply changes of gui if not already changed by ice
- setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
- }
- _guiTab.getValue<std::string>("tracking_frame").set(frameName);
+ _guiTab.receiveUpdates();
- _guiTab.sendUpdates();
+ if (oldEnabledGui == enabled)
+ {
+ // only apply changes of gui if not already changed by ice
+ _enableTracking(_guiTab.getValue<bool>("enabled").get());
+ }
+ _guiTab.getValue<bool>("enabled").set(enabled);
- if (_guiTab.getButton("button_look_at_frame").clicked())
- {
- lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
- }
+ if (oldFrameGui == frameName && oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
+ {
+ // only apply changes of gui if not already changed by ice
+ setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
+ }
+ _guiTab.getValue<std::string>("tracking_frame").set(frameName);
- if (_guiTab.getButton("button_look_at_global_point").clicked())
- {
- lookAtPointInGlobalFrame(Vector3f{_guiTab.getValue<float>("global_point_x").get(),
- _guiTab.getValue<float>("global_point_y").get(),
- _guiTab.getValue<float>("global_point_z").get()});
- }
+ _guiTab.sendUpdates();
- if (_guiTab.getButton("button_look_at_robot_point").clicked())
- {
- lookAtPointInRobotFrame(Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
- _guiTab.getValue<float>("robot_point_y").get(),
- _guiTab.getValue<float>("robot_point_z").get()});
- }
+ if (_guiTab.getButton("button_look_at_frame").clicked())
+ {
+ lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
+ }
- if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
- {
- scanInConfigurationSpace(_guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
- _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
- {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
- _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
- }
+ if (_guiTab.getButton("button_look_at_global_point").clicked())
+ {
+ lookAtPointInGlobalFrame(Vector3f{_guiTab.getValue<float>("global_point_x").get(),
+ _guiTab.getValue<float>("global_point_y").get(),
+ _guiTab.getValue<float>("global_point_z").get()});
+ }
- if (_guiTab.getButton("button_scan_in_workspace").clicked())
- {
- scanInWorkspace(
+ if (_guiTab.getButton("button_look_at_robot_point").clicked())
{
+ lookAtPointInRobotFrame(Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
+ _guiTab.getValue<float>("robot_point_y").get(),
+ _guiTab.getValue<float>("robot_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
+ {
+ scanInConfigurationSpace(_guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
+ _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
+ {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
+ _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
+ }
+
+ if (_guiTab.getButton("button_scan_in_workspace").clicked())
+ {
+ scanInWorkspace(
{
- _guiTab.getValue<float>("scan_in_workspace_from_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_z").get()
+ {
+ _guiTab.getValue<float>("scan_in_workspace_from_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_z").get()
+ },
+ {
+ _guiTab.getValue<float>("scan_in_workspace_to_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_z").get()
+ }
},
- {
- _guiTab.getValue<float>("scan_in_workspace_to_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_z").get()
- }
- },
- _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
- _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
- }
- }, 10);
+ _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
+ _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
+ }
+ }, 10);
- RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
+ RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
- _remoteGui->createTab(getName(), rootLayout);
- _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
+ _remoteGui->createTab(getName(), rootLayout);
+ _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
- _guiTask->start();
+ _guiTask->start();
+ }
}
-}
-void FrameTracking::onDisconnectComponent()
-{
- _enableTracking(false);
- if (_guiTask)
+ void FrameTracking::onDisconnectComponent()
{
- _guiTask->stop();
- _guiTask = nullptr;
+ _enableTracking(false);
+ if (_guiTask)
+ {
+ _guiTask->stop();
+ _guiTask = nullptr;
+ }
}
-}
-
-void FrameTracking::onExitComponent()
-{
-
-}
-armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(
- getConfigIdentifier()));
-}
+ void FrameTracking::onExitComponent()
+ {
-void FrameTracking::enableTracking(bool enable, const Ice::Current&)
-{
- _enableTracking(enable);
-}
+ }
-void FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
-{
- if (enabled)
+ armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
{
- ARMARX_WARNING << "Disable tracking to set new frame.";
- return;
+ return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(
+ getConfigIdentifier()));
}
- this->frameName = frameName;
-}
-
-std::string FrameTracking::getFrame(const Ice::Current&) const
-{
- return frameName;
-}
-void FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
-{
- if (enabled)
+ void FrameTracking::enableTracking(bool enable, const Ice::Current&)
{
- ARMARX_WARNING << "Disable tracking to use lookAt functions.";
- return;
+ _enableTracking(enable);
}
- if (!localRobot->hasRobotNode(frameName))
+
+ void FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
{
- ARMARX_ERROR << frameName << " does not exist.";
- return;
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to set new frame.";
+ return;
+ }
+ this->frameName = frameName;
}
- syncronizeLocalClone();
- _lookAtFrame(frameName);
-}
-void FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
-{
- if (enabled)
+ std::string FrameTracking::getFrame(const Ice::Current&) const
{
- ARMARX_WARNING << "Disable tracking to use lookAt functions.";
- return;
+ return frameName;
}
- syncronizeLocalClone();
- _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
-}
-void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
-{
- if (enabled)
+ void FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
{
- ARMARX_WARNING << "Disable tracking to use lookAt functions.";
- return;
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
+ if (!localRobot->hasRobotNode(frameName))
+ {
+ ARMARX_ERROR << frameName << " does not exist.";
+ return;
+ }
+ syncronizeLocalClone();
+ _lookAtFrame(frameName);
}
- syncronizeLocalClone();
- _lookAtPoint(ToEigen(point));
-}
-void FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
-{
- const float currentYaw = headYawJoint->getJointValue();
- const float currentPitch = headPitchJoint->getJointValue();
-
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
-
- FrameTracking::HeadState headState;
- headState.currentYawPos = currentYaw;
- headState.currentYawVel = currentYawVel;
- headState.currentPitchPos = currentPitch;
- headState.currentPitchVel = currentPitchVel;
-
-
- headState.desiredYawPos = yaw;
- headState.desiredPitchPos = pitch;
- _doPositionControl(headState);
- struct timespec req = {0, 30 * 1000000L};
- while (
- std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
- std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.)
- )
+ void FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
{
- ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
syncronizeLocalClone();
- // sleep for 30 milliseconds
- nanosleep(&req, nullptr);
+ _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
}
- auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
-}
-void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues, float velocity, const Ice::Current&)
-{
- if (enabled)
+ void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
{
- ARMARX_WARNING << "Disable tracking to use scan functions.";
- return;
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
+ syncronizeLocalClone();
+ _lookAtPoint(ToEigen(point));
}
- velocity = std::abs(velocity);
-
- syncronizeLocalClone();
- auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
- // to initial yaw
+ void FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
{
- bool wasGreater = headYawJoint->getJointValue() > yawFrom;
- float yawVelocityToInit = wasGreater ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
- // if the joint angle was greater before we want to run as long as it is greater
- // otherwise we want to run as long as it is smaler
- while ((wasGreater && headYawJoint->getJointValue() > yawFrom) || (!wasGreater && headYawJoint->getJointValue() < yawFrom))
+ const float currentYaw = headYawJoint->getJointValue();
+ const float currentPitch = headPitchJoint->getJointValue();
+
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+
+ FrameTracking::HeadState headState;
+ headState.currentYawPos = currentYaw;
+ headState.currentYawVel = currentYawVel;
+ headState.currentPitchPos = currentPitch;
+ headState.currentPitchVel = currentPitchVel;
+
+
+ headState.desiredYawPos = yaw;
+ headState.desiredPitchPos = pitch;
+ _doPositionControl(headState);
+ struct timespec req = {0, 30 * 1000000L};
+ while (
+ std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
+ std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.)
+ )
{
+ ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
syncronizeLocalClone();
+ // sleep for 30 milliseconds
+ nanosleep(&req, nullptr);
}
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- for (const auto& p : pitchValues)
+ void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues, float velocity, const Ice::Current&)
{
- // to pitch value
- bool wasGreaterP = headPitchJoint->getJointValue() > p;
- float velocityPitch = wasGreaterP ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
- while ((wasGreaterP && headPitchJoint->getJointValue() > p) || (!wasGreaterP && headPitchJoint->getJointValue() < p))
+ if (enabled)
{
- syncronizeLocalClone();
+ ARMARX_WARNING << "Disable tracking to use scan functions.";
+ return;
}
+ velocity = std::abs(velocity);
- // to yaw value
- bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
- float velocityYaw = wasGreaterY ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
- while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) || (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
+ syncronizeLocalClone();
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+
+ // to initial yaw
{
- syncronizeLocalClone();
+ bool wasGreater = headYawJoint->getJointValue() > yawFrom;
+ float yawVelocityToInit = wasGreater ? -velocity : velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
+ // if the joint angle was greater before we want to run as long as it is greater
+ // otherwise we want to run as long as it is smaler
+ while ((wasGreater && headYawJoint->getJointValue() > yawFrom) || (!wasGreater && headYawJoint->getJointValue() < yawFrom))
+ {
+ syncronizeLocalClone();
+ }
}
- std::swap(yawFrom, yawTo);
- }
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
-}
+ for (const auto& p : pitchValues)
+ {
+ // to pitch value
+ bool wasGreaterP = headPitchJoint->getJointValue() > p;
+ float velocityPitch = wasGreaterP ? -velocity : velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
+ while ((wasGreaterP && headPitchJoint->getJointValue() > p) || (!wasGreaterP && headPitchJoint->getJointValue() < p))
+ {
+ syncronizeLocalClone();
+ }
-void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current&)
-{
- if (enabled)
- {
- ARMARX_WARNING << "Disable tracking to use scan functions.";
- return;
+ // to yaw value
+ bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
+ float velocityYaw = wasGreaterY ? -velocity : velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
+ while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) || (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
+ {
+ syncronizeLocalClone();
+ }
+
+ std::swap(yawFrom, yawTo);
+ }
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- syncronizeLocalClone();
- auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
- struct timespec req = {0, 30 * 1000000L};
- for (const auto& p : points)
+
+ void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current&)
{
- auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
- auto target = _calculateJointAngles(pEigen);
- while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
- std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
+ if (enabled)
{
- ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: " << target.currentPitchPos << " - " << target.desiredPitchPos;
- syncronizeLocalClone();
- target = _calculateJointAngles(pEigen);
- _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
- // sleep for 30 milliseconds
- nanosleep(&req, nullptr);
+ ARMARX_WARNING << "Disable tracking to use scan functions.";
+ return;
}
+ syncronizeLocalClone();
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ struct timespec req = {0, 30 * 1000000L};
+ for (const auto& p : points)
+ {
+ auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
+ auto target = _calculateJointAngles(pEigen);
+ while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
+ std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
+ {
+ ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: " << target.currentPitchPos << " - " << target.desiredPitchPos;
+ syncronizeLocalClone();
+ target = _calculateJointAngles(pEigen);
+ _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
+ // sleep for 30 milliseconds
+ nanosleep(&req, nullptr);
+ }
+ }
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
-}
-void FrameTracking::process()
-{
- if (!localRobot->hasRobotNode(frameName))
+ void FrameTracking::process()
{
- ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
- std::thread([this]()
+ if (!localRobot->hasRobotNode(frameName))
{
- _enableTracking(false);
- }).detach();
- return;
+ ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
+ std::thread([this]()
+ {
+ _enableTracking(false);
+ }).detach();
+ return;
+ }
+ syncronizeLocalClone();
+ _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration, maxPitchVelocity, pitchAcceleration);
}
- syncronizeLocalClone();
- _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration, maxPitchVelocity, pitchAcceleration);
-}
-
-void FrameTracking::syncronizeLocalClone()
-{
- armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
-}
-
-void FrameTracking::_lookAtFrame(const std::string& frameName)
-{
- auto frame = localRobot->getRobotNode(frameName);
- auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
- _lookAtPoint(posInRobotFrame);
-}
-void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
-{
- _doPositionControl(_calculateJointAngles(point));
-}
-
-FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
-{
- auto frame = localRobot->getRobotNode(frameName);
- auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
- // do nothing if the robot works above his head
- // he should already look upwards because if this component runs continously
- if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+ void FrameTracking::syncronizeLocalClone()
{
- return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
+ armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
}
- return _calculateJointAngles(posInRobotFrame);
-}
-FrameTracking::HeadState FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
-{
- float yaw = -std::atan2(point.x(), point.y());
- // make shure the joint value satisfies the joint limits
- yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
- yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
- // we dont want the robot to move from one limit to the other in one step
- const float currentYaw = headYawJoint->getJointValue();
- if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+ void FrameTracking::_lookAtFrame(const std::string& frameName)
{
- yaw = currentYaw;
+ auto frame = localRobot->getRobotNode(frameName);
+ auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
+ _lookAtPoint(posInRobotFrame);
}
- const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
- const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
- const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
- float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
- // make shure the joint value satisfies the joint limits
- pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
- pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
- const float currentPitch = headPitchJoint->getJointValue();
-
- ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw << " and pitch=" << pitch;
-
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
-
- FrameTracking::HeadState headState;
- headState.currentYawPos = currentYaw;
- headState.desiredYawPos = yaw;
- headState.currentYawVel = currentYawVel;
- headState.currentPitchPos = currentPitch;
- headState.desiredPitchPos = pitch;
- headState.currentPitchVel = currentPitchVel;
- return headState;
-}
-
-void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
-{
- if (headState.stop)
+ void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- return;
+ _doPositionControl(_calculateJointAngles(point));
}
- float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentYawVel, maxYawVelocity,
- yawAcceleration, yawAcceleration,
- headState.currentYawPos, headState.desiredYawPos,
- 1.f);
- float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentPitchVel, maxPitchVelocity,
- pitchAcceleration, pitchAcceleration,
- headState.currentPitchPos, headState.desiredPitchPos,
- 1.f);
-
- // control mode is set when enable task
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity}, {headPitchJoint->getName(), desiredPitchVelocity}});
-}
+ FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
+ {
+ auto frame = localRobot->getRobotNode(frameName);
+ auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
+ // do nothing if the robot works above his head
+ // he should already look upwards because if this component runs continously
+ if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+ {
+ return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
+ }
+ return _calculateJointAngles(posInRobotFrame);
+ }
-void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
-{
- auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
- if (headState.stop)
+ FrameTracking::HeadState FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
{
- return;
+ float yaw = -std::atan2(point.x(), point.y());
+ // make shure the joint value satisfies the joint limits
+ yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
+ yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
+ // we dont want the robot to move from one limit to the other in one step
+ const float currentYaw = headYawJoint->getJointValue();
+ if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+ {
+ yaw = currentYaw;
+ }
+
+ const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
+ const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
+ const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
+ float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
+ // make shure the joint value satisfies the joint limits
+ pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
+ pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
+ const float currentPitch = headPitchJoint->getJointValue();
+
+ ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw << " and pitch=" << pitch;
+
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+
+ FrameTracking::HeadState headState;
+ headState.currentYawPos = currentYaw;
+ headState.desiredYawPos = yaw;
+ headState.currentYawVel = currentYawVel;
+ headState.currentPitchPos = currentPitch;
+ headState.desiredPitchPos = pitch;
+ headState.currentPitchVel = currentPitchVel;
+ return headState;
}
- kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos}, {headPitchJoint->getName(), headState.desiredPitchPos}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
-}
-void FrameTracking::_enableTracking(bool enable)
-{
- if (this->enabled == enable)
+ void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
{
- return;
+ if (headState.stop)
+ {
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ return;
+ }
+
+ float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(
+ 30.f / 1000, 35.f / 1000,
+ headState.currentYawVel, maxYawVelocity,
+ yawAcceleration, yawAcceleration,
+ headState.currentYawPos, headState.desiredYawPos,
+ 1.f);
+ float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(
+ 30.f / 1000, 35.f / 1000,
+ headState.currentPitchVel, maxPitchVelocity,
+ pitchAcceleration, pitchAcceleration,
+ headState.currentPitchPos, headState.desiredPitchPos,
+ 1.f);
+
+ // control mode is set when enable task
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity}, {headPitchJoint->getName(), desiredPitchVelocity}});
}
- this->enabled = enable;
- if (enable)
+
+ void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
{
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
- processorTask->start();
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
+ if (headState.stop)
+ {
+ return;
+ }
+ kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos}, {headPitchJoint->getName(), headState.desiredPitchPos}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- else
+
+ void FrameTracking::_enableTracking(bool enable)
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- processorTask->stop();
+ if (this->enabled == enable)
+ {
+ return;
+ }
+ this->enabled = enable;
+ if (enable)
+ {
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ processorTask->start();
+ }
+ else
+ {
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ processorTask->stop();
+ }
}
}
-
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index f4e94fa03..57bd492d1 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -26,58 +26,57 @@
-using namespace armarx;
-
-
-void GamepadControlUnit::onInitComponent()
+namespace armarx
{
- ARMARX_INFO << "oninit GamepadControlUnit";
- usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
- usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
-
+ void GamepadControlUnit::onInitComponent()
+ {
+ ARMARX_INFO << "oninit GamepadControlUnit";
+ usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
+ usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
- scaleX = getProperty<float>("ScaleX").getValue();
- scaleY = getProperty<float>("ScaleY").getValue();
- scaleRotation = getProperty<float>("ScaleAngle").getValue();
- ARMARX_INFO << "oninit GamepadControlUnit end";
-}
+ scaleX = getProperty<float>("ScaleX").getValue();
+ scaleY = getProperty<float>("ScaleY").getValue();
+ scaleRotation = getProperty<float>("ScaleAngle").getValue();
+ ARMARX_INFO << "oninit GamepadControlUnit end";
+ }
-void GamepadControlUnit::onConnectComponent()
-{
- ARMARX_INFO << "onConnect GamepadControlUnit";
- platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
-}
+ void GamepadControlUnit::onConnectComponent()
+ {
+ ARMARX_INFO << "onConnect GamepadControlUnit";
+ platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
+ }
-void GamepadControlUnit::onDisconnectComponent()
-{
-}
+ void GamepadControlUnit::onDisconnectComponent()
+ {
+ }
-void GamepadControlUnit::onExitComponent()
-{
- ARMARX_INFO << "exit GamepadControlUnit";
-}
-armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
- getConfigIdentifier()));
-}
+ void GamepadControlUnit::onExitComponent()
+ {
+ ARMARX_INFO << "exit GamepadControlUnit";
+ }
-void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- //scales are for the robot axis
- if (data.rightTrigger > 0)
+ armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
{
- platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ return armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
+ getConfigIdentifier()));
}
- else
+
+ void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- platformUnitPrx->move(0, 0, 0);
+ //scales are for the robot axis
+ if (data.rightTrigger > 0)
+ {
+ platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ }
+ else
+ {
+ platformUnitPrx->move(0, 0, 0);
+ }
+ //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
- //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
-
diff --git a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
index 21f418d6c..b6877129c 100644
--- a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
+++ b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
@@ -23,35 +23,34 @@
#include "KITProstheticHandObserver.h"
-using namespace armarx;
-
-
-void KITProstheticHandObserver::onInitComponent()
+namespace armarx
{
+ void KITProstheticHandObserver::onInitComponent()
+ {
-}
+ }
-void KITProstheticHandObserver::onConnectComponent()
-{
+ void KITProstheticHandObserver::onConnectComponent()
+ {
-}
+ }
-void KITProstheticHandObserver::onDisconnectComponent()
-{
+ void KITProstheticHandObserver::onDisconnectComponent()
+ {
-}
+ }
-void KITProstheticHandObserver::onExitComponent()
-{
+ void KITProstheticHandObserver::onExitComponent()
+ {
-}
+ }
-armarx::PropertyDefinitionsPtr KITProstheticHandObserver::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new KITProstheticHandObserverPropertyDefinitions(
- getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr KITProstheticHandObserver::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new KITProstheticHandObserverPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
index 9c78f07fa..b68904c7c 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -31,197 +31,196 @@
#include "KITProstheticHandUnit.h"
-using namespace armarx;
-
-
-armarx::PropertyDefinitionsPtr KITProstheticHandUnit::createPropertyDefinitions()
+namespace armarx
{
- return armarx::PropertyDefinitionsPtr(new KITProstheticHandUnitPropertyDefinitions(
- getConfigIdentifier()));
-}
+ armarx::PropertyDefinitionsPtr KITProstheticHandUnit::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new KITProstheticHandUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
-void KITProstheticHandUnit::onInitHandUnit()
-{
- _driver = std::make_unique<BLEProthesisInterface>(getProperty<std::string>("MAC"));
- //addShapeName("Open"); //is added by something else already
- addShapeName("Close");
- addShapeName("G0");
- addShapeName("G1");
- addShapeName("G2");
- addShapeName("G3");
- addShapeName("G4");
- addShapeName("G5");
- addShapeName("G6");
- addShapeName("G7");
- addShapeName("G8");
-
- offeringTopic(getProperty<std::string>("DebugObserverName"));
- if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ void KITProstheticHandUnit::onInitHandUnit()
{
- usingProxy(getProperty<std::string>("RemoteGuiName"));
+ _driver = std::make_unique<BLEProthesisInterface>(getProperty<std::string>("MAC"));
+ //addShapeName("Open"); //is added by something else already
+ addShapeName("Close");
+ addShapeName("G0");
+ addShapeName("G1");
+ addShapeName("G2");
+ addShapeName("G3");
+ addShapeName("G4");
+ addShapeName("G5");
+ addShapeName("G6");
+ addShapeName("G7");
+ addShapeName("G8");
+
+ offeringTopic(getProperty<std::string>("DebugObserverName"));
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ usingProxy(getProperty<std::string>("RemoteGuiName"));
+ }
}
-}
-void KITProstheticHandUnit::onStartHandUnit()
-{
- _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
- if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ void KITProstheticHandUnit::onStartHandUnit()
{
- _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
- RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
+ RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
- auto addFinger = [&](std::string name, float min, float max, float val, int steps)
- {
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name))
- .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
- .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps))
- .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max)))
- );
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name + " Pos "))
- .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
- .addChild(new RemoteGui::HSpacer())
- );
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name + " PWM "))
- .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
- .addChild(new RemoteGui::HSpacer())
- );
- };
-
- addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb());
- addFinger("Fingers", 0, 1, _lastGuiValueFingers, _driver->getMaxPosFingers());
-
- rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
-
- _guiTask = new SimplePeriodicTask<>([&]()
- {
- _guiTab.receiveUpdates();
- _driver->getMaxPosThumb();
- const float t = _guiTab.getValue<float>("Thumb").get();
- const float f = _guiTab.getValue<float>("Fingers").get();
+ auto addFinger = [&](std::string name, float min, float max, float val, int steps)
+ {
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name))
+ .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
+ .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps))
+ .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max)))
+ );
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name + " Pos "))
+ .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
+ .addChild(new RemoteGui::HSpacer())
+ );
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name + " PWM "))
+ .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
+ .addChild(new RemoteGui::HSpacer())
+ );
+ };
+
+ addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb());
+ addFinger("Fingers", 0, 1, _lastGuiValueFingers, _driver->getMaxPosFingers());
+
+ rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
+
+ _guiTask = new SimplePeriodicTask<>([&]()
+ {
+ _guiTab.receiveUpdates();
+ _driver->getMaxPosThumb();
+ const float t = _guiTab.getValue<float>("Thumb").get();
+ const float f = _guiTab.getValue<float>("Fingers").get();
+
+ bool updateT = t != _lastGuiValueThumb;
+ bool updateF = f != _lastGuiValueFingers;
+ _lastGuiValueThumb = t;
+ _lastGuiValueFingers = f;
+
+ if (updateT && updateF)
+ {
+ setJointAngles({{"Thumb", t}, {"Fingers", f}});
+ }
+ else if (updateT)
+ {
+ setJointAngles({{"Thumb", t}});
+ }
+ else if (updateF)
+ {
+ setJointAngles({{"Fingers", f}});
+ }
+
+ _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos()));
+ _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM()));
+ _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos()));
+ _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM()));
+ _guiTab.sendUpdates();
+ }, 10);
+
+ RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
+
+ _remoteGuiPrx->createTab("KITProstheticHandUnit", rootLayout);
+ _guiTab = RemoteGui::TabProxy(_remoteGuiPrx, "KITProstheticHandUnit");
+
+ _guiTask->start();
+ }
+ }
+
+ void KITProstheticHandUnit::onExitHandUnit()
+ {
+ _driver.reset();
+ }
- bool updateT = t != _lastGuiValueThumb;
- bool updateF = f != _lastGuiValueFingers;
- _lastGuiValueThumb = t;
- _lastGuiValueFingers = f;
+ void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
+ {
+ ARMARX_CHECK_NOT_NULL(_driver);
- if (updateT && updateF)
+ for (const std::pair<std::string, float>& pair : targetJointAngles)
+ {
+ if (pair.first == "Fingers")
{
- setJointAngles({{"Thumb", t}, {"Fingers", f}});
+ const std::uint64_t pos = boost::algorithm::clamp(
+ static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
+ 0, _driver->getMaxPosFingers());
+ ARMARX_INFO << "set fingers " << pos;
+ _driver->sendFingerPWM(200, 2999, pos);
+ // fix until hw driver is fixed to handle multiple commands at the same time
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
- else if (updateT)
+ else if (pair.first == "Thumb")
{
- setJointAngles({{"Thumb", t}});
+ const std::uint64_t pos = boost::algorithm::clamp(
+ static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
+ 0, _driver->getMaxPosThumb());
+ ARMARX_INFO << "set thumb " << pos;
+ _driver->sendThumbPWM(200, 2999, pos);
+ // fix until hw driver is fixed to handle multiple commands at the same time
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
- else if (updateF)
+ else
{
- setJointAngles({{"Fingers", f}});
+ ARMARX_WARNING << "Invalid HandJointName '" << pair.first << "', ignoring.";
}
+ }
+ }
- _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos()));
- _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM()));
- _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos()));
- _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM()));
- _guiTab.sendUpdates();
- }, 10);
-
- RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
-
- _remoteGuiPrx->createTab("KITProstheticHandUnit", rootLayout);
- _guiTab = RemoteGui::TabProxy(_remoteGuiPrx, "KITProstheticHandUnit");
-
- _guiTask->start();
+ NameValueMap KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
+ {
+ NameValueMap jointValues;
+ jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->getMaxPosFingers();
+ jointValues["Thumb"] = _driver->getThumbPos() * 1.f / _driver->getMaxPosThumb();
+ return jointValues;
}
-}
-void KITProstheticHandUnit::onExitHandUnit()
-{
- _driver.reset();
-}
+ void KITProstheticHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
+ {
+ _shapes[name] = shape;
+ addShapeName(name);
+ }
-void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
-{
- ARMARX_CHECK_NOT_NULL(_driver);
+ void KITProstheticHandUnit::addShapeName(const std::string& name)
+ {
+ Variant currentPreshape;
+ currentPreshape.setString(name);
+ shapeNames->addVariant(currentPreshape);
+ }
- for (const std::pair<std::string, float>& pair : targetJointAngles)
+ void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
{
- if (pair.first == "Fingers")
+ if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}))
+ {
+ _driver->sendGrasp(std::stoul(shapeName.substr(1)));
+ }
+ else if (shapeName == "Open")
+ {
+ _driver->sendGrasp(0);
+ }
+ else if (shapeName == "Close")
{
- const std::uint64_t pos = boost::algorithm::clamp(
- static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
- 0, _driver->getMaxPosFingers());
- ARMARX_INFO << "set fingers " << pos;
- _driver->sendFingerPWM(200, 2999, pos);
- // fix until hw driver is fixed to handle multiple commands at the same time
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ _driver->sendGrasp(1);
}
- else if (pair.first == "Thumb")
+ else if (!_shapes.count(shapeName))
{
- const std::uint64_t pos = boost::algorithm::clamp(
- static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
- 0, _driver->getMaxPosThumb());
- ARMARX_INFO << "set thumb " << pos;
- _driver->sendThumbPWM(200, 2999, pos);
- // fix until hw driver is fixed to handle multiple commands at the same time
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ ARMARX_WARNING << "Unknown shape name '" << shapeName
+ << "'\nKnown shapes: " << _shapes;
}
else
{
- ARMARX_WARNING << "Invalid HandJointName '" << pair.first << "', ignoring.";
+ setJointAngles(_shapes.at(shapeName));
}
}
}
-
-NameValueMap KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
-{
- NameValueMap jointValues;
- jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->getMaxPosFingers();
- jointValues["Thumb"] = _driver->getThumbPos() * 1.f / _driver->getMaxPosThumb();
- return jointValues;
-}
-
-void KITProstheticHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
-{
- _shapes[name] = shape;
- addShapeName(name);
-}
-
-void KITProstheticHandUnit::addShapeName(const std::string& name)
-{
- Variant currentPreshape;
- currentPreshape.setString(name);
- shapeNames->addVariant(currentPreshape);
-}
-
-void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
-{
- if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}))
- {
- _driver->sendGrasp(std::stoul(shapeName.substr(1)));
- }
- else if (shapeName == "Open")
- {
- _driver->sendGrasp(0);
- }
- else if (shapeName == "Close")
- {
- _driver->sendGrasp(1);
- }
- else if (!_shapes.count(shapeName))
- {
- ARMARX_WARNING << "Unknown shape name '" << shapeName
- << "'\nKnown shapes: " << _shapes;
- }
- else
- {
- setJointAngles(_shapes.at(shapeName));
- }
-}
-
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index f3430ec68..e6c314d47 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -27,273 +27,273 @@
#include <boost/regex.h>
-using namespace armarx;
-
-
-void RobotHealth::onInitComponent()
+namespace armarx
{
- ARMARX_TRACE;
- monitorHealthTask = new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthTaskClb, 10);
- defaultMaximumCycleTimeWarn = getProperty<int>("MaximumCycleTimeWarnMS").getValue();
- defaultMaximumCycleTimeErr = getProperty<int>("MaximumCycleTimeErrMS").getValue();
- usingTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
- reportErrorsWithSpeech = getProperty<bool>("ReportErrorsWithSpeech").getValue();
- speechMinimumReportInterval = getProperty<int>("SpeechMinimumReportInterval").getValue();
-
- ARMARX_TRACE;
- //robotUnitRequired = getProperty<bool>("RobotUnitRequired").getValue();
- /*if(robotUnitRequired)
+ void RobotHealth::onInitComponent()
{
- ARMARX_IMPORTANT << "RobotUnit is required";
- usingProxy(getProperty<std::string>("RobotUnitName").getValue());
- }*/
-
-
- //usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
-}
-
+ ARMARX_TRACE;
+ monitorHealthTask = new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthTaskClb, 10);
+ defaultMaximumCycleTimeWarn = getProperty<int>("MaximumCycleTimeWarnMS").getValue();
+ defaultMaximumCycleTimeErr = getProperty<int>("MaximumCycleTimeErrMS").getValue();
+ usingTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
+ reportErrorsWithSpeech = getProperty<bool>("ReportErrorsWithSpeech").getValue();
+ speechMinimumReportInterval = getProperty<int>("SpeechMinimumReportInterval").getValue();
-void RobotHealth::onConnectComponent()
-{
- ARMARX_TRACE;
- emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(getProperty<std::string>("EmergencyStopTopicName").getValue());
- //remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
- aggregatedRobotHealthTopicPrx = getTopic<AggregatedRobotHealthInterfacePrx>(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
- textToSpeechTopic = getTopic<TextListenerInterfacePrx>(getProperty<std::string>("TextToSpeechTopicName").getValue());
- lastSpeechOutput = TimeUtil::GetTime();
-
- /*if(robotUnitRequired)
- {
- robotUnitPrx = getTopic<RobotUnitInterfacePrx>(getProperty<std::string>("RobotUnitName").getValue());
- robotUnitPrx->enableHeartbeat();
- }*/
+ ARMARX_TRACE;
+ //robotUnitRequired = getProperty<bool>("RobotUnitRequired").getValue();
+ /*if(robotUnitRequired)
+ {
+ ARMARX_IMPORTANT << "RobotUnit is required";
+ usingProxy(getProperty<std::string>("RobotUnitName").getValue());
+ }*/
- monitorHealthTask->start();
-}
-void RobotHealth::monitorHealthTaskClb()
-{
- ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- bool hasNewErr = false;
+ //usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ }
- RobotHealthState healthState = RobotHealthState::HealthOK;
- for (size_t i = 0; i < validEntries; i++)
+ void RobotHealth::onConnectComponent()
{
ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
- if (e.required && delta > e.maximumCycleTimeErr)
- {
- healthState = RobotHealthState::HealthError;
- }
+ emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(getProperty<std::string>("EmergencyStopTopicName").getValue());
+ //remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ aggregatedRobotHealthTopicPrx = getTopic<AggregatedRobotHealthInterfacePrx>(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
+ textToSpeechTopic = getTopic<TextListenerInterfacePrx>(getProperty<std::string>("TextToSpeechTopicName").getValue());
+ lastSpeechOutput = TimeUtil::GetTime();
- if (!e.enabled)
+ /*if(robotUnitRequired)
{
- continue;
- }
+ robotUnitPrx = getTopic<RobotUnitInterfacePrx>(getProperty<std::string>("RobotUnitName").getValue());
+ robotUnitPrx->enableHeartbeat();
+ }*/
+
+ monitorHealthTask->start();
+ }
+
+ void RobotHealth::monitorHealthTaskClb()
+ {
ARMARX_TRACE;
+ long now = TimeUtil::GetTime().toMicroSeconds();
+ bool hasNewErr = false;
+ RobotHealthState healthState = RobotHealthState::HealthOK;
- if (delta > e.maximumCycleTimeErr)
+ for (size_t i = 0; i < validEntries; i++)
{
ARMARX_TRACE;
- healthState = RobotHealthState::HealthError;
- if (e.isRunning)
+ Entry& e = entries.at(i);
+ long delta = std::max(now - e.lastUpdate, e.lastDelta);
+ if (e.required && delta > e.maximumCycleTimeErr)
+ {
+ healthState = RobotHealthState::HealthError;
+ }
+
+ if (!e.enabled)
+ {
+ continue;
+ }
+ ARMARX_TRACE;
+
+
+ if (delta > e.maximumCycleTimeErr)
{
ARMARX_TRACE;
- ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name << " has died.";
- if (reportErrorsWithSpeech && (TimeUtil::GetTime() - lastSpeechOutput).toSecondsDouble() > speechMinimumReportInterval)
+ healthState = RobotHealthState::HealthError;
+ if (e.isRunning)
{
ARMARX_TRACE;
- lastSpeechOutput = TimeUtil::GetTime();
+ ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name << " has died.";
+ if (reportErrorsWithSpeech && (TimeUtil::GetTime() - lastSpeechOutput).toSecondsDouble() > speechMinimumReportInterval)
+ {
+ ARMARX_TRACE;
+ lastSpeechOutput = TimeUtil::GetTime();
- std::string name;
+ std::string name;
- boost::smatch m;
- boost::regex regex("^[a-zA-Z]+");
- if (boost::regex_search(e.name, m, regex))
- {
- if (m.empty())
- {
- name = "Whatever";
- }
- else
+ boost::smatch m;
+ boost::regex regex("^[a-zA-Z]+");
+ if (boost::regex_search(e.name, m, regex))
{
- name = m[0].str();
+ if (m.empty())
+ {
+ name = "Whatever";
+ }
+ else
+ {
+ name = m[0].str();
+ }
}
- }
- textToSpeechTopic->reportText("Oh no! Component " + name + " is no longer running.");
+ textToSpeechTopic->reportText("Oh no! Component " + name + " is no longer running.");
+ }
+ hasNewErr = true;
+ e.isRunning = false;
}
- hasNewErr = true;
- e.isRunning = false;
}
+ else if (e.isRunning && delta > e.maximumCycleTimeWarn)
+ {
+ ARMARX_TRACE;
+ ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name << " is running too slow.";
+ if (healthState == RobotHealthState::HealthOK)
+ {
+ healthState = RobotHealthState::HealthWarning;
+ }
+ }
+
}
- else if (e.isRunning && delta > e.maximumCycleTimeWarn)
+ if (hasNewErr)
{
ARMARX_TRACE;
- ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name << " is running too slow.";
- if (healthState == RobotHealthState::HealthOK)
+ emergencyStopTopicPrx->reportEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ }
+ ARMARX_TRACE;
+ aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(healthState);
+ /*if(allRequiredRunning && robotUnitPrx)
+ {
+ try
{
- healthState = RobotHealthState::HealthWarning;
+ robotUnitPrx->sendHeartbeat();
}
- }
-
+ catch(...)
+ {}
+ }*/
}
- if (hasNewErr)
+
+ void RobotHealth::onDisconnectComponent()
{
- ARMARX_TRACE;
- emergencyStopTopicPrx->reportEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ //robotUnitPrx = nullptr;
+ monitorHealthTask->stop();
}
- ARMARX_TRACE;
- aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(healthState);
- /*if(allRequiredRunning && robotUnitPrx)
- {
- try
- {
- robotUnitPrx->sendHeartbeat();
- }
- catch(...)
- {}
- }*/
-}
-
-void RobotHealth::onDisconnectComponent()
-{
- //robotUnitPrx = nullptr;
- monitorHealthTask->stop();
-}
-void RobotHealth::onExitComponent()
-{
+ void RobotHealth::onExitComponent()
+ {
-}
+ }
-armarx::PropertyDefinitionsPtr RobotHealth::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new RobotHealthPropertyDefinitions(
- getConfigIdentifier()));
-}
+ armarx::PropertyDefinitionsPtr RobotHealth::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new RobotHealthPropertyDefinitions(
+ getConfigIdentifier()));
+ }
-RobotHealth::Entry& RobotHealth::findOrCreateEntry(const std::string& name)
-{
- ARMARX_TRACE;
- for (size_t i = 0; i < entries.size(); i++)
+ RobotHealth::Entry& RobotHealth::findOrCreateEntry(const std::string& name)
{
- if (entries.at(i).name == name)
+ ARMARX_TRACE;
+ for (size_t i = 0; i < entries.size(); i++)
{
- return entries.at(i);
+ if (entries.at(i).name == name)
+ {
+ return entries.at(i);
+ }
}
+ ScopedLock lock(mutex);
+ ARMARX_INFO << "registering '" << name << "'";
+ entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
+ validEntries++;
+ return entries.back();
}
- ScopedLock lock(mutex);
- ARMARX_INFO << "registering '" << name << "'";
- entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
- validEntries++;
- return entries.back();
-}
-void RobotHealth::heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&)
-{
- ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- long now = TimeUtil::GetTime().toMicroSeconds();
- e.maximumCycleTimeWarn = args.maximumCycleTimeWarningMS * 1000;
- e.maximumCycleTimeErr = args.maximumCycleTimeErrorMS * 1000;
- if (!e.isRunning)
+ void RobotHealth::heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&)
{
- ARMARX_INFO << "'" << componentName << "' is now alive and running";
- e.lastDelta = 0;
- }
- else
- {
- e.lastDelta = now - e.lastUpdate;
- e.history.at(e.historyIndex) = e.lastDelta;
- e.historyIndex = (e.historyIndex + 1) % e.history.size();
+ ARMARX_TRACE;
+ Entry& e = findOrCreateEntry(componentName);
+ long now = TimeUtil::GetTime().toMicroSeconds();
+ e.maximumCycleTimeWarn = args.maximumCycleTimeWarningMS * 1000;
+ e.maximumCycleTimeErr = args.maximumCycleTimeErrorMS * 1000;
+ if (!e.isRunning)
+ {
+ ARMARX_INFO << "'" << componentName << "' is now alive and running";
+ e.lastDelta = 0;
+ }
+ else
+ {
+ e.lastDelta = now - e.lastUpdate;
+ e.history.at(e.historyIndex) = e.lastDelta;
+ e.historyIndex = (e.historyIndex + 1) % e.history.size();
+ }
+ ARMARX_TRACE;
+ e.lastUpdate = now;
+ e.isRunning = true;
+ e.enabled = true;
+ {
+ ScopedLock lock(e.messageMutex);
+ e.message = args.message;
+ }
}
- ARMARX_TRACE;
- e.lastUpdate = now;
- e.isRunning = true;
- e.enabled = true;
+
+ void armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
{
- ScopedLock lock(e.messageMutex);
- e.message = args.message;
+ ARMARX_TRACE;
+ Entry& e = findOrCreateEntry(componentName);
+ e.isRunning = false;
+ e.enabled = false;
+ ARMARX_INFO << "unregistering " << componentName;
}
-}
-void armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
-{
- ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- e.isRunning = false;
- e.enabled = false;
- ARMARX_INFO << "unregistering " << componentName;
-}
-
-std::string RobotHealth::getSummary(const Ice::Current&)
-{
- ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- std::stringstream ss;
- for (size_t i = 0; i < validEntries; i++)
+ std::string RobotHealth::getSummary(const Ice::Current&)
{
ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
-
- long minDelta = delta;
- long maxDelta = delta;
- for (size_t i = 1; i < e.history.size(); i++)
+ long now = TimeUtil::GetTime().toMicroSeconds();
+ std::stringstream ss;
+ for (size_t i = 0; i < validEntries; i++)
{
- long historicalDelta = e.history.at(i);
- if (historicalDelta < 0)
+ ARMARX_TRACE;
+ Entry& e = entries.at(i);
+ long delta = std::max(now - e.lastUpdate, e.lastDelta);
+
+ long minDelta = delta;
+ long maxDelta = delta;
+ for (size_t i = 1; i < e.history.size(); i++)
{
- break;
+ long historicalDelta = e.history.at(i);
+ if (historicalDelta < 0)
+ {
+ break;
+ }
+ minDelta = std::min(minDelta, historicalDelta);
+ maxDelta = std::max(maxDelta, historicalDelta);
}
- minDelta = std::min(minDelta, historicalDelta);
- maxDelta = std::max(maxDelta, historicalDelta);
- }
- ss << e.name;
- if (e.required)
- {
- ss << ", required";
- }
-
- if (!e.required && !e.enabled)
- {
- ss << ": disabled";
- }
- else
- {
- if (delta > e.maximumCycleTimeErr)
+ ss << e.name;
+ if (e.required)
{
- ss << ": ERROR";
+ ss << ", required";
}
- else if (delta > e.maximumCycleTimeWarn)
+
+ if (!e.required && !e.enabled)
{
- ss << ": warning";
+ ss << ": disabled";
}
else
{
- ss << ": ok";
+ if (delta > e.maximumCycleTimeErr)
+ {
+ ss << ": ERROR";
+ }
+ else if (delta > e.maximumCycleTimeWarn)
+ {
+ ss << ": warning";
+ }
+ else
+ {
+ ss << ": ok";
+ }
+ ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)";
}
- ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)";
- }
- ARMARX_TRACE;
+ ARMARX_TRACE;
- ScopedLock lock(e.messageMutex);
- if (e.message.size())
- {
- ss << " - " << e.message;
- }
+ ScopedLock lock(e.messageMutex);
+ if (e.message.size())
+ {
+ ss << " - " << e.message;
+ }
- ss << "\n";
+ ss << "\n";
+ }
+ return ss.str();
}
- return ss.str();
}
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index db8eaca64..b239fdd07 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -24,126 +24,125 @@
#include <time.h>
#include <thread>
-using namespace armarx;
-
-
-void RobotHealthDummy::onInitComponent()
-{
- dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
- getProperty(sleepmode, "SleepMode");
-}
-
-
-void RobotHealthDummy::onConnectComponent()
-{
- robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
- dummyTask->start();
-}
-
-#define NANOSECS_PER_SEC 1000000000
-int RobotHealthDummy::NanoSleep(long nanosec)
+namespace armarx
{
- struct timespec ts;
- ts.tv_sec = nanosec / NANOSECS_PER_SEC;
- ts.tv_nsec = (nanosec % NANOSECS_PER_SEC);
- return nanosleep(&ts, nullptr); // jitter up to +100ms!
-}
-
-void RobotHealthDummy::sleepwait(long microseconds)
-{
- long start = TimeUtil::GetTime().toMicroSeconds();
- auto end = start + microseconds;
- do
+ void RobotHealthDummy::onInitComponent()
{
- NanoSleep(1000);
+ dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
+ getProperty(sleepmode, "SleepMode");
}
- while (TimeUtil::GetTime().toMicroSeconds() < end);
-}
-void RobotHealthDummy::yieldwait(long microseconds)
-{
- long start = TimeUtil::GetTime().toMicroSeconds();
- auto end = start + microseconds;
- do
+
+
+ void RobotHealthDummy::onConnectComponent()
{
- std::this_thread::yield();
+ robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
+ dummyTask->start();
}
- while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
-}
-void RobotHealthDummy::busydwait(long microseconds)
-{
- long start = TimeUtil::GetTime().toMicroSeconds();
- auto end = start + microseconds;
- do
+
+#define NANOSECS_PER_SEC 1000000000
+ int RobotHealthDummy::NanoSleep(long nanosec)
{
- ;
+ struct timespec ts;
+ ts.tv_sec = nanosec / NANOSECS_PER_SEC;
+ ts.tv_nsec = (nanosec % NANOSECS_PER_SEC);
+ return nanosleep(&ts, nullptr); // jitter up to +100ms!
}
- while (TimeUtil::GetTime().toMicroSeconds() < end);
-}
-
-void RobotHealthDummy::runTask()
-{
-
-
- ARMARX_INFO << "starting rinning task";
- while (!dummyTask->isStopped())
+ void RobotHealthDummy::sleepwait(long microseconds)
{
- long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
- //ARMARX_INFO << "send heartbeat";
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
- if (sleepmode == "nanosleep")
- {
- NanoSleep(10 * 1000 * 1000);
- }
- else if (sleepmode == "sleepwait")
- {
- sleepwait(10 * 1000);
- }
- else if (sleepmode == "yieldwait")
+ long start = TimeUtil::GetTime().toMicroSeconds();
+ auto end = start + microseconds;
+ do
{
- yieldwait(10 * 1000);
+ NanoSleep(1000);
}
- else if (sleepmode == "busywait")
+ while (TimeUtil::GetTime().toMicroSeconds() < end);
+ }
+ void RobotHealthDummy::yieldwait(long microseconds)
+ {
+ long start = TimeUtil::GetTime().toMicroSeconds();
+ auto end = start + microseconds;
+ do
{
- busydwait(10 * 1000);
+ std::this_thread::yield();
}
- else
+ while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
+ }
+ void RobotHealthDummy::busydwait(long microseconds)
+ {
+ long start = TimeUtil::GetTime().toMicroSeconds();
+ auto end = start + microseconds;
+ do
{
- throw LocalException("Unknown sleepmode.");
+ ;
}
+ while (TimeUtil::GetTime().toMicroSeconds() < end);
+ }
- long afterSleep = TimeUtil::GetTime().toMicroSeconds();
- long topicCallDelay = afterTopicCall - beforeTopicCall;
- long sleepDelay = afterSleep - afterTopicCall;
- if (sleepDelay > 20000)
- {
- ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
- }
- if (topicCallDelay > 1000)
+ void RobotHealthDummy::runTask()
+ {
+
+
+
+ ARMARX_INFO << "starting rinning task";
+ while (!dummyTask->isStopped())
{
- ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ //ARMARX_INFO << "send heartbeat";
+ robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
+ long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ if (sleepmode == "nanosleep")
+ {
+ NanoSleep(10 * 1000 * 1000);
+ }
+ else if (sleepmode == "sleepwait")
+ {
+ sleepwait(10 * 1000);
+ }
+ else if (sleepmode == "yieldwait")
+ {
+ yieldwait(10 * 1000);
+ }
+ else if (sleepmode == "busywait")
+ {
+ busydwait(10 * 1000);
+ }
+ else
+ {
+ throw LocalException("Unknown sleepmode.");
+ }
+
+ long afterSleep = TimeUtil::GetTime().toMicroSeconds();
+ long topicCallDelay = afterTopicCall - beforeTopicCall;
+ long sleepDelay = afterSleep - afterTopicCall;
+ if (sleepDelay > 20000)
+ {
+ ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
+ }
+ if (topicCallDelay > 1000)
+ {
+ ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ }
}
}
-}
-void RobotHealthDummy::onDisconnectComponent()
-{
- //ARMARX_IMPORTANT << "onDisconnectComponent";
- dummyTask->stop();
-}
+ void RobotHealthDummy::onDisconnectComponent()
+ {
+ //ARMARX_IMPORTANT << "onDisconnectComponent";
+ dummyTask->stop();
+ }
-void RobotHealthDummy::onExitComponent()
-{
- //ARMARX_IMPORTANT << "onExitComponent";
- dummyTask->stop();
-}
+ void RobotHealthDummy::onExitComponent()
+ {
+ //ARMARX_IMPORTANT << "onExitComponent";
+ dummyTask->stop();
+ }
-armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions(
- getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
index 99b738ada..65f97877d 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
@@ -23,35 +23,34 @@
#include "RobotNameService.h"
-using namespace armarx;
-
-
-void RobotNameService::onInitComponent()
+namespace armarx
{
+ void RobotNameService::onInitComponent()
+ {
-}
+ }
-void RobotNameService::onConnectComponent()
-{
+ void RobotNameService::onConnectComponent()
+ {
-}
+ }
-void RobotNameService::onDisconnectComponent()
-{
+ void RobotNameService::onDisconnectComponent()
+ {
-}
+ }
-void RobotNameService::onExitComponent()
-{
+ void RobotNameService::onExitComponent()
+ {
-}
+ }
-armarx::PropertyDefinitionsPtr RobotNameService::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new RobotNameServicePropertyDefinitions(
- getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr RobotNameService::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new RobotNameServicePropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
index c57c63a8b..fbe94d4ea 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
@@ -60,8 +60,8 @@ namespace armarx
* Detailed description of class TopicTimingClient.
*/
class TopicTimingClient
- : virtual public armarx::Component
- , virtual public armarx::topic_timing::Topic
+ : virtual public armarx::Component
+ , virtual public armarx::topic_timing::Topic
{
public:
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
index a0d904b0d..48449f8b4 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
@@ -87,7 +87,7 @@ namespace armarx
armarx::PropertyDefinitionsPtr TopicTimingServer::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new TopicTimingServerPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
void TopicTimingServer::run()
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index 74ecf6ea6..3cb860a4d 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -31,496 +31,494 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
-using namespace armarx;
-
-
-void ViewSelection::onInitComponent()
+namespace armarx
{
- usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
- usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
+ void ViewSelection::onInitComponent()
+ {
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
- offeringTopic("DebugDrawerUpdates");
- offeringTopic(getName() + "Observer");
+ offeringTopic("DebugDrawerUpdates");
+ offeringTopic(getName() + "Observer");
- headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
- headFrameName = getProperty<std::string>("HeadFrameName").getValue();
- cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
- drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
- visuSaliencyThreshold = getProperty<float>("VisuSaliencyThreshold").getValue();
+ headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+ headFrameName = getProperty<std::string>("HeadFrameName").getValue();
+ cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
+ drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
+ visuSaliencyThreshold = getProperty<float>("VisuSaliencyThreshold").getValue();
- std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
+ std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
- armarx::CMakePackageFinder finder("RobotAPI");
- ArmarXDataPath::addDataPaths(finder.getDataDir());
+ armarx::CMakePackageFinder finder("RobotAPI");
+ ArmarXDataPath::addDataPaths(finder.getDataDir());
- if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
- {
- visibilityMaskGraph = new CIntensityGraph(graphFileName);
- TNodeList* nodes = visibilityMaskGraph->getNodes();
- const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
- const float borderAreaAngle = 10.0f;
- const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
-
- for (size_t i = 0; i < nodes->size(); i++)
+ if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
{
- CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+ visibilityMaskGraph = new CIntensityGraph(graphFileName);
+ TNodeList* nodes = visibilityMaskGraph->getNodes();
+ const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
+ const float borderAreaAngle = 10.0f;
+ const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
- if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
- {
- node->setIntensity(1.0f);
- }
- else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
+ for (size_t i = 0; i < nodes->size(); i++)
{
- node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
- }
- else
- {
- node->setIntensity(0.0f);
+ CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+
+ if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
+ {
+ node->setIntensity(1.0f);
+ }
+ else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
+ {
+ node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
+ }
+ else
+ {
+ node->setIntensity(0.0f);
+ }
}
}
- }
- else
- {
- ARMARX_ERROR << "Could not find required1 graph file";
- handleExceptions();
- return;
- }
-
- sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
- doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
+ else
+ {
+ ARMARX_ERROR << "Could not find required1 graph file";
+ handleExceptions();
+ return;
+ }
- timeOfLastViewChange = armarx::TimeUtil::GetTime();
+ sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
+ doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
- // this is robot model specific: offset from the used head coordinate system to the actual
- // head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
- offsetToHeadCenter << 0, 0, 150;
+ timeOfLastViewChange = armarx::TimeUtil::GetTime();
- processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
- processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
-}
+ // this is robot model specific: offset from the used head coordinate system to the actual
+ // head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
+ offsetToHeadCenter << 0, 0, 150;
+ processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
+ processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
+ }
-void ViewSelection::onConnectComponent()
-{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
- headIKUnitProxy->request();
+ void ViewSelection::onConnectComponent()
+ {
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- viewSelectionObserver = getTopic<ViewSelectionObserverPrx>(getName() + "Observer");
- drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+ headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
+ headIKUnitProxy->request();
- processorTask->start();
-}
+ viewSelectionObserver = getTopic<ViewSelectionObserverPrx>(getName() + "Observer");
+ drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+ processorTask->start();
+ }
-void ViewSelection::onDisconnectComponent()
-{
- processorTask->stop();
- if (drawer)
+ void ViewSelection::onDisconnectComponent()
{
- drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
- }
+ processorTask->stop();
- try
- {
- headIKUnitProxy->release();
- }
- catch (...)
- {
- ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
- handleExceptions();
+ if (drawer)
+ {
+ drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
+ }
+
+ try
+ {
+ headIKUnitProxy->release();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
+ handleExceptions();
+ }
}
-}
-void ViewSelection::onExitComponent()
-{
- delete visibilityMaskGraph;
-}
-
+ void ViewSelection::onExitComponent()
+ {
+ delete visibilityMaskGraph;
+ }
-void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
-{
- boost::mutex::scoped_lock lock(syncMutex);
- IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
- for (const auto& p : saliencyMaps)
+ void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
{
- if (p.second->validUntil)
+ boost::mutex::scoped_lock lock(syncMutex);
+
+ IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
+ for (const auto& p : saliencyMaps)
{
- TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(p.second->validUntil);
- if (time->toTime() > currentTime)
+ if (p.second->validUntil)
+ {
+ TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(p.second->validUntil);
+ if (time->toTime() > currentTime)
+ {
+ activeSaliencyMaps.push_back(p.second->name);
+ }
+ }
+ else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
{
activeSaliencyMaps.push_back(p.second->name);
}
}
- else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
- {
- activeSaliencyMaps.push_back(p.second->name);
- }
}
-}
-
-ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
-{
- std::vector<std::string> activeSaliencyMaps;
- getActiveSaliencyMaps(activeSaliencyMaps);
- if (activeSaliencyMaps.empty())
+ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
{
- return nullptr;
- }
-
-
- SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
- TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
+ std::vector<std::string> activeSaliencyMaps;
+ getActiveSaliencyMaps(activeSaliencyMaps);
- // find the direction with highest saliency
- int maxIndex = -1;
- float maxSaliency = std::numeric_limits<float>::min();
+ if (activeSaliencyMaps.empty())
+ {
+ return nullptr;
+ }
- boost::mutex::scoped_lock lock(syncMutex);
- hasNewSaliencyMap = false;
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
- for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
- {
- float saliency = 0.0f;
- for (const std::string& n : activeSaliencyMaps)
- {
- saliency += saliencyMaps[n]->map[i];
- }
+ // find the direction with highest saliency
+ int maxIndex = -1;
+ float maxSaliency = std::numeric_limits<float>::min();
- saliency /= activeSaliencyMaps.size();
+ boost::mutex::scoped_lock lock(syncMutex);
- CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
- saliency *= visibilityNode->getIntensity();
+ hasNewSaliencyMap = false;
- if (saliency > maxSaliency)
+ for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
{
- maxSaliency = saliency;
- maxIndex = i;
- }
- }
-
+ float saliency = 0.0f;
+ for (const std::string& n : activeSaliencyMaps)
+ {
+ saliency += saliencyMaps[n]->map[i];
+ }
- lock.unlock();
+ saliency /= activeSaliencyMaps.size();
- ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
+ CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ saliency *= visibilityNode->getIntensity();
- // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
- int timeSinceLastViewChange = (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
- float saliencyThreshold = 0;
+ if (saliency > maxSaliency)
+ {
+ maxSaliency = saliency;
+ maxIndex = i;
+ }
+ }
- if (timeSinceLastViewChange > 0)
- {
- saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
- }
- if (maxSaliency > saliencyThreshold)
- {
- Eigen::Vector3f target;
- MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
- const float distanceFactor = 1500;
- target = distanceFactor * target + offsetToHeadCenter;
- FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
+ lock.unlock();
- ViewTargetBasePtr viewTarget = new ViewTargetBase();
- viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // * maxSaliency;
- viewTarget->timeAdded = new TimestampVariant(armarx::TimeUtil::GetTime());
- viewTarget->target = viewTargetPositionPtr;
- viewTarget->duration = 0;
+ ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
+ // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
+ int timeSinceLastViewChange = (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
+ float saliencyThreshold = 0;
- if (visuSaliencyThreshold > 0.0)
+ if (timeSinceLastViewChange > 0)
{
- drawSaliencySphere(activeSaliencyMaps);
+ saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
}
- return viewTarget;
+ if (maxSaliency > saliencyThreshold)
+ {
+ Eigen::Vector3f target;
+ MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
+ const float distanceFactor = 1500;
+ target = distanceFactor * target + offsetToHeadCenter;
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
+
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // * maxSaliency;
+ viewTarget->timeAdded = new TimestampVariant(armarx::TimeUtil::GetTime());
+ viewTarget->target = viewTargetPositionPtr;
+ viewTarget->duration = 0;
- }
- return nullptr;
-}
+ if (visuSaliencyThreshold > 0.0)
+ {
+ drawSaliencySphere(activeSaliencyMaps);
+ }
+ return viewTarget;
+ }
-void ViewSelection::process()
-{
- /*
- while(getState() < eManagedIceObjectExiting)
- {
+ return nullptr;
}
- */
- ViewTargetBasePtr viewTarget;
- if (doAutomaticViewSelection)
- {
- viewTarget = nextAutomaticViewTarget();
- }
- /*
- //discard outdated view targets
- IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
- while (!manualViewTargets.empty())
+ void ViewSelection::process()
{
- ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
- TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(manualViewTarget->validUntil);
+ /*
+ while(getState() < eManagedIceObjectExiting)
+ {
+ }
+ */
- if (time->toTime() < currentTime)
+ ViewTargetBasePtr viewTarget;
+
+ if (doAutomaticViewSelection)
{
- ARMARX_INFO << "view target is no longer valid";
- manualViewTargets.pop();
+ viewTarget = nextAutomaticViewTarget();
}
- else
+
+ /*
+ //discard outdated view targets
+ IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
+ while (!manualViewTargets.empty())
{
- break;
+ ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
+ TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(manualViewTarget->validUntil);
+
+ if (time->toTime() < currentTime)
+ {
+ ARMARX_INFO << "view target is no longer valid";
+ manualViewTargets.pop();
+ }
+ else
+ {
+ break;
+ }
}
- }
- */
+ */
- if (!manualViewTargets.empty())
- {
- ScopedLock lock(manualViewTargetsMutex);
+ if (!manualViewTargets.empty())
+ {
+ ScopedLock lock(manualViewTargetsMutex);
- ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
+ ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
- CompareViewTargets c;
+ CompareViewTargets c;
- if (!viewTarget)
- {
- viewTarget = manualViewTarget;
- manualViewTargets.pop();
+ if (!viewTarget)
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
+ else if (c(viewTarget, manualViewTarget))
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
}
- else if (c(viewTarget, manualViewTarget))
+
+ if (viewTarget)
{
- viewTarget = manualViewTarget;
- manualViewTargets.pop();
- }
- }
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ // FramedPositionPtr viewTargetPositionPtr = FramedPositionPtr::dynamicCast(viewTarget->target);
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
- if (viewTarget)
- {
- SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
- // FramedPositionPtr viewTargetPositionPtr = FramedPositionPtr::dynamicCast(viewTarget->target);
- FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
+ if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
+ {
+ float arrowLength = 1500.0f;
+ Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
+ FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
+ drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, arrowLength, 5);
+ Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
+ Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
+ drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, arrowLength, 5);
+ }
- if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
- {
- float arrowLength = 1500.0f;
- Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
- FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
- drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, arrowLength, 5);
- Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
- Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
- drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, arrowLength, 5);
- }
+ ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
+ headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
+
+ timeOfLastViewChange = TimeUtil::GetTime();
- ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
- headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
+ long duration = viewTarget->duration;
+ if (!viewTarget->duration)
+ {
+ duration = sleepingTimeBetweenViewDirectionChanges;
+ }
- timeOfLastViewChange = TimeUtil::GetTime();
+ viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
- long duration = viewTarget->duration;
- if (!viewTarget->duration)
+ boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
+ }
+ else
{
- duration = sleepingTimeBetweenViewDirectionChanges;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5));
}
-
- viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
-
- boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
- }
- else
- {
- boost::this_thread::sleep(boost::posix_time::milliseconds(5));
}
-}
-void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
-{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
- //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
+ //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
- ViewTargetBasePtr viewTarget = new ViewTargetBase();
- viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY + 1;
- viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
- viewTarget->target = target;
- viewTarget->duration = 0;
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY + 1;
+ viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+ viewTarget->target = target;
+ viewTarget->duration = 0;
- manualViewTargets.push(viewTarget);
+ manualViewTargets.push(viewTarget);
- boost::mutex::scoped_lock lock2(syncMutex);
+ boost::mutex::scoped_lock lock2(syncMutex);
- condition.notify_all();
-}
-
-void ViewSelection::clearManualViewTargets(const Ice::Current& c)
-{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ condition.notify_all();
+ }
- while (!manualViewTargets.empty())
+ void ViewSelection::clearManualViewTargets(const Ice::Current& c)
{
- manualViewTargets.pop();
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ while (!manualViewTargets.empty())
+ {
+ manualViewTargets.pop();
+ }
+
+ // std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp;
+ // manualViewTargets.swap(temp);
}
- // std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp;
- // manualViewTargets.swap(temp);
-}
+ ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
-ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
-{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ ViewTargetList result;
- ViewTargetList result;
+ std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
- std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
+ while (!temp.empty())
+ {
+ result.push_back(temp.top());
- while (!temp.empty())
- {
- result.push_back(temp.top());
+ temp.pop();
+ }
- temp.pop();
+ return result;
}
- return result;
-}
-
-void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
-{
- boost::mutex::scoped_lock lock(syncMutex);
+ void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
+ {
+ boost::mutex::scoped_lock lock(syncMutex);
- hasNewSaliencyMap = true;
+ hasNewSaliencyMap = true;
- map->timeAdded = TimestampVariant::nowPtr();
+ map->timeAdded = TimestampVariant::nowPtr();
- saliencyMaps[map->name] = map;
+ saliencyMaps[map->name] = map;
- condition.notify_all();
+ condition.notify_all();
-}
+ }
-void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c)
-{
- ARMARX_LOG << "visualizing saliency map";
+ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c)
+ {
+ ARMARX_LOG << "visualizing saliency map";
- drawer->clearLayer("saliencyMap");
+ drawer->clearLayer("saliencyMap");
- SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
- Eigen::Matrix4f cameraToGlobal = FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())->toEigen();
+ Eigen::Matrix4f cameraToGlobal = FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())->toEigen();
- std::vector<std::string> activeSaliencyMaps;
+ std::vector<std::string> activeSaliencyMaps;
- if (names.size())
- {
- for (std::string n : names)
+ if (names.size())
{
- activeSaliencyMaps.push_back(n);
+ for (std::string n : names)
+ {
+ activeSaliencyMaps.push_back(n);
+ }
+ }
+ else
+ {
+ getActiveSaliencyMaps(activeSaliencyMaps);
}
- }
- else
- {
- getActiveSaliencyMaps(activeSaliencyMaps);
- }
- if (!activeSaliencyMaps.size())
- {
- return;
- }
+ if (!activeSaliencyMaps.size())
+ {
+ return;
+ }
- boost::mutex::scoped_lock lock(syncMutex);
+ boost::mutex::scoped_lock lock(syncMutex);
- TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
- DebugDrawer24BitColoredPointCloud cloud;
- cloud.points.reserve(visibilityMaskGraphNodes->size());
- for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
- {
- float saliency = 0.0f;
- for (const std::string& n : activeSaliencyMaps)
+ TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
+ DebugDrawer24BitColoredPointCloud cloud;
+ cloud.points.reserve(visibilityMaskGraphNodes->size());
+ for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
{
- saliency += saliencyMaps[n]->map[i];
- }
+ float saliency = 0.0f;
+ for (const std::string& n : activeSaliencyMaps)
+ {
+ saliency += saliencyMaps[n]->map[i];
+ }
- saliency /= activeSaliencyMaps.size();
+ saliency /= activeSaliencyMaps.size();
- CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
- saliency *= visibilityNode->getIntensity();
+ CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ saliency *= visibilityNode->getIntensity();
- Eigen::Vector3d out;
- TSphereCoord pos = visibilityNode->getPosition();
- MathTools::convert(pos, out);
+ Eigen::Vector3d out;
+ TSphereCoord pos = visibilityNode->getPosition();
+ MathTools::convert(pos, out);
- Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- pose.col(3).head<3>() = out.cast<float>() * 1000.0;
- pose = cameraToGlobal * pose;
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ pose.col(3).head<3>() = out.cast<float>() * 1000.0;
+ pose = cameraToGlobal * pose;
- // float r = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.8 + 0.2);
- // float g = std::min(1.0, (1.0 - saliency) * 0.8 + saliency * 0.2 + 0.2);
- // float b = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.2 + 0.2);
+ // float r = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.8 + 0.2);
+ // float g = std::min(1.0, (1.0 - saliency) * 0.8 + saliency * 0.2 + 0.2);
+ // float b = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.2 + 0.2);
- if (saliency < visuSaliencyThreshold)
- {
- continue;
+ if (saliency < visuSaliencyThreshold)
+ {
+ continue;
+ }
+ DebugDrawer24BitColoredPointCloudElement point;
+ point.color = colorutils::HeatMapRGBColor(saliency);
+ point.x = pose(0, 3);
+ point.y = pose(1, 3);
+ point.z = pose(2, 3);
+ cloud.points.push_back(point);
}
- DebugDrawer24BitColoredPointCloudElement point;
- point.color = colorutils::HeatMapRGBColor(saliency);
- point.x = pose(0, 3);
- point.y = pose(1, 3);
- point.z = pose(2, 3);
- cloud.points.push_back(point);
- }
- cloud.pointSize = 10;
- drawer->set24BitColoredPointCloudVisu("saliencyMap", "SaliencyCloud", cloud);
-
-}
-
-void ViewSelection::clearSaliencySphere(const Ice::Current& c)
-{
- drawer->clearLayer("saliencyMap");
-}
+ cloud.pointSize = 10;
+ drawer->set24BitColoredPointCloudVisu("saliencyMap", "SaliencyCloud", cloud);
-void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
-{
- boost::mutex::scoped_lock lock(syncMutex);
+ }
- if (saliencyMaps.count(name))
+ void ViewSelection::clearSaliencySphere(const Ice::Current& c)
{
- saliencyMaps.erase(name);
+ drawer->clearLayer("saliencyMap");
}
- condition.notify_all();
-}
+ void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
+ {
+ boost::mutex::scoped_lock lock(syncMutex);
-Ice::StringSeq ViewSelection::getSaliencyMapNames(const Ice::Current& c)
-{
- std::vector<std::string> names;
+ if (saliencyMaps.count(name))
+ {
+ saliencyMaps.erase(name);
+ }
- boost::mutex::scoped_lock lock(syncMutex);
+ condition.notify_all();
+ }
- for (const auto& p : saliencyMaps)
+ Ice::StringSeq ViewSelection::getSaliencyMapNames(const Ice::Current& c)
{
- names.push_back(p.second->name);
- }
+ std::vector<std::string> names;
- return names;
-}
+ boost::mutex::scoped_lock lock(syncMutex);
+ for (const auto& p : saliencyMaps)
+ {
+ names.push_back(p.second->name);
+ }
-PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
-}
+ return names;
+ }
+ PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
+ }
+}
diff --git a/source/RobotAPI/components/units/ATINetFTUnit.cpp b/source/RobotAPI/components/units/ATINetFTUnit.cpp
index f0cee2e4c..865751992 100644
--- a/source/RobotAPI/components/units/ATINetFTUnit.cpp
+++ b/source/RobotAPI/components/units/ATINetFTUnit.cpp
@@ -32,271 +32,269 @@
#include <stdlib.h>
#include <vector>
#include <inttypes.h>
-using namespace armarx;
+namespace armarx
+{
+ void ATINetFTUnit::onInitComponent()
+ {
+ }
+ void ATINetFTUnit::onConnectComponent()
+ {
+ /*
+ * Read ATINetFT hostname and port from properties
+ * std::string receiverHostName("192.168.1.1:49152");
+ */
+
+ std::string ATINetFTReceiverHostName = getProperty<std::string>("ATINetFTReceiveHostName").getValue();
+ std::string captureName = getProperty<std::string>("CaptureName").getValue();
+ ARMARX_INFO << "Capture Name " << captureName;
+ size_t pos = captureName.find("0");
+ captureNamePrefix = captureName.substr(0, pos);
+ ARMARX_INFO << "Capture Name Prefix " << captureNamePrefix;
+ std::string captureNameSuffix = captureName.substr(pos, captureName.size());
+ ARMARX_INFO << "Capture Name Suffix " << captureNameSuffix;
+ nPaddingZeros = captureNameSuffix.size();
+ captureNumber = std::stoi(captureNameSuffix);
+ establishATINetFTReceiveHostConnection(ATINetFTReceiverHostName);
+ ftDataSize = 6;
+ sendPacketSize = 8;
+ receivePacketSize = 36;
+ recordingStopped = true;
+ recordingCompleted = true;
+ recordingStarted = false;
+
+ /*
+ * Connect to ATINetFT
+ */
+ /*if (ATINetFT.Connect(hostName).Result != Result::Success)
+ {
+ ARMARX_ERROR << "Could not connect to ATINetFT at " << hostName << flush;
+ return;
+ }
+ else
+ {
+ ARMARX_IMPORTANT << "Connected to ATINetFT at " << hostName << flush;
+ }*/
-void ATINetFTUnit::onInitComponent()
-{
+ /*
+ * Enable Marker Data
+ */
+ /*Result::Enum result = ATINetFT.EnableMarkerData().Result;
-}
+ if (result != Result::Success)
+ {
+ ARMARX_ERROR << "ATINetFT EnableMarkerData() returned error code " << result << flush;
+ return;
+ }*/
+ }
-void ATINetFTUnit::onConnectComponent()
-{
- /*
- * Read ATINetFT hostname and port from properties
- * std::string receiverHostName("192.168.1.1:49152");
- */
-
- std::string ATINetFTReceiverHostName = getProperty<std::string>("ATINetFTReceiveHostName").getValue();
- std::string captureName = getProperty<std::string>("CaptureName").getValue();
- ARMARX_INFO << "Capture Name " << captureName;
- size_t pos = captureName.find("0");
- captureNamePrefix = captureName.substr(0, pos);
- ARMARX_INFO << "Capture Name Prefix " << captureNamePrefix;
- std::string captureNameSuffix = captureName.substr(pos, captureName.size());
- ARMARX_INFO << "Capture Name Suffix " << captureNameSuffix;
- nPaddingZeros = captureNameSuffix.size();
- captureNumber = std::stoi(captureNameSuffix);
- establishATINetFTReceiveHostConnection(ATINetFTReceiverHostName);
- ftDataSize = 6;
- sendPacketSize = 8;
- receivePacketSize = 36;
- recordingStopped = true;
- recordingCompleted = true;
- recordingStarted = false;
-
- /*
- * Connect to ATINetFT
- */
- /*if (ATINetFT.Connect(hostName).Result != Result::Success)
+ void ATINetFTUnit::onDisconnectComponent()
{
- ARMARX_ERROR << "Could not connect to ATINetFT at " << hostName << flush;
- return;
+ ARMARX_IMPORTANT << "Disconnecting from ATINetFT" << flush;
+
}
- else
- {
- ARMARX_IMPORTANT << "Connected to ATINetFT at " << hostName << flush;
- }*/
- /*
- * Enable Marker Data
- */
- /*Result::Enum result = ATINetFT.EnableMarkerData().Result;
- if (result != Result::Success)
+ void ATINetFTUnit::onExitComponent()
{
- ARMARX_ERROR << "ATINetFT EnableMarkerData() returned error code " << result << flush;
- return;
- }*/
-}
-void ATINetFTUnit::onDisconnectComponent()
-{
- ARMARX_IMPORTANT << "Disconnecting from ATINetFT" << flush;
+ }
-}
+ void ATINetFTUnit::periodicExec()
+ {
+ if (recordingStarted)
-void ATINetFTUnit::onExitComponent()
-{
+ {
+ //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
+ std::vector<float> ftdata(ftDataSize);
+ std::vector<char> receivePacketContent(receivePacketSize);
-}
+ if (!receivePacket(receivePacketContent.data()))
+ {
+ ARMARX_INFO << "recvfrom() failed :";
+ }
+ else
+ {
+ convertToFTValues(receivePacketContent.data(), ftdata.data(), ftdata.size());
+ writeFTValuesToFile(ftdata.data(), ftdata.size());
+ sampleIndex++;
+ }
-void ATINetFTUnit::periodicExec()
-{
- if (recordingStarted)
+ }
+ }
+ PropertyDefinitionsPtr ATINetFTUnit::createPropertyDefinitions()
{
- //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
- std::vector<float> ftdata(ftDataSize);
- std::vector<char> receivePacketContent(receivePacketSize);
+ return PropertyDefinitionsPtr(new ATINetFTUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
- if (!receivePacket(receivePacketContent.data()))
- {
- ARMARX_INFO << "recvfrom() failed :";
- }
- else
+ void ATINetFTUnit::startRecording(const std::string& customName, const Ice::Current& c)
+ {
+ if (remoteTriggerEnabled)
{
- convertToFTValues(receivePacketContent.data(), ftdata.data(), ftdata.size());
- writeFTValuesToFile(ftdata.data(), ftdata.size());
- sampleIndex++;
+ if (recordingCompleted)
+ {
+ int bpSize = sendPacketSize;
+ //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
+ std::vector<char> bytePacket(bpSize);
+ *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */
+ *(uint16_t*)&bytePacket[2] = htons(2); /* per table 9.1 in Net F/T user manual. */
+ *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */
+ recordingStarted = sendPacket(bytePacket.data(), bytePacket.size());
+
+ if (recordingStarted)
+ {
+ recordingStopped = false;
+ recordingCompleted = false;
+ //TODO build filename captureNamePrefic+customName
+ recordingFile.open(captureNamePrefix);
+ sampleIndex = 0;
+ }
+ }
}
-
}
-}
-
-PropertyDefinitionsPtr ATINetFTUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ATINetFTUnitPropertyDefinitions(
- getConfigIdentifier()));
-}
-
-void ATINetFTUnit::startRecording(const std::string& customName, const Ice::Current& c)
-{
- if (remoteTriggerEnabled)
+ void ATINetFTUnit::stopRecording(const Ice::Current& c)
{
- if (recordingCompleted)
+ if (remoteTriggerEnabled)
{
- int bpSize = sendPacketSize;
+ int bpSize = 8;
//ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
std::vector<char> bytePacket(bpSize);
*(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */
- *(uint16_t*)&bytePacket[2] = htons(2); /* per table 9.1 in Net F/T user manual. */
+ *(uint16_t*)&bytePacket[2] = htons(0); /* per table 9.1 in Net F/T user manual. */
*(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */
- recordingStarted = sendPacket(bytePacket.data(), bytePacket.size());
- if (recordingStarted)
+ recordingStopped = sendPacket(bytePacket.data(), bytePacket.size());
+ if (recordingStopped)
{
- recordingStopped = false;
- recordingCompleted = false;
- //TODO build filename captureNamePrefic+customName
- recordingFile.open(captureNamePrefix);
- sampleIndex = 0;
+ recordingFile.close();
+ recordingStarted = false;
}
+
}
}
-}
-void ATINetFTUnit::stopRecording(const Ice::Current& c)
-{
- if (remoteTriggerEnabled)
+ bool ATINetFTUnit::sendPacket(char* bytePacket, int bpSize)
{
- int bpSize = 8;
- //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
- std::vector<char> bytePacket(bpSize);
- *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */
- *(uint16_t*)&bytePacket[2] = htons(0); /* per table 9.1 in Net F/T user manual. */
- *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */
-
- recordingStopped = sendPacket(bytePacket.data(), bytePacket.size());
- if (recordingStopped)
+ if (remoteTriggerEnabled)
{
- recordingFile.close();
- recordingStarted = false;
- }
+ if (sendto(senderSocket, bytePacket, bpSize, 0, (struct sockaddr*)&receiveHostAddr, sizeof(receiveHostAddr)) != bpSize)
+ {
+ return false;
+ }
+ return true;
+ }
+ return false;
}
-}
-bool ATINetFTUnit::sendPacket(char* bytePacket, int bpSize)
-{
- if (remoteTriggerEnabled)
+ bool ATINetFTUnit::receivePacket(char* receiveBuf)
{
- if (sendto(senderSocket, bytePacket, bpSize, 0, (struct sockaddr*)&receiveHostAddr, sizeof(receiveHostAddr)) != bpSize)
+
+ int test = sizeof(receiveHostAddr);
+
+ int receiveint = recvfrom(senderSocket, receiveBuf, receivePacketSize, 0, (struct sockaddr*) &receiveHostAddr, ((socklen_t*)&test));
+
+ if (receiveint < 0)
{
return false;
}
- return true;
- }
- return false;
-}
-
-bool ATINetFTUnit::receivePacket(char* receiveBuf)
-{
- int test = sizeof(receiveHostAddr);
- int receiveint = recvfrom(senderSocket, receiveBuf, receivePacketSize, 0, (struct sockaddr*) &receiveHostAddr, ((socklen_t*)&test));
+ return true;
+ }
- if (receiveint < 0)
+ void ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize)
{
+ //uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); //was an unused variable
+ //uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); //was an unused variable
+ //uint32_t status = ntohl(*(uint32_t*)&receiveBuf[8]); //was an unused variable
- return false;
+ for (int i = 0; i < ftdataSize; i++)
+ {
+ ftdata[i] = float(int32_t(ntohl(*(uint32_t*)&receiveBuf[12 + i * 4]))) / 1000000.0f;
+ }
}
-
- return true;
-}
-
-void ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize)
-{
- //uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); //was an unused variable
- //uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); //was an unused variable
- //uint32_t status = ntohl(*(uint32_t*)&receiveBuf[8]); //was an unused variable
-
- for (int i = 0; i < ftdataSize; i++)
+ void ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize)
{
- ftdata[i] = float(int32_t(ntohl(*(uint32_t*)&receiveBuf[12 + i * 4]))) / 1000000.0f;
+ recordingFile << sampleIndex << " ";
+ for (int i = 0; i < ftdataSize; i++)
+ {
+ recordingFile << ftdata[i] << " ";
+ }
+ recordingFile << "\n";
}
-}
-void ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize)
-{
- recordingFile << sampleIndex << " ";
- for (int i = 0; i < ftdataSize; i++)
+ void ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName)
{
- recordingFile << ftdata[i] << " ";
- }
- recordingFile << "\n";
-}
+ remoteTriggerEnabled = false;
-void ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName)
-{
- remoteTriggerEnabled = false;
+ //Construct the server sockaddr_ structure
+ memset(&senderHostAddr, 0, sizeof(senderHostAddr));
+ senderHostAddr.sin_family = AF_INET;
+ senderHostAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+ senderHostAddr.sin_port = htons(0);
- //Construct the server sockaddr_ structure
- memset(&senderHostAddr, 0, sizeof(senderHostAddr));
- senderHostAddr.sin_family = AF_INET;
- senderHostAddr.sin_addr.s_addr = htonl(INADDR_ANY);
- senderHostAddr.sin_port = htons(0);
-
- //Create the socket
- if ((senderSocket = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
- {
- return;
- }
+ //Create the socket
+ if ((senderSocket = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
+ {
+ return;
+ }
- if (bind(senderSocket, (struct sockaddr*) &senderHostAddr, sizeof(senderHostAddr)) < 0)
- {
- return;
- }
+ if (bind(senderSocket, (struct sockaddr*) &senderHostAddr, sizeof(senderHostAddr)) < 0)
+ {
+ return;
+ }
- //Construct the server sockaddr_ structure
- memset(&receiveHostAddr, 0, sizeof(receiveHostAddr));
- size_t pos = receiverHostName.find(":");
- std::string hostname = receiverHostName.substr(0, pos);
+ //Construct the server sockaddr_ structure
+ memset(&receiveHostAddr, 0, sizeof(receiveHostAddr));
+ size_t pos = receiverHostName.find(":");
+ std::string hostname = receiverHostName.substr(0, pos);
- std::string hostport = receiverHostName.substr(pos + 1, receiverHostName.size());
+ std::string hostport = receiverHostName.substr(pos + 1, receiverHostName.size());
- struct hostent* he;
- struct in_addr** addr_list;
- int i;
+ struct hostent* he;
+ struct in_addr** addr_list;
+ int i;
- if ((he = gethostbyname(hostname.c_str())) == NULL)
- {
- // get the host info
- herror("gethostbyname");
- return;
- }
+ if ((he = gethostbyname(hostname.c_str())) == NULL)
+ {
+ // get the host info
+ herror("gethostbyname");
+ return;
+ }
- addr_list = (struct in_addr**) he->h_addr_list;
+ addr_list = (struct in_addr**) he->h_addr_list;
- for (i = 0; addr_list[i] != NULL; i++)
- {
+ for (i = 0; addr_list[i] != NULL; i++)
+ {
- char ip[100];
- //Return the first one;
- strcpy(ip, inet_ntoa(*addr_list[i]));
+ char ip[100];
+ //Return the first one;
+ strcpy(ip, inet_ntoa(*addr_list[i]));
- inet_pton(AF_INET, ip, &receiveHostAddr.sin_addr.s_addr);
+ inet_pton(AF_INET, ip, &receiveHostAddr.sin_addr.s_addr);
- receiveHostAddr.sin_port = htons(atoi(hostport.c_str()));
- remoteTriggerEnabled = true;
- ARMARX_INFO << "Connection established to " << hostname << ":" << hostport;
- //if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) {
- // return;
- //}
- //if(bind(receiverSocket,( struct sockaddr *) &receiveHostAddr, sizeof(receiveHostAddr))<0) {
- // return;
- //}
+ receiveHostAddr.sin_port = htons(atoi(hostport.c_str()));
+ remoteTriggerEnabled = true;
+ ARMARX_INFO << "Connection established to " << hostname << ":" << hostport;
+ //if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) {
+ // return;
+ //}
+ //if(bind(receiverSocket,( struct sockaddr *) &receiveHostAddr, sizeof(receiveHostAddr))<0) {
+ // return;
+ //}
+ return;
+ }
return;
- }
- return;
+ }
}
-
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index e896c60c2..31b2119d6 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -44,429 +44,430 @@
#define POD_SUFFIX "_pod"
-using namespace armarx;
-
-ForceTorqueObserver::ForceTorqueObserver()
-{
-}
-
-void ForceTorqueObserver::setTopicName(std::string topicName)
+namespace armarx
{
- this->topicName = topicName;
-}
-
-void ForceTorqueObserver::onInitObserver()
-{
- if (topicName.empty())
+ ForceTorqueObserver::ForceTorqueObserver()
{
- usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
}
- else
+
+ void ForceTorqueObserver::setTopicName(std::string topicName)
{
- usingTopic(topicName);
+ this->topicName = topicName;
}
- // register all checks
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
- offerConditionCheck("magnitudeinrange", new ConditionCheckMagnitudeInRange());
- offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
- offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
- offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
-
- usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
- offeringTopic("DebugDrawerUpdates");
-
- auto sensorRobotNodeSplit = armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
- for (auto& elem : sensorRobotNodeSplit)
+ void ForceTorqueObserver::onInitObserver()
{
- boost::trim(elem);
- auto split = armarx::Split(elem, ":");
- if (split.size() >= 2)
+ if (topicName.empty())
{
- sensorRobotNodeMapping.emplace(
- boost::trim_copy(split.at(0)),
- std::make_pair(boost::trim_copy(split.at(1)),
- split.size() >= 3 ? boost::trim_copy(split.at(2)) : ""));
+ usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+ }
+ else
+ {
+ usingTopic(topicName);
}
- }
-}
-void ForceTorqueObserver::onConnectObserver()
-{
- robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- localRobot = RemoteRobot::createLocalCloneFromFile(robot, VirtualRobot::RobotIO::eStructure);
- debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- if (getProperty<bool>("VisualizeForce").getValue())
- {
- visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
- visualizerTask->start();
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+ offerConditionCheck("magnitudeinrange", new ConditionCheckMagnitudeInRange());
+ offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
+ offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
+ offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
+
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ offeringTopic("DebugDrawerUpdates");
+
+ auto sensorRobotNodeSplit = armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
+ for (auto& elem : sensorRobotNodeSplit)
+ {
+ boost::trim(elem);
+ auto split = armarx::Split(elem, ":");
+ if (split.size() >= 2)
+ {
+ sensorRobotNodeMapping.emplace(
+ boost::trim_copy(split.at(0)),
+ std::make_pair(boost::trim_copy(split.at(1)),
+ split.size() >= 3 ? boost::trim_copy(split.at(2)) : ""));
+ }
+ }
}
- updateRobotTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::updateRobot, 1000 / getProperty<int>("RobotUpdateFrequency").getValue(), false, "updateRobot", false);
- updateRobotTask->start();
-
-}
-void ForceTorqueObserver::visualizerFunction()
-{
- if (!localRobot)
+ void ForceTorqueObserver::onConnectObserver()
{
- return;
+ robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ localRobot = RemoteRobot::createLocalCloneFromFile(robot, VirtualRobot::RobotIO::eStructure);
+ debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+ if (getProperty<bool>("VisualizeForce").getValue())
+ {
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+ visualizerTask->start();
+ }
+ updateRobotTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::updateRobot, 1000 / getProperty<int>("RobotUpdateFrequency").getValue(), false, "updateRobot", false);
+ updateRobotTask->start();
+
}
- float maxTorque = getProperty<float>("MaxExpectedTorqueValue");
- float torqueVisuDeadZone = getProperty<float>("TorqueVisuDeadZone");
- float arrowLength = getProperty<float>("MaxForceArrowLength").getValue();
- float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
- auto channels = getAvailableChannels(false);
- auto batchPrx = debugDrawer->ice_batchOneway();
- {
- ScopedLock lock(dataMutex);
- std::set<std::string> frameAlreadyReported;
- for (auto& channel : channels)
+ void ForceTorqueObserver::visualizerFunction()
+ {
+ if (!localRobot)
{
- try
+ return;
+ }
+ float maxTorque = getProperty<float>("MaxExpectedTorqueValue");
+ float torqueVisuDeadZone = getProperty<float>("TorqueVisuDeadZone");
+ float arrowLength = getProperty<float>("MaxForceArrowLength").getValue();
+ float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
+ auto channels = getAvailableChannels(false);
+ auto batchPrx = debugDrawer->ice_batchOneway();
+ {
+ ScopedLock lock(dataMutex);
+
+ std::set<std::string> frameAlreadyReported;
+ for (auto& channel : channels)
{
- // if (localRobot->hasRobotNode(channel.first))
+ try
{
- DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::emptyCurrent));
-
- FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
- if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
- {
- continue;
- }
- frameAlreadyReported.insert(value->frame);
- auto force = value->toGlobal(localRobot);
- ARMARX_DEBUG << deactivateSpam(5, channel.first) << "new force " << channel.first << " : " << force->toEigen() << " original frame: " << value->frame;
-
- float forceMag = force->toEigen().norm() * forceFactor;
- Vector3Ptr pos = new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
- forceMag = std::min(1.0f, forceMag);
- batchPrx->setArrowVisu("Forces",
- "Force" + channel.first,
- pos,
- force,
- DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
- std::max(arrowLength * forceMag, 10.f),
- 3);
-
- field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::emptyCurrent));
- value = field->getDataField()->get<FramedDirection>();
- auto torque = value;
- // ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
-
- Eigen::Vector3f dirXglobal = FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirYglobal = FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirZglobal = FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- // ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
- if (fabs(torque->x) > torqueVisuDeadZone)
- {
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueX" + channel.first,
- pos,
- new Vector3(dirXglobal),
- 50,
- torque->x / maxTorque,
- 3 * std::max(1.0f, torque->x / maxTorque),
- DrawColor {1.0f, 0.0f, 0.0f, 0.5f}
- );
- }
- else
- {
- batchPrx->removeCircleVisu("Torques",
- "TorqueX" + channel.first);
- }
- if (fabs(torque->y) > torqueVisuDeadZone)
- {
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueY" + channel.first,
- pos,
- new Vector3(dirYglobal),
- 50,
- torque->y / maxTorque,
- 3 * std::max(1.0f, torque->y / maxTorque),
- DrawColor {0.0f, 1.0f, 0.0f, 0.5f}
- );
- }
- else
- {
- batchPrx->removeCircleVisu("Torques",
- "TorqueY" + channel.first);
-
- }
- if (fabs(torque->z) > torqueVisuDeadZone)
- {
- batchPrx->setCircleArrowVisu("Torques",
- "TorqueZ" + channel.first,
- pos,
- new Vector3(dirZglobal),
- 50,
- torque->z / maxTorque,
- 3 * std::max(1.0f, torque->z / maxTorque),
- DrawColor {0.0f, 0.0f, 1.0f, 0.5f}
- );
- }
- else
+ // if (localRobot->hasRobotNode(channel.first))
{
- batchPrx->removeCircleVisu("Torques",
- "TorqueZ" + channel.first);
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::emptyCurrent));
+
+ FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
+ if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
+ {
+ continue;
+ }
+ frameAlreadyReported.insert(value->frame);
+ auto force = value->toGlobal(localRobot);
+ ARMARX_DEBUG << deactivateSpam(5, channel.first) << "new force " << channel.first << " : " << force->toEigen() << " original frame: " << value->frame;
+
+ float forceMag = force->toEigen().norm() * forceFactor;
+ Vector3Ptr pos = new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
+ forceMag = std::min(1.0f, forceMag);
+ batchPrx->setArrowVisu("Forces",
+ "Force" + channel.first,
+ pos,
+ force,
+ DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
+ std::max(arrowLength * forceMag, 10.f),
+ 3);
+
+ field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::emptyCurrent));
+ value = field->getDataField()->get<FramedDirection>();
+ auto torque = value;
+ // ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
+
+ Eigen::Vector3f dirXglobal = FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ Eigen::Vector3f dirYglobal = FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ Eigen::Vector3f dirZglobal = FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ // ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
+ if (fabs(torque->x) > torqueVisuDeadZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueX" + channel.first,
+ pos,
+ new Vector3(dirXglobal),
+ 50,
+ torque->x / maxTorque,
+ 3 * std::max(1.0f, torque->x / maxTorque),
+ DrawColor {1.0f, 0.0f, 0.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueX" + channel.first);
+ }
+ if (fabs(torque->y) > torqueVisuDeadZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueY" + channel.first,
+ pos,
+ new Vector3(dirYglobal),
+ 50,
+ torque->y / maxTorque,
+ 3 * std::max(1.0f, torque->y / maxTorque),
+ DrawColor {0.0f, 1.0f, 0.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueY" + channel.first);
+
+ }
+ if (fabs(torque->z) > torqueVisuDeadZone)
+ {
+ batchPrx->setCircleArrowVisu("Torques",
+ "TorqueZ" + channel.first,
+ pos,
+ new Vector3(dirZglobal),
+ 50,
+ torque->z / maxTorque,
+ 3 * std::max(1.0f, torque->z / maxTorque),
+ DrawColor {0.0f, 0.0f, 1.0f, 0.5f}
+ );
+ }
+ else
+ {
+ batchPrx->removeCircleVisu("Torques",
+ "TorqueZ" + channel.first);
+
+ }
}
+ // else
+ // {
+ // // ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+ // }
}
+ catch (...)
+ {
- // else
- // {
- // // ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
- // }
- }
- catch (...)
- {
-
+ }
}
}
+ batchPrx->ice_flushBatchRequests();
}
- batchPrx->ice_flushBatchRequests();
-}
-PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
- getConfigIdentifier()));
-}
+ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
+ getConfigIdentifier()));
+ }
-void ForceTorqueObserver::updateRobot()
-{
- ScopedLock lock(dataMutex);
- RemoteRobot::synchronizeLocalClone(localRobot, robot);
-}
+ void ForceTorqueObserver::updateRobot()
+ {
+ ScopedLock lock(dataMutex);
+ RemoteRobot::synchronizeLocalClone(localRobot, robot);
+ }
-void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
-{
- FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
+ void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
+ {
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
- try
- {
- setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
- }
- catch (...)
- {
- ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id " << id->getIdentifierStr();
- if (!existsDataField(id->channelName, id->datafieldName))
+ try
{
- if (!existsChannel(id->channelName))
+ setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id " << id->getIdentifierStr();
+ if (!existsDataField(id->channelName, id->datafieldName))
{
- offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+ if (!existsChannel(id->channelName))
+ {
+ offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+ }
+ offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
}
- offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
}
- }
- // pod = plain old data
- std::string pod_channelName = id->channelName + POD_SUFFIX;
+ // pod = plain old data
+ std::string pod_channelName = id->channelName + POD_SUFFIX;
- try
- {
- StringVariantBaseMap values;
- values[id->datafieldName + "_x" ] = new Variant(vec->x);
- values[id->datafieldName + "_y" ] = new Variant(vec->y);
- values[id->datafieldName + "_z" ] = new Variant(vec->z);
- values[id->datafieldName + "_frame"] = new Variant(vec->frame);
- setDataFieldsFlatCopy(pod_channelName, values);
- }
- catch (...)
- {
- ARMARX_INFO << "Creating force/torque pod fields";
- if (!existsChannel(pod_channelName))
+ try
{
- offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
-
+ StringVariantBaseMap values;
+ values[id->datafieldName + "_x" ] = new Variant(vec->x);
+ values[id->datafieldName + "_y" ] = new Variant(vec->y);
+ values[id->datafieldName + "_z" ] = new Variant(vec->z);
+ values[id->datafieldName + "_frame"] = new Variant(vec->frame);
+ setDataFieldsFlatCopy(pod_channelName, values);
}
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
- }
+ catch (...)
+ {
+ ARMARX_INFO << "Creating force/torque pod fields";
+ if (!existsChannel(pod_channelName))
+ {
+ offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+ }
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
+ }
-}
-void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
+ }
- auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr & forces, const FramedDirectionBasePtr & torques)
+ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
{
- try
- {
- DataFieldIdentifierPtr id;
+ ScopedLock lock(dataMutex);
- if (forces)
+ auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr & forces, const FramedDirectionBasePtr & torques)
+ {
+ try
{
- if (channelName.empty())
+ DataFieldIdentifierPtr id;
+
+ if (forces)
{
- id = getForceDatafieldId(sensorNodeName, frame);
+ if (channelName.empty())
+ {
+ id = getForceDatafieldId(sensorNodeName, frame);
+ }
+ else
+ {
+ id = new DataFieldIdentifier(getName(), channelName, "forces");
+ }
+
+ offerValue(sensorNodeName, id->datafieldName, forces, id);
}
- else
+
+ if (torques)
{
- id = new DataFieldIdentifier(getName(), channelName, "forces");
+ if (channelName.empty())
+ {
+ id = getTorqueDatafieldId(sensorNodeName, frame);
+ }
+ else
+ {
+ id = new DataFieldIdentifier(getName(), channelName, "torques");
+ }
+
+ offerValue(sensorNodeName, id->datafieldName, torques, id);
}
- offerValue(sensorNodeName, id->datafieldName, forces, id);
+ updateChannel(id->channelName);
+ updateChannel(id->channelName + POD_SUFFIX);
}
-
- if (torques)
+ catch (std::exception&)
{
- if (channelName.empty())
- {
- id = getTorqueDatafieldId(sensorNodeName, frame);
- }
- else
- {
- id = new DataFieldIdentifier(getName(), channelName, "torques");
- }
-
- offerValue(sensorNodeName, id->datafieldName, torques, id);
+ ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
+ handleExceptions();
}
+ };
- updateChannel(id->channelName);
- updateChannel(id->channelName + POD_SUFFIX);
- }
- catch (std::exception&)
+ if (!localRobot)
{
- ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
- handleExceptions();
+ return;
}
- };
-
- if (!localRobot)
- {
- return;
- }
- FramedDirectionPtr realTorques = FramedDirectionPtr::dynamicCast(torques);
- realTorques->changeFrame(localRobot, forces->frame);
+ FramedDirectionPtr realTorques = FramedDirectionPtr::dynamicCast(torques);
+ realTorques->changeFrame(localRobot, forces->frame);
- FramedDirectionPtr realForces = FramedDirectionPtr::dynamicCast(forces);
- offerForceAndTorqueValue(sensorNodeName, forces->frame, "", realForces, realTorques);
+ FramedDirectionPtr realForces = FramedDirectionPtr::dynamicCast(forces);
+ offerForceAndTorqueValue(sensorNodeName, forces->frame, "", realForces, realTorques);
- auto sensorMappingRange = sensorRobotNodeMapping.equal_range(sensorNodeName);
- for (auto iter = sensorMappingRange.first; iter != sensorMappingRange.second; ++iter)
- {
- auto& sensorName = iter->first;
- auto& robotNodeName = iter->second.first;
- auto& channelName = iter->second.second;
- FramedDirectionPtr forcesCopy = FramedDirectionPtr::dynamicCast(realForces->clone());
- FramedDirectionPtr torquesCopy = FramedDirectionPtr::dynamicCast(realTorques->clone());
- forcesCopy->changeFrame(localRobot, robotNodeName);
- torquesCopy->changeFrame(localRobot, robotNodeName);
-
- offerForceAndTorqueValue(sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
+ auto sensorMappingRange = sensorRobotNodeMapping.equal_range(sensorNodeName);
+ for (auto iter = sensorMappingRange.first; iter != sensorMappingRange.second; ++iter)
+ {
+ auto& sensorName = iter->first;
+ auto& robotNodeName = iter->second.first;
+ auto& channelName = iter->second.second;
+ FramedDirectionPtr forcesCopy = FramedDirectionPtr::dynamicCast(realForces->clone());
+ FramedDirectionPtr torquesCopy = FramedDirectionPtr::dynamicCast(realTorques->clone());
+ forcesCopy->changeFrame(localRobot, robotNodeName);
+ torquesCopy->changeFrame(localRobot, robotNodeName);
+
+ offerForceAndTorqueValue(sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
+ }
}
-}
-
-
-DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&)
-{
- return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
-}
-DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
-{
- auto id = getForceDatafieldId(nodeName, nodeName);
- if (!existsChannel(id->channelName))
+ DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&)
{
- throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
+ return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
}
- if (!existsDataField(id->channelName, id->datafieldName))
+ DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
{
- throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
- }
+ auto id = getForceDatafieldId(nodeName, nodeName);
- return new DatafieldRef(this, id->channelName, id->datafieldName);
+ if (!existsChannel(id->channelName))
+ {
+ throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
+ }
-}
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
+ throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ }
-DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
-{
- auto id = getTorqueDatafieldId(nodeName, nodeName);
+ return new DatafieldRef(this, id->channelName, id->datafieldName);
- if (!existsChannel(id->channelName))
- {
- throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
}
- if (!existsDataField(id->channelName, id->datafieldName))
+ DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
{
- throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
- }
+ auto id = getTorqueDatafieldId(nodeName, nodeName);
- return new DatafieldRef(this, id->channelName, id->datafieldName);
-}
+ if (!existsChannel(id->channelName))
+ {
+ throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
+ }
-DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
-{
- std::string channelName;
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
+ throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ }
- if (frame == nodeName)
- {
- channelName = nodeName;
- }
- else
- {
- channelName = nodeName + "_" + frame;
+ return new DatafieldRef(this, id->channelName, id->datafieldName);
}
- std::string datafieldName = "forces";
- DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
- return id;
-}
+ DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
+ {
+ std::string channelName;
-DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
-{
- std::string channelName;
+ if (frame == nodeName)
+ {
+ channelName = nodeName;
+ }
+ else
+ {
+ channelName = nodeName + "_" + frame;
+ }
- if (frame == nodeName)
- {
- channelName = nodeName;
+ std::string datafieldName = "forces";
+ DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
+ return id;
}
- else
+
+ DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
{
- channelName = nodeName + "_" + frame;
- }
+ std::string channelName;
- std::string datafieldName = "torques";
- DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
- return id;
-}
+ if (frame == nodeName)
+ {
+ channelName = nodeName;
+ }
+ else
+ {
+ channelName = nodeName + "_" + frame;
+ }
-void ForceTorqueObserver::onDisconnectComponent()
-{
- if (visualizerTask)
- {
- visualizerTask->stop();
+ std::string datafieldName = "torques";
+ DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
+ return id;
}
- if (updateRobotTask)
+
+ void ForceTorqueObserver::onDisconnectComponent()
{
- updateRobotTask->stop();
+ if (visualizerTask)
+ {
+ visualizerTask->stop();
+ }
+ if (updateRobotTask)
+ {
+ updateRobotTask->stop();
+ }
}
-}
-void ForceTorqueObserver::onExitObserver()
-{
+ void ForceTorqueObserver::onExitObserver()
+ {
+ }
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.cpp b/source/RobotAPI/components/units/ForceTorqueUnit.cpp
index e529bfac7..50cebe8c5 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.cpp
@@ -24,28 +24,29 @@
#include "ForceTorqueUnit.h"
-using namespace armarx;
-
-void ForceTorqueUnit::onInitComponent()
+namespace armarx
{
- listenerName = getProperty<std::string>("ForceTorqueTopicName").getValue();
- offeringTopic(listenerName);
- onInitForceTorqueUnit();
-}
+ void ForceTorqueUnit::onInitComponent()
+ {
+ listenerName = getProperty<std::string>("ForceTorqueTopicName").getValue();
+ offeringTopic(listenerName);
+ onInitForceTorqueUnit();
+ }
-void ForceTorqueUnit::onConnectComponent()
-{
- ARMARX_INFO << "setting report topic to " << listenerName << flush;
- listenerPrx = getTopic<ForceTorqueUnitListenerPrx>(listenerName);
- onStartForceTorqueUnit();
-}
+ void ForceTorqueUnit::onConnectComponent()
+ {
+ ARMARX_INFO << "setting report topic to " << listenerName << flush;
+ listenerPrx = getTopic<ForceTorqueUnitListenerPrx>(listenerName);
+ onStartForceTorqueUnit();
+ }
-void ForceTorqueUnit::onExitComponent()
-{
- onExitForceTorqueUnit();
-}
+ void ForceTorqueUnit::onExitComponent()
+ {
+ onExitForceTorqueUnit();
+ }
-PropertyDefinitionsPtr ForceTorqueUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ForceTorqueUnitPropertyDefinitions(getConfigIdentifier()));
+ PropertyDefinitionsPtr ForceTorqueUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new ForceTorqueUnitPropertyDefinitions(getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index 17f8c26b4..f7784390b 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -27,67 +27,67 @@
#include <boost/algorithm/string.hpp>
-using namespace armarx;
-
-void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
+namespace armarx
{
- int intervalMs = getProperty<int>("IntervalMs").getValue();
-
- sensorNamesList = Split(getProperty<std::string>("SensorNames").getValue(), ",");
- for (auto& sensor : sensorNamesList)
+ void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
{
- boost::trim(sensor);
+ int intervalMs = getProperty<int>("IntervalMs").getValue();
+
+ sensorNamesList = Split(getProperty<std::string>("SensorNames").getValue(), ",");
+ for (auto& sensor : sensorNamesList)
+ {
+ boost::trim(sensor);
+ }
+ //std::vector<std::string> sensorNamesList;
+ //boost::split(sensorNamesList, sensorNames, boost::is_any_of(","));
+
+ for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
+ {
+ forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+ torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+ }
+
+ ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
+ simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation", false);
}
- //std::vector<std::string> sensorNamesList;
- //boost::split(sensorNamesList, sensorNames, boost::is_any_of(","));
- for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
+ void ForceTorqueUnitSimulation::onStartForceTorqueUnit()
{
- forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
- torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+ simulationTask->start();
}
- ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
- simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation", false);
-}
-void ForceTorqueUnitSimulation::onStartForceTorqueUnit()
-{
- simulationTask->start();
-}
+ void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
+ {
+ simulationTask->stop();
+ }
-void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
-{
- simulationTask->stop();
-}
+ void ForceTorqueUnitSimulation::simulationFunction()
+ {
+ for (auto& sensor : sensorNamesList)
+ {
+ listenerPrx->reportSensorValues(sensor, forces[sensor], torques[sensor]);
+ }
-void ForceTorqueUnitSimulation::simulationFunction()
-{
+ //listenerPrx->reportSensorValues(forces);
+ //listenerPrx->reportSensorValues(torques);
+ }
- for (auto& sensor : sensorNamesList)
+ void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c)
{
- listenerPrx->reportSensorValues(sensor, forces[sensor], torques[sensor]);
+ // Ignore
}
- //listenerPrx->reportSensorValues(forces);
- //listenerPrx->reportSensorValues(torques);
-}
-
-void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c)
-{
- // Ignore
-}
-
-void ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
-{
- // Ignore
-}
+ void ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
+ {
+ // Ignore
+ }
-PropertyDefinitionsPtr ForceTorqueUnitSimulation::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ForceTorqueUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ PropertyDefinitionsPtr ForceTorqueUnitSimulation::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new ForceTorqueUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.cpp b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
index 437648bee..e6e522632 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.cpp
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
@@ -33,79 +33,76 @@
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-
-using namespace armarx;
-
-
-void GamepadUnitObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
-
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
+ void GamepadUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
-void GamepadUnitObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void GamepadUnitObserver::onExitObserver()
-{
- debugDrawerPrx->removePoseVisu("IMU", "orientation");
- debugDrawerPrx->removeLineVisu("IMU", "acceleration");
-}
+ void GamepadUnitObserver::onConnectObserver()
+ {
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-PropertyDefinitionsPtr GamepadUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
-}
-void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
+ void GamepadUnitObserver::onExitObserver()
+ {
+ debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+ }
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- if (!existsChannel(device))
+ PropertyDefinitionsPtr GamepadUnitObserver::createPropertyDefinitions()
{
- offerChannel(device, "Gamepad data");
+ return PropertyDefinitionsPtr(new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
- //ARMARX_IMPORTANT << deactivateSpam(1) << "observed " << device << " with name " << name;
-
- //axis
- offerOrUpdateDataField(device, "leftStickX", Variant(data.leftStickX), "X value of the left analog stick");
- offerOrUpdateDataField(device, "leftStickY", Variant(data.leftStickY), "Y value of the left analog stick");
- offerOrUpdateDataField(device, "rightStickX", Variant(data.rightStickX), "X value of the right analog stick");
- offerOrUpdateDataField(device, "rightStickY", Variant(data.rightStickY), "Y value of the right analog stick");
- offerOrUpdateDataField(device, "dPadX", Variant(data.dPadX), "X value of the digital pad");
- offerOrUpdateDataField(device, "dPadY", Variant(data.dPadY), "y value of the digital pad");
- offerOrUpdateDataField(device, "leftTrigger", Variant(data.leftTrigger), "value of the left analog trigger");
- offerOrUpdateDataField(device, "rightTrigger", Variant(data.rightTrigger), "value of the right analog trigger");
- //buttons
- offerOrUpdateDataField(device, "aButton", Variant(data.aButton), "A button pressed");
- offerOrUpdateDataField(device, "backButton", Variant(data.backButton), "Back button pressed");
- offerOrUpdateDataField(device, "bButton", Variant(data.bButton), "B button pressed");
- offerOrUpdateDataField(device, "leftButton", Variant(data.leftButton), "Left shoulder button pressed");
- offerOrUpdateDataField(device, "leftStickButton", Variant(data.leftStickButton), "Left stick button pressed");
- offerOrUpdateDataField(device, "rightButton", Variant(data.rightButton), "Right shoulder button pressed");
- offerOrUpdateDataField(device, "rightStickButton", Variant(data.rightStickButton), "Right stick button pressed");
- offerOrUpdateDataField(device, "startButton", Variant(data.startButton), "Start button pressed");
- offerOrUpdateDataField(device, "theMiddleButton", Variant(data.theMiddleButton), "The middle button pressed");
- offerOrUpdateDataField(device, "xButton", Variant(data.xButton), "X button pressed");
- offerOrUpdateDataField(device, "yButton", Variant(data.yButton), "Y button pressed");
-
- updateChannel(device);
+ void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ {
+ ScopedLock lock(dataMutex);
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "Gamepad data");
+ }
+
+ //ARMARX_IMPORTANT << deactivateSpam(1) << "observed " << device << " with name " << name;
+
+ //axis
+ offerOrUpdateDataField(device, "leftStickX", Variant(data.leftStickX), "X value of the left analog stick");
+ offerOrUpdateDataField(device, "leftStickY", Variant(data.leftStickY), "Y value of the left analog stick");
+ offerOrUpdateDataField(device, "rightStickX", Variant(data.rightStickX), "X value of the right analog stick");
+ offerOrUpdateDataField(device, "rightStickY", Variant(data.rightStickY), "Y value of the right analog stick");
+ offerOrUpdateDataField(device, "dPadX", Variant(data.dPadX), "X value of the digital pad");
+ offerOrUpdateDataField(device, "dPadY", Variant(data.dPadY), "y value of the digital pad");
+ offerOrUpdateDataField(device, "leftTrigger", Variant(data.leftTrigger), "value of the left analog trigger");
+ offerOrUpdateDataField(device, "rightTrigger", Variant(data.rightTrigger), "value of the right analog trigger");
+ //buttons
+ offerOrUpdateDataField(device, "aButton", Variant(data.aButton), "A button pressed");
+ offerOrUpdateDataField(device, "backButton", Variant(data.backButton), "Back button pressed");
+ offerOrUpdateDataField(device, "bButton", Variant(data.bButton), "B button pressed");
+ offerOrUpdateDataField(device, "leftButton", Variant(data.leftButton), "Left shoulder button pressed");
+ offerOrUpdateDataField(device, "leftStickButton", Variant(data.leftStickButton), "Left stick button pressed");
+ offerOrUpdateDataField(device, "rightButton", Variant(data.rightButton), "Right shoulder button pressed");
+ offerOrUpdateDataField(device, "rightStickButton", Variant(data.rightStickButton), "Right stick button pressed");
+ offerOrUpdateDataField(device, "startButton", Variant(data.startButton), "Start button pressed");
+ offerOrUpdateDataField(device, "theMiddleButton", Variant(data.theMiddleButton), "The middle button pressed");
+ offerOrUpdateDataField(device, "xButton", Variant(data.xButton), "X button pressed");
+ offerOrUpdateDataField(device, "yButton", Variant(data.yButton), "Y button pressed");
+
+ updateChannel(device);
+ }
}
-
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.cpp b/source/RobotAPI/components/units/HandUnitSimulation.cpp
index bf8bfcdc7..40688ad93 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/HandUnitSimulation.cpp
@@ -32,91 +32,92 @@
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/RobotConfig.h>
-using namespace armarx;
-
-void HandUnitSimulation::onInitHandUnit()
-{
- kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
- usingProxy(kinematicUnitName);
-}
-
-void HandUnitSimulation::onStartHandUnit()
+namespace armarx
{
- kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
- if (!kinematicUnitPrx)
+ void HandUnitSimulation::onInitHandUnit()
{
- ARMARX_ERROR << "Failed to obtain kinematic unit proxy";
+ kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+ usingProxy(kinematicUnitName);
}
-}
-
-void HandUnitSimulation::onExitHandUnit()
-{
-}
-
-PropertyDefinitionsPtr HandUnitSimulation::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new HandUnitSimulationPropertyDefinitions(getConfigIdentifier()));
-}
-void armarx::HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current&)
-{
- std::string myShapeName = shapeName;
- ARMARX_INFO << "Setting shape " << myShapeName;
+ void HandUnitSimulation::onStartHandUnit()
+ {
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ if (!kinematicUnitPrx)
+ {
+ ARMARX_ERROR << "Failed to obtain kinematic unit proxy";
+ }
+ }
- if (!eef)
+ void HandUnitSimulation::onExitHandUnit()
{
- ARMARX_WARNING << "No EEF";
- return;
}
+ PropertyDefinitionsPtr HandUnitSimulation::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new HandUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ }
- if (!eef->hasPreshape(myShapeName))
+ void armarx::HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current&)
{
- ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
+ std::string myShapeName = shapeName;
+ ARMARX_INFO << "Setting shape " << myShapeName;
- bool foundMatch = false;
+ if (!eef)
+ {
+ ARMARX_WARNING << "No EEF";
+ return;
+ }
- for (std::string name : eef->getPreshapes())
+
+ if (!eef->hasPreshape(myShapeName))
{
- if (name.find(myShapeName) != std::string::npos)
+ ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
+
+ bool foundMatch = false;
+
+ for (std::string name : eef->getPreshapes())
{
- foundMatch = true;
- myShapeName = name;
- ARMARX_INFO << "Using matching shape: " << name;
- break;
+ if (name.find(myShapeName) != std::string::npos)
+ {
+ foundMatch = true;
+ myShapeName = name;
+ ARMARX_INFO << "Using matching shape: " << name;
+ break;
+ }
}
- }
- if (!foundMatch)
- {
- ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
- return;
+ if (!foundMatch)
+ {
+ ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
+ return;
+ }
}
- }
- VirtualRobot::RobotConfigPtr config = eef->getPreshape(myShapeName);
- std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
+ VirtualRobot::RobotConfigPtr config = eef->getPreshape(myShapeName);
+ std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
- NameControlModeMap controlModes;
+ NameControlModeMap controlModes;
- for (std::pair<std::string, float> pair : jointAngles)
- {
- controlModes.insert(std::make_pair(pair.first, ePositionControl));
+ for (std::pair<std::string, float> pair : jointAngles)
+ {
+ controlModes.insert(std::make_pair(pair.first, ePositionControl));
+ }
+
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointAngles(jointAngles);
}
- kinematicUnitPrx->switchControlMode(controlModes);
- kinematicUnitPrx->setJointAngles(jointAngles);
-}
+ void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
+ {
+ NameControlModeMap controlModes;
-void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
-{
- NameControlModeMap controlModes;
+ for (std::pair<std::string, float> pair : jointAngles)
+ {
+ controlModes.insert(std::make_pair(pair.first, ePositionControl));
+ }
- for (std::pair<std::string, float> pair : jointAngles)
- {
- controlModes.insert(std::make_pair(pair.first, ePositionControl));
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointAngles(jointAngles);
}
-
- kinematicUnitPrx->switchControlMode(controlModes);
- kinematicUnitPrx->setJointAngles(jointAngles);
}
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 9eedc200f..759927bd8 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -34,141 +34,142 @@
#include <Eigen/Dense>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-using namespace armarx;
-
-HapticObserver::HapticObserver()
-{
- statisticsTask = new PeriodicTask<HapticObserver>(this, &HapticObserver::updateStatistics, 10, false);
-}
-
-void HapticObserver::setTopicName(std::string topicName)
-{
- this->topicName = topicName;
-}
-
-void HapticObserver::onInitObserver()
+namespace armarx
{
- if (topicName.empty())
+ HapticObserver::HapticObserver()
{
- usingTopic(getProperty<std::string>("HapticTopicName").getValue());
+ statisticsTask = new PeriodicTask<HapticObserver>(this, &HapticObserver::updateStatistics, 10, false);
}
- else
+
+ void HapticObserver::setTopicName(std::string topicName)
{
- usingTopic(topicName);
+ this->topicName = topicName;
}
- // register all checks
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-}
-
-void HapticObserver::onConnectObserver()
-{
- statisticsTask->start();
-}
-
-void HapticObserver::onExitObserver()
-{
- statisticsTask->stop();
-}
+ void HapticObserver::onInitObserver()
+ {
+ if (topicName.empty())
+ {
+ usingTopic(getProperty<std::string>("HapticTopicName").getValue());
+ }
+ else
+ {
+ usingTopic(topicName);
+ }
-void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+ }
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(values);
- if (matrix->cols == 0)
+ void HapticObserver::onConnectObserver()
{
- // Empty matrix received, silently ignore
- return;
+ statisticsTask->start();
}
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- Eigen::MatrixXf eigenMatrix = matrix->toEigen();
- float max = eigenMatrix.maxCoeff();
- float mean = eigenMatrix.mean();
- std::string channelName = name;
- Eigen::MatrixXf M = matrix->toEigen();
+ void HapticObserver::onExitObserver()
+ {
+ statisticsTask->stop();
+ }
- if (!existsChannel(channelName))
+ void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
{
- offerChannel(channelName, "Haptic data");
- offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
- offerDataFieldWithDefault(channelName, "name", Variant(name), "Name of the tactile sensor");
- offerDataFieldWithDefault(channelName, "matrix", matrix, "Raw tactile matrix data");
- offerDataFieldWithDefault(channelName, "max", Variant(max), "Maximum value");
- offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
- offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
- offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
-
- for (int i = 0; i < M.rows(); i++)
+ ScopedLock lock(dataMutex);
+
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(values);
+ if (matrix->cols == 0)
+ {
+ // Empty matrix received, silently ignore
+ return;
+ }
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+ Eigen::MatrixXf eigenMatrix = matrix->toEigen();
+ float max = eigenMatrix.maxCoeff();
+ float mean = eigenMatrix.mean();
+ std::string channelName = name;
+ Eigen::MatrixXf M = matrix->toEigen();
+
+ if (!existsChannel(channelName))
{
- for (int j = 0; j < M.cols(); j++)
+ offerChannel(channelName, "Haptic data");
+ offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
+ offerDataFieldWithDefault(channelName, "name", Variant(name), "Name of the tactile sensor");
+ offerDataFieldWithDefault(channelName, "matrix", matrix, "Raw tactile matrix data");
+ offerDataFieldWithDefault(channelName, "max", Variant(max), "Maximum value");
+ offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
+ offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
+ offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
+
+ for (int i = 0; i < M.rows(); i++)
{
- std::stringstream s;
- s << "entry_" << i << "," << j;
- offerDataFieldWithDefault(channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
+ for (int j = 0; j < M.cols(); j++)
+ {
+ std::stringstream s;
+ s << "entry_" << i << "," << j;
+ offerDataFieldWithDefault(channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
+ }
}
- }
- ARMARX_INFO << "Offering new channel: " << channelName;
- }
- else
- {
- setDataField(channelName, "device", Variant(device));
- setDataField(channelName, "name", Variant(name));
- setDataField(channelName, "matrix", matrix);
- setDataField(channelName, "max", Variant(max));
- setDataField(channelName, "mean", Variant(mean));
- setDataField(channelName, "timestamp", timestampPtr);
-
- for (int i = 0; i < M.rows(); i++)
+ ARMARX_INFO << "Offering new channel: " << channelName;
+ }
+ else
{
- for (int j = 0; j < M.cols(); j++)
+ setDataField(channelName, "device", Variant(device));
+ setDataField(channelName, "name", Variant(name));
+ setDataField(channelName, "matrix", matrix);
+ setDataField(channelName, "max", Variant(max));
+ setDataField(channelName, "mean", Variant(mean));
+ setDataField(channelName, "timestamp", timestampPtr);
+
+ for (int i = 0; i < M.rows(); i++)
{
- std::stringstream s;
- s << "entry_" << i << "," << j;
- setDataField(channelName, s.str(), Variant(M(i, j)));
+ for (int j = 0; j < M.cols(); j++)
+ {
+ std::stringstream s;
+ s << "entry_" << i << "," << j;
+ setDataField(channelName, s.str(), Variant(M(i, j)));
+ }
}
+
}
+ /*if(statistics.count(device) > 0)
+ {
+ statistics.at(device).add(timestamp->timestamp);
+ HapticSampleStatistics stats = statistics.at(device);
+ long avg = stats.average();
+ float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
+ setDataField(device, "rate", Variant(rate));
+ }
+ else
+ {
+ statistics.insert(std::map<std::string,HapticSampleStatistics>::value_type(device, HapticSampleStatistics(100, timestamp->timestamp)));
+ }*/
+
+ updateChannel(channelName);
}
- /*if(statistics.count(device) > 0)
+ PropertyDefinitionsPtr HapticObserver::createPropertyDefinitions()
{
- statistics.at(device).add(timestamp->timestamp);
- HapticSampleStatistics stats = statistics.at(device);
- long avg = stats.average();
- float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
- setDataField(device, "rate", Variant(rate));
+ return PropertyDefinitionsPtr(new HapticObserverPropertyDefinitions(getConfigIdentifier()));
}
- else
- {
- statistics.insert(std::map<std::string,HapticSampleStatistics>::value_type(device, HapticSampleStatistics(100, timestamp->timestamp)));
- }*/
- updateChannel(channelName);
-}
-
-PropertyDefinitionsPtr HapticObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new HapticObserverPropertyDefinitions(getConfigIdentifier()));
-}
-
-void HapticObserver::updateStatistics()
-{
- /*ScopedLock lock(dataMutex);
- //ARMARX_LOG << "updateStatistics";
- long now = TimestampVariant::nowLong();
- for (std::map<std::string, HapticSampleStatistics>::iterator it = statistics.begin(); it != statistics.end(); ++it)
+ void HapticObserver::updateStatistics()
{
- HapticSampleStatistics stats = it->second;
- std::string device = it->first;
- long avg = stats.average(now);
- float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
- setDataField(device, "rate", Variant(rate));
- updateChannel(device);
- }*/
+ /*ScopedLock lock(dataMutex);
+ //ARMARX_LOG << "updateStatistics";
+ long now = TimestampVariant::nowLong();
+ for (std::map<std::string, HapticSampleStatistics>::iterator it = statistics.begin(); it != statistics.end(); ++it)
+ {
+ HapticSampleStatistics stats = it->second;
+ std::string device = it->first;
+ long avg = stats.average(now);
+ float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
+ setDataField(device, "rate", Variant(rate));
+ updateChannel(device);
+ }*/
+ }
}
diff --git a/source/RobotAPI/components/units/HapticUnit.cpp b/source/RobotAPI/components/units/HapticUnit.cpp
index 3de1bfa4d..4f1c3a103 100644
--- a/source/RobotAPI/components/units/HapticUnit.cpp
+++ b/source/RobotAPI/components/units/HapticUnit.cpp
@@ -24,26 +24,27 @@
#include "HapticUnit.h"
-using namespace armarx;
-
-void HapticUnit::onInitComponent()
+namespace armarx
{
- offeringTopic(getProperty<std::string>("HapticTopicName").getValue());
- onInitHapticUnit();
-}
+ void HapticUnit::onInitComponent()
+ {
+ offeringTopic(getProperty<std::string>("HapticTopicName").getValue());
+ onInitHapticUnit();
+ }
-void HapticUnit::onConnectComponent()
-{
- hapticTopicPrx = getTopic<HapticUnitListenerPrx>(getProperty<std::string>("HapticTopicName").getValue());
- onStartHapticUnit();
-}
+ void HapticUnit::onConnectComponent()
+ {
+ hapticTopicPrx = getTopic<HapticUnitListenerPrx>(getProperty<std::string>("HapticTopicName").getValue());
+ onStartHapticUnit();
+ }
-void HapticUnit::onExitComponent()
-{
- onExitHapticUnit();
-}
+ void HapticUnit::onExitComponent()
+ {
+ onExitHapticUnit();
+ }
-PropertyDefinitionsPtr HapticUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new HapticUnitPropertyDefinitions(getConfigIdentifier()));
+ PropertyDefinitionsPtr HapticUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new HapticUnitPropertyDefinitions(getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnit.cpp b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
index d5bdc468b..5b4610fb2 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
@@ -25,36 +25,35 @@
#include "InertialMeasurementUnit.h"
-using namespace armarx;
-
-
-void InertialMeasurementUnit::onInitComponent()
+namespace armarx
{
- offeringTopic(getProperty<std::string>("IMUTopicName").getValue());
- onInitIMU();
-}
+ void InertialMeasurementUnit::onInitComponent()
+ {
+ offeringTopic(getProperty<std::string>("IMUTopicName").getValue());
+ onInitIMU();
+ }
-void InertialMeasurementUnit::onConnectComponent()
-{
- IMUTopicPrx = getTopic<InertialMeasurementUnitListenerPrx>(getProperty<std::string>("IMUTopicName").getValue());
- onStartIMU();
-}
+ void InertialMeasurementUnit::onConnectComponent()
+ {
+ IMUTopicPrx = getTopic<InertialMeasurementUnitListenerPrx>(getProperty<std::string>("IMUTopicName").getValue());
+ onStartIMU();
+ }
-void InertialMeasurementUnit::onDisconnectComponent()
-{
-}
+ void InertialMeasurementUnit::onDisconnectComponent()
+ {
+ }
-void InertialMeasurementUnit::onExitComponent()
-{
- onExitIMU();
-}
+ void InertialMeasurementUnit::onExitComponent()
+ {
+ onExitIMU();
+ }
-PropertyDefinitionsPtr InertialMeasurementUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new InertialMeasurementUnitPropertyDefinitions(
- getConfigIdentifier()));
+ PropertyDefinitionsPtr InertialMeasurementUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new InertialMeasurementUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index e5ae05a54..52e6f92ea 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -33,118 +33,119 @@
#include <RobotAPI/libraries/core/Pose.h>
-using namespace armarx;
-
-void InertialMeasurementUnitObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("IMUTopicName").getValue());
+ void InertialMeasurementUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("IMUTopicName").getValue());
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
- if (getProperty<bool>("EnableVisualization").getValue())
- {
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ if (getProperty<bool>("EnableVisualization").getValue())
+ {
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
}
-}
-void InertialMeasurementUnitObserver::onConnectObserver()
-{
- if (getProperty<bool>("EnableVisualization").getValue())
+ void InertialMeasurementUnitObserver::onConnectObserver()
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ if (getProperty<bool>("EnableVisualization").getValue())
+ {
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
}
-}
-void InertialMeasurementUnitObserver::onExitObserver()
-{
- if (getProperty<bool>("EnableVisualization").getValue())
+ void InertialMeasurementUnitObserver::onExitObserver()
{
- debugDrawerPrx->removePoseVisu("IMU", "orientation");
- debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+ if (getProperty<bool>("EnableVisualization").getValue())
+ {
+ debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+ }
}
-}
-void InertialMeasurementUnitObserver::reportSensorValues(
- const std::string& device, const std::string& name, const IMUData& values,
- const TimestampBasePtr& timestamp, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
-
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
-
-
- if (!existsChannel(device))
+ void InertialMeasurementUnitObserver::reportSensorValues(
+ const std::string& device, const std::string& name, const IMUData& values,
+ const TimestampBasePtr& timestamp, const Ice::Current&)
{
- offerChannel(device, "IMU data");
+ ScopedLock lock(dataMutex);
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "IMU data");
+ }
+
+ offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
+ Vector3Ptr acceleration;
+ QuaternionPtr orientationQuaternion;
+ if (values.acceleration.size() > 0)
+ {
+ ARMARX_CHECK_EXPRESSION(values.acceleration.size() == 3);
+ acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+ offerValue(device, "acceleration", acceleration);
+ }
+ if (values.gyroscopeRotation.size() > 0)
+ {
+ ARMARX_CHECK_EXPRESSION(values.gyroscopeRotation.size() == 3);
+ Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+ offerValue(device, "gyroscopeRotation", gyroscopeRotation);
+ }
+ if (values.magneticRotation.size() > 0)
+ {
+ ARMARX_CHECK_EXPRESSION(values.magneticRotation.size() == 3);
+ Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+ offerValue(device, "magneticRotation", magneticRotation);
+ }
+ if (values.orientationQuaternion.size() > 0)
+ {
+ ARMARX_CHECK_EXPRESSION(values.orientationQuaternion.size() == 4);
+ orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+ offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
+ }
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+ updateChannel(device);
+
+
+
+ if (orientationQuaternion && acceleration && getProperty<bool>("EnableVisualization").getValue())
+ {
+ Eigen::Vector3f zero;
+ zero.setZero();
+
+ DrawColor color;
+ color.r = 1;
+ color.g = 1;
+ color.b = 0;
+ color.a = 0.5;
+
+ Eigen::Vector3f ac = acceleration->toEigen();
+ ac *= 10;
+
+ debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+
+ PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+ debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
+ // debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+ }
}
- offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
- Vector3Ptr acceleration;
- QuaternionPtr orientationQuaternion;
- if (values.acceleration.size() > 0)
- {
- ARMARX_CHECK_EXPRESSION(values.acceleration.size() == 3);
- acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
- offerValue(device, "acceleration", acceleration);
- }
- if (values.gyroscopeRotation.size() > 0)
- {
- ARMARX_CHECK_EXPRESSION(values.gyroscopeRotation.size() == 3);
- Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
- offerValue(device, "gyroscopeRotation", gyroscopeRotation);
- }
- if (values.magneticRotation.size() > 0)
+ void InertialMeasurementUnitObserver::offerValue(const std::string& device, const std::string& fieldName, const Vector3Ptr& vec)
{
- ARMARX_CHECK_EXPRESSION(values.magneticRotation.size() == 3);
- Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
- offerValue(device, "magneticRotation", magneticRotation);
+ offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
+ offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
+ offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
+ offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
}
- if (values.orientationQuaternion.size() > 0)
- {
- ARMARX_CHECK_EXPRESSION(values.orientationQuaternion.size() == 4);
- orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
- offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
- }
- offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
-
- updateChannel(device);
-
-
- if (orientationQuaternion && acceleration && getProperty<bool>("EnableVisualization").getValue())
+ PropertyDefinitionsPtr InertialMeasurementUnitObserver::createPropertyDefinitions()
{
- Eigen::Vector3f zero;
- zero.setZero();
-
- DrawColor color;
- color.r = 1;
- color.g = 1;
- color.b = 0;
- color.a = 0.5;
-
- Eigen::Vector3f ac = acceleration->toEigen();
- ac *= 10;
-
- debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
-
- PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
- debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
- // debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+ return PropertyDefinitionsPtr(new InertialMeasurementUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
}
-
-void InertialMeasurementUnitObserver::offerValue(const std::string& device, const std::string& fieldName, const Vector3Ptr& vec)
-{
- offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
- offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
- offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
- offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
-}
-
-PropertyDefinitionsPtr InertialMeasurementUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new InertialMeasurementUnitObserverPropertyDefinitions(getConfigIdentifier()));
-}
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 0348c5914..045972525 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -34,154 +34,155 @@
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-using namespace armarx;
-
-void KinematicUnit::onInitComponent()
+namespace armarx
{
- // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
- // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
-
- const std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
+ void KinematicUnit::onInitComponent()
{
- std::vector<std::string> result;
- const auto& packages = armarx::Application::GetProjectDependencies();
- std::set<std::string> packageSet {packages.begin(), packages.end()};
- packageSet.emplace(Application::GetProjectName());
- packageSet.emplace(project);
- packageSet.erase("");
- armarXPackages.assign(packageSet.begin(), packageSet.end());
- }
+ // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
+ // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- if (relativeRobotFile.empty())
- {
- relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
- }
-
- if (!robot)
- {
- Ice::StringSeq includePaths;
-
- if (!project.empty())
+ const std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
{
- CMakePackageFinder finder(project);
- Ice::StringSeq projectIncludePaths;
- auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ std::vector<std::string> result;
+ const auto& packages = armarx::Application::GetProjectDependencies();
+ std::set<std::string> packageSet {packages.begin(), packages.end()};
+ packageSet.emplace(Application::GetProjectName());
+ packageSet.emplace(project);
+ packageSet.erase("");
+ armarXPackages.assign(packageSet.begin(), packageSet.end());
}
- std::string robotFile = relativeRobotFile;
- if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
+
+ if (relativeRobotFile.empty())
{
- throw UserException("Could not find robot file " + robotFile);
+ relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
}
- // read the robot
- try
+ if (!robot)
{
- robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ Ice::StringSeq includePaths;
+
+ if (!project.empty())
+ {
+ CMakePackageFinder finder(project);
+ Ice::StringSeq projectIncludePaths;
+ auto pathsString = finder.getDataDir();
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
+ std::string robotFile = relativeRobotFile;
+ if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
+
+ // read the robot
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ }
+ catch (VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
}
- catch (VirtualRobot::VirtualRobotException& e)
+ // read the robot node set and initialize the joints of this kinematic unit
+ if (robotNodeSetName == "")
{
- throw UserException(e.what());
+ throw UserException("RobotNodeSet not defined");
}
- }
- // read the robot node set and initialize the joints of this kinematic unit
- if (robotNodeSetName == "")
- {
- throw UserException("RobotNodeSet not defined");
- }
-
- VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
- robotNodes = robotNodeSetPtr->getAllRobotNodes();
- // component dependencies
- listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
- offeringTopic(listenerName);
+ VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
- this->onInitKinematicUnit();
-}
+ // component dependencies
+ listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
+ offeringTopic(listenerName);
+ this->onInitKinematicUnit();
+ }
-void KinematicUnit::onConnectComponent()
-{
- ARMARX_INFO << "setting report topic to " << listenerName << flush;
- listenerPrx = getTopic<KinematicUnitListenerPrx>(listenerName);
- this->onStartKinematicUnit();
-}
+ void KinematicUnit::onConnectComponent()
+ {
+ ARMARX_INFO << "setting report topic to " << listenerName << flush;
+ listenerPrx = getTopic<KinematicUnitListenerPrx>(listenerName);
+ this->onStartKinematicUnit();
+ }
-void KinematicUnit::onExitComponent()
-{
- this->onExitKinematicUnit();
-}
+ void KinematicUnit::onExitComponent()
+ {
+ this->onExitKinematicUnit();
+ }
-void KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
-{
-}
-void KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
-{
-}
+ void KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
+ {
+ }
-void KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
-{
-}
+ void KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
+ {
+ }
-void KinematicUnit::requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
-{
- SensorActorUnit::request(c);
-}
+ void KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
+ {
+ }
-void KinematicUnit::releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
-{
- SensorActorUnit::release(c);
-}
+ void KinematicUnit::requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
+ {
+ SensorActorUnit::request(c);
+ }
+ void KinematicUnit::releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
+ {
+ SensorActorUnit::release(c);
+ }
-std::string KinematicUnit::getRobotFilename(const Ice::Current&) const
-{
- return relativeRobotFile;
-}
-std::vector<std::string> KinematicUnit::getArmarXPackages(const Ice::Current&) const
-{
- return armarXPackages;
-}
+ std::string KinematicUnit::getRobotFilename(const Ice::Current&) const
+ {
+ return relativeRobotFile;
+ }
-std::string KinematicUnit::getRobotName(const Ice::Current&) const
-{
- if (robot)
+ std::vector<std::string> KinematicUnit::getArmarXPackages(const Ice::Current&) const
{
- return robot->getName();
+ return armarXPackages;
}
- else
+
+ std::string KinematicUnit::getRobotName(const Ice::Current&) const
{
- throw NotInitializedException("Robot Ptr is NULL", "getName");
+ if (robot)
+ {
+ return robot->getName();
+ }
+ else
+ {
+ throw NotInitializedException("Robot Ptr is NULL", "getName");
+ }
}
-}
-std::string KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
-{
- if (robotNodeSetName.empty())
+ std::string KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
{
- throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
+ if (robotNodeSetName.empty())
+ {
+ throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
+ }
+ return robotNodeSetName;
}
- return robotNodeSetName;
-}
-std::string KinematicUnit::getReportTopicName(const Ice::Current&) const
-{
- return listenerName;
-}
+ std::string KinematicUnit::getReportTopicName(const Ice::Current&) const
+ {
+ return listenerName;
+ }
-PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
- getConfigIdentifier()));
+ PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index ea9b8da61..15712aa55 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -38,294 +38,295 @@
#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
-using namespace armarx;
-
-// ********************************************************************
-// observer framework hooks
-// ********************************************************************
-void KinematicUnitObserver::onInitObserver()
-{
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
-
- // register all checks
- offerConditionCheck("valid", new ConditionCheckValid());
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("inrange", new ConditionCheckInRange());
- offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
-}
-
-void KinematicUnitObserver::onConnectObserver()
+namespace armarx
{
- // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
- // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
- std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
- std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
- Ice::StringSeq includePaths;
-
- if (!project.empty())
+ // ********************************************************************
+ // observer framework hooks
+ // ********************************************************************
+ void KinematicUnitObserver::onInitObserver()
{
- CMakePackageFinder finder(project);
- Ice::StringSeq projectIncludePaths;
- auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+
+ // register all checks
+ offerConditionCheck("valid", new ConditionCheckValid());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
}
- if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
+ void KinematicUnitObserver::onConnectObserver()
{
- throw UserException("Could not find robot file " + robotFile);
- }
+ // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
+ // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
+ Ice::StringSeq includePaths;
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ if (!project.empty())
+ {
+ CMakePackageFinder finder(project);
+ Ice::StringSeq projectIncludePaths;
+ auto pathsString = finder.getDataDir();
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
- if (robotNodeSetName == "")
- {
- throw UserException("RobotNodeSet not defined");
- }
+ if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
- auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
- std::vector< VirtualRobot::RobotNodePtr > robotNodes;
- robotNodes = robotNodeSetPtr->getAllRobotNodes();
- auto robotNodeNames = robotNodeSetPtr->getNodeNames();
- this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
- // register all channels
- offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointaccelerations", "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointtorques", "Joint torques of the" + robotNodeSetName + " kinematic chain");
- offerChannel("jointcurrents", "Joint currents of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointmotortemperatures", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointcontrolmodes", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ if (robotNodeSetName == "")
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+ auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
- // register all data fields
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
- {
- std::string jointName = (*it)->getName();
- ARMARX_VERBOSE << "* " << jointName << std::endl;
- offerDataFieldWithDefault("jointcontrolmodes", jointName, ControlModeToString(eUnknown), "Controlmode of the " + jointName + " joint");
- offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
- offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
- offerDataField("jointaccelerations", jointName, VariantType::Float, "Joint acceleration of the " + jointName + " joint");
- offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
- offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
- offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
- }
+ std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
+ auto robotNodeNames = robotNodeSetPtr->getNodeNames();
+ this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
+ // register all channels
+ offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointaccelerations", "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointtorques", "Joint torques of the" + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcurrents", "Joint currents of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointmotortemperatures", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcontrolmodes", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
- updateChannel("jointcontrolmodes");
-}
+ // register all data fields
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ ARMARX_VERBOSE << "* " << jointName << std::endl;
+ offerDataFieldWithDefault("jointcontrolmodes", jointName, ControlModeToString(eUnknown), "Controlmode of the " + jointName + " joint");
+ offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
+ offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
+ offerDataField("jointaccelerations", jointName, VariantType::Float, "Joint acceleration of the " + jointName + " joint");
+ offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
+ offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
+ offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
+ }
+
+ updateChannel("jointcontrolmodes");
+ }
-// ********************************************************************
-// KinematicUnitListener interface implementation
-// ********************************************************************
-void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+
+ // ********************************************************************
+ // KinematicUnitListener interface implementation
+ // ********************************************************************
+ void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointModes.size() == 0)
+ try
{
- return;
- }
+ if (jointModes.size() == 0)
+ {
+ return;
+ }
+
+ for (auto elem : jointModes)
+ {
+ setDataFieldFlatCopy("jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
+ }
- for (auto elem : jointModes)
+ updateChannel("jointcontrolmodes");
+ }
+ catch (...)
{
- setDataFieldFlatCopy("jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
+ handleExceptions();
}
-
- updateChannel("jointcontrolmodes");
- }
- catch (...)
- {
- handleExceptions();
}
-}
-void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
+ try
+ {
- if (jointAngles.size() == 0)
- {
- return;
- }
+ if (jointAngles.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointangles", jointAngles, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointangles", jointAngles, timestamp, aValueChanged);
- updateChannel("jointangles");
+ updateChannel("jointangles");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
- catch (...)
- {
- handleExceptions();
- }
-}
-void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointVelocities.size() == 0)
+ try
{
- return;
- }
+ if (jointVelocities.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointvelocities", jointVelocities, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointvelocities", jointVelocities, timestamp, aValueChanged);
- updateChannel("jointvelocities");
- }
- catch (...)
- {
- handleExceptions();
+ updateChannel("jointvelocities");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
-}
-void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointTorques.size() == 0)
+ try
{
- return;
- }
+ if (jointTorques.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointtorques", jointTorques, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointtorques", jointTorques, timestamp, aValueChanged);
- updateChannel("jointtorques");
- }
- catch (...)
- {
- handleExceptions();
+ updateChannel("jointtorques");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
-}
-void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointAccelerations.size() == 0)
+ try
{
- return;
- }
+ if (jointAccelerations.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointaccelerations", jointAccelerations, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointaccelerations", jointAccelerations, timestamp, aValueChanged);
- updateChannel("jointaccelerations");
- }
- catch (...)
- {
- handleExceptions();
+ updateChannel("jointaccelerations");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
-}
-void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointCurrents.size() == 0)
+ try
{
- return;
- }
+ if (jointCurrents.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointcurrents", jointCurrents, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointcurrents", jointCurrents, timestamp, aValueChanged);
- updateChannel("jointcurrents");
- }
- catch (...)
- {
- handleExceptions();
+ updateChannel("jointcurrents");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
-}
-void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- try
+ void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- if (jointMotorTemperatures.size() == 0)
+ try
{
- return;
- }
+ if (jointMotorTemperatures.size() == 0)
+ {
+ return;
+ }
- nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures, timestamp, aValueChanged);
+ nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures, timestamp, aValueChanged);
- updateChannel("jointmotortemperatures");
- }
- catch (...)
- {
- handleExceptions();
+ updateChannel("jointmotortemperatures");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
-}
-void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
-}
-
-// ********************************************************************
-// private methods
-// ********************************************************************
-void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged)
-{
- // ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
- bool newChannel;
+ void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- ScopedLock lock(initializedChannelsMutex);
- newChannel = initializedChannels.count(channelName) == 0;
- initializedChannels.insert(channelName);
}
- if (aValueChanged || newChannel)
- {
- boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
- if (timestamp < 0)
+ // ********************************************************************
+ // private methods
+ // ********************************************************************
+ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged)
+ {
+ // ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
+ bool newChannel;
{
- for (const auto& it : nameValueMap)
+ ScopedLock lock(initializedChannelsMutex);
+ newChannel = initializedChannels.count(channelName) == 0;
+ initializedChannels.insert(channelName);
+ }
+ if (aValueChanged || newChannel)
+ {
+
+ boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
+ if (timestamp < 0)
{
- if (robotNodes.count(it.first))
+ for (const auto& it : nameValueMap)
{
- map[it.first] = new Variant(it.second);
+ if (robotNodes.count(it.first))
+ {
+ map[it.first] = new Variant(it.second);
+ }
}
}
- }
- else
- {
- for (const auto& it : nameValueMap)
+ else
{
- if (robotNodes.count(it.first))
+ for (const auto& it : nameValueMap)
{
- map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+ if (robotNodes.count(it.first))
+ {
+ map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+ }
}
}
+ setDataFieldsFlatCopy(channelName, map);
+ }
+ else
+ {
+ updateDatafieldTimestamps(channelName, timestamp);
}
- setDataFieldsFlatCopy(channelName, map);
+
}
- else
+
+ PropertyDefinitionsPtr KinematicUnitObserver::createPropertyDefinitions()
{
- updateDatafieldTimestamps(channelName, timestamp);
+ return PropertyDefinitionsPtr(new KinematicUnitObserverPropertyDefinitions(
+ getConfigIdentifier()));
}
-
-}
-
-PropertyDefinitionsPtr KinematicUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new KinematicUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
}
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 92cf7f720..8d7266006 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -199,20 +199,13 @@ namespace armarx
private:
std::string _channelName;
};
-
-
- namespace channels
- {
-
- namespace KinematicUnitObserver
- {
- const KinematicUnitDatafieldCreator jointAngles("jointAngles");
- const KinematicUnitDatafieldCreator jointVelocities("jointVelocities");
- const KinematicUnitDatafieldCreator jointTorques("jointTorques");
- const KinematicUnitDatafieldCreator jointCurrents("jointCurrents");
- const KinematicUnitDatafieldCreator jointTemperatures("jointTemperatures");
- }
- }
-
}
+namespace armarx::channels::KinematicUnitObserver
+{
+ const KinematicUnitDatafieldCreator jointAngles("jointAngles");
+ const KinematicUnitDatafieldCreator jointVelocities("jointVelocities");
+ const KinematicUnitDatafieldCreator jointTorques("jointTorques");
+ const KinematicUnitDatafieldCreator jointCurrents("jointCurrents");
+ const KinematicUnitDatafieldCreator jointTemperatures("jointTemperatures");
+}
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 8b4a354af..0cb76bb4d 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -34,80 +34,79 @@
-using namespace armarx;
-
-
-void LaserScannerUnitObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("LaserScannerTopicName").getValue());
-
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-}
+ void LaserScannerUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("LaserScannerTopicName").getValue());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+ }
-void LaserScannerUnitObserver::onConnectObserver()
-{
-}
-
-void LaserScannerUnitObserver::onExitObserver()
-{
-}
+ void LaserScannerUnitObserver::onConnectObserver()
+ {
+ }
-PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
-}
+ void LaserScannerUnitObserver::onExitObserver()
+ {
+ }
-void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
- if (!existsChannel(device))
+ PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
{
- offerChannel(device, "laser scans");
+ return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
-
- // Calculate some statistics on the laser scan
- float minDistance = FLT_MAX;
- float minAngle = 0.0f;
- float maxDistance = -FLT_MAX;
- float maxAngle = 0.0f;
- float distanceSum = 0.0f;
- for (LaserScanStep const& step : scan)
+ void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- distanceSum += step.distance;
- if (step.distance < minDistance)
+ ScopedLock lock(dataMutex);
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "laser scans");
+ }
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+ // Calculate some statistics on the laser scan
+ float minDistance = FLT_MAX;
+ float minAngle = 0.0f;
+ float maxDistance = -FLT_MAX;
+ float maxAngle = 0.0f;
+ float distanceSum = 0.0f;
+ for (LaserScanStep const& step : scan)
{
- minDistance = step.distance;
- minAngle = step.angle;
+ distanceSum += step.distance;
+ if (step.distance < minDistance)
+ {
+ minDistance = step.distance;
+ minAngle = step.angle;
+ }
+ if (step.distance > maxDistance)
+ {
+ maxDistance = step.distance;
+ maxAngle = step.angle;
+ }
}
- if (step.distance > maxDistance)
+
+ if (scan.size() > 0)
{
- maxDistance = step.distance;
- maxAngle = step.angle;
+ offerOrUpdateDataField(device, "minDistance", minDistance, "minimal distance in scan");
+ offerOrUpdateDataField(device, "minAngle", minAngle, "angle with minimal distance in scan");
+ offerOrUpdateDataField(device, "maxDistance", maxDistance, "maximal distance in scan");
+ offerOrUpdateDataField(device, "maxAngle", maxAngle, "angle with maximal distance in scan");
+ float averageDistance = distanceSum / scan.size();
+ offerOrUpdateDataField(device, "averageDistance", averageDistance, "average distance in scan");
}
- }
- if (scan.size() > 0)
- {
- offerOrUpdateDataField(device, "minDistance", minDistance, "minimal distance in scan");
- offerOrUpdateDataField(device, "minAngle", minAngle, "angle with minimal distance in scan");
- offerOrUpdateDataField(device, "maxDistance", maxDistance, "maximal distance in scan");
- offerOrUpdateDataField(device, "maxAngle", maxAngle, "angle with maximal distance in scan");
- float averageDistance = distanceSum / scan.size();
- offerOrUpdateDataField(device, "averageDistance", averageDistance, "average distance in scan");
+ updateChannel(device);
}
-
- updateChannel(device);
}
-
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
index bd36b219b..b6885037d 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
@@ -35,82 +35,78 @@
-using namespace armarx;
-
-
-void MetaWearIMUObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("MetaWearTopicName").getValue());
-
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
+ void MetaWearIMUObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("MetaWearTopicName").getValue());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
-void MetaWearIMUObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void MetaWearIMUObserver::onExitObserver()
-{
-}
+ void MetaWearIMUObserver::onConnectObserver()
+ {
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- if (!existsChannel(name))
+ void MetaWearIMUObserver::onExitObserver()
{
- offerChannel(name, "MetaWear IMU data");
+
}
- offerVector3(name, "acceleration", data.acceleration);
- offerVector3(name, "gyro", data.gyro);
- offerVector3(name, "magnetic", data.magnetic);
- offerQuaternion(name, "orientationQuaternion", data.orientationQuaternion);
- updateChannel(name);
+ void armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&)
+ {
+ ScopedLock lock(dataMutex);
-}
+ if (!existsChannel(name))
+ {
+ offerChannel(name, "MetaWear IMU data");
+ }
+ offerVector3(name, "acceleration", data.acceleration);
+ offerVector3(name, "gyro", data.gyro);
+ offerVector3(name, "magnetic", data.magnetic);
+ offerQuaternion(name, "orientationQuaternion", data.orientationQuaternion);
-PropertyDefinitionsPtr MetaWearIMUObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new MetaWearIMUObserverPropertyDefinitions(getConfigIdentifier()));
-}
+ updateChannel(name);
-void MetaWearIMUObserver::offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
-{
- if (data.size() == 3)
- {
- Vector3Ptr vec3 = new Vector3(data.at(0), data.at(1), data.at(2));
- offerOrUpdateDataField(channelName, dfName, vec3, dfName);
}
- else if (data.size() != 0)
+
+ PropertyDefinitionsPtr MetaWearIMUObserver::createPropertyDefinitions()
{
- ARMARX_WARNING << "data." << dfName << ".size() != 3 && data." << dfName << ".size() != 0";
+ return PropertyDefinitionsPtr(new MetaWearIMUObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
-void MetaWearIMUObserver::offerQuaternion(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
-{
- if (data.size() == 4)
+ void MetaWearIMUObserver::offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
{
- QuaternionPtr quat = new Quaternion(data.at(0), data.at(1), data.at(2), data.at(3));
- offerOrUpdateDataField(channelName, dfName, quat, dfName);
+ if (data.size() == 3)
+ {
+ Vector3Ptr vec3 = new Vector3(data.at(0), data.at(1), data.at(2));
+ offerOrUpdateDataField(channelName, dfName, vec3, dfName);
+ }
+ else if (data.size() != 0)
+ {
+ ARMARX_WARNING << "data." << dfName << ".size() != 3 && data." << dfName << ".size() != 0";
+ }
}
- else if (data.size() != 0)
+
+ void MetaWearIMUObserver::offerQuaternion(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
{
- ARMARX_WARNING << "data." << dfName << ".size() != 4 && data." << dfName << ".size() != 0";
+ if (data.size() == 4)
+ {
+ QuaternionPtr quat = new Quaternion(data.at(0), data.at(1), data.at(2), data.at(3));
+ offerOrUpdateDataField(channelName, dfName, quat, dfName);
+ }
+ else if (data.size() != 0)
+ {
+ ARMARX_WARNING << "data." << dfName << ".size() != 4 && data." << dfName << ".size() != 0";
+ }
}
}
-
-
-
-
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
index 0b43e68d9..ce09bacd4 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -35,113 +35,112 @@
-using namespace armarx;
-
-
-void OptoForceUnitObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("OptoForceTopicName").getValue());
-
- //offerConditionCheck("updated", new ConditionCheckUpdated());
- //offerConditionCheck("larger", new ConditionCheckLarger());
- //offerConditionCheck("equals", new ConditionCheckEquals());
- //offerConditionCheck("smaller", new ConditionCheckSmaller());
+ void OptoForceUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("OptoForceTopicName").getValue());
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
+ //offerConditionCheck("updated", new ConditionCheckUpdated());
+ //offerConditionCheck("larger", new ConditionCheckLarger());
+ //offerConditionCheck("equals", new ConditionCheckEquals());
+ //offerConditionCheck("smaller", new ConditionCheckSmaller());
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void OptoForceUnitObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
+ void OptoForceUnitObserver::onConnectObserver()
+ {
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void OptoForceUnitObserver::onExitObserver()
-{
- //debugDrawerPrx->removePoseVisu("IMU", "orientation");
- //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
-}
-
-void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- if (!existsChannel(name))
+ void OptoForceUnitObserver::onExitObserver()
{
- offerChannel(name, "Force data");
+ //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
- offerOrUpdateDataField(name, "name", Variant(name), "Name of the sensor");
- offerOrUpdateDataField(name, "device", Variant(device), "Device name");
- offerOrUpdateDataField(name, "fx", Variant(fx), "Force x");
- offerOrUpdateDataField(name, "fy", Variant(fy), "Force y");
- offerOrUpdateDataField(name, "fz", Variant(fz), "Force z");
- offerOrUpdateDataField(name, "timestamp", timestampPtr, "Timestamp");
- offerOrUpdateDataField(name, "force", Vector3(fx, fy, fz), "Force");
-
- updateChannel(name);
-}
+ void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ {
+ ScopedLock lock(dataMutex);
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ if (!existsChannel(name))
+ {
+ offerChannel(name, "Force data");
+ }
+
+ offerOrUpdateDataField(name, "name", Variant(name), "Name of the sensor");
+ offerOrUpdateDataField(name, "device", Variant(device), "Device name");
+ offerOrUpdateDataField(name, "fx", Variant(fx), "Force x");
+ offerOrUpdateDataField(name, "fy", Variant(fy), "Force y");
+ offerOrUpdateDataField(name, "fz", Variant(fz), "Force z");
+ offerOrUpdateDataField(name, "timestamp", timestampPtr, "Timestamp");
+ offerOrUpdateDataField(name, "force", Vector3(fx, fy, fz), "Force");
+
+ updateChannel(name);
+ }
-/*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
+ /*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ {
+ ScopedLock lock(dataMutex);
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
- Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
- Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
- QuaternionPtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+ Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+ Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+ Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+ QuaternionPtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
- if (!existsChannel(device))
- {
- offerChannel(device, "IMU data");
- }
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "IMU data");
+ }
- offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
- offerValue(device, "acceleration", acceleration);
- offerValue(device, "gyroscopeRotation", gyroscopeRotation);
- offerValue(device, "magneticRotation", magneticRotation);
- offerValue(device, "acceleration", acceleration);
- offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
- offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+ offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
+ offerValue(device, "acceleration", acceleration);
+ offerValue(device, "gyroscopeRotation", gyroscopeRotation);
+ offerValue(device, "magneticRotation", magneticRotation);
+ offerValue(device, "acceleration", acceleration);
+ offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
- updateChannel(device);
+ updateChannel(device);
- Eigen::Vector3f zero;
- zero.setZero();
+ Eigen::Vector3f zero;
+ zero.setZero();
- DrawColor color;
- color.r = 1;
- color.g = 1;
- color.b = 0;
- color.a = 0.5;
+ DrawColor color;
+ color.r = 1;
+ color.g = 1;
+ color.b = 0;
+ color.a = 0.5;
- Eigen::Vector3f ac = acceleration->toEigen();
- ac *= 10;
+ Eigen::Vector3f ac = acceleration->toEigen();
+ ac *= 10;
- debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+ debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
- PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
- debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
- debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
-}*/
+ PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+ debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
+ debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+ }*/
-void OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
-{
- offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
- offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
- offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
- offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
+ void OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
+ {
+ offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
+ offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
+ offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
+ offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
-}
+ }
-PropertyDefinitionsPtr OptoForceUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ PropertyDefinitionsPtr OptoForceUnitObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ }
}
-
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 7762139ad..514a1767b 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -35,75 +35,75 @@
-using namespace armarx;
-
-
-void OrientedTactileSensorUnitObserver::onInitObserver()
+namespace armarx
{
- usingTopic(getProperty<std::string>("SensorTopicName").getValue());
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
-
+ void OrientedTactileSensorUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("SensorTopicName").getValue());
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
-void OrientedTactileSensorUnitObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-void OrientedTactileSensorUnitObserver::onExitObserver()
-{
- //debugDrawerPrx->removePoseVisu("IMU", "orientation");
- //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
-}
-
-void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+ void OrientedTactileSensorUnitObserver::onConnectObserver()
+ {
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ }
- std::stringstream ss;
- ss << "sensor" << id;
- std::string channelName = ss.str();
- if (!existsChannel(channelName))
+ void OrientedTactileSensorUnitObserver::onExitObserver()
{
- offerChannel(channelName, "Sensor Data");
+ //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
- offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
- QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
- offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current orientation");
- offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
- offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
- offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
- offerOrUpdateDataField(channelName, "linear acceleration", Variant(new Vector3(accelx, accely, accelz)), "current linear acceleration");
-}
-/* TODO: for integration with debug drawer
- updateChannel(device);
+ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
+ {
+ ScopedLock lock(dataMutex);
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ std::stringstream ss;
+ ss << "sensor" << id;
+ std::string channelName = ss.str();
+
+ if (!existsChannel(channelName))
+ {
+ offerChannel(channelName, "Sensor Data");
+ }
+
+ offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
+ QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
+ offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current orientation");
+ offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
+ offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
+ offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
+ offerOrUpdateDataField(channelName, "linear acceleration", Variant(new Vector3(accelx, accely, accelz)), "current linear acceleration");
+ }
+ /* TODO: for integration with debug drawer
+ updateChannel(device);
- Eigen::Vector3f zero;
- zero.setZero();
+ Eigen::Vector3f zero;
+ zero.setZero();
- DrawColor color;
- color.r = 1;
- color.g = 1;
- color.b = 0;
- color.a = 0.5;
+ DrawColor color;
+ color.r = 1;
+ color.g = 1;
+ color.b = 0;
+ color.a = 0.5;
- Eigen::Vector3f ac = acceleration->toEigen();
- ac *= 10;
+ Eigen::Vector3f ac = acceleration->toEigen();
+ ac *= 10;
- debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+ debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
- PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
- debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
- debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
-*/
+ PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+ debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
+ debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+ */
-PropertyDefinitionsPtr OrientedTactileSensorUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ PropertyDefinitionsPtr OrientedTactileSensorUnitObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index ac67bfaaf..7afa0eefd 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -26,51 +26,51 @@
#include "PlatformUnit.h"
-using namespace armarx;
-
-
-void PlatformUnit::onInitComponent()
+namespace armarx
{
- std::string platformName = getProperty<std::string>("PlatformName").getValue();
+ void PlatformUnit::onInitComponent()
+ {
+ std::string platformName = getProperty<std::string>("PlatformName").getValue();
- // component dependencies
- listenerChannelName = platformName + "State";
- offeringTopic(listenerChannelName);
+ // component dependencies
+ listenerChannelName = platformName + "State";
+ offeringTopic(listenerChannelName);
- this->onInitPlatformUnit();
-}
+ this->onInitPlatformUnit();
+ }
-void PlatformUnit::onConnectComponent()
-{
- ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
- listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
+ void PlatformUnit::onConnectComponent()
+ {
+ ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
+ listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
- this->onStartPlatformUnit();
-}
+ this->onStartPlatformUnit();
+ }
-void PlatformUnit::onDisconnectComponent()
-{
- listenerPrx = NULL;
+ void PlatformUnit::onDisconnectComponent()
+ {
+ listenerPrx = NULL;
- this->onStopPlatformUnit();
-}
+ this->onStopPlatformUnit();
+ }
-void PlatformUnit::onExitComponent()
-{
- this->onExitPlatformUnit();
-}
+ void PlatformUnit::onExitComponent()
+ {
+ this->onExitPlatformUnit();
+ }
-void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
-{
-}
+ void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+ {
+ }
-PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
- getConfigIdentifier()));
+ PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index 73661c4ce..d0e0cd8e0 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -33,98 +33,99 @@
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
-using namespace armarx;
-
-// ********************************************************************
-// observer framework hooks
-// ********************************************************************
-void PlatformUnitObserver::onInitObserver()
-{
- // TODO: check if platformNodeName exists?
- platformNodeName = getProperty<std::string>("PlatformName").getValue();
-
-
- // register all checks
- offerConditionCheck("valid", new ConditionCheckValid());
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("inrange", new ConditionCheckInRange());
- offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- usingTopic(platformNodeName + "State");
-}
-
-void PlatformUnitObserver::onConnectObserver()
-{
- // register all channels
- offerChannel("platformPose", "Current Position of " + platformNodeName);
- offerChannel("targetPose", "Target Position of " + platformNodeName);
- offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
- offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
-
- // register all data fields
- offerDataField("platformPose", "positionX", VariantType::Float, "Current X position of " + platformNodeName + " in mm");
- offerDataField("platformPose", "positionY", VariantType::Float, "Current Y position of " + platformNodeName + " in mm");
- offerDataField("platformPose", "rotation", VariantType::Float, "Current Rotation of " + platformNodeName + " in radian");
-
- offerDataField("targetPose", "positionX", VariantType::Float, "Target X position of " + platformNodeName + " in mm");
- offerDataField("targetPose", "positionY", VariantType::Float, "Target Y position of " + platformNodeName + " in mm");
- offerDataField("targetPose", "rotation", VariantType::Float, "Target Rotation of " + platformNodeName + " in radian");
-
- offerDataField("platformVelocity", "velocityX", VariantType::Float, "Current X velocity of " + platformNodeName + " in mm/s");
- offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
- offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
-
- offerDataField("platformOdometryPose", "positionX", VariantType::Float, "Current Odometry X position of " + platformNodeName + " in mm");
- offerDataField("platformOdometryPose", "positionY", VariantType::Float, "Current Odometry Y position of " + platformNodeName + " in mm");
- offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
-
- // odometry pose is always zero in the beginning - set it so that it can be queried
- reportPlatformOdometryPose(0, 0, 0, Ice::emptyCurrent);
-
-}
-
-void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
-{
- nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
- updateChannel("platformPose");
-}
-
-void PlatformUnitObserver::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
-{
- nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
- updateChannel("targetPose");
-}
-
-void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
-{
- setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
- setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
- setDataField("platformVelocity", "velocityRotation", currentPlatformVelocityRotation);
- updateChannel("platformVelocity");
-}
-
-// ********************************************************************
-// private methods
-// ********************************************************************
-void PlatformUnitObserver::nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation)
-{
- setDataField(channelName, "positionX", platformPositionX);
- setDataField(channelName, "positionY", platformPositionY);
- setDataField(channelName, "rotation", platformRotation);
-}
-
-PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new PlatformUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
-}
-
-
-void armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&)
+namespace armarx
{
- nameValueMapToDataFields("platformOdometryPose", x, y, angle);
- updateChannel("platformOdometryPose");
+ // ********************************************************************
+ // observer framework hooks
+ // ********************************************************************
+ void PlatformUnitObserver::onInitObserver()
+ {
+ // TODO: check if platformNodeName exists?
+ platformNodeName = getProperty<std::string>("PlatformName").getValue();
+
+
+ // register all checks
+ offerConditionCheck("valid", new ConditionCheckValid());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ usingTopic(platformNodeName + "State");
+ }
+
+ void PlatformUnitObserver::onConnectObserver()
+ {
+ // register all channels
+ offerChannel("platformPose", "Current Position of " + platformNodeName);
+ offerChannel("targetPose", "Target Position of " + platformNodeName);
+ offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
+ offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
+
+ // register all data fields
+ offerDataField("platformPose", "positionX", VariantType::Float, "Current X position of " + platformNodeName + " in mm");
+ offerDataField("platformPose", "positionY", VariantType::Float, "Current Y position of " + platformNodeName + " in mm");
+ offerDataField("platformPose", "rotation", VariantType::Float, "Current Rotation of " + platformNodeName + " in radian");
+
+ offerDataField("targetPose", "positionX", VariantType::Float, "Target X position of " + platformNodeName + " in mm");
+ offerDataField("targetPose", "positionY", VariantType::Float, "Target Y position of " + platformNodeName + " in mm");
+ offerDataField("targetPose", "rotation", VariantType::Float, "Target Rotation of " + platformNodeName + " in radian");
+
+ offerDataField("platformVelocity", "velocityX", VariantType::Float, "Current X velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
+
+ offerDataField("platformOdometryPose", "positionX", VariantType::Float, "Current Odometry X position of " + platformNodeName + " in mm");
+ offerDataField("platformOdometryPose", "positionY", VariantType::Float, "Current Odometry Y position of " + platformNodeName + " in mm");
+ offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
+
+ // odometry pose is always zero in the beginning - set it so that it can be queried
+ reportPlatformOdometryPose(0, 0, 0, Ice::emptyCurrent);
+
+ }
+
+ void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+ {
+ nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
+ updateChannel("platformPose");
+ }
+
+ void PlatformUnitObserver::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
+ {
+ nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
+ updateChannel("targetPose");
+ }
+
+ void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+ {
+ setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
+ setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
+ setDataField("platformVelocity", "velocityRotation", currentPlatformVelocityRotation);
+ updateChannel("platformVelocity");
+ }
+
+ // ********************************************************************
+ // private methods
+ // ********************************************************************
+ void PlatformUnitObserver::nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation)
+ {
+ setDataField(channelName, "positionX", platformPositionX);
+ setDataField(channelName, "positionY", platformPositionY);
+ setDataField(channelName, "rotation", platformRotation);
+ }
+
+ PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new PlatformUnitObserverPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+
+ void armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&)
+ {
+ nameValueMapToDataFields("platformOdometryPose", x, y, angle);
+ updateChannel("platformOdometryPose");
+ }
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index a79ee0792..66b24989d 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -138,16 +138,11 @@ namespace armarx
private:
std::string channelName;
};
-
-
- namespace channels
- {
- namespace PlatformUnitObserver
- {
- const PlatformUnitDatafieldCreator platformPose("platformPose");
- const PlatformUnitDatafieldCreator targetPose("targetPose");
- const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
- }
- }
}
+namespace armarx::channels::PlatformUnitObserver
+{
+ const PlatformUnitDatafieldCreator platformPose("platformPose");
+ const PlatformUnitDatafieldCreator targetPose("targetPose");
+ const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
+}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index b526cff93..b4b4879b4 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -30,90 +30,119 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <cmath>
-using namespace armarx;
-
-void PlatformUnitSimulation::onInitPlatformUnit()
+namespace armarx
{
- platformMode = eUndefined;
- referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
- targetPositionX = currentPositionX = getProperty<float>("InitialPosition.x").getValue();
- targetPositionY = currentPositionY = getProperty<float>("InitialPosition.y").getValue();
- targetRotation = 0.0;
- targetRotation = currentRotation = getProperty<float>("InitialRotation").getValue();
+ void PlatformUnitSimulation::onInitPlatformUnit()
+ {
+ platformMode = eUndefined;
+ referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
+ targetPositionX = currentPositionX = getProperty<float>("InitialPosition.x").getValue();
+ targetPositionY = currentPositionY = getProperty<float>("InitialPosition.y").getValue();
+ targetRotation = 0.0;
+ targetRotation = currentRotation = getProperty<float>("InitialRotation").getValue();
- maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
- maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
+ maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
+ maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
- velPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(), 0, 0, getProperty<float>("MaxLinearAcceleration").getValue()));
- posPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(), 0, 0, maxLinearVelocity, getProperty<float>("MaxLinearAcceleration").getValue()));
+ velPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(), 0, 0, getProperty<float>("MaxLinearAcceleration").getValue()));
+ posPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(), 0, 0, maxLinearVelocity, getProperty<float>("MaxLinearAcceleration").getValue()));
- positionalAccuracy = 0.01;
+ positionalAccuracy = 0.01;
- intervalMs = getProperty<int>("IntervalMs").getValue();
- ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval";
- simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
+ intervalMs = getProperty<int>("IntervalMs").getValue();
+ ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval";
+ simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
-}
+ }
-void PlatformUnitSimulation::onStartPlatformUnit()
-{
- PlatformPose currentPose;
- // FIXME: Take the timestamp from simulation
- currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- currentPose.x = currentPositionX;
- currentPose.y = currentPositionY;
- currentPose.rotationAroundZ = currentRotation;
- listenerPrx->reportPlatformPose(currentPose);
- simulationTask->start();
-}
+ void PlatformUnitSimulation::onStartPlatformUnit()
+ {
+ PlatformPose currentPose;
+ // FIXME: Take the timestamp from simulation
+ currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.x = currentPositionX;
+ currentPose.y = currentPositionY;
+ currentPose.rotationAroundZ = currentRotation;
+ listenerPrx->reportPlatformPose(currentPose);
+ simulationTask->start();
+ }
-void PlatformUnitSimulation::onStopPlatformUnit()
-{
- if (simulationTask)
+ void PlatformUnitSimulation::onStopPlatformUnit()
{
- simulationTask->stop();
+ if (simulationTask)
+ {
+ simulationTask->stop();
+ }
}
-}
-void PlatformUnitSimulation::onExitPlatformUnit()
-{
- if (simulationTask)
+ void PlatformUnitSimulation::onExitPlatformUnit()
{
- simulationTask->stop();
+ if (simulationTask)
+ {
+ simulationTask->stop();
+ }
}
-}
-void PlatformUnitSimulation::simulationFunction()
-{
- // the time it took until this method was called again
- auto now = TimeUtil::GetTime();
- double timeDeltaInSeconds = (now - lastExecutionTime).toSecondsDouble();
- lastExecutionTime = now;
- std::vector<float> vel(3, 0.0f);
+ void PlatformUnitSimulation::simulationFunction()
{
- ScopedLock lock(currentPoseMutex);
- switch (platformMode)
+ // the time it took until this method was called again
+ auto now = TimeUtil::GetTime();
+ double timeDeltaInSeconds = (now - lastExecutionTime).toSecondsDouble();
+ lastExecutionTime = now;
+ std::vector<float> vel(3, 0.0f);
{
- case ePositionControl:
+ ScopedLock lock(currentPoseMutex);
+ switch (platformMode)
{
- posPID->update(timeDeltaInSeconds,
- Eigen::Vector2f(currentPositionX, currentPositionY),
- Eigen::Vector2f(targetPositionX, targetPositionY));
- float newXTickMotion = posPID->getControlValue()[0] * timeDeltaInSeconds;
- currentPositionX += newXTickMotion;
- currentPositionY += posPID->getControlValue()[1] * timeDeltaInSeconds;
- vel[0] = posPID->getControlValue()[0];
- vel[1] = posPID->getControlValue()[1];
-
- float diff = fabs(targetRotation - currentRotation);
-
- if (diff > orientationalAccuracy)
+ case ePositionControl:
{
- int sign = copysignf(1.0f, (targetRotation - currentRotation));
- currentRotation += sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
+ posPID->update(timeDeltaInSeconds,
+ Eigen::Vector2f(currentPositionX, currentPositionY),
+ Eigen::Vector2f(targetPositionX, targetPositionY));
+ float newXTickMotion = posPID->getControlValue()[0] * timeDeltaInSeconds;
+ currentPositionX += newXTickMotion;
+ currentPositionY += posPID->getControlValue()[1] * timeDeltaInSeconds;
+ vel[0] = posPID->getControlValue()[0];
+ vel[1] = posPID->getControlValue()[1];
+
+ float diff = fabs(targetRotation - currentRotation);
+
+ if (diff > orientationalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetRotation - currentRotation));
+ currentRotation += sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
+
+ // stay between +/- M_2_PI
+ if (currentRotation > 2 * M_PI)
+ {
+ currentRotation -= 2 * M_PI;
+ }
+
+ if (currentRotation < -2 * M_PI)
+ {
+ currentRotation += 2 * M_PI;
+ }
+
+ vel[2] = angularVelocity;
+
+ }
+ }
+ break;
+ case eVelocityControl:
+ {
+ Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
+ Eigen::Rotation2Df rot(currentRotation);
+ targetVel = rot * targetVel;
+ velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
+ currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
+ currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
+ currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
+
+
+ currentRotation += angularVelocity * timeDeltaInSeconds;
// stay between +/- M_2_PI
if (currentRotation > 2 * M_PI)
@@ -125,109 +154,80 @@ void PlatformUnitSimulation::simulationFunction()
{
currentRotation += 2 * M_PI;
}
-
+ vel[0] = currentTranslationVelocity[0];
+ vel[1] = currentTranslationVelocity[1];
vel[2] = angularVelocity;
-
- }
- }
- break;
- case eVelocityControl:
- {
- Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
- Eigen::Rotation2Df rot(currentRotation);
- targetVel = rot * targetVel;
- velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
- currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
- currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
- currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
-
-
- currentRotation += angularVelocity * timeDeltaInSeconds;
-
- // stay between +/- M_2_PI
- if (currentRotation > 2 * M_PI)
- {
- currentRotation -= 2 * M_PI;
- }
-
- if (currentRotation < -2 * M_PI)
- {
- currentRotation += 2 * M_PI;
}
- vel[0] = currentTranslationVelocity[0];
- vel[1] = currentTranslationVelocity[1];
- vel[2] = angularVelocity;
- }
- break;
- default:
break;
+ default:
+ break;
+ }
}
+ PlatformPose currentPose;
+ // FIXME: Take the timestamp from simulation
+ currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.x = currentPositionX;
+ currentPose.y = currentPositionY;
+ currentPose.rotationAroundZ = currentRotation;
+ listenerPrx->reportPlatformPose(currentPose);
+ listenerPrx->reportPlatformVelocity(vel[0], vel[1], vel[2]);
}
- PlatformPose currentPose;
- // FIXME: Take the timestamp from simulation
- currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- currentPose.x = currentPositionX;
- currentPose.y = currentPositionY;
- currentPose.rotationAroundZ = currentRotation;
- listenerPrx->reportPlatformPose(currentPose);
- listenerPrx->reportPlatformVelocity(vel[0], vel[1], vel[2]);
-}
-void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
-{
- ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
+ void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
{
- ScopedLock lock(currentPoseMutex);
- platformMode = ePositionControl;
- targetPositionX = targetPlatformPositionX;
- targetPositionY = targetPlatformPositionY;
- targetRotation = targetPlatformRotation;
- this->positionalAccuracy = positionalAccuracy;
- this->orientationalAccuracy = orientationalAccuracy;
+ ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
+ {
+ ScopedLock lock(currentPoseMutex);
+ platformMode = ePositionControl;
+ targetPositionX = targetPlatformPositionX;
+ targetPositionY = targetPlatformPositionY;
+ targetRotation = targetPlatformRotation;
+ this->positionalAccuracy = positionalAccuracy;
+ this->orientationalAccuracy = orientationalAccuracy;
+ }
+ listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
}
- listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
-}
-PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(
- new PlatformUnitSimulationPropertyDefinitions(getConfigIdentifier()));
-}
+ PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(
+ new PlatformUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ }
-void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
-{
- ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
- ScopedLock lock(currentPoseMutex);
- platformMode = eVelocityControl;
- linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
- linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
- angularVelocity = std::min(maxAngularVelocity, targetPlatformVelocityRotation);
+ void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
+ ScopedLock lock(currentPoseMutex);
+ platformMode = eVelocityControl;
+ linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
+ linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
+ angularVelocity = std::min(maxAngularVelocity, targetPlatformVelocityRotation);
-}
+ }
-void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
-{
- float targetPositionX, targetPositionY, targetRotation;
+ void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
{
- ScopedLock lock(currentPoseMutex);
- targetPositionX = currentPositionX + targetPlatformOffsetX;
- targetPositionY = currentPositionY + targetPlatformOffsetY;
- targetRotation = currentRotation + targetPlatformOffsetRotation;
+ float targetPositionX, targetPositionY, targetRotation;
+ {
+ ScopedLock lock(currentPoseMutex);
+ targetPositionX = currentPositionX + targetPlatformOffsetX;
+ targetPositionY = currentPositionY + targetPlatformOffsetY;
+ targetRotation = currentRotation + targetPlatformOffsetRotation;
+ }
+ moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
}
- moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
-}
-void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
-{
- ScopedLock lock(currentPoseMutex);
- maxLinearVelocity = positionalVelocity;
- maxAngularVelocity = orientaionalVelocity;
-}
+ void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
+ {
+ ScopedLock lock(currentPoseMutex);
+ maxLinearVelocity = positionalVelocity;
+ maxAngularVelocity = orientaionalVelocity;
+ }
-void PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
-{
- move(0, 0, 0);
+ void PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
+ {
+ move(0, 0, 0);
+ }
}
-
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.cpp b/source/RobotAPI/components/units/RobotPoseUnit.cpp
index d3b051a4a..85bb9fbdc 100644
--- a/source/RobotAPI/components/units/RobotPoseUnit.cpp
+++ b/source/RobotAPI/components/units/RobotPoseUnit.cpp
@@ -26,51 +26,51 @@
#include "RobotPoseUnit.h"
-using namespace armarx;
-
-
-void RobotPoseUnit::onInitComponent()
+namespace armarx
{
- std::string RobotPoseName = getProperty<std::string>("RobotName").getValue();
+ void RobotPoseUnit::onInitComponent()
+ {
+ std::string RobotPoseName = getProperty<std::string>("RobotName").getValue();
- // component dependencies
- listenerChannelName = RobotPoseName + "6DPoseState";
- offeringTopic(listenerChannelName);
+ // component dependencies
+ listenerChannelName = RobotPoseName + "6DPoseState";
+ offeringTopic(listenerChannelName);
- this->onInitRobotPoseUnit();
-}
+ this->onInitRobotPoseUnit();
+ }
-void RobotPoseUnit::onConnectComponent()
-{
- ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
- listenerPrx = getTopic<RobotPoseUnitListenerPrx>(listenerChannelName);
+ void RobotPoseUnit::onConnectComponent()
+ {
+ ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
+ listenerPrx = getTopic<RobotPoseUnitListenerPrx>(listenerChannelName);
- this->onStartRobotPoseUnit();
-}
+ this->onStartRobotPoseUnit();
+ }
-void RobotPoseUnit::onDisconnectComponent()
-{
- listenerPrx = NULL;
+ void RobotPoseUnit::onDisconnectComponent()
+ {
+ listenerPrx = NULL;
- this->onStopRobotPoseUnit();
-}
+ this->onStopRobotPoseUnit();
+ }
-void RobotPoseUnit::onExitComponent()
-{
- this->onExitRobotPoseUnit();
-}
+ void RobotPoseUnit::onExitComponent()
+ {
+ this->onExitRobotPoseUnit();
+ }
-void RobotPoseUnit:: moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
-{
-}
+ void RobotPoseUnit:: moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+ {
+ }
-PropertyDefinitionsPtr RobotPoseUnit::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new RobotPoseUnitPropertyDefinitions(
- getConfigIdentifier()));
+ PropertyDefinitionsPtr RobotPoseUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new RobotPoseUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 2fbf0d924..0a65025a7 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
EmergencyStop
ArmarXGuiInterfaces
+ RobotAPIInterfaces
RobotAPIUnits
DefaultWidgetDescriptions
)
diff --git a/source/RobotAPI/components/units/RobotUnit/Constants.h b/source/RobotAPI/components/units/RobotUnit/Constants.h
index 81724864c..b8452dcd4 100644
--- a/source/RobotAPI/components/units/RobotUnit/Constants.h
+++ b/source/RobotAPI/components/units/RobotUnit/Constants.h
@@ -25,14 +25,11 @@
#include <cmath>
#include <string>
-namespace armarx
+namespace armarx::ControllerConstants
{
- namespace ControllerConstants
- {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
- static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
+ static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
#pragma GCC diagnostic pop
- }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlModes.h b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
index 3eb6750d0..fa6490a1f 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlModes.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
@@ -24,39 +24,36 @@
#include <string>
-namespace armarx
+namespace armarx::ControlModes
{
- namespace ControlModes
- {
- //'normal' actor modes
- static const std::string Position1DoF = "ControlMode_Position1DoF";
- static const std::string Torque1DoF = "ControlMode_Torque1DoF";
- static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
- static const std::string Velocity1DoF = "ControlMode_Velocity1DoF";
- static const std::string Current1DoF = "ControlMode_Current1DoF";
-
- // 'extended' actor modes
- static const std::string VelocityTorque = "ControlMode_VelocityTorque";
- static const std::string ActiveImpedance = "ControlMode_ActiveImpedance";
-
-
- //modes for holonomic platforms
- static const std::string HolonomicPlatformVelocity = "ControlMode_HolonomicPlatformVelocity";
- static const std::string HolonomicPlatformRelativePosition = "ControlMode_HolonomicPlatformRelativePosition";
-
- //special sentinel modes
- static const std::string EmergencyStop = "ControlMode_EmergencyStop";
- static const std::string StopMovement = "ControlMode_StopMovement";
- }
-
- namespace HardwareControlModes
- {
- //'normal' actor modes
- static const std::string Position1DoF = "Position1DoF";
- static const std::string Velocity1DoF = "Velocity1DoF";
- static const std::string Current1DoF = "Current1DoF";
-
- //modes for holonomic platforms
- static const std::string HolonomicPlatformVelocity = "HolonomicPlatformVelocity";
- }
+ //'normal' actor modes
+ static const std::string Position1DoF = "ControlMode_Position1DoF";
+ static const std::string Torque1DoF = "ControlMode_Torque1DoF";
+ static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
+ static const std::string Velocity1DoF = "ControlMode_Velocity1DoF";
+ static const std::string Current1DoF = "ControlMode_Current1DoF";
+
+ // 'extended' actor modes
+ static const std::string VelocityTorque = "ControlMode_VelocityTorque";
+ static const std::string ActiveImpedance = "ControlMode_ActiveImpedance";
+
+
+ //modes for holonomic platforms
+ static const std::string HolonomicPlatformVelocity = "ControlMode_HolonomicPlatformVelocity";
+ static const std::string HolonomicPlatformRelativePosition = "ControlMode_HolonomicPlatformRelativePosition";
+
+ //special sentinel modes
+ static const std::string EmergencyStop = "ControlMode_EmergencyStop";
+ static const std::string StopMovement = "ControlMode_StopMovement";
+}
+
+namespace armarx::HardwareControlModes
+{
+ //'normal' actor modes
+ static const std::string Position1DoF = "Position1DoF";
+ static const std::string Velocity1DoF = "Velocity1DoF";
+ static const std::string Current1DoF = "Current1DoF";
+
+ //modes for holonomic platforms
+ static const std::string HolonomicPlatformVelocity = "HolonomicPlatformVelocity";
}
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
index ca6b0a2e4..a83b78a52 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
@@ -22,71 +22,68 @@
#include "DefaultWidgetDescriptions.h"
-namespace armarx
+namespace armarx::WidgetDescription
{
- namespace WidgetDescription
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options)
{
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options)
- {
- StringComboBoxPtr rns = new StringComboBox;
- rns->name = std::move(name);
- rns->options = std::move(options);
- rns->defaultIndex = 0;
- return rns;
- }
+ StringComboBoxPtr rns = new StringComboBox;
+ rns->name = std::move(name);
+ rns->options = std::move(options);
+ rns->defaultIndex = 0;
+ return rns;
+ }
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet)
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet)
+ {
+ StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
+ for (std::size_t i = 0; i < rns->options.size(); ++i)
{
- StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
- for (std::size_t i = 0; i < rns->options.size(); ++i)
+ if (preferredSet.count(rns->options.at(i)))
{
- if (preferredSet.count(rns->options.at(i)))
- {
- rns->defaultIndex = i;
- break;
- }
+ rns->defaultIndex = i;
+ break;
}
- return rns;
}
+ return rns;
+ }
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred)
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred)
+ {
+ StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
+ for (std::size_t i = 0; i < rns->options.size(); ++i)
{
- StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
- for (std::size_t i = 0; i < rns->options.size(); ++i)
+ if (mostPreferred == rns->options.at(i))
{
- if (mostPreferred == rns->options.at(i))
- {
- rns->defaultIndex = i;
- break;
- }
+ rns->defaultIndex = i;
+ break;
}
- return rns;
}
+ return rns;
+ }
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred)
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred)
+ {
+ StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
+ for (std::size_t i = 0; i < rns->options.size(); ++i)
{
- StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
- for (std::size_t i = 0; i < rns->options.size(); ++i)
+ if (mostPreferred == rns->options.at(i))
{
- if (mostPreferred == rns->options.at(i))
- {
- rns->defaultIndex = i;
- break;
- }
+ rns->defaultIndex = i;
+ break;
}
- return rns;
}
+ return rns;
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
index cdff566ee..ec9f4c590 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
@@ -30,63 +30,60 @@
#include <ArmarXGui/interface/WidgetDescription.h>
#include <ArmarXGui/libraries/DefaultWidgetDescriptions/DefaultWidgetDescriptions.h>
-namespace armarx
+namespace armarx::WidgetDescription
{
- namespace WidgetDescription
- {
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options);
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet);
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::initializer_list<std::string>& preferredSet)
- {
- return makeStringSelectionComboBox(std::move(name), std::move(options), std::set<std::string> {preferredSet});
- }
+ inline StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::initializer_list<std::string>& preferredSet)
+ {
+ return makeStringSelectionComboBox(std::move(name), std::move(options), std::set<std::string> {preferredSet});
+ }
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred,
- const std::set<std::string>& preferredSet)
- {
- return makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet, mostPreferred);
- }
+ inline StringComboBoxPtr makeStringSelectionComboBox(
+ std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred,
+ const std::set<std::string>& preferredSet)
+ {
+ return makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet, mostPreferred);
+ }
- inline StringComboBoxPtr makeRNSComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNodeSet",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
- {
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
- }
+ inline StringComboBoxPtr makeRNSComboBox(
+ const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNodeSet",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
+ {
+ return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
+ }
- inline StringComboBoxPtr makeRobotNodeComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNode",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
- {
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
- }
+ inline StringComboBoxPtr makeRobotNodeComboBox(
+ const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNode",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
+ {
+ return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index ce8f870d7..2ba5dd359 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -26,10 +26,6 @@
namespace armarx
{
-
- namespace DeviceTags
- {
- }
class DeviceBase
{
public:
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
index 2fae772fc..bf0b2bf34 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
@@ -24,15 +24,16 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-using namespace armarx;
-
-ControlDevice& JointController::getParent() const
+namespace armarx
{
- ARMARX_CHECK_EXPRESSION_W_HINT(parent, "This JointController is not owned by a ControlDevice");
- return *parent;
-}
+ ControlDevice& JointController::getParent() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(parent, "This JointController is not owned by a ControlDevice");
+ return *parent;
+ }
-StringVariantBaseMap JointController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-{
- return StringVariantBaseMap {};
+ StringVariantBaseMap JointController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+ {
+ return StringVariantBaseMap {};
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index e42356516..3bd188efe 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -24,60 +24,61 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-using namespace armarx;
-
-NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnitPtr prov,
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
- cfg, const VirtualRobot::RobotPtr&) :
- maxCommandDelay(IceUtil::Time::milliSeconds(500))
+namespace armarx
{
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity);
-
- initialSettings.velocityX = cfg->initialVelocityX;
- initialSettings.velocityY = cfg->initialVelocityY;
- initialSettings.velocityRotation = cfg->initialVelocityRotation;
- reinitTripleBuffer(initialSettings);
-}
+ NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
+ RobotUnitPtr prov,
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
+ cfg, const VirtualRobot::RobotPtr&) :
+ maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ {
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity);
-void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
-{
- auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+ initialSettings.velocityX = cfg->initialVelocityX;
+ initialSettings.velocityY = cfg->initialVelocityY;
+ initialSettings.velocityRotation = cfg->initialVelocityRotation;
+ reinitTripleBuffer(initialSettings);
+ }
- if (commandAge > maxCommandDelay && // command must be recent
- (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f || rtGetControlStruct().velocityRotation != 0.0f)) // only throw error if any command is not zero
+ void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
{
- throw LocalException("Platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f || rtGetControlStruct().velocityRotation != 0.0f)) // only throw error if any command is not zero
+ {
+ throw LocalException("Platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ }
+ else
+ {
+ target->velocityX = rtGetControlStruct().velocityX;
+ target->velocityY = rtGetControlStruct().velocityY;
+ target->velocityRotation = rtGetControlStruct().velocityRotation;
+ }
}
- else
+
+ void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
+ float velocityRotation)
{
- target->velocityX = rtGetControlStruct().velocityX;
- target->velocityY = rtGetControlStruct().velocityY;
- target->velocityRotation = rtGetControlStruct().velocityRotation;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
+ getWriterControlStruct().velocityRotation = velocityRotation;
+ getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
+ writeControlStruct();
}
-}
-void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
- float velocityRotation)
-{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
- getWriterControlStruct().velocityRotation = velocityRotation;
- getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
- writeControlStruct();
-}
+ IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
+ {
+ return maxCommandDelay;
+ }
-IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
-{
- return maxCommandDelay;
-}
+ void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
+ {
+ maxCommandDelay = value;
+ }
-void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
-{
- maxCommandDelay = value;
+ NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
+ registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");
}
-
-NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
-registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 9dac25286..1da488120 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -21,55 +21,56 @@
*/
#include "NJointKinematicUnitPassThroughController.h"
-using namespace armarx;
-
-NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
-
-NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
- RobotUnitPtr prov,
- const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&)
+namespace armarx
{
- const SensorValueBase* sv = useSensorValue(cfg->deviceName);
-
- std::string controlMode = cfg->controlMode;
- std::string deviceName = cfg->deviceName;
+ NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
- /*if (deviceName == "ArmL5_Elb1" && controlMode == ControlModes::Velocity1DoF)
+ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
+ RobotUnitPtr prov,
+ const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&)
{
- ARMARX_IMPORTANT << "overriding velocity mode for " << deviceName;
- controlMode = ControlModes::VelocityTorque;
- }*/
+ const SensorValueBase* sv = useSensorValue(cfg->deviceName);
- ControlTargetBase* ct = useControlTarget(cfg->deviceName, controlMode);
- //get sensor
- if (ct->isA<ControlTarget1DoFActuatorPosition>())
- {
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
- sensor = &(sv->asA<SensorValue1DoFActuatorPosition>()->position);
- ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
- target = &(ct->asA<ControlTarget1DoFActuatorPosition>()->position);
- sensorToControlOnActivateFactor = 1;
- }
- else if (ct->isA<ControlTarget1DoFActuatorVelocity>())
- {
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
- sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
- ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorVelocity>());
- target = &(ct->asA<ControlTarget1DoFActuatorVelocity>()->velocity);
- sensorToControlOnActivateFactor = 1;
- resetZeroThreshold = 0.1f; // TODO: way to big value?!
- }
- else if (ct->isA<ControlTarget1DoFActuatorTorque>())
- {
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorTorque>());
- sensor = &(sv->asA<SensorValue1DoFActuatorTorque>()->torque);
- ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorTorque>());
- target = &(ct->asA<ControlTarget1DoFActuatorTorque>()->torque);
- sensorToControlOnActivateFactor = 0;
- }
- else
- {
- throw InvalidArgumentException {"Unsupported control mode: " + controlMode};
+ std::string controlMode = cfg->controlMode;
+ std::string deviceName = cfg->deviceName;
+
+ /*if (deviceName == "ArmL5_Elb1" && controlMode == ControlModes::Velocity1DoF)
+ {
+ ARMARX_IMPORTANT << "overriding velocity mode for " << deviceName;
+ controlMode = ControlModes::VelocityTorque;
+ }*/
+
+ ControlTargetBase* ct = useControlTarget(cfg->deviceName, controlMode);
+ //get sensor
+ if (ct->isA<ControlTarget1DoFActuatorPosition>())
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorPosition>()->position);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
+ target = &(ct->asA<ControlTarget1DoFActuatorPosition>()->position);
+ sensorToControlOnActivateFactor = 1;
+ }
+ else if (ct->isA<ControlTarget1DoFActuatorVelocity>())
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ target = &(ct->asA<ControlTarget1DoFActuatorVelocity>()->velocity);
+ sensorToControlOnActivateFactor = 1;
+ resetZeroThreshold = 0.1f; // TODO: way to big value?!
+ }
+ else if (ct->isA<ControlTarget1DoFActuatorTorque>())
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorTorque>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorTorque>()->torque);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorTorque>());
+ target = &(ct->asA<ControlTarget1DoFActuatorTorque>()->torque);
+ sensorToControlOnActivateFactor = 0;
+ }
+ else
+ {
+ throw InvalidArgumentException {"Unsupported control mode: " + controlMode};
+ }
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h b/source/RobotAPI/components/units/RobotUnit/util.h
index 091333dbb..ef3e5c326 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.h
+++ b/source/RobotAPI/components/units/RobotUnit/util.h
@@ -31,3 +31,5 @@
#include "util/introspection/ClassMemberInfo.h"
#include <type_traits>
+
+namespace armarx::DeviceTags {}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
index 16b168012..706f9396e 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
@@ -25,162 +25,159 @@
#include <cmath>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <iostream>
-namespace armarx
+
+namespace armarx::ctrlutil
{
- namespace ctrlutil
+ double s(double t, double s0, double v0, double a0, double j)
{
- double s(double t, double s0, double v0, double a0, double j)
- {
- return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
- }
- double v(double t, double v0, double a0, double j)
- {
- return v0 + a0 * t + 0.5 * j * t * t;
- }
- double a(double t, double a0, double j)
- {
- return a0 + j * t;
- }
+ return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
+ }
+ double v(double t, double v0, double a0, double j)
+ {
+ return v0 + a0 * t + 0.5 * j * t * t;
+ }
+ double a(double t, double a0, double j)
+ {
+ return a0 + j * t;
+ }
- double t_to_v(double v, double a, double j)
- {
- // time to accelerate to v with jerk j starting at acceleration a0
- //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
- // 0 = (-v + a0*t) /(0.5*j) + t*t
- // 0 = -2v/j + 2a0t/j + t*t
- // --> p = 2a0/j; q = -2v/j
- // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
- double tmp = a / j;
- return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
- }
+ double t_to_v(double v, double a, double j)
+ {
+ // time to accelerate to v with jerk j starting at acceleration a0
+ //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
+ // 0 = (-v + a0*t) /(0.5*j) + t*t
+ // 0 = -2v/j + 2a0t/j + t*t
+ // --> p = 2a0/j; q = -2v/j
+ // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
+ double tmp = a / j;
+ return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
+ }
- double t_to_v_with_wedge_acc(double v, double a0, double j)
- {
- // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
- return 2 * t_to_v(v / 2.0, a0, j);
- }
+ double t_to_v_with_wedge_acc(double v, double a0, double j)
+ {
+ // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
+ return 2 * t_to_v(v / 2.0, a0, j);
+ }
- struct BrakingData
- {
- double s_total, v1, v2, v3, dt1, dt2, dt3;
- };
+ struct BrakingData
+ {
+ double s_total, v1, v2, v3, dt1, dt2, dt3;
+ };
- double brakingDistance(double v0, double a0, double j)
- {
- auto signV = math::MathUtils::Sign(v0);
- auto t = t_to_v(v0, -signV * a0, signV * j);
- return s(t, 0, v0, a0, -signV * j);
- }
+ double brakingDistance(double v0, double a0, double j)
+ {
+ auto signV = math::MathUtils::Sign(v0);
+ auto t = t_to_v(v0, -signV * a0, signV * j);
+ return s(t, 0, v0, a0, -signV * j);
+ }
- BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
+ BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
+ {
+ double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
+ dec_max *= -d;
+ // std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
+ double dt1 = std::abs((dec_max - a0) / (-j * d));
+ // double dt1 = t_to_v(v_max-v0, a0, j);
+ // double a1 = a(dt1, a0, -d * j);
+ double v1 = v(dt1, v0, a0, -d * j);
+ double acc_duration = std::abs(dec_max) / j;
+ double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
+ v2 = math::MathUtils::LimitTo(v2, v_max);
+ // v2 = v1 + dv2
+ double dt2 = d * ((v1 - v2) / dec_max);
+ // double a2 = a(dt2, a1, 0);
+
+ double dt3 = t_to_v(std::abs(v2), 0, j);
+ double s1 = s(dt1, 0, v0, a0, d * j);
+ double s2 = s(dt2, 0, v1, dec_max, 0);
+ double s3 = s(dt3, 0, v2, dec_max, d * j);
+ double v3 = v(dt3, v2, dec_max, d * j);
+ double s_total = s1 + s2 + s3;
+
+ if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
{
- double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
+ double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
+ double delta_a = a(excess_time / 2, 0, d * j);
+ a_max -= d * delta_a;
+ dec_max = std::abs(dec_max) - std::abs(delta_a);
dec_max *= -d;
- // std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
- double dt1 = std::abs((dec_max - a0) / (-j * d));
- // double dt1 = t_to_v(v_max-v0, a0, j);
- // double a1 = a(dt1, a0, -d * j);
- double v1 = v(dt1, v0, a0, -d * j);
- double acc_duration = std::abs(dec_max) / j;
- double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
- v2 = math::MathUtils::LimitTo(v2, v_max);
- // v2 = v1 + dv2
- double dt2 = d * ((v1 - v2) / dec_max);
- // double a2 = a(dt2, a1, 0);
-
- double dt3 = t_to_v(std::abs(v2), 0, j);
- double s1 = s(dt1, 0, v0, a0, d * j);
- double s2 = s(dt2, 0, v1, dec_max, 0);
- double s3 = s(dt3, 0, v2, dec_max, d * j);
- double v3 = v(dt3, v2, dec_max, d * j);
- double s_total = s1 + s2 + s3;
-
- if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
- {
- double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
- double delta_a = a(excess_time / 2, 0, d * j);
- a_max -= d * delta_a;
- dec_max = std::abs(dec_max) - std::abs(delta_a);
- dec_max *= -d;
- return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
- }
-
- return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
- // return (s_total, v1, v2, v3, dt1, dt2, dt3);
+ return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
}
- struct WedgeBrakingData
- {
- double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
- };
- bool isPossibleToBrake(double v0, double a0, double j)
- {
- return j * v0 - a0 * a0 / 2 > 0.0f;
- }
+ return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
+ // return (s_total, v1, v2, v3, dt1, dt2, dt3);
+ }
- double jerk(double t, double s0, double v0, double a0)
- {
- return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
- }
+ struct WedgeBrakingData
+ {
+ double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
+ };
+ bool isPossibleToBrake(double v0, double a0, double j)
+ {
+ return j * v0 - a0 * a0 / 2 > 0.0f;
+ }
- std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
- {
- s0 *= -1;
- double t = - (3 * s0) / v0;
- double a0 = - (2 * v0) / t;
- double j = 2 * v0 / (t * t);
- return std::make_tuple(t, a0, j);
- }
+ double jerk(double t, double s0, double v0, double a0)
+ {
+ return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
+ }
+
+ std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
+ {
+ s0 *= -1;
+ double t = - (3 * s0) / v0;
+ double a0 = - (2 * v0) / t;
+ double j = 2 * v0 / (t * t);
+ return std::make_tuple(t, a0, j);
+ }
- WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
+ WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
+ {
+ // # v0 = v1 + v2
+ // # v1 = t1 * a0 + 0.5*j*t1**2
+ // # v2 = 0.5*j*t2**2
+ // # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
+ // # t2 = (a0 - t1 * j) / j
+ // # t2 = a0/j - t1
+ // # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
+ // # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
+ // # t1 = t1_2
+ double d = math::MathUtils::Sign(v0);
+ v0 *= d;
+ a0 *= d;
+ // j *= d;
+ double part = j * v0 - a0 * a0 / 2.0;
+ if (part < 0)
{
- // # v0 = v1 + v2
- // # v1 = t1 * a0 + 0.5*j*t1**2
- // # v2 = 0.5*j*t2**2
- // # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
- // # t2 = (a0 - t1 * j) / j
- // # t2 = a0/j - t1
- // # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
- // # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
- // # t1 = t1_2
- double d = math::MathUtils::Sign(v0);
- v0 *= d;
- a0 *= d;
- // j *= d;
- double part = j * v0 - a0 * a0 / 2.0;
- if (part < 0)
- {
- WedgeBrakingData r;
- r.s_total = -1;
- r.dt2 = -1;
- return r;
- throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
- " a0: " + std::to_string(a0) +
- " v0: " + std::to_string(v0));
- }
- double t1 = std::sqrt(part) / j;
- double t2 = (a0 / j) + t1;
- double v1 = v(t1, v0, a0, -j);
- // double dv1 = v(t1, 0, a0, -j);
- // double diff_v1 = v0 - v1;
- double a1 = a(t1, -a0, -j);
- // double a1_2 = a(t2, 0, j);
- double v2 = v(t2, v1, a1, j);
- // double dv2 = v(t2, 0, 0, j);
- // double v_sum = abs(dv1)+dv2;
- double a2 = a(t2, a1, j);
- // assert(abs(v_sum - v0) < 0.001);
- // assert(abs(v2) < 0.0001);
- double s1 = s(t1, 0, v0, a0, -j);
- double s2 = s(t2, 0, v1, a1, j);
- double s_total = s1 + s2;
- return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
+ WedgeBrakingData r;
+ r.s_total = -1;
+ r.dt2 = -1;
+ return r;
+ throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
+ " a0: " + std::to_string(a0) +
+ " v0: " + std::to_string(v0));
}
+ double t1 = std::sqrt(part) / j;
+ double t2 = (a0 / j) + t1;
+ double v1 = v(t1, v0, a0, -j);
+ // double dv1 = v(t1, 0, a0, -j);
+ // double diff_v1 = v0 - v1;
+ double a1 = a(t1, -a0, -j);
+ // double a1_2 = a(t2, 0, j);
+ double v2 = v(t2, v1, a1, j);
+ // double dv2 = v(t2, 0, 0, j);
+ // double v_sum = abs(dv1)+dv2;
+ double a2 = a(t2, a1, j);
+ // assert(abs(v_sum - v0) < 0.001);
+ // assert(abs(v2) < 0.0001);
+ double s1 = s(t1, 0, v0, a0, -j);
+ double s2 = s(t2, 0, v1, a1, j);
+ double s_total = s1 + s2;
+ return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
}
-
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
index 4212b3ae5..de313e9ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -27,13 +27,14 @@
#include <Eigen/Core>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+namespace armarx::rtfilters
+{
+ class RTFilterBase;
+ using RTFilterBasePtr = std::shared_ptr<RTFilterBase>;
+}
+
namespace armarx
{
- namespace rtfilters
- {
- class RTFilterBase;
- using RTFilterBasePtr = std::shared_ptr<RTFilterBase>;
- }
class RobotUnit;
class DynamicsHelper
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index 860e627af..00800f190 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -25,182 +25,177 @@
#include "ClassMemberInfoEntry.h"
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ /**
+ * @brief
+ */
+ template<class CommonBaseT, class ClassT>
+ struct ClassMemberInfo
{
- /**
- * @brief
- */
- template<class CommonBaseT, class ClassT>
- struct ClassMemberInfo
- {
- using ClassType = ClassT;
- using CommonBaseType = CommonBaseT;
- static_assert(std::is_base_of<CommonBaseType, ClassType>::value, "The class has to inherit the common base class");
- using Entry = introspection::ClassMemberInfoEntry<CommonBaseType>;
- template<class T>
- using EntryConfigurator = introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
-
- static const ClassMemberInfo<CommonBaseT, ClassT>& GetInstance();
-
- /// @brief add a member variable of the current class
- template<class MemberType>
- EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::* ptr, const std::string& name);
-
- /// @brief add all variables of a base class of the current class
- template<class BaseClassType>
- void addBaseClass();
-
- /// @brief Get the name of the current class
- static const std::string& GetClassName();
- /// @brief Get all entries for member variables
- static const KeyValueVector<std::string, Entry>& GetEntries();
- static std::size_t GetNumberOfDataFields();
- static std::string GetDataFieldAsString(const ClassType* ptr, std::size_t i);
- static std::vector<std::string> GetDataFieldNames();
-
- static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
-
- private:
- KeyValueVector<std::string, Entry> entries;
- std::set<std::string> addedBases;
- };
- }
+ using ClassType = ClassT;
+ using CommonBaseType = CommonBaseT;
+ static_assert(std::is_base_of<CommonBaseType, ClassType>::value, "The class has to inherit the common base class");
+ using Entry = introspection::ClassMemberInfoEntry<CommonBaseType>;
+ template<class T>
+ using EntryConfigurator = introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
+
+ static const ClassMemberInfo<CommonBaseT, ClassT>& GetInstance();
+
+ /// @brief add a member variable of the current class
+ template<class MemberType>
+ EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::* ptr, const std::string& name);
+
+ /// @brief add all variables of a base class of the current class
+ template<class BaseClassType>
+ void addBaseClass();
+
+ /// @brief Get the name of the current class
+ static const std::string& GetClassName();
+ /// @brief Get all entries for member variables
+ static const KeyValueVector<std::string, Entry>& GetEntries();
+ static std::size_t GetNumberOfDataFields();
+ static std::string GetDataFieldAsString(const ClassType* ptr, std::size_t i);
+ static std::vector<std::string> GetDataFieldNames();
+
+ static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
+
+ private:
+ KeyValueVector<std::string, Entry> entries;
+ std::set<std::string> addedBases;
+ };
}
+
//implementation
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ template<class CommonBaseT, class ClassT>
+ template<class MemberType>
+ ClassMemberInfo<CommonBaseT, ClassT>::EntryConfigurator<ClassT> ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::* ptr, const std::string& name)
{
- template<class CommonBaseT, class ClassT>
- template<class MemberType>
- ClassMemberInfo<CommonBaseT, ClassT>::EntryConfigurator<ClassT> ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::* ptr, const std::string& name)
+ ARMARX_CHECK_EQUAL(0, entries.count(name));
+ entries.add(name, Entry(name, ptr));
+ return entries.at(name);
+ }
+
+ template<class CommonBaseT, class ClassT>
+ template<class BaseClassType>
+ void ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
+ {
+ if (std::is_same<BaseClassType, CommonBaseType>::value)
{
- ARMARX_CHECK_EQUAL(0, entries.count(name));
- entries.add(name, Entry(name, ptr));
- return entries.at(name);
+ return;
}
+ static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value, "The base class has to inherit the common base class");
+ static_assert(std::is_base_of<BaseClassType, ClassType>::value, "The base class has to be a base class");
+ static_assert(!std::is_same<BaseClassType, ClassType>::value, "The base class has must not be the class");
- template<class CommonBaseT, class ClassT>
- template<class BaseClassType>
- void ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
+ std::set<std::string> basesAddedInCall;
+ if (addedBases.count(ClassMemberInfo<CommonBaseType, BaseClassType>::GetClassName()))
{
- if (std::is_same<BaseClassType, CommonBaseType>::value)
- {
- return;
- }
- static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value, "The base class has to inherit the common base class");
- static_assert(std::is_base_of<BaseClassType, ClassType>::value, "The base class has to be a base class");
- static_assert(!std::is_same<BaseClassType, ClassType>::value, "The base class has must not be the class");
-
- std::set<std::string> basesAddedInCall;
- if (addedBases.count(ClassMemberInfo<CommonBaseType, BaseClassType>::GetClassName()))
- {
- return;
- }
- for (const Entry& e : ClassMemberInfo<CommonBaseType, BaseClassType>::GetEntries().values())
- {
- if (!addedBases.count(e.getClassName()))
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(
- !entries.count(e.getMemberName()),
- "Adding the base class '" << GetTypeString<BaseClassType>()
- << "' adds an entry for the member '" << e.getMemberName()
- << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'");
- basesAddedInCall.emplace(e.getClassName());
- entries.add(e.getMemberName(), e);
- }
- }
- addedBases.insert(basesAddedInCall.begin(), basesAddedInCall.end());
+ return;
}
-
- template<class CommonBaseT, class ClassT>
- std::vector<std::string> ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
+ for (const Entry& e : ClassMemberInfo<CommonBaseType, BaseClassType>::GetEntries().values())
{
- std::vector<std::string> dataFieldNames;
- dataFieldNames.reserve(GetNumberOfDataFields());
- for (const Entry& e : GetEntries().values())
+ if (!addedBases.count(e.getClassName()))
{
- for (std::size_t i = 0; i < e.getNumberOfFields(); ++i)
- {
- dataFieldNames.emplace_back(e.getFieldName(i));
- }
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ !entries.count(e.getMemberName()),
+ "Adding the base class '" << GetTypeString<BaseClassType>()
+ << "' adds an entry for the member '" << e.getMemberName()
+ << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'");
+ basesAddedInCall.emplace(e.getClassName());
+ entries.add(e.getMemberName(), e);
}
- ARMARX_CHECK_EQUAL(GetNumberOfDataFields(), dataFieldNames.size());
- return dataFieldNames;
}
+ addedBases.insert(basesAddedInCall.begin(), basesAddedInCall.end());
+ }
- template<class CommonBaseT, class ClassT>
- std::map<std::string, VariantBasePtr> ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr)
+ template<class CommonBaseT, class ClassT>
+ std::vector<std::string> ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
+ {
+ std::vector<std::string> dataFieldNames;
+ dataFieldNames.reserve(GetNumberOfDataFields());
+ for (const Entry& e : GetEntries().values())
{
- std::map<std::string, VariantBasePtr> result;
- for (const auto& e : GetEntries().values())
+ for (std::size_t i = 0; i < e.getNumberOfFields(); ++i)
{
- mergeMaps(result, e.toVariants(timestamp, ptr), MergeMapsMode::OverrideNoValues);
+ dataFieldNames.emplace_back(e.getFieldName(i));
}
- return result;
}
+ ARMARX_CHECK_EQUAL(GetNumberOfDataFields(), dataFieldNames.size());
+ return dataFieldNames;
+ }
- template<class CommonBaseT, class ClassT>
- const ClassMemberInfo<CommonBaseT, ClassT>& ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
+ template<class CommonBaseT, class ClassT>
+ std::map<std::string, VariantBasePtr> ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr)
+ {
+ std::map<std::string, VariantBasePtr> result;
+ for (const auto& e : GetEntries().values())
{
- static const ClassMemberInfo<CommonBaseT, ClassT> i = ClassT::GetClassMemberInfo();
- return i;
+ mergeMaps(result, e.toVariants(timestamp, ptr), MergeMapsMode::OverrideNoValues);
}
+ return result;
+ }
- template<class CommonBaseT, class ClassT>
- const std::string& ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
- {
- return GetTypeString<ClassType>();
- }
+ template<class CommonBaseT, class ClassT>
+ const ClassMemberInfo<CommonBaseT, ClassT>& ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
+ {
+ static const ClassMemberInfo<CommonBaseT, ClassT> i = ClassT::GetClassMemberInfo();
+ return i;
+ }
- template<class CommonBaseT, class ClassT>
- const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>& ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
- {
- return GetInstance().entries;
- }
+ template<class CommonBaseT, class ClassT>
+ const std::string& ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
+ {
+ return GetTypeString<ClassType>();
+ }
- template<class CommonBaseT, class ClassT>
- std::size_t ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
+ template<class CommonBaseT, class ClassT>
+ const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>& ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
+ {
+ return GetInstance().entries;
+ }
+
+ template<class CommonBaseT, class ClassT>
+ std::size_t ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
+ {
+ static const std::size_t numberOfDataFields = []
{
- static const std::size_t numberOfDataFields = []
+ std::size_t n = 0;
+ for (const Entry& e : GetEntries().values())
{
- std::size_t n = 0;
- for (const Entry& e : GetEntries().values())
- {
- n += e.getNumberOfFields();
- }
- return n;
- }();
- return numberOfDataFields;
- }
+ n += e.getNumberOfFields();
+ }
+ return n;
+ }();
+ return numberOfDataFields;
+ }
- template<class CommonBaseT, class ClassT>
- std::string ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAsString(const ClassType* ptr, std::size_t i)
+ template<class CommonBaseT, class ClassT>
+ std::string ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAsString(const ClassType* ptr, std::size_t i)
+ {
+ static const auto toString = []
{
- static const auto toString = []
- {
- std::vector<std::function<std::string(const ClassType*)>> functions;
+ std::vector<std::function<std::string(const ClassType*)>> functions;
- for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
+ for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
+ {
+ for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
{
- for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ functions.emplace_back(
+ [idxEntr, idxField](const ClassType * classptr)
{
- functions.emplace_back(
- [idxEntr, idxField](const ClassType * classptr)
- {
- const Entry& e = GetEntries().at(idxEntr);
- return e.getFieldAsString(idxField, classptr);
- });
- }
+ const Entry& e = GetEntries().at(idxEntr);
+ return e.getFieldAsString(idxField, classptr);
+ });
}
- ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
- return functions;
- }();
- ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
- return toString.at(i)(ptr);
- }
+ }
+ ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
+ return functions;
+ }();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ return toString.at(i)(ptr);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 9c1de849e..1f503de05 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -22,216 +22,213 @@
#pragma once
#include "DataFieldsInfo.h"
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
- {
- template<class CommonBaseT, class ClassT>
- struct ClassMemberInfo;
- template<class CommonBaseT, class ClassT>
- struct ClassMemberInfoEntryConfigurator;
-
- template<class CommonBaseT>
- struct ClassMemberInfoEntry
- {
- public:
- using CommonBaseType = CommonBaseT;
-
- ClassMemberInfoEntry(ClassMemberInfoEntry&&) = default;
- ClassMemberInfoEntry(const ClassMemberInfoEntry&) = default;
- ClassMemberInfoEntry& operator=(ClassMemberInfoEntry&&) = default;
- ClassMemberInfoEntry& operator=(const ClassMemberInfoEntry&) = default;
-
- template<class ClassType, class MemberType>
- ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::* ptr):
- className {GetTypeString<ClassType>()},
- memberName {memberName},
- memberTypeName {GetTypeString<MemberType>()},
- numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
- fieldToString {MakeFieldToStringFunctors<ClassType>(numberOfFields, ptr)},
- toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
+ template<class CommonBaseT, class ClassT>
+ struct ClassMemberInfo;
+ template<class CommonBaseT, class ClassT>
+ struct ClassMemberInfoEntryConfigurator;
+
+ template<class CommonBaseT>
+ struct ClassMemberInfoEntry
+ {
+ public:
+ using CommonBaseType = CommonBaseT;
+
+ ClassMemberInfoEntry(ClassMemberInfoEntry&&) = default;
+ ClassMemberInfoEntry(const ClassMemberInfoEntry&) = default;
+ ClassMemberInfoEntry& operator=(ClassMemberInfoEntry&&) = default;
+ ClassMemberInfoEntry& operator=(const ClassMemberInfoEntry&) = default;
+
+ template<class ClassType, class MemberType>
+ ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::* ptr):
+ className {GetTypeString<ClassType>()},
+ memberName {memberName},
+ memberTypeName {GetTypeString<MemberType>()},
+ numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
+ fieldToString {MakeFieldToStringFunctors<ClassType>(numberOfFields, ptr)},
+ toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
+ {
+ ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
+ //field names
{
- ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
- //field names
+ fieldNames.reserve(numberOfFields);
+ if (numberOfFields == 1)
{
- fieldNames.reserve(numberOfFields);
- if (numberOfFields == 1)
- {
- fieldNames.emplace_back(memberName);
- }
- else
- {
- for (auto& name : DataFieldsInfo<MemberType>::GetFieldNames())
- {
- fieldNames.emplace_back(memberName + "." + std::move(name));
- }
- }
+ fieldNames.emplace_back(memberName);
}
- }
-
- //general
- const std::string& getClassName() const
- {
- return className;
- }
- const std::string& getMemberName() const
- {
- return memberName;
- }
- const std::string& getMemberTypeName() const
- {
- return memberTypeName;
- }
- //fields
- std::size_t getNumberOfFields() const
- {
- return numberOfFields;
- }
- const std::string& getFieldName(std::size_t i) const
- {
- ARMARX_CHECK_LESS(i, numberOfFields);
- return fieldNames.at(i);
- }
- std::string getFieldAsString(std::size_t i, const CommonBaseType* ptr) const
- {
- ARMARX_CHECK_NOT_NULL(ptr);
- ARMARX_CHECK_LESS(i, numberOfFields);
- return fieldToString.at(i)(ptr);
- }
-
- //variants
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
- {
- return toVariants_(this, timestamp, ptr);
- }
-
- private:
- using ToVariantsFunctorType = std::function <
- std::map<std::string, VariantBasePtr>(
- const ClassMemberInfoEntry* thisptr,
- const IceUtil::Time&,
- const CommonBaseType*) >;
- using FieldToStringFunctorType = std::function<std::string(const CommonBaseType*)>;
-
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static std::vector<FieldToStringFunctorType> MakeFieldToStringFunctors(
- std::size_t numberOfFields,
- MemberType MemberPtrClassType::* ptr)
- {
- std::vector<std::function<std::string(const CommonBaseType*)>> result;
- result.reserve(numberOfFields);
- for (std::size_t i = 0; i < numberOfFields; ++i)
+ else
{
- result.emplace_back(
- [i, ptr](const CommonBaseType * ptrBase)
+ for (auto& name : DataFieldsInfo<MemberType>::GetFieldNames())
{
- const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
- ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::GetFieldAsString(i, cptr->*ptr);
+ fieldNames.emplace_back(memberName + "." + std::move(name));
}
- );
}
- return result;
}
+ }
+
+ //general
+ const std::string& getClassName() const
+ {
+ return className;
+ }
+ const std::string& getMemberName() const
+ {
+ return memberName;
+ }
+ const std::string& getMemberTypeName() const
+ {
+ return memberTypeName;
+ }
+ //fields
+ std::size_t getNumberOfFields() const
+ {
+ return numberOfFields;
+ }
+ const std::string& getFieldName(std::size_t i) const
+ {
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldNames.at(i);
+ }
+ std::string getFieldAsString(std::size_t i, const CommonBaseType* ptr) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToString.at(i)(ptr);
+ }
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static ToVariantsFunctorType MakeToVariantsFunctor(MemberType MemberPtrClassType::* ptr)
+ //variants
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
+ {
+ return toVariants_(this, timestamp, ptr);
+ }
+
+ private:
+ using ToVariantsFunctorType = std::function <
+ std::map<std::string, VariantBasePtr>(
+ const ClassMemberInfoEntry* thisptr,
+ const IceUtil::Time&,
+ const CommonBaseType*) >;
+ using FieldToStringFunctorType = std::function<std::string(const CommonBaseType*)>;
+
+ template<class ClassType, class MemberType, class MemberPtrClassType>
+ static std::vector<FieldToStringFunctorType> MakeFieldToStringFunctors(
+ std::size_t numberOfFields,
+ MemberType MemberPtrClassType::* ptr)
+ {
+ std::vector<std::function<std::string(const CommonBaseType*)>> result;
+ result.reserve(numberOfFields);
+ for (std::size_t i = 0; i < numberOfFields; ++i)
{
- return [ptr](
- const ClassMemberInfoEntry * thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
+ result.emplace_back(
+ [i, ptr](const CommonBaseType * ptrBase)
{
const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::ToVariants(
- cptr->*ptr, thisptr->getMemberName(),
- timestamp,
- thisptr->variantReportFrame(), thisptr->variantReportAgent());
- };
+ return DataFieldsInfo<MemberType>::GetFieldAsString(i, cptr->*ptr);
+ }
+ );
}
+ return result;
+ }
- template<class BaseType, class ClassT>
- friend class ClassMemberInfo;
- template<class BaseType, class ClassT>
- friend class ClassMemberInfoEntryConfigurator;
-
- const std::string className;
- const std::string memberName;
- const std::string memberTypeName;
- //elementar subparts
- const std::size_t numberOfFields;
- const std::vector<FieldToStringFunctorType> fieldToString;
- std::vector<std::string> fieldNames;
- //variants
- ToVariantsFunctorType toVariants_;
- bool setVariantReportFrame_ {false};
- std::function<std::string()> variantReportFrame {[]{return "";}};
- std::function<std::string()> variantReportAgent {[]{return "";}};
- };
-
- template<class CommonBaseT, class ClassT>
- struct ClassMemberInfoEntryConfigurator
- {
- using ClassType = ClassT;
- using CommonBaseType = CommonBaseT;
- using Entry = ClassMemberInfoEntry<CommonBaseType>;
-
- ClassMemberInfoEntryConfigurator& setFieldNames(std::vector<std::string> fieldNames)
- {
- ARMARX_CHECK_EQUAL(fieldNames.size(), e.numberOfFields);
- e.fieldNames = std::move(fieldNames);
- return *this;
- }
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, const std::string& frame)
- {
- e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
- e.variantReportAgent = [agent] {return agent;};
- return *this;
- }
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
- {
- e.setVariantReportFrame_ = true;
- e.variantReportFrame = std::move(frame);
- e.variantReportAgent = [agent] {return agent;};
- return *this;
- }
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
- {
- e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
- e.variantReportAgent = std::move(agent);
- return *this;
- }
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, std::function<std::string()> frame)
- {
- e.setVariantReportFrame_ = true;
- e.variantReportFrame = std::move(frame);
- e.variantReportAgent = std::move(agent);
- return *this;
- }
+ template<class ClassType, class MemberType, class MemberPtrClassType>
+ static ToVariantsFunctorType MakeToVariantsFunctor(MemberType MemberPtrClassType::* ptr)
+ {
+ return [ptr](
+ const ClassMemberInfoEntry * thisptr,
+ const IceUtil::Time & timestamp,
+ const CommonBaseType * ptrBase)
+ {
+ const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
+ ARMARX_CHECK_NOT_NULL(cptr);
+ return DataFieldsInfo<MemberType>::ToVariants(
+ cptr->*ptr, thisptr->getMemberName(),
+ timestamp,
+ thisptr->variantReportFrame(), thisptr->variantReportAgent());
+ };
+ }
+
+ template<class BaseType, class ClassT>
+ friend class ClassMemberInfo;
+ template<class BaseType, class ClassT>
+ friend class ClassMemberInfoEntryConfigurator;
+
+ const std::string className;
+ const std::string memberName;
+ const std::string memberTypeName;
+ //elementar subparts
+ const std::size_t numberOfFields;
+ const std::vector<FieldToStringFunctorType> fieldToString;
+ std::vector<std::string> fieldNames;
+ //variants
+ ToVariantsFunctorType toVariants_;
+ bool setVariantReportFrame_ {false};
+ std::function<std::string()> variantReportFrame {[]{return "";}};
+ std::function<std::string()> variantReportAgent {[]{return "";}};
+ };
+
+ template<class CommonBaseT, class ClassT>
+ struct ClassMemberInfoEntryConfigurator
+ {
+ using ClassType = ClassT;
+ using CommonBaseType = CommonBaseT;
+ using Entry = ClassMemberInfoEntry<CommonBaseType>;
- ClassMemberInfoEntryConfigurator& setVariantReportFunction(
- std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&, const ClassType*)> f)
+ ClassMemberInfoEntryConfigurator& setFieldNames(std::vector<std::string> fieldNames)
+ {
+ ARMARX_CHECK_EQUAL(fieldNames.size(), e.numberOfFields);
+ e.fieldNames = std::move(fieldNames);
+ return *this;
+ }
+ ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, const std::string& frame)
+ {
+ e.setVariantReportFrame_ = true;
+ e.variantReportFrame = [frame] {return frame;};
+ e.variantReportAgent = [agent] {return agent;};
+ return *this;
+ }
+ ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
+ {
+ e.setVariantReportFrame_ = true;
+ e.variantReportFrame = std::move(frame);
+ e.variantReportAgent = [agent] {return agent;};
+ return *this;
+ }
+ ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
+ {
+ e.setVariantReportFrame_ = true;
+ e.variantReportFrame = [frame] {return frame;};
+ e.variantReportAgent = std::move(agent);
+ return *this;
+ }
+ ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, std::function<std::string()> frame)
+ {
+ e.setVariantReportFrame_ = true;
+ e.variantReportFrame = std::move(frame);
+ e.variantReportAgent = std::move(agent);
+ return *this;
+ }
+
+ ClassMemberInfoEntryConfigurator& setVariantReportFunction(
+ std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&, const ClassType*)> f)
+ {
+ e.toVariants_ = [f](
+ const ClassMemberInfoEntry<CommonBaseType>* thisptr,
+ const IceUtil::Time & timestamp,
+ const CommonBaseType * ptrBase)
{
- e.toVariants_ = [f](
- const ClassMemberInfoEntry<CommonBaseType>* thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
- {
- const ClassType* ptr = dynamic_cast<const ClassType*>(ptrBase);
- ARMARX_CHECK_NOT_NULL(ptr);
- return f(timestamp, ptr);
- };
- return *this;
- }
-
- private:
- friend struct ClassMemberInfo<CommonBaseType, ClassType>;
- ClassMemberInfoEntryConfigurator(Entry& e): e(e) {}
- Entry& e;
- };
- }
+ const ClassType* ptr = dynamic_cast<const ClassType*>(ptrBase);
+ ARMARX_CHECK_NOT_NULL(ptr);
+ return f(timestamp, ptr);
+ };
+ return *this;
+ }
+
+ private:
+ friend struct ClassMemberInfo<CommonBaseType, ClassType>;
+ ClassMemberInfoEntryConfigurator(Entry& e): e(e) {}
+ Entry& e;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index 4691dd99d..60f62a097 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -31,39 +31,35 @@
#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ std::string DataFieldsInfo<Eigen::Vector3f, void>::GetFieldAsString(std::size_t i, const Eigen::Vector3f& field)
{
- std::string DataFieldsInfo<Eigen::Vector3f, void>::GetFieldAsString(std::size_t i, const Eigen::Vector3f& field)
- {
- ARMARX_CHECK_LESS(i, 3);
- return to_string(field(i));
- }
+ ARMARX_CHECK_LESS(i, 3);
+ return to_string(field(i));
+ }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
+ const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ if (!frame.empty())
{
- if (!frame.empty())
- {
- return {{name, {new TimedVariant(FramedPosition{value, frame, agent}, timestamp)}}};
- }
- ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
- return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
+ return {{name, {new TimedVariant(FramedPosition{value, frame, agent}, timestamp)}}};
}
+ ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
+ return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
}
}
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
- {
#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
std::string DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field) \
{ \
@@ -95,167 +91,154 @@ namespace armarx
return result; \
}
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, 6)
#undef make_DataFieldsInfo_for_eigen_vector
- }
}
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ std::string DataFieldsInfo<JointStatus, void>::GetFieldAsString(
+ std::size_t i, const JointStatus& field)
{
- std::string DataFieldsInfo<JointStatus, void>::GetFieldAsString(
- std::size_t i, const JointStatus& field)
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
{
- ARMARX_CHECK_LESS(i, 4);
- switch (i)
- {
- case 0:
- return to_string(field.error);
- case 1:
- return to_string(field.operation);
- case 2:
- return to_string(field.enabled);
- case 3:
- return to_string(field.emergencyStop);
- }
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ case 0:
+ return to_string(field.error);
+ case 1:
+ return to_string(field.operation);
+ case 2:
+ return to_string(field.enabled);
+ case 3:
+ return to_string(field.emergencyStop);
}
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
+ const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for JointStatus");
+ static const auto fnames = GetFieldNames();
+ return
{
- ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for JointStatus");
- static const auto fnames = GetFieldNames();
- return
- {
- {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
- {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
- {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
- {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
- };
- }
+ {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
+ {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
+ {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
+ {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
+ };
}
}
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ std::string DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldAsString(
+ std::size_t i,
+ const Eigen::Quaternionf& field)
{
- std::string DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldAsString(
- std::size_t i,
- const Eigen::Quaternionf& field)
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
{
- ARMARX_CHECK_LESS(i, 4);
- switch (i)
- {
- case 0:
- return to_string(field.w());
- case 1:
- return to_string(field.x());
- case 2:
- return to_string(field.y());
- case 3:
- return to_string(field.z());
- }
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ case 0:
+ return to_string(field.w());
+ case 1:
+ return to_string(field.x());
+ case 2:
+ return to_string(field.y());
+ case 3:
+ return to_string(field.z());
}
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
+ const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ if (!frame.empty())
{
- if (!frame.empty())
- {
- return {{name, {new TimedVariant(FramedOrientation{value, frame, agent}, timestamp)}}};
- }
- ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
- return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
+ return {{name, {new TimedVariant(FramedOrientation{value, frame, agent}, timestamp)}}};
}
+ ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
+ return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
}
}
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ std::string DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field)
{
- std::string DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field)
- {
- ARMARX_CHECK_LESS(i, 16);
- return to_string(field(i / 4, i % 4));
- }
+ ARMARX_CHECK_LESS(i, 16);
+ return to_string(field(i / 4, i % 4));
+ }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
+ const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ if (!frame.empty())
{
- if (!frame.empty())
- {
- return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
- }
- ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
- return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
+ return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
}
+ ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given");
+ return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
}
}
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
+ std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
- std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant");
- return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
- }
+ ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant");
+ return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
+ }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant");
- return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
- }
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
+ IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant");
+ return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 15582f2ec..ac9a8cc66 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -27,68 +27,66 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
-namespace armarx
+namespace armarx::introspection
{
- namespace introspection
+ template<class T, class = void> struct DataFieldsInfo;
+ // static std::size_t GetNumberOfFields();
+ // static std::string GetFieldAsString(std::size_t i, T field);
+ // static std::vector<std::string> GetFieldNames();
+ // static std::map<std::string, VariantBasePtr> ToVariants(
+ // T value,
+ // const std::string& name,
+ // const IceUtil::Time& timestamp,
+ // const std::string& frame = "",
+ // const std::string& agent = "")
+
+ template<class T>
+ struct DataFieldsInfo<T, typename std::enable_if<std::is_fundamental<T>::value>::type>
{
- template<class T, class = void> struct DataFieldsInfo;
- // static std::size_t GetNumberOfFields();
- // static std::string GetFieldAsString(std::size_t i, T field);
- // static std::vector<std::string> GetFieldNames();
- // static std::map<std::string, VariantBasePtr> ToVariants(
- // T value,
- // const std::string& name,
- // const IceUtil::Time& timestamp,
- // const std::string& frame = "",
- // const std::string& agent = "")
-
- template<class T>
- struct DataFieldsInfo<T, typename std::enable_if<std::is_fundamental<T>::value>::type>
- {
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, T field)
- {
- ARMARX_CHECK_EQUAL(i, 0);
- return to_string(field);
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"Fundamental types have no field names"};
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- T value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "")
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for fundamental types");
- return {{name, {new TimedVariant(value, timestamp)}}};
- }
- };
+ static std::size_t GetNumberOfFields()
+ {
+ return 1;
+ }
+ static std::string GetFieldAsString(std::size_t i, T field)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ return to_string(field);
+ }
+ static std::vector<std::string> GetFieldNames()
+ {
+ throw std::logic_error {"Fundamental types have no field names"};
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ T value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "")
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for fundamental types");
+ return {{name, {new TimedVariant(value, timestamp)}}};
+ }
+ };
- template<>
- struct DataFieldsInfo<Eigen::Vector3f, void>
+ template<>
+ struct DataFieldsInfo<Eigen::Vector3f, void>
+ {
+ static std::size_t GetNumberOfFields()
{
- static std::size_t GetNumberOfFields()
- {
- return 3;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Vector3f& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"x", "y", "z"};
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
- };
+ return 3;
+ }
+ static std::string GetFieldAsString(std::size_t i, const Eigen::Vector3f& field);
+ static std::vector<std::string> GetFieldNames()
+ {
+ return {"x", "y", "z"};
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
+ };
@@ -110,141 +108,142 @@ namespace armarx
const std::string& agent = ""); \
};
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, 6)
#undef make_DataFieldsInfo_for_eigen_vector
- template<>
- struct DataFieldsInfo<Eigen::Matrix4f, void>
+ template<>
+ struct DataFieldsInfo<Eigen::Matrix4f, void>
+ {
+ static std::size_t GetNumberOfFields()
{
- static std::size_t GetNumberOfFields()
- {
- return 16;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field);
- static std::vector<std::string> GetFieldNames()
- {
- return
- {
- "00", "01", "02", "03",
- "10", "11", "12", "13",
- "20", "21", "22", "23",
- "30", "31", "32", "33"
- };
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
- };
-
- template<>
- struct DataFieldsInfo<Eigen::Quaternionf, void>
- {
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Quaternionf& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"qw", "qx", "qy", "qz"};
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
- };
-
- template<>
- struct DataFieldsInfo<std::chrono::microseconds, void>
- {
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, std::chrono::microseconds field)
- {
- return to_string(field.count());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
- };
- template<>
- struct DataFieldsInfo<IceUtil::Time, void>
- {
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, IceUtil::Time field)
- {
- return to_string(field.toMicroSeconds());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
- static std::map<std::string, VariantBasePtr> ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
- };
- }
+ return 16;
+ }
+ static std::string GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field);
+ static std::vector<std::string> GetFieldNames()
+ {
+ return
+ {
+ "00", "01", "02", "03",
+ "10", "11", "12", "13",
+ "20", "21", "22", "23",
+ "30", "31", "32", "33"
+ };
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
+ };
+
+ template<>
+ struct DataFieldsInfo<Eigen::Quaternionf, void>
+ {
+ static std::size_t GetNumberOfFields()
+ {
+ return 4;
+ }
+ static std::string GetFieldAsString(std::size_t i, const Eigen::Quaternionf& field);
+ static std::vector<std::string> GetFieldNames()
+ {
+ return {"qw", "qx", "qy", "qz"};
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
+ };
+
+ template<>
+ struct DataFieldsInfo<std::chrono::microseconds, void>
+ {
+ static std::size_t GetNumberOfFields()
+ {
+ return 1;
+ }
+ static std::string GetFieldAsString(std::size_t i, std::chrono::microseconds field)
+ {
+ return to_string(field.count());
+ }
+ static std::vector<std::string> GetFieldNames()
+ {
+ throw std::logic_error {"should never be called"};
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
+ };
+ template<>
+ struct DataFieldsInfo<IceUtil::Time, void>
+ {
+ static std::size_t GetNumberOfFields()
+ {
+ return 1;
+ }
+ static std::string GetFieldAsString(std::size_t i, IceUtil::Time field)
+ {
+ return to_string(field.toMicroSeconds());
+ }
+ static std::vector<std::string> GetFieldNames()
+ {
+ throw std::logic_error {"should never be called"};
+ }
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
+ };
}
namespace armarx
{
struct JointStatus;
- namespace introspection
+}
+
+namespace armarx::introspection
+{
+ template<>
+ struct DataFieldsInfo<JointStatus, void>
{
- template<>
- struct DataFieldsInfo<JointStatus, void>
+ static std::size_t GetNumberOfFields()
{
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const JointStatus& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"error", "operation", "enabled", "emergencyStop"};
- }
-
- static std::map<std::string, VariantBasePtr> ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent);
- };
- }
+ return 4;
+ }
+ static std::string GetFieldAsString(std::size_t i, const JointStatus& field);
+ static std::vector<std::string> GetFieldNames()
+ {
+ return {"error", "operation", "enabled", "emergencyStop"};
+ }
+
+ static std::map<std::string, VariantBasePtr> ToVariants(
+ const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent);
+ };
}
+
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
index 231cc0618..67d2a5e4a 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
@@ -14,23 +14,20 @@
#include "XsensMTiModule.h"
-namespace IMU
+namespace IMU::Xsens
{
- namespace Xsens
+ struct XsensMTiFrame
{
- struct XsensMTiFrame
+ XsensMTiFrame() :
+ m_DataLength(0)
{
- XsensMTiFrame() :
- m_DataLength(0)
- {
- memset(m_Data, 0, sizeof(unsigned char) * MAXMSGLEN);
- }
-
- short m_DataLength;
- unsigned char m_Data[MAXMSGLEN ];
- IMUState m_IMUState;
- };
- }
+ memset(m_Data, 0, sizeof(unsigned char) * MAXMSGLEN);
+ }
+
+ short m_DataLength;
+ unsigned char m_Data[MAXMSGLEN ];
+ IMUState m_IMUState;
+ };
}
#endif
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
index 468349d5a..334aa4c34 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
@@ -121,773 +121,815 @@ static char THIS_FILE[] = __FILE__;
#endif
#endif
-namespace IMU
+namespace IMU::Xsens
{
- namespace Xsens
+ //////////////////////////////////////////////////////////////////////
+ // Construction/Destruction
+ //////////////////////////////////////////////////////////////////////
+ //
+ CXsensMTiModule::CXsensMTiModule()
{
- //////////////////////////////////////////////////////////////////////
- // Construction/Destruction
- //////////////////////////////////////////////////////////////////////
- //
- CXsensMTiModule::CXsensMTiModule()
- {
- m_handle = -1;
- m_portOpen = false;
- m_fileOpen = false;
- m_deviceError = 0; // No error
- m_retVal = MTRV_OK;
- m_timeOut = TO_DEFAULT;
- m_nTempBufferLen = 0;
- m_clkEnd = 0;
-
- for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
- {
- m_storedOutputMode[i] = INVALIDSETTINGVALUE;
- m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
- m_storedDataLength[i] = 0;
- }
- }
-
- CXsensMTiModule::~CXsensMTiModule()
+ m_handle = -1;
+ m_portOpen = false;
+ m_fileOpen = false;
+ m_deviceError = 0; // No error
+ m_retVal = MTRV_OK;
+ m_timeOut = TO_DEFAULT;
+ m_nTempBufferLen = 0;
+ m_clkEnd = 0;
+
+ for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
{
- close();
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
}
+ }
- ////////////////////////////////////////////////////////////////////
- // clockms
- //
- // Calculates the processor time used by the calling process.
- // For linux use gettimeofday instead of clock() function
- // When using read from FTDI serial port in a loop the
- // clock() function very often keeps returning 0.
- //
- // Output
- // returns processor time in milliseconds
- //
- clock_t CXsensMTiModule::clockms()
- {
- clock_t clk;
+ CXsensMTiModule::~CXsensMTiModule()
+ {
+ close();
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // clockms
+ //
+ // Calculates the processor time used by the calling process.
+ // For linux use gettimeofday instead of clock() function
+ // When using read from FTDI serial port in a loop the
+ // clock() function very often keeps returning 0.
+ //
+ // Output
+ // returns processor time in milliseconds
+ //
+ clock_t CXsensMTiModule::clockms()
+ {
+ clock_t clk;
#ifdef WIN32
- clk = clock(); // Get current processor time
+ clk = clock(); // Get current processor time
#if (CLOCKS_PER_SEC != 1000)
- clk /= (CLOCKS_PER_SEC / 1000);
- // clk = (clk * 1000) / CLOCKS_PER_SEC;
+ clk /= (CLOCKS_PER_SEC / 1000);
+ // clk = (clk * 1000) / CLOCKS_PER_SEC;
#endif
#else
- struct timeval tv;
- struct timezone tz;
- gettimeofday(&tv, &tz);
- clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
+ struct timeval tv;
+ struct timezone tz;
+ gettimeofday(&tv, &tz);
+ clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
#endif
- return clk;
- }
+ return clk;
+ }
- ////////////////////////////////////////////////////////////////////
- // openPort (serial port number as input parameter)
- //
- // Opens a 'live' connection to a MT or XM
- //
- // Input
- // portNumber : number of serial port to open (1-99)
- // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
- // inqueueSize : size of input queue, default = 4096
- // outqueueSize: size of output queue, default = 1024
- //
- // Output
- // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
- //
- short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
- {
- m_clkEnd = 0;
+ ////////////////////////////////////////////////////////////////////
+ // openPort (serial port number as input parameter)
+ //
+ // Opens a 'live' connection to a MT or XM
+ //
+ // Input
+ // portNumber : number of serial port to open (1-99)
+ // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+ // inqueueSize : size of input queue, default = 4096
+ // outqueueSize: size of output queue, default = 1024
+ //
+ // Output
+ // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+ //
+ short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
- if (m_fileOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
#ifdef WIN32
- char pchFileName[10];
+ char pchFileName[10];
- sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
+ sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
- m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
- // Once here, port is open
- m_portOpen = true;
+ // Once here, port is open
+ m_portOpen = true;
- //Get the current state & then change it
- DCB dcb;
+ //Get the current state & then change it
+ DCB dcb;
- GetCommState(m_handle, &dcb);// Get current state
+ GetCommState(m_handle, &dcb);// Get current state
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
- dcb.fOutxDsrFlow = FALSE;
- dcb.fOutX = FALSE;
- dcb.fInX = FALSE;
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
if (!SetCommState(m_handle, (LPDCB)&dcb))
{
- // Set new state
- // Bluetooth ports cannot always be opened with 2 stopbits
- // Now try to open port with 1 stopbit.
- dcb.StopBits = ONESTOPBIT;
-
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- {
- CloseHandle(m_handle);
- m_handle = INVALID_HANDLE_VALUE;
- m_portOpen = false;
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
}
+ }
- // Set COM timeouts
- COMMTIMEOUTS CommTimeouts;
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
- GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
+ GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
- // immediate return if data is available, wait 1ms otherwise
- CommTimeouts.ReadTotalTimeoutConstant = 1;
- CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
- // immediate return whether data is available or not
- // CommTimeouts.ReadTotalTimeoutConstant = 0;
- // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+ // immediate return whether data is available or not
+ // CommTimeouts.ReadTotalTimeoutConstant = 0;
+ // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
- // Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
- SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
- return (m_retVal = MTRV_OK);
+ return (m_retVal = MTRV_OK);
#else
- char chPort[32];
- struct termios options;
-
- /* Open port */
- sprintf(chPort, "/dev/ttyS%d", (portNumber - 1));
- m_handle = open(chPort, O_RDWR | O_NOCTTY);
-
- // O_RDWR: Read+Write
- // O_NOCTTY: Raw input, no "controlling terminal"
- // O_NDELAY: Don't care about DCD signal
+ char chPort[32];
+ struct termios options;
- if (m_handle < 0)
- {
- // Port not open
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ /* Open port */
+ sprintf(chPort, "/dev/ttyS%d", (portNumber - 1));
+ m_handle = open(chPort, O_RDWR | O_NOCTTY);
- // Once here, port is open
- m_portOpen = true;
-
- /* Start configuring of port for non-canonical transfer mode */
- // Get current options for the port
- tcgetattr(m_handle, &options);
-
- // Set baudrate.
- cfsetispeed(&options, baudrate);
- cfsetospeed(&options, baudrate);
-
- // Enable the receiver and set local mode
- options.c_cflag |= (CLOCAL | CREAD);
- // Set character size to data bits and set no parity Mask the characte size bits
- options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
- // Disable hardware flow control
- options.c_cflag &= ~CRTSCTS;
- options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
- // Disable software flow control
- options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
- // Set Raw output
- options.c_oflag &= ~OPOST;
- // Timeout 0.005 sec for first byte, read minimum of 0 bytes
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 5;
-
- // Set the new options for the port
- tcsetattr(m_handle, TCSANOW, &options);
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
- tcflush(m_handle, TCIOFLUSH);
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
- return (m_retVal = MTRV_OK);
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle, &options);
+
+ // Set baudrate.
+ cfsetispeed(&options, baudrate);
+ cfsetospeed(&options, baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle, TCSANOW, &options);
+
+ tcflush(m_handle, TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
#endif
- }
+ }
- ////////////////////////////////////////////////////////////////////
- // openPort (string as input parameter)
- //
- // Opens a 'live' connection to a MT or XM
- //
- // Input
- // portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
- // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
- // inqueueSize : size of input queue, default = 4096
- // outqueueSize: size of output queue, default = 1024
- //
- // Output
- // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
- //
- short CXsensMTiModule::openPort(const char* portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
- {
- m_clkEnd = 0;
+ ////////////////////////////////////////////////////////////////////
+ // openPort (string as input parameter)
+ //
+ // Opens a 'live' connection to a MT or XM
+ //
+ // Input
+ // portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
+ // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+ // inqueueSize : size of input queue, default = 4096
+ // outqueueSize: size of output queue, default = 1024
+ //
+ // Output
+ // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+ //
+ short CXsensMTiModule::openPort(const char* portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
- if (m_fileOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
#ifdef WIN32
- m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
- // Once here, port is open
- m_portOpen = true;
+ // Once here, port is open
+ m_portOpen = true;
- //Get the current state & then change it
- DCB dcb;
+ //Get the current state & then change it
+ DCB dcb;
- GetCommState(m_handle, &dcb);// Get current state
+ GetCommState(m_handle, &dcb);// Get current state
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
- dcb.fOutxDsrFlow = FALSE;
- dcb.fOutX = FALSE;
- dcb.fInX = FALSE;
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
if (!SetCommState(m_handle, (LPDCB)&dcb))
{
- // Set new state
- // Bluetooth ports cannot always be opened with 2 stopbits
- // Now try to open port with 1 stopbit.
- dcb.StopBits = ONESTOPBIT;
-
- if (!SetCommState(m_handle, (LPDCB)&dcb))
- {
- CloseHandle(m_handle);
- m_handle = INVALID_HANDLE_VALUE;
- m_portOpen = false;
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
}
+ }
- // Set COM timeouts
- COMMTIMEOUTS CommTimeouts;
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
- GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
+ GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
- // immediate return if data is available, wait 1ms otherwise
- CommTimeouts.ReadTotalTimeoutConstant = 1;
- CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
- // immediate return whether data is available or not
- // CommTimeouts.ReadTotalTimeoutConstant = 0;
- // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
- // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+ // immediate return whether data is available or not
+ // CommTimeouts.ReadTotalTimeoutConstant = 0;
+ // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
- // Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
- SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
- return (m_retVal = MTRV_OK);
+ return (m_retVal = MTRV_OK);
#else
- struct termios options;
+ struct termios options;
- /* Open port */
+ /* Open port */
- m_handle = open(portName, O_RDWR | O_NOCTTY);
+ m_handle = open(portName, O_RDWR | O_NOCTTY);
- // O_RDWR: Read+Write
- // O_NOCTTY: Raw input, no "controlling terminal"
- // O_NDELAY: Don't care about DCD signal
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
- if (m_handle < 0)
- {
- // Port not open
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
-
- // Once here, port is open
- m_portOpen = true;
-
- /* Start configuring of port for non-canonical transfer mode */
- // Get current options for the port
- tcgetattr(m_handle, &options);
-
- // Set baudrate.
- cfsetispeed(&options, baudrate);
- cfsetospeed(&options, baudrate);
-
- // Enable the receiver and set local mode
- options.c_cflag |= (CLOCAL | CREAD);
- // Set character size to data bits and set no parity Mask the characte size bits
- options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
- // Disable hardware flow control
- options.c_cflag &= ~CRTSCTS;
- options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
- // Disable software flow control
- options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
- // Set Raw output
- options.c_oflag &= ~OPOST;
- // Timeout 0.005 sec for first byte, read minimum of 0 bytes
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 5;
-
- // Set the new options for the port
- tcsetattr(m_handle, TCSANOW, &options);
-
- tcflush(m_handle, TCIOFLUSH);
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
- return (m_retVal = MTRV_OK);
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle, &options);
+
+ // Set baudrate.
+ cfsetispeed(&options, baudrate);
+ cfsetospeed(&options, baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle, TCSANOW, &options);
+
+ tcflush(m_handle, TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
#endif
- }
+ }
- ////////////////////////////////////////////////////////////////////
- // openFile
- //
- // Open file for reading and writing
- // Filepos is at start of file
- //
- // Input
- // fileName : file name including path
- // createAlways: empties the log file, if existing
- //
- // Output
- // returns MTRV_OK if the file is opened
- // returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
- // returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
- //
- short CXsensMTiModule::openFile(const char* fileName, bool createAlways)
- {
- m_clkEnd = 0;
+ ////////////////////////////////////////////////////////////////////
+ // openFile
+ //
+ // Open file for reading and writing
+ // Filepos is at start of file
+ //
+ // Input
+ // fileName : file name including path
+ // createAlways: empties the log file, if existing
+ //
+ // Output
+ // returns MTRV_OK if the file is opened
+ // returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
+ // returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
+ //
+ short CXsensMTiModule::openFile(const char* fileName, bool createAlways)
+ {
+ m_clkEnd = 0;
- if (m_portOpen || m_portOpen)
- {
- return (m_retVal = MTRV_ANINPUTALREADYOPEN);
- }
+ if (m_portOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
#ifdef WIN32
- DWORD disposition = OPEN_ALWAYS;
+ DWORD disposition = OPEN_ALWAYS;
- if (createAlways == true)
- {
- disposition = CREATE_ALWAYS;
- }
+ if (createAlways == true)
+ {
+ disposition = CREATE_ALWAYS;
+ }
- m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
+ m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
- if (m_handle == INVALID_HANDLE_VALUE)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
#else
- int openMode = O_RDWR | O_CREAT;
+ int openMode = O_RDWR | O_CREAT;
- if (createAlways == true)
- {
- openMode |= O_TRUNC;
- }
+ if (createAlways == true)
+ {
+ openMode |= O_TRUNC;
+ }
- m_handle = open(fileName, openMode, S_IRWXU);
+ m_handle = open(fileName, openMode, S_IRWXU);
- if (m_handle < 0)
- {
- return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
- }
+ if (m_handle < 0)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
#endif
- m_timeOut = 0; // No timeout when using file input
- m_fileOpen = true;
- return (m_retVal = MTRV_OK);
+ m_timeOut = 0; // No timeout when using file input
+ m_fileOpen = true;
+ return (m_retVal = MTRV_OK);
- }
+ }
- ////////////////////////////////////////////////////////////////////
- // isPortOpen
- //
- // Return if serial port is open or not
- //
- bool CXsensMTiModule::isPortOpen()
- {
- return m_portOpen;
- }
+ ////////////////////////////////////////////////////////////////////
+ // isPortOpen
+ //
+ // Return if serial port is open or not
+ //
+ bool CXsensMTiModule::isPortOpen()
+ {
+ return m_portOpen;
+ }
- ////////////////////////////////////////////////////////////////////
- // isFileOpen
- //
- // Return if serial port is open or not
- //
- bool CXsensMTiModule::isFileOpen()
- {
- return m_fileOpen;
- }
+ ////////////////////////////////////////////////////////////////////
+ // isFileOpen
+ //
+ // Return if serial port is open or not
+ //
+ bool CXsensMTiModule::isFileOpen()
+ {
+ return m_fileOpen;
+ }
- ////////////////////////////////////////////////////////////////////
- // readData
- //
- // Reads bytes from serial port or file
- //
- // Input
- // msgBuffer : pointer to buffer in which next string will be stored
- // nBytesToRead : number of buffer bytes to read from serial port
- // retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
- //
- // Output
- // number of bytes actually read
- int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
+ ////////////////////////////////////////////////////////////////////
+ // readData
+ //
+ // Reads bytes from serial port or file
+ //
+ // Input
+ // msgBuffer : pointer to buffer in which next string will be stored
+ // nBytesToRead : number of buffer bytes to read from serial port
+ // retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
+ //
+ // Output
+ // number of bytes actually read
+ int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
+ {
+ //if(!m_fileOpen && !m_portOpen)
+ if (!(m_portOpen || m_fileOpen))
{
- //if(!m_fileOpen && !m_portOpen)
- if (!(m_portOpen || m_fileOpen))
- {
- m_retVal = MTRV_NOINPUTINITIALIZED;
- return 0;
- }
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
#ifdef WIN32
- DWORD nBytesRead;
- BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
+ DWORD nBytesRead;
+ BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
- if (retval && nBytesRead == 0 && m_fileOpen)
- {
- m_retVal = MTRV_ENDOFFILE;
- }
- else
- {
- m_retVal = MTRV_OK;
- }
+ if (retval && nBytesRead == 0 && m_fileOpen)
+ {
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ {
+ m_retVal = MTRV_OK;
+ }
- return nBytesRead;
+ return nBytesRead;
#else
- const int nBytesRead = read(m_handle, msgBuffer, nBytesToRead);
-
- m_retVal = MTRV_OK;
+ const int nBytesRead = read(m_handle, msgBuffer, nBytesToRead);
- if (nBytesRead)
- {
- return nBytesRead;
- }
- else if (m_fileOpen)
- {
- m_retVal = MTRV_ENDOFFILE;
- }
+ m_retVal = MTRV_OK;
+ if (nBytesRead)
+ {
return nBytesRead;
- /*
- if (nBytesRead == 0 && m_fileOpen)
- {
- nBytesRead = 0;
- m_retVal = MTRV_ENDOFFILE;
- }
- else
- m_retVal = MTRV_OK;
- return nBytesRead;*/
-
- // In Linux it is sometimes better to read per byte instead of a block of bytes at once
- // Use this block if experiencing problems with the above code
- /*
- int j = 0; // Index in buffer for read data
- int k = 0; // Timeout factor
- int nRead = 0; // Bytes read from port, return by read function
-
- do {
- nRead = read(m_handle, &msgBuffer[j], 1);
- if (nRead == 1) { // Byte read
- k = 0;
- j++;
- }
- else {
- k++;
- }
-
- if (k == 3) { // Timeout, too long no data read from port
- return nRead;
- }
- }
- while (j < nBytesToRead);
-
- return j;
- */
-#endif
+ }
+ else if (m_fileOpen)
+ {
+ m_retVal = MTRV_ENDOFFILE;
}
- ////////////////////////////////////////////////////////////////////
- // writeData
- //
- // Writes bytes to serial port (to do: file)
- //
- // Input
- // msgBuffer : a pointer to a char buffer containing
- // the characters to be written to serial port
- // nBytesToWrite : number of buffer bytes to write to serial port
- //
- // Output
- // number of bytes actually written
- //
- // The MTComm return value is != MTRV_OK if serial port is closed
- int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
+ return nBytesRead;
+ /*
+ if (nBytesRead == 0 && m_fileOpen)
+ {
+ nBytesRead = 0;
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ m_retVal = MTRV_OK;
+ return nBytesRead;*/
+
+ // In Linux it is sometimes better to read per byte instead of a block of bytes at once
+ // Use this block if experiencing problems with the above code
+ /*
+ int j = 0; // Index in buffer for read data
+ int k = 0; // Timeout factor
+ int nRead = 0; // Bytes read from port, return by read function
+
+ do {
+ nRead = read(m_handle, &msgBuffer[j], 1);
+ if (nRead == 1) { // Byte read
+ k = 0;
+ j++;
+ }
+ else {
+ k++;
+ }
+
+ if (k == 3) { // Timeout, too long no data read from port
+ return nRead;
+ }
+ }
+ while (j < nBytesToRead);
+
+ return j;
+ */
+#endif
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // writeData
+ //
+ // Writes bytes to serial port (to do: file)
+ //
+ // Input
+ // msgBuffer : a pointer to a char buffer containing
+ // the characters to be written to serial port
+ // nBytesToWrite : number of buffer bytes to write to serial port
+ //
+ // Output
+ // number of bytes actually written
+ //
+ // The MTComm return value is != MTRV_OK if serial port is closed
+ int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
+ {
+ if (!m_fileOpen && !m_portOpen)
{
- if (!m_fileOpen && !m_portOpen)
- {
- m_retVal = MTRV_NOINPUTINITIALIZED;
- return 0;
- }
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
- m_retVal = MTRV_OK;
+ m_retVal = MTRV_OK;
#ifdef WIN32
- DWORD nBytesWritten;
- WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
- return nBytesWritten;
+ DWORD nBytesWritten;
+ WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
+ return nBytesWritten;
#else
- return write(m_handle, msgBuffer, nBytesToWrite);
+ return write(m_handle, msgBuffer, nBytesToWrite);
#endif
- }
+ }
- ////////////////////////////////////////////////////////////////////
- // flush
- //
- // Remove any 'old' data in COM port buffer and flushes internal
- // temporary buffer
- //
- void CXsensMTiModule::flush()
+ ////////////////////////////////////////////////////////////////////
+ // flush
+ //
+ // Remove any 'old' data in COM port buffer and flushes internal
+ // temporary buffer
+ //
+ void CXsensMTiModule::flush()
+ {
+ if (m_portOpen)
{
- if (m_portOpen)
- {
#ifdef WIN32
- // Remove any 'old' data in buffer
- PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
#else
- tcflush(m_handle, TCIOFLUSH);
+ tcflush(m_handle, TCIOFLUSH);
#endif
- }
-
- m_nTempBufferLen = 0;
- m_retVal = MTRV_OK;
}
- ////////////////////////////////////////////////////////////////////
- // escape
- //
- // Directs a COM port to perform an extended function
- //
- // Input
- // function : Windows define. Can be one of following:
- // CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
- void CXsensMTiModule::escape(unsigned long /*function*/)
- {
+ m_nTempBufferLen = 0;
+ m_retVal = MTRV_OK;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // escape
+ //
+ // Directs a COM port to perform an extended function
+ //
+ // Input
+ // function : Windows define. Can be one of following:
+ // CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
+ void CXsensMTiModule::escape(unsigned long /*function*/)
+ {
#ifdef WIN32
- EscapeCommFunction(m_handle, function);
+ EscapeCommFunction(m_handle, function);
#else
#endif
- }
+ }
- ////////////////////////////////////////////////////////////////////
- // setPortQueueSize
- //
- // Set input and output buffer size of serial port
- //
- void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
+ ////////////////////////////////////////////////////////////////////
+ // setPortQueueSize
+ //
+ // Set input and output buffer size of serial port
+ //
+ void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
+ {
+ if (m_portOpen)
{
- if (m_portOpen)
- {
#ifdef WIN32
- SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
+ SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
#else
- // Not yet implemented
+ // Not yet implemented
#endif
- }
-
- m_retVal = MTRV_OK;
}
- ////////////////////////////////////////////////////////////////////
- // setFilePos
- //
- // Sets the current position of the file pointer for file input
- //
- // Input
- // relPos : 32-bit value specifying the relative move in bytes
- // origin is specified in moveMethod
- // moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
- // Output
- //
- // returns MTRV_OK if file pointer is successfully set
- //
- short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
- {
+ m_retVal = MTRV_OK;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setFilePos
+ //
+ // Sets the current position of the file pointer for file input
+ //
+ // Input
+ // relPos : 32-bit value specifying the relative move in bytes
+ // origin is specified in moveMethod
+ // moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
+ // Output
+ //
+ // returns MTRV_OK if file pointer is successfully set
+ //
+ short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
+ {
#ifdef WIN32
- if (m_fileOpen)
+ if (m_fileOpen)
+ {
+ if (SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
{
- if (SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
- {
- return (m_retVal = MTRV_OK);
- }
+ return (m_retVal = MTRV_OK);
}
+ }
#else
- if (m_fileOpen)
+ if (m_fileOpen)
+ {
+ if (lseek(m_handle, relPos, moveMethod) != -1)
{
- if (lseek(m_handle, relPos, moveMethod) != -1)
- {
- return (m_retVal = MTRV_OK);
- }
+ return (m_retVal = MTRV_OK);
}
+ }
#endif
- return (m_retVal = MTRV_NOTSUCCESSFUL);
- }
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
- ////////////////////////////////////////////////////////////////////
- // getFileSize
- //
- // Retrieves the file size of the opened file
- //
- // Input
- // Output
- // fileSize : Number of bytes of the current file
- //
- // returns MTRV_OK if successful
- //
- short CXsensMTiModule::getFileSize(unsigned long& fileSize)
- {
+ ////////////////////////////////////////////////////////////////////
+ // getFileSize
+ //
+ // Retrieves the file size of the opened file
+ //
+ // Input
+ // Output
+ // fileSize : Number of bytes of the current file
+ //
+ // returns MTRV_OK if successful
+ //
+ short CXsensMTiModule::getFileSize(unsigned long& fileSize)
+ {
#ifdef WIN32
- if (m_fileOpen)
+ if (m_fileOpen)
+ {
+ if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
{
- if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
- {
- return (m_retVal = MTRV_OK);
- }
+ return (m_retVal = MTRV_OK);
}
+ }
#else
- if (m_fileOpen)
- {
- struct stat buf;
+ if (m_fileOpen)
+ {
+ struct stat buf;
- if (fstat(m_handle, &buf) == 0)
- {
- fileSize = buf.st_size;
- return (m_retVal = MTRV_OK);
- }
+ if (fstat(m_handle, &buf) == 0)
+ {
+ fileSize = buf.st_size;
+ return (m_retVal = MTRV_OK);
}
+ }
#endif
- return (m_retVal = MTRV_NOTSUCCESSFUL);
- }
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
- ////////////////////////////////////////////////////////////////////
- // close
- //
- // Closes handle of file or serial port
- //
- short CXsensMTiModule::close()
+ ////////////////////////////////////////////////////////////////////
+ // close
+ //
+ // Closes handle of file or serial port
+ //
+ short CXsensMTiModule::close()
+ {
+ if (m_portOpen || m_fileOpen)
{
- if (m_portOpen || m_fileOpen)
- {
#ifdef WIN32
- if (m_portOpen)
- {
- // Because of USB-serial driver bug
- flush();
- }
+ if (m_portOpen)
+ {
+ // Because of USB-serial driver bug
+ flush();
+ }
- CloseHandle(m_handle);
+ CloseHandle(m_handle);
#else
- ::close(m_handle);
+ ::close(m_handle);
#endif
- }
+ }
- m_fileOpen = m_portOpen = false;
- m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
- m_clkEnd = 0;
- m_nTempBufferLen = 0;
- m_deviceError = 0; // No error
+ m_fileOpen = m_portOpen = false;
+ m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
+ m_clkEnd = 0;
+ m_nTempBufferLen = 0;
+ m_deviceError = 0; // No error
- for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
- {
- m_storedOutputMode[i] = INVALIDSETTINGVALUE;
- m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
- m_storedDataLength[i] = 0;
- }
+ for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ {
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
+ }
- return (m_retVal = MTRV_OK);
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readMessage
+ //
+ // Reads the next message from serial port buffer or file.
+ // To be read within current time out period
+ //
+ // Input
+ // Output
+ // mid : MessageID of message received
+ // data : array pointer to data bytes (no header)
+ // dataLen : number of data bytes read
+ // bid : BID or address of message read (optional)
+ //
+ // returns MTRV_OK if a message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
}
- ////////////////////////////////////////////////////////////////////
- // readMessage
- //
- // Reads the next message from serial port buffer or file.
- // To be read within current time out period
- //
- // Input
- // Output
- // mid : MessageID of message received
- // data : array pointer to data bytes (no header)
- // dataLen : number of data bytes read
- // bid : BID or address of message read (optional)
- //
- // returns MTRV_OK if a message has been read else <>MTRV_OK
- //
- // Remarks
- // allocate enough memory for message buffer
- // use setTimeOut for different timeout value
- short CXsensMTiModule::readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid)
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ // Message read
+ mid = buffer[IND_MID];
- if (!(m_fileOpen || m_portOpen))
+ if (bid != nullptr)
{
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ *bid = buffer[IND_BID];
}
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
{
- // Message read
- mid = buffer[IND_MID];
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
+ }
- if (bid != nullptr)
- {
- *bid = buffer[IND_BID];
- }
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // readDataMessage
+ //
+ // Read a MTData or XMData message from serial port (using TimeOut val)
+ //
+ // Input
+ // data : pointer to buffer in which the DATA field of MTData/XMData is stored
+ // dataLen : number of bytes in buffer (= number bytes in DATA field)
+ // Output
+ // returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for data buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
+ {
+ if (!(m_portOpen || m_fileOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
+ if (readMessageRaw(buffer, &buflen) == MTRV_OK)
+ {
+ if (buffer[IND_MID] == MID_MTDATA)
+ {
+ // MID_XMDATA is same
if (buffer[IND_LEN] != EXTLENCODE)
{
dataLen = buffer[IND_LEN];
@@ -898,2290 +940,2245 @@ namespace IMU
dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
memcpy(data, &buffer[IND_DATAEXT0], dataLen);
}
- }
- return m_retVal;
- }
-
- ////////////////////////////////////////////////////////////////////
- // readDataMessage
- //
- // Read a MTData or XMData message from serial port (using TimeOut val)
- //
- // Input
- // data : pointer to buffer in which the DATA field of MTData/XMData is stored
- // dataLen : number of bytes in buffer (= number bytes in DATA field)
- // Output
- // returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
- //
- // Remarks
- // allocate enough memory for data buffer
- // use setTimeOut for different timeout value
- short CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
- {
- if (!(m_portOpen || m_fileOpen))
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
{
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- unsigned char buffer[MAXMSGLEN ];
- short buflen;
+ return m_retVal;
+ }
- if (readMessageRaw(buffer, &buflen) == MTRV_OK)
+ ////////////////////////////////////////////////////////////////////
+ // readMessageRaw
+ //
+ // Read a message from serial port for a certain period
+ //
+ // Input
+ // msgBuffer : pointer to buffer in which next msg will be stored
+ // msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
+ // Output
+ // returns MTRV_OK if a message has been read else <>MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
+ {
+ int state = 0;
+ int nBytesToRead = 1;
+ int nBytesRead = 0;
+ int nOffset = 0;
+ int nMsgDataLen = 0;
+ int nMsgLen;
+ unsigned char chCheckSum;
+ bool Synced = false;
+
+ if (!(m_portOpen || m_fileOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ // Copy previous read bytes if available
+ if (m_nTempBufferLen > 0)
+ {
+ memcpy(msgBuffer, m_tempBuffer, m_nTempBufferLen);
+ nBytesRead = m_nTempBufferLen;
+ m_nTempBufferLen = 0;
+ }
+
+ clock_t clkStart = clockms(); // Get current processor time
+ clock_t clkEnd = m_clkEnd; // check if the end timer is already set
+
+ if (clkEnd == 0)
+ {
+ clkEnd = clkStart + m_timeOut;
+ }
+
+ while (true)
+ {
+ do
{
- if (buffer[IND_MID] == MID_MTDATA)
+ // First check if we already have some bytes read
+ if (nBytesRead > 0 && nBytesToRead > 0)
{
- // MID_XMDATA is same
- if (buffer[IND_LEN] != EXTLENCODE)
+ if (nBytesToRead > nBytesRead)
{
- dataLen = buffer[IND_LEN];
- memcpy(data, &buffer[IND_DATA0], dataLen);
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
}
else
{
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ nOffset += nBytesToRead;
+ nBytesRead -= nBytesToRead;
+ nBytesToRead = 0;
}
-
- return (m_retVal = MTRV_OK);
}
- else if (buffer[IND_MID] == MID_ERROR)
+
+ // Check if serial port buffer must be read
+ if (nBytesToRead > 0)
{
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ nBytesRead = readData(msgBuffer + nOffset, nBytesToRead);
- return m_retVal;
- }
+ if (m_retVal == MTRV_ENDOFFILE)
+ {
+ return (m_retVal = MTRV_ENDOFFILE);
+ }
- ////////////////////////////////////////////////////////////////////
- // readMessageRaw
- //
- // Read a message from serial port for a certain period
- //
- // Input
- // msgBuffer : pointer to buffer in which next msg will be stored
- // msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
- // Output
- // returns MTRV_OK if a message has been read else <>MTRV_OK
- //
- // Remarks
- // allocate enough memory for message buffer
- // use setTimeOut for different timeout value
- short CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
- {
- int state = 0;
- int nBytesToRead = 1;
- int nBytesRead = 0;
- int nOffset = 0;
- int nMsgDataLen = 0;
- int nMsgLen;
- unsigned char chCheckSum;
- bool Synced = false;
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
+ }
- if (!(m_portOpen || m_fileOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (!nBytesToRead)
+ {
+ switch (state)
+ {
+ case 0: // Check preamble
+ if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
+ {
+ state++;
+ nBytesToRead = 3;
+ }
+ else
+ {
+ nOffset = 0;
+ nBytesToRead = 1;
+ }
- // Copy previous read bytes if available
- if (m_nTempBufferLen > 0)
- {
- memcpy(msgBuffer, m_tempBuffer, m_nTempBufferLen);
- nBytesRead = m_nTempBufferLen;
- m_nTempBufferLen = 0;
- }
+ break;
- clock_t clkStart = clockms(); // Get current processor time
- clock_t clkEnd = m_clkEnd; // check if the end timer is already set
+ case 1: // Check ADDR, MID, LEN
+ if (msgBuffer[IND_LEN] != 0xFF)
+ {
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
+ }
+ else
+ {
+ state = 2;
+ nBytesToRead = 2; // Read extended length
+ }
- if (clkEnd == 0)
- {
- clkEnd = clkStart + m_timeOut;
- }
+ break;
- while (true)
- {
- do
- {
- // First check if we already have some bytes read
- if (nBytesRead > 0 && nBytesToRead > 0)
- {
- if (nBytesToRead > nBytesRead)
- {
- nOffset += nBytesRead;
- nBytesToRead -= nBytesRead;
- nBytesRead = 0;
- }
- else
- {
- nOffset += nBytesToRead;
- nBytesRead -= nBytesToRead;
- nBytesToRead = 0;
- }
- }
+ case 2:
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
- // Check if serial port buffer must be read
- if (nBytesToRead > 0)
- {
- nBytesRead = readData(msgBuffer + nOffset, nBytesToRead);
+ if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
+ {
+ // Not synced - check for preamble in the bytes read
+ for (int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = LEN_MSGEXTHEADER - i;
+ memmove(msgBuffer, msgBuffer + i, nBytesRead);
+ break;
+ }
- if (m_retVal == MTRV_ENDOFFILE)
- {
- return (m_retVal = MTRV_ENDOFFILE);
- }
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
- nOffset += nBytesRead;
- nBytesToRead -= nBytesRead;
- nBytesRead = 0;
- }
+ break;
- if (!nBytesToRead)
- {
- switch (state)
- {
- case 0: // Check preamble
- if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
- {
- state++;
- nBytesToRead = 3;
- }
- else
- {
- nOffset = 0;
- nBytesToRead = 1;
- }
+ case 3: // Check msg
+ chCheckSum = 0;
+ nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
- break;
+ for (int i = 1 ; i < nMsgLen ; i++)
+ {
+ chCheckSum += msgBuffer[i];
+ }
- case 1: // Check ADDR, MID, LEN
- if (msgBuffer[IND_LEN] != 0xFF)
- {
- state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
- }
- else
+ if (chCheckSum == 0)
+ {
+ // Checksum ok?
+ if (nBytesRead > 0)
{
- state = 2;
- nBytesToRead = 2; // Read extended length
+ // Store bytes if read too much
+ memcpy(m_tempBuffer, msgBuffer + nMsgLen, nBytesRead);
+ m_nTempBufferLen = nBytesRead;
}
- break;
-
- case 2:
- state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
-
- if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
- {
- // Not synced - check for preamble in the bytes read
- for (int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
+ *msgBufferLength = nMsgLen;
+ Synced = true;
+ return (m_retVal = MTRV_OK);
+ }
+ else
+ {
+ if (!Synced)
+ for (int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
if (msgBuffer[i] == PREAMBLE)
{
// Found a maybe preamble - 'fill buffer'
- nBytesRead = LEN_MSGEXTHEADER - i;
+ nBytesRead = nMsgLen - i;
memmove(msgBuffer, msgBuffer + i, nBytesRead);
break;
}
- Synced = false;
- nOffset = 0;
- state = 0;
- nBytesToRead = 1; // Start looking for preamble
- }
-
- break;
-
- case 3: // Check msg
- chCheckSum = 0;
- nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
-
- for (int i = 1 ; i < nMsgLen ; i++)
- {
- chCheckSum += msgBuffer[i];
- }
-
- if (chCheckSum == 0)
- {
- // Checksum ok?
- if (nBytesRead > 0)
- {
- // Store bytes if read too much
- memcpy(m_tempBuffer, msgBuffer + nMsgLen, nBytesRead);
- m_nTempBufferLen = nBytesRead;
- }
-
- *msgBufferLength = nMsgLen;
- Synced = true;
- return (m_retVal = MTRV_OK);
- }
- else
- {
- if (!Synced)
- for (int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
- if (msgBuffer[i] == PREAMBLE)
- {
- // Found a maybe preamble - 'fill buffer'
- nBytesRead = nMsgLen - i;
- memmove(msgBuffer, msgBuffer + i, nBytesRead);
- break;
- }
-
- Synced = false;
- nOffset = 0;
- state = 0;
- nBytesToRead = 1; // Start looking for preamble
- }
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
- break;
- }
+ break;
}
}
- while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
-
- // Check if pending message has a valid message
- if (state > 0)
- {
- int i;
+ }
+ while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
- // Search for preamble
- for (i = 1; i < nOffset ; i++)
- if (msgBuffer[i] == PREAMBLE)
- {
- // Found a maybe preamble - 'fill buffer'
- nBytesRead = nOffset - i - 1; // no preamble
- memmove(msgBuffer + 1, msgBuffer + i + 1, nBytesRead);
- break;
- }
+ // Check if pending message has a valid message
+ if (state > 0)
+ {
+ int i;
- if (i < nOffset)
+ // Search for preamble
+ for (i = 1; i < nOffset ; i++)
+ if (msgBuffer[i] == PREAMBLE)
{
- // Found preamble in message - recheck
- nOffset = 1;
- state = 1;
- nBytesToRead = 3; // Start looking for preamble
- continue;
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = nOffset - i - 1; // no preamble
+ memmove(msgBuffer + 1, msgBuffer + i + 1, nBytesRead);
+ break;
}
- }
- break;
+ if (i < nOffset)
+ {
+ // Found preamble in message - recheck
+ nOffset = 1;
+ state = 1;
+ nBytesToRead = 3; // Start looking for preamble
+ continue;
+ }
}
- return (m_retVal = MTRV_TIMEOUT);
+ break;
}
- ////////////////////////////////////////////////////////////////////
- // writeMessage (optional: integer value)
- //
- // Sends a message and in case of an serial port interface it checks
- // if the return message (ack, error or timeout). See return value
- // In case an file is opened the functions returns directly after
- // 'sending' the message
- //
- // Use this function for GotoConfig, Reset, ResetOrientation etc
- //
- // Input
- // mid : MessageID of message to send
- // dataValue : A integer value that will be included into the data message field
- // can be a 1,2 or 4 bytes values
- // dataValueLen : Size of dataValue in bytes
- // bid : BID or address to use in message to send (default = 0xFF)
- //
- // Return value
- // = MTRV_OK if an Ack message received / or data successfully written to file
- // = MTRV_RECVERRORMSG if an error message received
- // = MTRV_TIMEOUT if timeout occurred
- // = MTRV_NOINPUTINITIALIZED
- //
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // writeMessage (optional: integer value)
+ //
+ // Sends a message and in case of an serial port interface it checks
+ // if the return message (ack, error or timeout). See return value
+ // In case an file is opened the functions returns directly after
+ // 'sending' the message
+ //
+ // Use this function for GotoConfig, Reset, ResetOrientation etc
+ //
+ // Input
+ // mid : MessageID of message to send
+ // dataValue : A integer value that will be included into the data message field
+ // can be a 1,2 or 4 bytes values
+ // dataValueLen : Size of dataValue in bytes
+ // bid : BID or address to use in message to send (default = 0xFF)
+ //
+ // Return value
+ // = MTRV_OK if an Ack message received / or data successfully written to file
+ // = MTRV_RECVERRORMSG if an error message received
+ // = MTRV_TIMEOUT if timeout occurred
+ // = MTRV_NOINPUTINITIALIZED
+ //
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = dataValueLen;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = dataValueLen;
+ if (dataValueLen)
+ {
+ swapEndian((const unsigned char*) &dataValue, &buffer[IND_DATA0], dataValueLen);
+ }
- if (dataValueLen)
- {
- swapEndian((const unsigned char*) &dataValue, &buffer[IND_DATA0], dataValueLen);
- }
+ calcChecksum(buffer, LEN_MSGHEADER + dataValueLen);
- calcChecksum(buffer, LEN_MSGHEADER + dataValueLen);
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + dataValueLen);
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + dataValueLen);
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
- // Return if file opened
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_OK);
- }
+ // Keep reading until an Ack or Error message is received (or timeout)
+ clock_t clkStart, clkOld;
+ bool msgRead = false;
- // Keep reading until an Ack or Error message is received (or timeout)
- clock_t clkStart, clkOld;
- bool msgRead = false;
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
- if (clkOld == 0)
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
{
- m_clkEnd = m_timeOut + clkStart;
- }
+ // Message received
+ msgRead = true;
- while (m_clkEnd >= clockms() || (m_timeOut == 0))
- {
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ if (buffer[IND_MID] == (mid + 1))
{
- // Message received
- msgRead = true;
-
- if (buffer[IND_MID] == (mid + 1))
- {
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
+ }
- m_clkEnd = clkOld;
+ m_clkEnd = clkOld;
- if (msgRead)
- {
- return (m_retVal = MTRV_TIMEOUT);
- }
- else
- {
- return (m_retVal = MTRV_TIMEOUTNODATA);
- }
+ if (msgRead)
+ {
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+ else
+ {
+ return (m_retVal = MTRV_TIMEOUTNODATA);
}
+ }
- ////////////////////////////////////////////////////////////////////
- // writeMessage (data array)
- //
- // Sends a message and in case of an serial port interface it checks
- // if the return message (ack, error or timeout). See return value
- // In case an file is opened the functions returns directly after
- // 'sending' the message
- //
- // Input
- // mid : MessageID of message to send
- // data : array pointer to data bytes
- // dataLen : number of bytes to include in message
- // bid : BID or address to use in message to send (default = 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message received
- // = MTRV_RECVERRORMSG if an error message received
- // = MTRV_TIMEOUT if timeout occurred
- // = MTRV_NOINPUTINITIALIZED
- //
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid)
+ ////////////////////////////////////////////////////////////////////
+ // writeMessage (data array)
+ //
+ // Sends a message and in case of an serial port interface it checks
+ // if the return message (ack, error or timeout). See return value
+ // In case an file is opened the functions returns directly after
+ // 'sending' the message
+ //
+ // Input
+ // mid : MessageID of message to send
+ // data : array pointer to data bytes
+ // dataLen : number of bytes to include in message
+ // bid : BID or address to use in message to send (default = 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message received
+ // = MTRV_RECVERRORMSG if an error message received
+ // = MTRV_TIMEOUT if timeout occurred
+ // = MTRV_NOINPUTINITIALIZED
+ //
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+ short headerLength;
+
+ if (!(m_fileOpen || m_portOpen))
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
- short headerLength;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ // Build message to send
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- // Build message to send
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ if (dataLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char)(dataLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char)(dataLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
- if (dataLen < EXTLENCODE)
- {
- buffer[IND_LEN] = (unsigned char) dataLen;
- headerLength = LEN_MSGHEADER;
- }
- else
- {
- buffer[IND_LEN] = EXTLENCODE;
- buffer[IND_LENEXTH] = (unsigned char)(dataLen >> 8);
- buffer[IND_LENEXTL] = (unsigned char)(dataLen & 0x00FF);
- headerLength = LEN_MSGEXTHEADER;
- }
+ memcpy(&buffer[headerLength], data, dataLen);
+ calcChecksum(buffer, headerLength + dataLen);
- memcpy(&buffer[headerLength], data, dataLen);
- calcChecksum(buffer, headerLength + dataLen);
+ // Send message
+ writeData(buffer, headerLength + dataLen + LEN_CHECKSUM);
- // Send message
- writeData(buffer, headerLength + dataLen + LEN_CHECKSUM);
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
- // Return if file opened
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_OK);
- }
+ // Keep reading until an Ack or Error message is received (or timeout)
+ bool msgRead = false;
+ clock_t clkStart, clkOld;
- // Keep reading until an Ack or Error message is received (or timeout)
- bool msgRead = false;
- clock_t clkStart, clkOld;
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
- if (clkOld == 0)
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
{
- m_clkEnd = m_timeOut + clkStart;
- }
+ // Message received
+ msgRead = true;
- while (m_clkEnd >= clockms() || (m_timeOut == 0))
- {
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ if (buffer[IND_MID] == (mid + 1))
{
- // Message received
- msgRead = true;
-
- if (buffer[IND_MID] == (mid + 1))
- {
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
+ }
- m_clkEnd = clkOld;
+ m_clkEnd = clkOld;
- if (msgRead)
- {
- return (m_retVal = MTRV_TIMEOUT);
- }
- else
- {
- return (m_retVal = MTRV_TIMEOUTNODATA);
- }
+ if (msgRead)
+ {
+ return (m_retVal = MTRV_TIMEOUT);
}
-
- ////////////////////////////////////////////////////////////////////
- // waitForMessage
- //
- // Read messages from serial port or file using the current timeout period
- // until the received message is equal to a specific message identifier
- // By default the timeout period by file input is set to infinity (= until
- // end of file is reached)
- //
- // Input/Output
- // mid : message identifier of message that should be returned
- // data : pointer to buffer in which the data of the requested msg will be stored
- // dataLen : integer to number of data bytes
- // bid : optional, pointer which holds the bid of the returned message
- // Output
- // returns MTRV_OK if the message has been read else != MTRV_OK
- //
- // Remarks
- // allocate enough memory for data message buffer
- // use setTimeOut for different timeout value
- short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short* dataLen, unsigned char* bid)
+ else
{
- unsigned char buffer[MAXMSGLEN ];
- short buflen;
+ return (m_retVal = MTRV_TIMEOUTNODATA);
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // waitForMessage
+ //
+ // Read messages from serial port or file using the current timeout period
+ // until the received message is equal to a specific message identifier
+ // By default the timeout period by file input is set to infinity (= until
+ // end of file is reached)
+ //
+ // Input/Output
+ // mid : message identifier of message that should be returned
+ // data : pointer to buffer in which the data of the requested msg will be stored
+ // dataLen : integer to number of data bytes
+ // bid : optional, pointer which holds the bid of the returned message
+ // Output
+ // returns MTRV_OK if the message has been read else != MTRV_OK
+ //
+ // Remarks
+ // allocate enough memory for data message buffer
+ // use setTimeOut for different timeout value
+ short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short* dataLen, unsigned char* bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
- clock_t clkStart, clkOld;
+ clock_t clkStart, clkOld;
- if (!(m_fileOpen || m_portOpen))
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- clkStart = clockms(); // Get current processor time
- clkOld = m_clkEnd;
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
- if (clkOld == 0)
- {
- m_clkEnd = m_timeOut + clkStart;
- }
+ if (clkOld == 0)
+ {
+ m_clkEnd = m_timeOut + clkStart;
+ }
- while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ while (m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer, &buflen) == MTRV_OK)
{
- if (readMessageRaw(buffer, &buflen) == MTRV_OK)
+ if (buffer[IND_MID] == mid)
{
- if (buffer[IND_MID] == mid)
+ if (bid != nullptr)
{
- if (bid != nullptr)
- {
- *bid = buffer[IND_BID];
- }
+ *bid = buffer[IND_BID];
+ }
- if (data != nullptr && dataLen != nullptr)
+ if (data != nullptr && dataLen != nullptr)
+ {
+ if (buffer[IND_LEN] != EXTLENCODE)
{
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- *dataLen = buffer[IND_LEN];
- memcpy(data, &buffer[IND_DATA0], *dataLen);
- }
- else
- {
- *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data, &buffer[IND_DATAEXT0], *dataLen);
- }
+ *dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], *dataLen);
}
- else if (dataLen != nullptr)
+ else
{
- dataLen = nullptr;
+ *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], *dataLen);
}
-
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK);
}
- }
- else if (m_retVal == MTRV_ENDOFFILE)
- {
+ else if (dataLen != nullptr)
+ {
+ dataLen = nullptr;
+ }
+
m_clkEnd = clkOld;
- return (m_retVal = MTRV_ENDOFFILE);
+ return (m_retVal = MTRV_OK);
}
}
-
- m_clkEnd = clkOld;
- return (m_retVal = MTRV_TIMEOUT);
+ else if (m_retVal == MTRV_ENDOFFILE)
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_ENDOFFILE);
+ }
+ }
+
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (integer & no param variant)
+ //
+ // Request a integer setting from the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the integer value of the data field of the ack message
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
}
- ////////////////////////////////////////////////////////////////////
- // reqSetting (integer & no param variant)
- //
- // Request a integer setting from the device. This setting
- // can be an unsigned 1,2 or 4 bytes setting. Only valid
- // for serial port connections
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // value contains the integer value of the data field of the ack message
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid)
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (integer & param variant)
+ //
+ // Request a integer setting from the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the integer value of the data field of the ack message
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
buffer[IND_LEN] = 0;
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+ }
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- // Message received
- if (buffer[IND_MID] == (mid + 1))
+ // Acknowledge received
+ value = 0;
+
+ if (param == 0xFF)
{
- // Acknowledge received
- value = 0;
swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
}
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
+
+ return m_retVal;
+ }
- return m_retVal;
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (float & no param variant)
+ //
+ // Request a float setting from the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the float value of the acknowledge data field
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // reqSetting (integer & param variant)
- //
- // Request a integer setting from the device. This setting
- // can be an unsigned 1,2 or 4 bytes setting. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // value contains the integer value of the data field of the ack message
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- if (!m_portOpen)
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
}
-
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
-
- if (param != 0xFF)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- buffer[IND_LEN] = 0;
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
-
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
-
- if (param == 0xFF)
- {
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
- }
- else
- {
- swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
- }
+ return m_retVal;
+ }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (float & param variant)
+ //
+ // Request a float setting from the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // value contains the float value of the acknowledge data field
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // reqSetting (float & no param variant)
- //
- // Request a float setting from the device. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // value contains the float value of the acknowledge data field
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
buffer[IND_LEN] = 0;
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+ }
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- // Message received
- if (buffer[IND_MID] == (mid + 1))
+ // Acknowledge received
+ value = 0;
+
+ if (param == 0xFF)
{
- // Acknowledge received
- value = 0;
swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
}
- }
-
- return m_retVal;
- }
- ////////////////////////////////////////////////////////////////////
- // reqSetting (float & param variant)
- //
- // Request a float setting from the device. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter (optional)
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // value contains the float value of the acknowledge data field
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
-
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ return (m_retVal = MTRV_OK);
}
-
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
-
- if (param != 0xFF)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- buffer[IND_LEN] = 0;
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array & no param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // data[] contains the data of the acknowledge message
+ // dataLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- value = 0;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
- if (param == 0xFF)
- {
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
- }
- else
- {
- swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
- }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
{
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
}
+
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array in + out & no param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // dataIn : Data to be included in request
+ // dataInLen : Number of bytes in dataIn
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // dataOut[] contains the data of the acknowledge message
+ // dataOutLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short headerLength;
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // reqSetting (byte array & no param variant)
- //
- // Send a message to the device and the data of acknowledge message
- // will be returned. Only valid for serial port connections
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // data[] contains the data of the acknowledge message
- // dataLen contains the number bytes returned
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (dataInLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataInLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char)(dataInLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char)(dataInLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = 0;
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+ memcpy(&buffer[headerLength], dataIn, dataInLen);
+ calcChecksum(buffer, headerLength + dataInLen);
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ // Send message
+ writeData(buffer, headerLength + dataInLen + LEN_CHECKSUM);
- dataLen = 0;
+ dataOutLen = 0;
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data, &buffer[IND_DATA0], dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data, &buffer[IND_DATAEXT0], dataLen);
- }
-
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
{
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ dataOutLen = buffer[IND_LEN];
+ memcpy(dataOut, &buffer[IND_DATA0], dataOutLen);
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(dataOut, &buffer[IND_DATAEXT0], dataOutLen);
}
- }
-
- return m_retVal;
- }
- ////////////////////////////////////////////////////////////////////
- // reqSetting (byte array in + out & no param variant)
- //
- // Send a message to the device and the data of acknowledge message
- // will be returned. Only valid for serial port connections
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- // dataIn : Data to be included in request
- // dataInLen : Number of bytes in dataIn
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // dataOut[] contains the data of the acknowledge message
- // dataOutLen contains the number bytes returned
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid)
- {
- unsigned char buffer[MAXMSGLEN ];
- short headerLength;
- short msgLen;
-
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
-
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ return (m_retVal = MTRV_OK);
}
-
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
-
- if (dataInLen < EXTLENCODE)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = (unsigned char) dataInLen;
- headerLength = LEN_MSGHEADER;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- buffer[IND_LEN] = EXTLENCODE;
- buffer[IND_LENEXTH] = (unsigned char)(dataInLen >> 8);
- buffer[IND_LENEXTL] = (unsigned char)(dataInLen & 0x00FF);
- headerLength = LEN_MSGEXTHEADER;
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
- memcpy(&buffer[headerLength], dataIn, dataInLen);
- calcChecksum(buffer, headerLength + dataInLen);
+ return m_retVal;
+ }
- // Send message
- writeData(buffer, headerLength + dataInLen + LEN_CHECKSUM);
+ ////////////////////////////////////////////////////////////////////
+ // reqSetting (byte array & param variant)
+ //
+ // Send a message to the device and the data of acknowledge message
+ // will be returned. Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ // data[] contains the data of the acknowledge message (including param!!)
+ // dataLen contains the number bytes returned
+ //
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- dataOutLen = 0;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataOutLen = buffer[IND_LEN];
- memcpy(dataOut, &buffer[IND_DATA0], dataOutLen);
- }
- else
- {
- dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(dataOut, &buffer[IND_DATAEXT0], dataOutLen);
- }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- return m_retVal;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
}
-
- ////////////////////////////////////////////////////////////////////
- // reqSetting (byte array & param variant)
- //
- // Send a message to the device and the data of acknowledge message
- // will be returned. Only valid for serial port connections
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter (optional)
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- // data[] contains the data of the acknowledge message (including param!!)
- // dataLen contains the number bytes returned
- //
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid)
+ else
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ buffer[IND_LEN] = 0;
+ }
+
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- if (m_fileOpen)
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data, &buffer[IND_DATA0], dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data, &buffer[IND_DATAEXT0], dataLen);
+ }
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ return (m_retVal = MTRV_OK);
}
-
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
-
- if (param != 0xFF)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = 1;
- buffer[IND_DATA0] = param;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- buffer[IND_LEN] = 0;
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
+ }
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (integer & no param variant)
+ //
+ // Sets a integer setting of the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the integer value to be used
+ // valuelen : Length in bytes of the value
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- dataLen = 0;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
- // Read next message or else timeout
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- // Acknowledge received
- if (buffer[IND_LEN] != EXTLENCODE)
- {
- dataLen = buffer[IND_LEN];
- memcpy(data, &buffer[IND_DATA0], dataLen);
- }
- else
- {
- dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
- memcpy(data, &buffer[IND_DATAEXT0], dataLen);
- }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- return (m_retVal = MTRV_OK);
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- else
- {
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
- }
- }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- return m_retVal;
- }
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
- ////////////////////////////////////////////////////////////////////
- // setSetting (integer & no param variant)
- //
- // Sets a integer setting of the device. This setting
- // can be an unsigned 1,2 or 4 bytes setting. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- // value : Contains the integer value to be used
- // valuelen : Length in bytes of the value
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- //
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
-
- if (m_fileOpen)
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
-
- if (!m_portOpen)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = (unsigned char) valuelen;
- swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Read next received message
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (integer & param variant)
+ //
+ // Sets a integer setting of the device. This setting
+ // can be an unsigned 1,2 or 4 bytes setting. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the integer value to be used
+ // valuelen : Length in bytes of the value
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // setSetting (integer & param variant)
- //
- // Sets a integer setting of the device. This setting
- // can be an unsigned 1,2 or 4 bytes setting. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter (optional)
- // bid : Bus ID of message to send (def 0xFF)
- // value : Contains the integer value to be used
- // valuelen : Length in bytes of the value
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- //
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = valuelen + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- if (param != 0xFF)
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- msgLen++;
- buffer[IND_LEN] = valuelen + 1;
- buffer[IND_DATA0] = param;
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
- else
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = (unsigned char) valuelen;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Read next received message
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & no param variant)
+ //
+ // Sets a float setting of the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the float value to be used
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // setSetting (float & no param variant)
- //
- // Sets a float setting of the device. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // bid : Bus ID of message to send (def 0xFF)
- // value : Contains the float value to be used
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = LEN_FLOAT;
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ calcChecksum(buffer, LEN_MSGHEADER + LEN_FLOAT);
+
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + LEN_FLOAT);
- if (m_fileOpen)
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
-
- if (!m_portOpen)
+ else if (buffer[IND_MID] == MID_ERROR)
{
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
- buffer[IND_LEN] = LEN_FLOAT;
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
- calcChecksum(buffer, LEN_MSGHEADER + LEN_FLOAT);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + LEN_FLOAT);
+ return m_retVal;
+ }
- // Read next received message
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & param variant)
+ //
+ // Sets a float setting of the device. Only valid
+ // for serial port connections.
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // bid : Bus ID of message to send (def 0xFF)
+ // value : Contains the float value to be used
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // setSetting (float & param variant)
- //
- // Sets a float setting of the device. Only valid
- // for serial port connections.
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter (optional)
- // bid : Bus ID of message to send (def 0xFF)
- // value : Contains the float value to be used
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- //
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT;
+ }
+
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
- if (param != 0xFF)
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- msgLen++;
- buffer[IND_LEN] = LEN_FLOAT + 1;
- buffer[IND_DATA0] = param;
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
- else
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = LEN_FLOAT;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Read next received message
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // setSetting (float & param & store variant)
+ //
+ // Sets a float setting of the device and with the Store field.
+ // Only valid for serial port connections
+ //
+ // Input
+ // mid : Message ID of message to send
+ // param : For messages that need a parameter (optional)
+ // value : Contains the float value to be used
+ // store ; Store in non-volatile memory (1) or not (0)
+ // bid : Bus ID of message to send (def 0xFF)
+ //
+ // Output
+ // = MTRV_OK if an Ack message is received
+ // = MTRV_RECVERRORMSG if an error message is received
+ // = MTRV_TIMEOUT if timeout occurred
+ //
+ //
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
- return m_retVal;
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
}
- ////////////////////////////////////////////////////////////////////
- // setSetting (float & param & store variant)
- //
- // Sets a float setting of the device and with the Store field.
- // Only valid for serial port connections
- //
- // Input
- // mid : Message ID of message to send
- // param : For messages that need a parameter (optional)
- // value : Contains the float value to be used
- // store ; Store in non-volatile memory (1) or not (0)
- // bid : Bus ID of message to send (def 0xFF)
- //
- // Output
- // = MTRV_OK if an Ack message is received
- // = MTRV_RECVERRORMSG if an error message is received
- // = MTRV_TIMEOUT if timeout occurred
- //
- //
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
+ if (!m_portOpen)
{
- unsigned char buffer[MAXMSGLEN ];
- short msgLen;
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
- if (m_fileOpen)
- {
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
- }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
- if (!m_portOpen)
- {
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
- }
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 2;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ }
+
+ swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ buffer[msgLen + LEN_FLOAT ] = store;
+ calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
- msgLen = LEN_MSGHEADER;
- buffer[IND_PREAMBLE] = PREAMBLE;
- buffer[IND_BID] = bid;
- buffer[IND_MID] = mid;
+ // Send message
+ writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
- if (param != 0xFF)
+ // Read next received message
+ if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
{
- msgLen++;
- buffer[IND_LEN] = LEN_FLOAT + 2;
- buffer[IND_DATA0] = param;
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
- else
+ else if (buffer[IND_MID] == MID_ERROR)
{
- buffer[IND_LEN] = LEN_FLOAT + 1;
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
+ }
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
- buffer[msgLen + LEN_FLOAT ] = store;
- calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
-
- // Send message
- writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
+ return m_retVal;
+ }
- // Read next received message
- if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
- {
- // Message received
- if (buffer[IND_MID] == (mid + 1))
- {
- return (m_retVal = MTRV_OK); // Acknowledge received
- }
- else if (buffer[IND_MID] == MID_ERROR)
- {
- m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
- }
- }
+ ////////////////////////////////////////////////////////////////////
+ // getDeviceMode
+ //
+ // Requests the current output mode/setting of input (file or serialport)
+ // the Outputmode, Outputsettings, DataLength & number of devices
+ // are stored in member variables of the MTComm class. These values
+ // are needed for the GetValue functions.
+ // The function optionally returns the number of devices
+ //
+ // File: expects the Configuration message at the start of the file
+ // which holds the OutputMode & OutputSettings. File position
+ // is after the first message
+ //
+ // Input
+ // Output
+ // numDevices : [optional] number of devices connected to port or
+ // found in configuration file
+ //
+ // returns MTRV_OK if the mode & settings are read
+ //
+ short CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
+ {
+ unsigned char mid = 0, data[MAXMSGLEN ];
+ short datalen;
- return m_retVal;
- }
-
- ////////////////////////////////////////////////////////////////////
- // getDeviceMode
- //
- // Requests the current output mode/setting of input (file or serialport)
- // the Outputmode, Outputsettings, DataLength & number of devices
- // are stored in member variables of the MTComm class. These values
- // are needed for the GetValue functions.
- // The function optionally returns the number of devices
- //
- // File: expects the Configuration message at the start of the file
- // which holds the OutputMode & OutputSettings. File position
- // is after the first message
- //
- // Input
- // Output
- // numDevices : [optional] number of devices connected to port or
- // found in configuration file
- //
- // returns MTRV_OK if the mode & settings are read
- //
- short CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
- {
- unsigned char mid = 0, data[MAXMSGLEN ];
- short datalen;
+ if (numDevices != nullptr)
+ {
+ *numDevices = 0;
+ }
- if (numDevices != nullptr)
+ // In case serial port is used (live device / XM or MT)
+ if (m_portOpen)
+ {
+ if (reqSetting(MID_INITBUS, data, datalen) != MTRV_OK)
{
- *numDevices = 0;
+ return m_retVal;
}
- // In case serial port is used (live device / XM or MT)
- if (m_portOpen)
+ // Retrieve outputmode + outputsettings
+ for (int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
{
- if (reqSetting(MID_INITBUS, data, datalen) != MTRV_OK)
+ if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) != MTRV_OK)
{
return m_retVal;
}
- // Retrieve outputmode + outputsettings
- for (int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
+ if (reqSetting(MID_REQOUTPUTSETTINGS, m_storedOutputSettings[BID_MT + i], BID_MT + i) != MTRV_OK)
{
- if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
-
- if (reqSetting(MID_REQOUTPUTSETTINGS, m_storedOutputSettings[BID_MT + i], BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
-
- if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) != MTRV_OK)
- {
- return m_retVal;
- }
+ return m_retVal;
}
- if (numDevices != nullptr)
+ if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) != MTRV_OK)
{
- *numDevices = datalen / LEN_DEVICEID;
+ return m_retVal;
}
+ }
- unsigned char masterDID[4];
- short DIDlen;
+ if (numDevices != nullptr)
+ {
+ *numDevices = datalen / LEN_DEVICEID;
+ }
- if (reqSetting(MID_REQDID, masterDID, DIDlen) != MTRV_OK)
- {
- return m_retVal;
- }
+ unsigned char masterDID[4];
+ short DIDlen;
- if (memcmp(masterDID, data, LEN_DEVICEID) != 0)
- {
- // Using an XbusMaster
- m_storedOutputMode[0] = OUTPUTMODE_XM;
- m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
- else
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
- }
+ if (reqSetting(MID_REQDID, masterDID, DIDlen) != MTRV_OK)
+ {
+ return m_retVal;
+ }
- return (m_retVal = MTRV_OK);
+ if (memcmp(masterDID, data, LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
}
- else if (m_fileOpen)
+ else
{
- // Configuration message should be the first message in the file
- setFilePos(0);
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ }
- if (readMessage(mid, data, datalen) == MTRV_OK)
- {
- if (mid == MID_CONFIGURATION)
- {
- unsigned short _numDevices = 0;
- swapEndian(data + CONF_NUMDEVICES, (unsigned char*) &_numDevices, CONF_NUMDEVICESLEN);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (m_fileOpen)
+ {
+ // Configuration message should be the first message in the file
+ setFilePos(0);
- for (unsigned int i = 0 ; i < _numDevices ; i++)
- {
- m_storedOutputMode[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputMode + BID_MT + i),
- CONF_OUTPUTMODELEN);
- m_storedOutputSettings[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputSettings + BID_MT + i),
- CONF_OUTPUTSETTINGSLEN);
- m_storedDataLength[BID_MT + i] = 0;
- swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN, (unsigned char*)(m_storedDataLength + BID_MT + i),
- CONF_DATALENGTHLEN);
- }
+ if (readMessage(mid, data, datalen) == MTRV_OK)
+ {
+ if (mid == MID_CONFIGURATION)
+ {
+ unsigned short _numDevices = 0;
+ swapEndian(data + CONF_NUMDEVICES, (unsigned char*) &_numDevices, CONF_NUMDEVICESLEN);
- if (numDevices != nullptr)
- {
- *numDevices = _numDevices;
- }
+ for (unsigned int i = 0 ; i < _numDevices ; i++)
+ {
+ m_storedOutputMode[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputMode + BID_MT + i),
+ CONF_OUTPUTMODELEN);
+ m_storedOutputSettings[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputSettings + BID_MT + i),
+ CONF_OUTPUTSETTINGSLEN);
+ m_storedDataLength[BID_MT + i] = 0;
+ swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN, (unsigned char*)(m_storedDataLength + BID_MT + i),
+ CONF_DATALENGTHLEN);
+ }
- if (memcmp(data + CONF_MASTERDID, data + CONF_DID, LEN_DEVICEID) != 0)
- {
- // Using an XbusMaster
- m_storedOutputMode[0] = OUTPUTMODE_XM;
- m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
- else
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
- }
+ if (numDevices != nullptr)
+ {
+ *numDevices = _numDevices;
+ }
- return (m_retVal = MTRV_OK);
+ if (memcmp(data + CONF_MASTERDID, data + CONF_DID, LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ else
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
}
- }
- return (m_retVal = MTRV_NOTSUCCESSFUL);
+ return (m_retVal = MTRV_OK);
+ }
}
- return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
}
- ////////////////////////////////////////////////////////////////////
- // setDeviceMode
- //
- // Sets the current output mode/setting of input (not for file-based
- // inputs)
- //
- // Input
- // OutputMode : OutputMode to be set in device & stored in MTComm
- // class member variable
- // OutputSettings : OutputSettings to be set in device & stored in
- // MTComm class member variable
- // Output
- //
- // returns MTRV_OK if the mode & settings are read
- //
- short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
- {
- // In case serial port is used (live XM / MT)
- if (m_portOpen)
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setDeviceMode
+ //
+ // Sets the current output mode/setting of input (not for file-based
+ // inputs)
+ //
+ // Input
+ // OutputMode : OutputMode to be set in device & stored in MTComm
+ // class member variable
+ // OutputSettings : OutputSettings to be set in device & stored in
+ // MTComm class member variable
+ // Output
+ //
+ // returns MTRV_OK if the mode & settings are read
+ //
+ short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ // In case serial port is used (live XM / MT)
+ if (m_portOpen)
+ {
+ // Set OutputMode
+ if (setSetting(MID_SETOUTPUTMODE, OutputMode, LEN_OUTPUTMODE, bid) != MTRV_OK)
{
- // Set OutputMode
- if (setSetting(MID_SETOUTPUTMODE, OutputMode, LEN_OUTPUTMODE, bid) != MTRV_OK)
- {
- return m_retVal;
- }
+ return m_retVal;
+ }
+
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
+ }
+ else
+ {
+ m_storedOutputMode[bid] = OutputMode;
+ }
- if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
- {
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
- }
- else
- {
- m_storedOutputMode[bid] = OutputMode;
- }
+ // Set OutputSettings
+ if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) != MTRV_OK)
+ {
+ return m_retVal;
+ }
- // Set OutputSettings
- if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) != MTRV_OK)
- {
- return m_retVal;
- }
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
+ }
+ else
+ {
+ m_storedOutputSettings[bid] = OutputSettings;
+ }
- if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
- {
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
- }
- else
- {
- m_storedOutputSettings[bid] = OutputSettings;
- }
+ // Get DataLength from device
+ if (OutputMode != OUTPUTMODE_XM)
+ {
+ unsigned long value;
- // Get DataLength from device
- if (OutputMode != OUTPUTMODE_XM)
+ if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK)
{
- unsigned long value;
-
- if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK)
+ if ((bid == BID_MASTER) || ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
{
- if ((bid == BID_MASTER) || ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
- {
- m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
- }
- else
- {
- m_storedDataLength[bid] = value;
- }
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
+ }
+ else
+ {
+ m_storedDataLength[bid] = value;
}
}
- else
- {
- m_storedDataLength[0] = LEN_SAMPLECNT;
- }
-
- return (m_retVal = MTRV_OK);
+ }
+ else
+ {
+ m_storedDataLength[0] = LEN_SAMPLECNT;
}
- return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ return (m_retVal = MTRV_OK);
}
- ////////////////////////////////////////////////////////////////////
- // getMode
- //
- // Gets the output mode/setting used in MTComm class and the corresponding
- // datalength. These variables are set by the functions GetDeviceMode,
- // SetDeviceMode or SetMode
- //
- // Input
- // Output
- // OutputMode : OutputMode stored in MTComm class member variable
- // OutputSettings : OutputSettings stored in MTComm class member variable
- //
- // returns always MTRV_OK
- //
- short CXsensMTiModule::getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid)
- {
- unsigned char nbid = (bid == BID_MASTER) ? 0 : bid;
- OutputMode = m_storedOutputMode[nbid];
- OutputSettings = m_storedOutputSettings[nbid];
- dataLength = (unsigned short) m_storedDataLength[nbid];
- return (m_retVal = MTRV_OK);
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getMode
+ //
+ // Gets the output mode/setting used in MTComm class and the corresponding
+ // datalength. These variables are set by the functions GetDeviceMode,
+ // SetDeviceMode or SetMode
+ //
+ // Input
+ // Output
+ // OutputMode : OutputMode stored in MTComm class member variable
+ // OutputSettings : OutputSettings stored in MTComm class member variable
+ //
+ // returns always MTRV_OK
+ //
+ short CXsensMTiModule::getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid)
+ {
+ unsigned char nbid = (bid == BID_MASTER) ? 0 : bid;
+ OutputMode = m_storedOutputMode[nbid];
+ OutputSettings = m_storedOutputSettings[nbid];
+ dataLength = (unsigned short) m_storedDataLength[nbid];
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // setMode
+ //
+ // Sets the output mode/setting used in MTComm class. Use the function
+ // GetDeviceMode to retrieve the current values of file/device.
+ // This function will also calculate the data length field
+ //
+ // Input
+ // OutputMode : OutputMode to be stored in MTComm class member variable
+ // OutputSettings : OutputSettings to be stored in MTComm class member variable
+ // Output
+ //
+ // returns always MTRV_OK
+ //
+ short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
}
- ////////////////////////////////////////////////////////////////////
- // setMode
- //
- // Sets the output mode/setting used in MTComm class. Use the function
- // GetDeviceMode to retrieve the current values of file/device.
- // This function will also calculate the data length field
- //
- // Input
- // OutputMode : OutputMode to be stored in MTComm class member variable
- // OutputSettings : OutputSettings to be stored in MTComm class member variable
- // Output
- //
- // returns always MTRV_OK
- //
- short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ m_storedOutputMode[nbid] = OutputMode;
+ m_storedOutputSettings[nbid] = OutputSettings;
+
+ if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
+ {
+ m_storedDataLength[nbid] = 0;
+ }
+ else
{
- unsigned char nbid = bid;
+ unsigned short dataLength = 0;
- if (nbid == BID_MASTER)
+ if (OutputMode & OUTPUTMODE_MT9)
{
- nbid = 0;
+ dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
}
-
- m_storedOutputMode[nbid] = OutputMode;
- m_storedOutputSettings[nbid] = OutputSettings;
-
- if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
+ else if (OutputMode == OUTPUTMODE_XM)
{
- m_storedDataLength[nbid] = 0;
+ // XbusMaster concatenates sample counter
+ dataLength = LEN_SAMPLECNT;
}
else
{
- unsigned short dataLength = 0;
-
- if (OutputMode & OUTPUTMODE_MT9)
+ if (OutputMode & OUTPUTMODE_RAW)
{
- dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
- }
- else if (OutputMode == OUTPUTMODE_XM)
- {
- // XbusMaster concatenates sample counter
- dataLength = LEN_SAMPLECNT;
+ dataLength = LEN_RAWDATA;
}
else
{
- if (OutputMode & OUTPUTMODE_RAW)
+ if (OutputMode & OUTPUTMODE_CALIB)
{
- dataLength = LEN_RAWDATA;
+ dataLength = LEN_CALIBDATA;
}
- else
- {
- if (OutputMode & OUTPUTMODE_CALIB)
- {
- dataLength = LEN_CALIBDATA;
- }
- if (OutputMode & OUTPUTMODE_ORIENT)
+ if (OutputMode & OUTPUTMODE_ORIENT)
+ {
+ switch (OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
{
- switch (OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
- {
- case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
- dataLength += LEN_ORIENT_QUATDATA;
- break;
+ case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
+ dataLength += LEN_ORIENT_QUATDATA;
+ break;
- case OUTPUTSETTINGS_ORIENTMODE_EULER:
- dataLength += LEN_ORIENT_EULERDATA;
- break;
+ case OUTPUTSETTINGS_ORIENTMODE_EULER:
+ dataLength += LEN_ORIENT_EULERDATA;
+ break;
- case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
- dataLength += LEN_ORIENT_MATRIXDATA;
- break;
+ case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
+ dataLength += LEN_ORIENT_MATRIXDATA;
+ break;
- default:
- break;
- }
+ default:
+ break;
}
}
+ }
- switch (OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
- {
- case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
- dataLength += LEN_SAMPLECNT;
- break;
+ switch (OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
+ {
+ case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
+ dataLength += LEN_SAMPLECNT;
+ break;
- default:
- break;
- }
+ default:
+ break;
}
-
- m_storedDataLength[nbid] = dataLength;
}
- // If not XbusMaster store also in BID_MT
- if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
- {
- m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
- m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
- m_storedDataLength[BID_MT ] = m_storedDataLength[0];
- }
+ m_storedDataLength[nbid] = dataLength;
+ }
- return (m_retVal = MTRV_OK);
+ // If not XbusMaster store also in BID_MT
+ if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
+ {
+ m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
+ m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
+ m_storedDataLength[BID_MT ] = m_storedDataLength[0];
}
- ////////////////////////////////////////////////////////////////////
- // getValue (unsigned short variant)
- //
- // Retrieves a unsigned short value from the data input parameter
- // This function is valid for the following value specifiers:
- // VALUE_RAW_TEMP
- // VALUE_SAMPLECNT
- //
- // Use getDeviceMode or setMode to initialize the Outputmode
- // and Outputsettings member variables used to retrieve the correct
- // value
- //
- // Input
- // valueSpec : Specifier of the value to be retrieved
- // data[] : Data field of a MTData / BusData message
- // bid : bus identifier of the device of which the
- // value should be returned (default = BID_MT)
- // Output
- // value : reference to unsigned short in which the retrieved
- // value will be returned
- //
- // Return value
- // MTRV_OK : value is successfully retrieved
- // != MTRV_OK : not successful
- //
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid)
+ return (m_retVal = MTRV_OK);
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getValue (unsigned short variant)
+ //
+ // Retrieves a unsigned short value from the data input parameter
+ // This function is valid for the following value specifiers:
+ // VALUE_RAW_TEMP
+ // VALUE_SAMPLECNT
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value : reference to unsigned short in which the retrieved
+ // value will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
{
- short offset = 0;
- unsigned char nbid = bid;
+ nbid = 0;
+ }
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
- {
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
- }
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ while (i < nbid)
{
- int i = 0;
-
- while (i < nbid)
- {
- offset += (short) m_storedDataLength[i++];
- }
+ offset += (short) m_storedDataLength[i++];
}
+ }
- // Check if data is unsigned short & available in data
- m_retVal = MTRV_INVALIDVALUESPEC;
+ // Check if data is unsigned short & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
- if (valueSpec == VALUE_RAW_TEMP)
+ if (valueSpec == VALUE_RAW_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
{
- if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
- {
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
- swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3, (unsigned char*) &value, LEN_RAW_TEMP);
- m_retVal = MTRV_OK;
- }
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3, (unsigned char*) &value, LEN_RAW_TEMP);
+ m_retVal = MTRV_OK;
}
- else if (valueSpec == VALUE_SAMPLECNT)
+ }
+ else if (valueSpec == VALUE_SAMPLECNT)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
{
- if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
{
- if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
- {
- offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
- }
-
- swapEndian(data + offset, (unsigned char*) &value, LEN_SAMPLECNT);
- m_retVal = MTRV_OK;
+ offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
}
+
+ swapEndian(data + offset, (unsigned char*) &value, LEN_SAMPLECNT);
+ m_retVal = MTRV_OK;
}
+ }
+
+ return m_retVal;
+ }
+
+ ////////////////////////////////////////////////////////////////////
+ // getValue (array of unsigned short variant)
+ //
+ // Retrieves an array of unsigned short values from the data input
+ // parameter. This function is valid for the following value specifiers:
+ // VALUE_RAW_ACC
+ // VALUE_RAW_GYR
+ // VALUE_RAW_MAG
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value[] : pointer to array of unsigned shorts in which the
+ // retrieved values will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
- return m_retVal;
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
}
- ////////////////////////////////////////////////////////////////////
- // getValue (array of unsigned short variant)
- //
- // Retrieves an array of unsigned short values from the data input
- // parameter. This function is valid for the following value specifiers:
- // VALUE_RAW_ACC
- // VALUE_RAW_GYR
- // VALUE_RAW_MAG
- //
- // Use getDeviceMode or setMode to initialize the Outputmode
- // and Outputsettings member variables used to retrieve the correct
- // value
- //
- // Input
- // valueSpec : Specifier of the value to be retrieved
- // data[] : Data field of a MTData / BusData message
- // bid : bus identifier of the device of which the
- // value should be returned (default = BID_MT)
- // Output
- // value[] : pointer to array of unsigned shorts in which the
- // retrieved values will be returned
- //
- // Return value
- // MTRV_OK : value is successfully retrieved
- // != MTRV_OK : not successful
- //
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
{
- short offset = 0;
- unsigned char nbid = bid;
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
- if (nbid == BID_MASTER)
- {
- nbid = 0;
- }
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ while (i < nbid)
{
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ offset += (short) m_storedDataLength[i++];
}
+ }
+
+ // Check if data is unsigned short, available in data & retrieve data
+ m_retVal = MTRV_INVALIDVALUESPEC;
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
+ if (valueSpec <= VALUE_RAW_MAG)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
{
- int i = 0;
+ offset += (short)(valueSpec * LEN_UNSIGSHORT * 3);
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
- while (i < nbid)
+ for (int i = 0 ; i < 3 ; i++)
{
- offset += (short) m_storedDataLength[i++];
+ swapEndian(data + offset + i * LEN_UNSIGSHORT, (unsigned char*) value + i * LEN_UNSIGSHORT, LEN_UNSIGSHORT);
}
- }
- // Check if data is unsigned short, available in data & retrieve data
- m_retVal = MTRV_INVALIDVALUESPEC;
+ m_retVal = MTRV_OK;
+ }
+ }
- //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
- if (valueSpec <= VALUE_RAW_MAG)
- {
- if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
- {
- offset += (short)(valueSpec * LEN_UNSIGSHORT * 3);
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ return m_retVal;
+ }
- for (int i = 0 ; i < 3 ; i++)
- {
- swapEndian(data + offset + i * LEN_UNSIGSHORT, (unsigned char*) value + i * LEN_UNSIGSHORT, LEN_UNSIGSHORT);
- }
+ ////////////////////////////////////////////////////////////////////
+ // getValue (array of floats variant)
+ //
+ // Retrieves an array of float values from the data input parameter.
+ // This function is valid for the following value specifiers:
+ // VALUE_TEMP
+ // VALUE_CALIB_ACC
+ // VALUE_CALIB_GYR
+ // VALUE_CALIB_MAG
+ // VALUE_ORIENT_QUAT
+ // VALUE_ORIENT_EULER
+ // VALUE_ORIENT_MATRIX
+ //
+ // Use getDeviceMode or setMode to initialize the Outputmode
+ // and Outputsettings member variables used to retrieve the correct
+ // value
+ //
+ // Input
+ // valueSpec : Specifier of the value to be retrieved
+ // data[] : Data field of a MTData / BusData message
+ // bid : bus identifier of the device of which the
+ // value should be returned (default = BID_MT)
+ // Output
+ // value[] : pointer to array of floats in which the
+ // retrieved values will be returned
+ //
+ // Return value
+ // MTRV_OK : value is successfully retrieved
+ // != MTRV_OK : not successful
+ //
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ int nElements = 0;
+ unsigned char nbid = bid;
- m_retVal = MTRV_OK;
- }
- }
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
- return m_retVal;
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
}
- ////////////////////////////////////////////////////////////////////
- // getValue (array of floats variant)
- //
- // Retrieves an array of float values from the data input parameter.
- // This function is valid for the following value specifiers:
- // VALUE_TEMP
- // VALUE_CALIB_ACC
- // VALUE_CALIB_GYR
- // VALUE_CALIB_MAG
- // VALUE_ORIENT_QUAT
- // VALUE_ORIENT_EULER
- // VALUE_ORIENT_MATRIX
- //
- // Use getDeviceMode or setMode to initialize the Outputmode
- // and Outputsettings member variables used to retrieve the correct
- // value
- //
- // Input
- // valueSpec : Specifier of the value to be retrieved
- // data[] : Data field of a MTData / BusData message
- // bid : bus identifier of the device of which the
- // value should be returned (default = BID_MT)
- // Output
- // value[] : pointer to array of floats in which the
- // retrieved values will be returned
- //
- // Return value
- // MTRV_OK : value is successfully retrieved
- // != MTRV_OK : not successful
- //
- short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
{
- short offset = 0;
- int nElements = 0;
- unsigned char nbid = bid;
+ int i = 0;
- if (nbid == BID_MASTER)
+ while (i < nbid)
{
- nbid = 0;
+ offset += (short) m_storedDataLength[i++];
}
+ }
+
+ // Check if data is float & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
- // Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ if (valueSpec == VALUE_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
{
- return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ nElements = LEN_TEMPDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
}
+ }
+ else if (valueSpec == VALUE_CALIB_ACC)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- // Calculate offset for XM input
- if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
{
- int i = 0;
-
- while (i < nbid)
- {
- offset += (short) m_storedDataLength[i++];
- }
+ nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
}
+ }
+ else if (valueSpec == VALUE_CALIB_GYR)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- // Check if data is float & available in data
- m_retVal = MTRV_INVALIDVALUESPEC;
-
- if (valueSpec == VALUE_TEMP)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
{
- if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
- {
- nElements = LEN_TEMPDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
}
- else if (valueSpec == VALUE_CALIB_ACC)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ }
+ else if (valueSpec == VALUE_CALIB_MAG)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
- {
- nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec == VALUE_CALIB_GYR)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
{
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
-
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
}
- else if (valueSpec == VALUE_CALIB_MAG)
- {
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ }
+ else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
- nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
- }
- else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
{
- offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
+ }
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
- {
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
- }
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
+ {
+ unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
- if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
+ switch (valueSpec)
{
- unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
-
- switch (valueSpec)
- {
- case VALUE_ORIENT_QUAT:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
- {
- nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
+ case VALUE_ORIENT_QUAT:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
+ {
+ nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
- break;
+ break;
- case VALUE_ORIENT_EULER:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
- {
- nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
+ case VALUE_ORIENT_EULER:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
+ {
+ nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
- break;
+ break;
- case VALUE_ORIENT_MATRIX:
- if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
- {
- nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
- m_retVal = MTRV_OK;
- }
+ case VALUE_ORIENT_MATRIX:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
+ {
+ nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
- break;
+ break;
- default:
- break;
- }
+ default:
+ break;
}
}
+ }
- if (m_retVal == MTRV_OK)
+ if (m_retVal == MTRV_OK)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
{
- if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
+ for (int i = 0 ; i < nElements ; i++)
{
- for (int i = 0 ; i < nElements ; i++)
- {
- swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) value + i * LEN_FLOAT, LEN_FLOAT);
- }
+ swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) value + i * LEN_FLOAT, LEN_FLOAT);
}
- else
- {
- int temp;
+ }
+ else
+ {
+ int temp;
- for (int i = 0 ; i < nElements ; i++)
- {
- swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) &temp, 4);
- value[i] = (float) temp / 1048576;
- }
+ for (int i = 0 ; i < nElements ; i++)
+ {
+ swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) &temp, 4);
+ value[i] = (float) temp / 1048576;
}
}
-
- return m_retVal;
}
- //////////////////////////////////////////////////////////////////////
- // getLastDeviceError
- //
- // Returns the last reported device error of the latest received Error
- // message
- //
- // Output
- // Error code
- short CXsensMTiModule::getLastDeviceError()
- {
- return m_deviceError;
- }
+ return m_retVal;
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // getLastDeviceError
+ //
+ // Returns the last reported device error of the latest received Error
+ // message
+ //
+ // Output
+ // Error code
+ short CXsensMTiModule::getLastDeviceError()
+ {
+ return m_deviceError;
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // getLastRetVal
+ //
+ // Returns the returned value of the last called function
+ //
+ // Output
+ // Return value
+ short CXsensMTiModule::getLastRetVal()
+ {
+ return m_retVal;
+ }
- //////////////////////////////////////////////////////////////////////
- // getLastRetVal
- //
- // Returns the returned value of the last called function
- //
- // Output
- // Return value
- short CXsensMTiModule::getLastRetVal()
+ //////////////////////////////////////////////////////////////////////
+ // setTimeOut
+ //
+ // Sets the time out value in milliseconds used by the functions
+ // Use 0 for infinite timeout
+ //
+ // Output
+ // MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
+ short CXsensMTiModule::setTimeOut(short timeOutMs)
+ {
+ if (timeOutMs >= 0)
{
- return m_retVal;
+ m_timeOut = timeOutMs;
+ return (m_retVal = MTRV_OK);
}
-
- //////////////////////////////////////////////////////////////////////
- // setTimeOut
- //
- // Sets the time out value in milliseconds used by the functions
- // Use 0 for infinite timeout
- //
- // Output
- // MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
- short CXsensMTiModule::setTimeOut(short timeOutMs)
+ else
{
- if (timeOutMs >= 0)
- {
- m_timeOut = timeOutMs;
- return (m_retVal = MTRV_OK);
- }
- else
- {
- return (m_retVal = MTRV_INVALIDTIMEOUT);
- }
+ return (m_retVal = MTRV_INVALIDTIMEOUT);
}
+ }
- //////////////////////////////////////////////////////////////////////
- // swapEndian
- //
- // Convert 2 or 4 bytes data from little to big endian or back
- //
- // Input
- // input : Pointer to data to be converted
- // output : Pointer where converted data is stored
- // length : Length of setting (0,2 & 4)
- //
- // Remarks:
- // Allocate enough bytes for output buffer
-
- void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
+ //////////////////////////////////////////////////////////////////////
+ // swapEndian
+ //
+ // Convert 2 or 4 bytes data from little to big endian or back
+ //
+ // Input
+ // input : Pointer to data to be converted
+ // output : Pointer where converted data is stored
+ // length : Length of setting (0,2 & 4)
+ //
+ // Remarks:
+ // Allocate enough bytes for output buffer
+
+ void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
+ {
+ switch (length)
{
- switch (length)
- {
- case 4:
- output[0] = input[3];
- output[1] = input[2];
- output[2] = input[1];
- output[3] = input[0];
- break;
+ case 4:
+ output[0] = input[3];
+ output[1] = input[2];
+ output[2] = input[1];
+ output[3] = input[0];
+ break;
- case 2:
- output[0] = input[1];
- output[1] = input[0];
- break;
+ case 2:
+ output[0] = input[1];
+ output[1] = input[0];
+ break;
- case 1:
- output[0] = input[0];
- break;
+ case 1:
+ output[0] = input[0];
+ break;
- default:
- for (int i = 0, j = length - 1 ; i < length ; i++, j--)
- {
- output[j] = input[i];
- }
+ default:
+ for (int i = 0, j = length - 1 ; i < length ; i++, j--)
+ {
+ output[j] = input[i];
+ }
- break;
- }
+ break;
}
+ }
- //////////////////////////////////////////////////////////////////////
- // calcChecksum
- //
- // Calculate and append checksum to msgBuffer
- //
- void CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
+ //////////////////////////////////////////////////////////////////////
+ // calcChecksum
+ //
+ // Calculate and append checksum to msgBuffer
+ //
+ void CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
+
+ for (i = 1; i < msgBufferLength ; i++)
{
- unsigned char checkSum = 0;
- int i;
+ checkSum += msgBuffer[i];
+ }
- for (i = 1; i < msgBufferLength ; i++)
- {
- checkSum += msgBuffer[i];
- }
+ msgBuffer[msgBufferLength] = -checkSum; // Store chksum
+ }
- msgBuffer[msgBufferLength] = -checkSum; // Store chksum
- }
+ //////////////////////////////////////////////////////////////////////
+ // checkChecksum
+ //
+ // Checks if message checksum is valid
+ //
+ // Output
+ // returns true checksum is OK
+ bool CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
- //////////////////////////////////////////////////////////////////////
- // checkChecksum
- //
- // Checks if message checksum is valid
- //
- // Output
- // returns true checksum is OK
- bool CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
+ for (i = 1; i < msgBufferLength ; i++)
{
- unsigned char checkSum = 0;
- int i;
-
- for (i = 1; i < msgBufferLength ; i++)
- {
- checkSum += msgBuffer[i];
- }
+ checkSum += msgBuffer[i];
+ }
- if (checkSum == 0)
- {
- return true;
- }
- else
- {
- return false;
- }
+ if (checkSum == 0)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
}
}
}
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
index 0a7bde26a..99fac5a3d 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
@@ -128,16 +128,14 @@
#ifdef _IMU_USE_XSENS_DEVICE_
-namespace IMU
+namespace IMU::Xsens
{
- namespace Xsens
- {
#ifndef INVALID_SET_FILE_POINTER
#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
#endif
- // Field message indexes
+ // Field message indexes
#define IND_PREAMBLE 0
#define IND_BID 1
#define IND_MID 2
@@ -147,7 +145,7 @@ namespace IMU
#define IND_LENEXTL 5
#define IND_DATAEXT0 6
- // Maximum number of sensors supported
+ // Maximum number of sensors supported
#define MAXDEVICES 20
#define PREAMBLE (unsigned char)0xFA
@@ -164,24 +162,24 @@ namespace IMU
#define LEN_UNSIGINT (unsigned short)4
#define LEN_FLOAT (unsigned short)4
- // Maximum message/data length
+ // Maximum message/data length
#define MAXDATALEN (unsigned short)2048
#define MAXSHORTDATALEN (unsigned short)254
#define MAXMSGLEN (unsigned short)(MAXDATALEN+7)
#define MAXSHORTMSGLEN (unsigned short)(MAXSHORTDATALEN+5)
- // DID Type (high nibble)
+ // DID Type (high nibble)
#define DID_TYPEH_MASK (unsigned long)0x00F00000
#define DID_TYPEH_MT (unsigned long)0x00000000
#define DID_TYPEH_XM (unsigned long)0x00100000
#define DID_TYPEH_MTI_MTX (unsigned long)0x00300000
- // All Message identifiers
- // WakeUp state messages
+ // All Message identifiers
+ // WakeUp state messages
#define MID_WAKEUP (unsigned char)0x3E
#define MID_WAKEUPACK (unsigned char)0x3F
- // Config state messages
+ // Config state messages
#define MID_REQDID (unsigned char)0x00
#define MID_DEVICEID (unsigned char)0x01
#define LEN_DEVICEID (unsigned short)4
@@ -193,14 +191,14 @@ namespace IMU
#define LEN_PERIOD (unsigned short)2
#define MID_SETPERIOD (unsigned char)0x04
#define MID_SETPERIODACK (unsigned char)0x05
- // XbusMaster
+ // XbusMaster
#define MID_SETBID (unsigned char)0x06
#define MID_SETBIDACK (unsigned char)0x07
#define MID_AUTOSTART (unsigned char)0x06
#define MID_AUTOSTARTACK (unsigned char)0x07
#define MID_BUSPWROFF (unsigned char)0x08
#define MID_BUSPWROFFACK (unsigned char)0x09
- // End XbusMaster
+ // End XbusMaster
#define MID_REQDATALENGTH (unsigned char)0x0A
#define MID_DATALENGTH (unsigned char)0x0B
#define LEN_DATALENGTH (unsigned short)2
@@ -215,7 +213,7 @@ namespace IMU
#define MID_REQFWREV (unsigned char)0x12
#define MID_FIRMWAREREV (unsigned char)0x13
#define LEN_FIRMWAREREV (unsigned short)3
- // XbusMaster
+ // XbusMaster
#define MID_REQBTDISABLE (unsigned char)0x14
#define MID_REQBTDISABLEACK (unsigned char)0x15
#define MID_DISABLEBT (unsigned char)0x14
@@ -224,18 +222,18 @@ namespace IMU
#define MID_REQOPMODEACK (unsigned char)0x17
#define MID_SETOPMODE (unsigned char)0x16
#define MID_SETOPMODEACK (unsigned char)0x17
- // End XbusMaster
+ // End XbusMaster
#define MID_REQBAUDRATE (unsigned char)0x18
#define MID_REQBAUDRATEACK (unsigned char)0x19
#define LEN_BAUDRATE (unsigned short)1
#define MID_SETBAUDRATE (unsigned char)0x18
#define MID_SETBAUDRATEACK (unsigned char)0x19
- // XbusMaster
+ // XbusMaster
#define MID_REQSYNCMODE (unsigned char)0x1A
#define MID_REQSYNCMODEACK (unsigned char)0x1B
#define MID_SETSYNCMODE (unsigned char)0x1A
#define MID_SETSYNCMODEACK (unsigned char)0x1B
- // End XbusMaster
+ // End XbusMaster
#define MID_REQPRODUCTCODE (unsigned char)0x1C
#define MID_PRODUCTCODE (unsigned char)0x1D
@@ -286,7 +284,7 @@ namespace IMU
#define MID_SETTRANSMITDELAY (unsigned char)0xDC
#define MID_SETTRANSMITDELAYACK (unsigned char)0xDD
- // Xbus Master
+ // Xbus Master
#define MID_REQXMERRORMODE (unsigned char)0x82
#define MID_REQXMERRORMODEACK (unsigned char)0x83
#define LEN_XMERRORMODE (unsigned short)2
@@ -298,7 +296,7 @@ namespace IMU
#define LEN_BUFFERSIZE (unsigned short)2
#define MID_SETBUFFERSIZE (unsigned char)0x84
#define MID_SETBUFFERSIZEACK (unsigned char)0x85
- // End Xbus Master
+ // End Xbus Master
#define MID_REQHEADING (unsigned char)0x82
#define MID_REQHEADINGACK (unsigned char)0x83
@@ -318,10 +316,10 @@ namespace IMU
#define MID_SETEXTOUTPUTMODE (unsigned char)0x86
#define MID_SETEXTOUTPUTMODEACK (unsigned char)0x87
- // XbusMaster
+ // XbusMaster
#define MID_REQBATLEVEL (unsigned char)0x88
#define MID_BATLEVEL (unsigned char)0x89
- // End XbusMaster
+ // End XbusMaster
#define MID_REQINITTRACKMODE (unsigned char)0x88
#define MID_REQINITTRACKMODEACK (unsigned char)0x89
@@ -332,19 +330,19 @@ namespace IMU
#define MID_STOREFILTERSTATE (unsigned char)0x8A
#define MID_STOREFILTERSTATEACK (unsigned char)0x8B
- // Measurement state
+ // Measurement state
#define MID_GOTOCONFIG (unsigned char)0x30
#define MID_GOTOCONFIGACK (unsigned char)0x31
#define MID_BUSDATA (unsigned char)0x32
#define MID_MTDATA (unsigned char)0x32
- // Manual
+ // Manual
#define MID_PREPAREDATA (unsigned char)0x32
#define MID_REQDATA (unsigned char)0x34
#define MID_REQDATAACK (unsigned char)0x35
- // MTData defines
- // Length of data blocks in bytes
+ // MTData defines
+ // Length of data blocks in bytes
#define LEN_RAWDATA (unsigned short)20
#define LEN_CALIBDATA (unsigned short)36
#define LEN_CALIB_ACCDATA (unsigned short)12
@@ -356,15 +354,15 @@ namespace IMU
#define LEN_SAMPLECNT (unsigned short)2
#define LEN_TEMPDATA (unsigned short)4
- // Length of data blocks in floats
+ // Length of data blocks in floats
#define LEN_CALIBDATA_FLT (unsigned short)9
#define LEN_ORIENT_QUATDATA_FLT (unsigned short)4
#define LEN_ORIENT_EULERDATA_FLT (unsigned short)3
#define LEN_ORIENT_MATRIXDATA_FLT (unsigned short)9
- // Indices of fields in DATA field of MTData message in bytes
- // use in combination with LEN_CALIB etc
- // Un-calibrated raw data
+ // Indices of fields in DATA field of MTData message in bytes
+ // use in combination with LEN_CALIB etc
+ // Un-calibrated raw data
#define IND_RAW_ACCX 0
#define IND_RAW_ACCY 2
#define IND_RAW_ACCZ 4
@@ -375,7 +373,7 @@ namespace IMU
#define IND_RAW_MAGY 14
#define IND_RAW_MAGZ 16
#define IND_RAW_TEMP 18
- // Calibrated data
+ // Calibrated data
#define IND_CALIB_ACCX 0
#define IND_CALIB_ACCY 4
#define IND_CALIB_ACCZ 8
@@ -385,16 +383,16 @@ namespace IMU
#define IND_CALIB_MAGX 24
#define IND_CALIB_MAGY 28
#define IND_CALIB_MAGZ 32
- // Orientation data - quat
+ // Orientation data - quat
#define IND_ORIENT_Q0 0
#define IND_ORIENT_Q1 4
#define IND_ORIENT_Q2 8
#define IND_ORIENT_Q3 12
- // Orientation data - euler
+ // Orientation data - euler
#define IND_ORIENT_ROLL 0
#define IND_ORIENT_PITCH 4
#define IND_ORIENT_YAW 8
- // Orientation data - matrix
+ // Orientation data - matrix
#define IND_ORIENT_A 0
#define IND_ORIENT_B 4
#define IND_ORIENT_C 8
@@ -404,12 +402,12 @@ namespace IMU
#define IND_ORIENT_G 24
#define IND_ORIENT_H 28
#define IND_ORIENT_I 32
- // Orientation data - euler
+ // Orientation data - euler
#define IND_SAMPLECNTH 0
#define IND_SAMPLECNTL 1
- // Indices of fields in DATA field of MTData message
- // Un-calibrated raw data
+ // Indices of fields in DATA field of MTData message
+ // Un-calibrated raw data
#define FLDNUM_RAW_ACCX 0
#define FLDNUM_RAW_ACCY 1
#define FLDNUM_RAW_ACCZ 2
@@ -420,7 +418,7 @@ namespace IMU
#define FLDNUM_RAW_MAGY 7
#define FLDNUM_RAW_MAGZ 8
#define FLDNUM_RAW_TEMP 9
- // Calibrated data
+ // Calibrated data
#define FLDNUM_CALIB_ACCX 0
#define FLDNUM_CALIB_ACCY 1
#define FLDNUM_CALIB_ACCZ 2
@@ -430,16 +428,16 @@ namespace IMU
#define FLDNUM_CALIB_MAGX 6
#define FLDNUM_CALIB_MAGY 7
#define FLDNUM_CALIB_MAGZ 8
- // Orientation data - quat
+ // Orientation data - quat
#define FLDNUM_ORIENT_Q0 0
#define FLDNUM_ORIENT_Q1 1
#define FLDNUM_ORIENT_Q2 2
#define FLDNUM_ORIENT_Q3 3
- // Orientation data - euler
+ // Orientation data - euler
#define FLDNUM_ORIENT_ROLL 0
#define FLDNUM_ORIENT_PITCH 1
#define FLDNUM_ORIENT_YAW 2
- // Orientation data - matrix
+ // Orientation data - matrix
#define FLDNUM_ORIENT_A 0
#define FLDNUM_ORIENT_B 1
#define FLDNUM_ORIENT_C 2
@@ -449,8 +447,8 @@ namespace IMU
#define FLDNUM_ORIENT_G 6
#define FLDNUM_ORIENT_H 7
#define FLDNUM_ORIENT_I 8
- // Length
- // Uncalibrated raw data
+ // Length
+ // Uncalibrated raw data
#define LEN_RAW_ACCX 2
#define LEN_RAW_ACCY 2
#define LEN_RAW_ACCZ 2
@@ -461,7 +459,7 @@ namespace IMU
#define LEN_RAW_MAGY 2
#define LEN_RAW_MAGZ 2
#define LEN_RAW_TEMP 2
- // Calibrated data
+ // Calibrated data
#define LEN_CALIB_ACCX 4
#define LEN_CALIB_ACCY 4
#define LEN_CALIB_ACCZ 4
@@ -471,16 +469,16 @@ namespace IMU
#define LEN_CALIB_MAGX 4
#define LEN_CALIB_MAGY 4
#define LEN_CALIB_MAGZ 4
- // Orientation data - quat
+ // Orientation data - quat
#define LEN_ORIENT_Q0 4
#define LEN_ORIENT_Q1 4
#define LEN_ORIENT_Q2 4
#define LEN_ORIENT_Q3 4
- // Orientation data - euler
+ // Orientation data - euler
#define LEN_ORIENT_ROLL 4
#define LEN_ORIENT_PITCH 4
#define LEN_ORIENT_YAW 4
- // Orientation data - matrix
+ // Orientation data - matrix
#define LEN_ORIENT_A 4
#define LEN_ORIENT_B 4
#define LEN_ORIENT_C 4
@@ -491,7 +489,7 @@ namespace IMU
#define LEN_ORIENT_H 4
#define LEN_ORIENT_I 4
- // Defines for getDataValue
+ // Defines for getDataValue
#define VALUE_RAW_ACC 0
#define VALUE_RAW_GYR 1
#define VALUE_RAW_MAG 2
@@ -507,14 +505,14 @@ namespace IMU
#define INVALIDSETTINGVALUE 0xFFFFFFFF
- // Valid in all states
+ // Valid in all states
#define MID_RESET (unsigned char)0x40
#define MID_RESETACK (unsigned char)0x41
#define MID_ERROR (unsigned char)0x42
#define LEN_ERROR (unsigned short)1
- // XbusMaster
+ // XbusMaster
#define MID_XMPWROFF (unsigned char)0x44
- // End XbusMaster
+ // End XbusMaster
#define MID_REQFILTERSETTINGS (unsigned char)0xA0
#define MID_REQFILTERSETTINGSACK (unsigned char)0xA1
@@ -530,12 +528,12 @@ namespace IMU
#define MID_RESETORIENTATIONACK (unsigned char)0xA5
#define LEN_RESETORIENTATION (unsigned short)2
- // All Messages
- // WakeUp state messages
+ // All Messages
+ // WakeUp state messages
#define MSG_WAKEUPLEN 5
#define MSG_WAKEUPACK (const unsigned char *)"\xFA\xFF\x3F\x00"
#define MSG_WAKEUPACKLEN 4
- // Config state messages
+ // Config state messages
#define MSG_REQDID (const unsigned char *)"\xFA\xFF\x00\x00"
#define MSG_REQDIDLEN 4
#define MSG_DEVICEIDLEN 9
@@ -620,11 +618,11 @@ namespace IMU
#define MSG_REQERRORMODE (const unsigned char *)"\xFA\xFF\xDA\x00"
#define MSG_REQERRORMODELEN 4
#define MSG_REQERRORMODEACKLEN 7
- // Measurement state - auto messages
+ // Measurement state - auto messages
#define MSG_GOTOCONFIG (const unsigned char *)"\xFA\xFF\x30\x00"
#define MSG_GOTOCONFIGLEN 4
#define MSG_GOTOCONFIGACKLEN 5
- // Measurement state - manual messages (Use DID = 0x01)
+ // Measurement state - manual messages (Use DID = 0x01)
#define MSG_GOTOCONFIGM (const unsigned char *)"\xFA\x01\x30\x00"
#define MSG_GOTOCONFIGMLEN 4
#define MSG_GOTOCONFIGMACKLEN 5
@@ -632,7 +630,7 @@ namespace IMU
#define MSG_PREPAREDATALEN 4
#define MSG_REQDATA (const unsigned char *)"\xFA\x01\x34\x00"
#define MSG_REQDATALEN 4
- // Valid in all states
+ // Valid in all states
#define MSG_RESET (const unsigned char *)"\xFA\xFF\x40\x00"
#define MSG_RESETLEN 4
#define MSG_RESETACKLEN 5
@@ -640,7 +638,7 @@ namespace IMU
#define MSG_XMPWROFFLEN 4
#define MSG_XMPWROFFACKLEN 5
- // Baudrate defines for SetBaudrate message
+ // Baudrate defines for SetBaudrate message
#define BAUDRATE_9K6 0x09
#define BAUDRATE_14K4 0x08
#define BAUDRATE_19K2 0x07
@@ -653,7 +651,7 @@ namespace IMU
#define BAUDRATE_460K8 0x00
#define BAUDRATE_921K6 0x80
- // Xbus protocol error codes (Error)
+ // Xbus protocol error codes (Error)
#define ERROR_NOBUSCOMM 0x01
#define ERROR_BUSNOTREADY 0x02
#define ERROR_PERIODINVALID 0x03
@@ -674,13 +672,13 @@ namespace IMU
#define ERROR_PARAMETERINVALID 0x21
#define ERROR_MEASUREMENTFAILED7 0x23
- // Error modes (SetErrorMode)
+ // Error modes (SetErrorMode)
#define ERRORMODE_IGNORE 0x0000
#define ERRORMODE_INCSAMPLECNT 0x0001
#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
- // Configuration message defines
+ // Configuration message defines
#define CONF_MASTERDID 0
#define CONF_PERIOD 4
#define CONF_OUTPUTSKIPFACTOR 6
@@ -690,14 +688,14 @@ namespace IMU
#define CONF_DATE 16
#define CONF_TIME 24
#define CONF_NUMDEVICES 96
- // Configuration sensor block properties
+ // Configuration sensor block properties
#define CONF_DID 98
#define CONF_DATALENGTH 102
#define CONF_OUTPUTMODE 104
#define CONF_OUTPUTSETTINGS 106
#define CONF_BLOCKLEN 20
- // To calculate the offset in data field for output mode of sensor #2 use
- // CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
+ // To calculate the offset in data field for output mode of sensor #2 use
+ // CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
#define CONF_MASTERDIDLEN 4
#define CONF_PERIODLEN 2
#define CONF_OUTPUTSKIPFACTORLEN 2
@@ -709,21 +707,21 @@ namespace IMU
#define CONF_RESERVED_CLIENTLEN 32
#define CONF_RESERVED_HOSTLEN 32
#define CONF_NUMDEVICESLEN 2
- // Configuration sensor block properties
+ // Configuration sensor block properties
#define CONF_DIDLEN 4
#define CONF_DATALENGTHLEN 2
#define CONF_OUTPUTMODELEN 2
#define CONF_OUTPUTSETTINGSLEN 4
- // Clock frequency for offset & pulse width
+ // Clock frequency for offset & pulse width
#define SYNC_CLOCKFREQ 29.4912e6
- // SyncIn params
+ // SyncIn params
#define PARAM_SYNCIN_MODE (const unsigned char)0x00
#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
- // SyncIn mode
+ // SyncIn mode
#define SYNCIN_DISABLED 0x0000
#define SYNCIN_EDGE_RISING 0x0001
#define SYNCIN_EDGE_FALLING 0x0002
@@ -733,13 +731,13 @@ namespace IMU
#define SYNCIN_EDGE_MASK 0x0003
#define SYNCIN_TYPE_MASK 0x000C
- // SyncOut params
+ // SyncOut params
#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
- // SyncOut mode
+ // SyncOut mode
#define SYNCOUT_DISABLED 0x0000
#define SYNCOUT_TYPE_TOGGLE 0x0001
#define SYNCOUT_TYPE_PULSE 0x0002
@@ -748,7 +746,7 @@ namespace IMU
#define SYNCOUT_TYPE_MASK 0x000F
#define SYNCOUT_POL_MASK 0x0010
- // Sample frequencies (SetPeriod)
+ // Sample frequencies (SetPeriod)
#define PERIOD_10HZ 0x2D00
#define PERIOD_12HZ 0x2580
#define PERIOD_15HZ 0x1E00
@@ -793,7 +791,7 @@ namespace IMU
#define PERIOD_480HZ 0x00F0
#define PERIOD_512HZ 0x00E1
- // OutputModes
+ // OutputModes
#define OUTPUTMODE_MT9 0x8000
#define OUTPUTMODE_XM 0x0000
#define OUTPUTMODE_RAW 0x4000
@@ -801,7 +799,7 @@ namespace IMU
#define OUTPUTMODE_CALIB 0x0002
#define OUTPUTMODE_ORIENT 0x0004
- // OutputSettings
+ // OutputSettings
#define OUTPUTSETTINGS_XM 0x00000001
#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
@@ -825,47 +823,47 @@ namespace IMU
#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
- // Extended Output Modes
+ // Extended Output Modes
#define EXTOUTPUTMODE_DISABLED 0x0000
#define EXTOUTPUTMODE_EULER 0x0001
- // Factory Output Mode
+ // Factory Output Mode
#define FACTORYOUTPUTMODE_DISABLE 0x0000
#define FACTORYOUTPUTMODE_DEFAULT 0x0001
#define FACTORYOUTPUTMODE_CUSTOM 0x0002
- // Initial tracking mode (SetInitTrackMode)
+ // Initial tracking mode (SetInitTrackMode)
#define INITTRACKMODE_DISABLED 0x0000
#define INITTRACKMODE_ENABLED 0x0001
- // Filter settings params
+ // Filter settings params
#define PARAM_FILTER_GAIN (const unsigned char)0x00
#define PARAM_FILTER_RHO (const unsigned char)0x01
#define DONOTSTORE 0x00
#define STORE 0x01
- // AMDSetting (SetAMD)
+ // AMDSetting (SetAMD)
#define AMDSETTING_DISABLED 0x0000
#define AMDSETTING_ENABLED 0x0001
- // Reset orientation message type
+ // Reset orientation message type
#define RESETORIENTATION_STORE 0
#define RESETORIENTATION_HEADING 1
#define RESETORIENTATION_GLOBAL 2
#define RESETORIENTATION_OBJECT 3
#define RESETORIENTATION_ALIGN 4
- // Send raw string mode
+ // Send raw string mode
#define SENDRAWSTRING_INIT 0
#define SENDRAWSTRING_DEFAULT 1
#define SENDRAWSTRING_SEND 2
- // Timeouts
+ // Timeouts
#define TO_DEFAULT 500
#define TO_INIT 250
#define TO_RETRY 50
- // openPort baudrates
+ // openPort baudrates
#ifdef WIN32
#define PBR_9600 CBR_9600
#define PBR_14K4 CBR_14400
@@ -892,7 +890,7 @@ namespace IMU
#define PBR_921K6 B921600
#endif
- // setFilePos defines
+ // setFilePos defines
#ifdef WIN32
#define FILEPOS_BEGIN FILE_BEGIN
#define FILEPOS_CURRENT FILE_CURRENT
@@ -903,7 +901,7 @@ namespace IMU
#define FILEPOS_END SEEK_END
#endif
- // Return values
+ // Return values
#define MTRV_OK 0 // Operation successful
#define MTRV_NOTSUCCESSFUL 1 // General no success return value
#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
@@ -920,97 +918,96 @@ namespace IMU
#define MTRV_ENDOFFILE 13 // End of file is reached
#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
- // mtGetDeviceMode or mtSetMode)
+ // mtGetDeviceMode or mtSetMode)
#define MTRV_INVALIDVALUESPEC 16 // Value specifier does not match value type or not available in data
#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
- class CXsensMTiModule
- {
- public:
- CXsensMTiModule();
- virtual ~CXsensMTiModule();
-
- // Low level general functions
- clock_t clockms();
-
- // Low level COM port / file functions
- short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short openPort(const char* portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short openFile(const char* fileName, bool createAlways = false);
- bool isPortOpen();
- bool isFileOpen();
- int readData(unsigned char* msgBuffer, const int nBytesToRead);
- int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
- void flush();
- void escape(unsigned long function);
- void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
- short getFileSize(unsigned long& fileSize);
- short close();
-
- // Read & write message functions
- short readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid = NULL);
- short readDataMessage(unsigned char data[], short& dataLen);
- short readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength);
- short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
- short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid = BID_MASTER);
- short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short* dataLen = NULL, unsigned char* bid = NULL);
-
- // Request & set setting functions
- short reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, float& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
- // Data-related functions
- short getDeviceMode(unsigned short* numDevices = NULL);
- short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid = BID_MASTER);
- short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
-
- // Generic MTComm functions
- short getLastDeviceError();
- short getLastRetVal();
- short setTimeOut(short timeOutMs);
- static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
- void calcChecksum(unsigned char* msgBuffer, const int msgBufferLength);
- bool checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength);
- protected:
- // member variables
+ class CXsensMTiModule
+ {
+ public:
+ CXsensMTiModule();
+ virtual ~CXsensMTiModule();
+
+ // Low level general functions
+ clock_t clockms();
+
+ // Low level COM port / file functions
+ short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openPort(const char* portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openFile(const char* fileName, bool createAlways = false);
+ bool isPortOpen();
+ bool isFileOpen();
+ int readData(unsigned char* msgBuffer, const int nBytesToRead);
+ int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
+ void flush();
+ void escape(unsigned long function);
+ void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
+ short getFileSize(unsigned long& fileSize);
+ short close();
+
+ // Read & write message functions
+ short readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid = NULL);
+ short readDataMessage(unsigned char data[], short& dataLen);
+ short readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength);
+ short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
+ short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid = BID_MASTER);
+ short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short* dataLen = NULL, unsigned char* bid = NULL);
+
+ // Request & set setting functions
+ short reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, float& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
+ // Data-related functions
+ short getDeviceMode(unsigned short* numDevices = NULL);
+ short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid = BID_MASTER);
+ short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
+
+ // Generic MTComm functions
+ short getLastDeviceError();
+ short getLastRetVal();
+ short setTimeOut(short timeOutMs);
+ static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
+ void calcChecksum(unsigned char* msgBuffer, const int msgBufferLength);
+ bool checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength);
+ protected:
+ // member variables
#ifdef WIN32
- HANDLE m_handle;
+ HANDLE m_handle;
#else
- int m_handle;
+ int m_handle;
#endif
- bool m_portOpen;
- bool m_fileOpen;
- short m_deviceError;
- short m_retVal;
- short m_timeOut;
- clock_t m_clkEnd;
-
- // OutputMode, OutputSettings & DataLength for connected devices + 1 for master
- unsigned long m_storedOutputMode[MAXDEVICES + 1];
- unsigned long m_storedOutputSettings[MAXDEVICES + 1];
- unsigned long m_storedDataLength[MAXDEVICES + 1];
-
- // Temporary buffer for excess bytes read in ReadMessageRaw
- unsigned char m_tempBuffer[MAXMSGLEN ];
- int m_nTempBufferLen;
-
- private:
- };
- }
+ bool m_portOpen;
+ bool m_fileOpen;
+ short m_deviceError;
+ short m_retVal;
+ short m_timeOut;
+ clock_t m_clkEnd;
+
+ // OutputMode, OutputSettings & DataLength for connected devices + 1 for master
+ unsigned long m_storedOutputMode[MAXDEVICES + 1];
+ unsigned long m_storedOutputSettings[MAXDEVICES + 1];
+ unsigned long m_storedDataLength[MAXDEVICES + 1];
+
+ // Temporary buffer for excess bytes read in ReadMessageRaw
+ unsigned char m_tempBuffer[MAXMSGLEN ];
+ int m_nTempBufferLen;
+
+ private:
+ };
}
#endif
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
index 77659db22..16b3e89e0 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
@@ -25,141 +25,141 @@
#include "XsensIMU.h"
-using namespace armarx;
-using namespace IMU;
-
-
-PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
+namespace armarx
{
- return PropertyDefinitionsPtr(new XsensIMUPropertyDefinitions(getConfigIdentifier()));
-}
+ using namespace IMU;
-void XsensIMU::frameAcquisitionTaskLoop()
-{
+ PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new XsensIMUPropertyDefinitions(getConfigIdentifier()));
+ }
- std::vector<float> offset(3, 0.0);
+ void XsensIMU::frameAcquisitionTaskLoop()
+ {
- int count = 0;
+ std::vector<float> offset(3, 0.0);
- IceUtil::Time startTime = armarx::TimeUtil::GetTime();
+ int count = 0;
- if (getProperty<bool>("EnableSimpleCalibration").getValue())
- {
- ARMARX_WARNING << "Estimation of the offset for the IMU, please do not move the IMU";
- while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) && !sensorTask->isStopped())
- {
+ IceUtil::Time startTime = armarx::TimeUtil::GetTime();
- while (HasPendingEvents())
+ if (getProperty<bool>("EnableSimpleCalibration").getValue())
+ {
+ ARMARX_WARNING << "Estimation of the offset for the IMU, please do not move the IMU";
+ while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) && !sensorTask->isStopped())
{
- ProcessPendingEvent();
- offset[0] += m_GyroscopeRotation[0];
- offset[1] += m_GyroscopeRotation[1];
- offset[2] += m_GyroscopeRotation[2];
+ while (HasPendingEvents())
+ {
+ ProcessPendingEvent();
+
+ offset[0] += m_GyroscopeRotation[0];
+ offset[1] += m_GyroscopeRotation[1];
+ offset[2] += m_GyroscopeRotation[2];
- count ++;
+ count ++;
+ }
}
- }
- offset[0] /= count;
- offset[1] /= count;
- offset[2] /= count;
+ offset[0] /= count;
+ offset[1] /= count;
+ offset[2] /= count;
- }
+ }
- while (!sensorTask->isStopped())
- {
-
- while (HasPendingEvents())
+ while (!sensorTask->isStopped())
{
- ProcessPendingEvent();
+ while (HasPendingEvents())
+ {
- TimestampVariantPtr now = TimestampVariant::nowPtr();
- IMUData data;
+ ProcessPendingEvent();
- data.acceleration.push_back(m_Accelaration[0]);
- data.acceleration.push_back(m_Accelaration[1]);
- data.acceleration.push_back(m_Accelaration[2]);
+ TimestampVariantPtr now = TimestampVariant::nowPtr();
+ IMUData data;
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[0] - offset[0]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[1] - offset[1]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[2] - offset[2]);
+ data.acceleration.push_back(m_Accelaration[0]);
+ data.acceleration.push_back(m_Accelaration[1]);
+ data.acceleration.push_back(m_Accelaration[2]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[0] - offset[0]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[1] - offset[1]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[2] - offset[2]);
- data.magneticRotation.push_back(m_MagneticRotation[0]);
- data.magneticRotation.push_back(m_MagneticRotation[1]);
- data.magneticRotation.push_back(m_MagneticRotation[2]);
+ data.magneticRotation.push_back(m_MagneticRotation[0]);
+ data.magneticRotation.push_back(m_MagneticRotation[1]);
+ data.magneticRotation.push_back(m_MagneticRotation[2]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[0]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[1]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
- data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
- IMUTopicPrx->reportSensorValues("XsensIMU", "XsensIMU", data, now);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[0]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[1]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
- }
+ IMUTopicPrx->reportSensorValues("XsensIMU", "XsensIMU", data, now);
- usleep(10000);
+ }
+
+ usleep(10000);
+ }
}
-}
-/*
-void XsensIMU::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
-{
- const IMUState CurrentState = pIMUDevice->GetIMUState();
+ /*
+ void XsensIMU::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
- TimestampVariantPtr now = TimestampVariant::nowPtr();
- IMUData data;
+ TimestampVariantPtr now = TimestampVariant::nowPtr();
+ IMUData data;
- data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[0]);
- data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[1]);
- data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[2]);
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[0]);
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[1]);
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[2]);
- data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[0]);
- data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[1]);
- data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[2]);
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[0]);
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[1]);
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[2]);
- data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[0]);
- data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[1]);
- data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[2]);
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[0]);
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[1]);
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[2]);
- data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[0]);
- data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[1]);
- data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[2]);
- data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[3]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[0]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[1]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[2]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[3]);
- IMUTopicPrx->reportSensorValues("XsensIMU", pIMUDevice->GetDeviceId(), data, now);
+ IMUTopicPrx->reportSensorValues("XsensIMU", pIMUDevice->GetDeviceId(), data, now);
-}
+ }
-*/
+ */
-void XsensIMU::onInitIMU()
-{
- sensorTask = new RunningTask<XsensIMU>(this, &XsensIMU::frameAcquisitionTaskLoop);
+ void XsensIMU::onInitIMU()
+ {
+ sensorTask = new RunningTask<XsensIMU>(this, &XsensIMU::frameAcquisitionTaskLoop);
- SetDispatchingMode(IMU::IIMUEventDispatcher::eDecoupled);
- SetMaximalPendingEvents(5);
+ SetDispatchingMode(IMU::IIMUEventDispatcher::eDecoupled);
+ SetMaximalPendingEvents(5);
- //IMUDevice.SetFusion(IMU::CIMUDevice::eGaussianFusion, 2);
- IMUDevice.RegisterEventDispatcher(this);
+ //IMUDevice.SetFusion(IMU::CIMUDevice::eGaussianFusion, 2);
+ IMUDevice.RegisterEventDispatcher(this);
- IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_120HZ);
-}
+ IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_120HZ);
+ }
-void XsensIMU::onStartIMU()
-{
- IMUDevice.Start(false);
- sensorTask->start();
-}
+ void XsensIMU::onStartIMU()
+ {
+ IMUDevice.Start(false);
+ sensorTask->start();
+ }
-void XsensIMU::onExitIMU()
-{
- IMUDevice.Stop();
- sensorTask->stop();
+ void XsensIMU::onExitIMU()
+ {
+ IMUDevice.Stop();
+ sensorTask->stop();
+ }
}
-
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
index a37459d3e..c73c555c8 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
@@ -24,13 +24,10 @@
#include "ArVizWidgetController.h"
-using namespace armarx;
-
-ArVizGuiPlugin::ArVizGuiPlugin()
+namespace armarx
{
- addWidget < ArVizWidgetController > ();
+ ArVizGuiPlugin::ArVizGuiPlugin()
+ {
+ addWidget < ArVizWidgetController > ();
+ }
}
-
-#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
-Q_EXPORT_PLUGIN2(armarx_gui_ArVizGuiPlugin, ArVizGuiPlugin)
-#endif
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
index 844f53569..5b2ab4dd6 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
@@ -38,10 +38,9 @@ namespace armarx
public armarx::ArmarXGuiPlugin
{
Q_OBJECT
-#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
Q_INTERFACES(ArmarXGuiInterface)
Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
-#endif
+
public:
/**
* All widgets exposed by this plugin are added in the constructor
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 1093b19bf..881438c62 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -30,860 +30,861 @@
#include <QMessageBox>
#include <QTimer>
-using namespace armarx;
-
-struct armarx::ArVizWidgetBatchCallback : IceUtil::Shared
+namespace armarx
{
- class ArVizWidgetController* this_;
-
- void onSuccess(viz::data::RecordingBatch const& batch)
- {
- this_->onGetBatchAsync(batch);
- }
-
- void onFailure(Ice::Exception const& ex)
+ struct ArVizWidgetBatchCallback : IceUtil::Shared
{
- ARMARX_WARNING << "Failed to get batch async.\nReason:"
- << ex;
- }
-};
+ class ArVizWidgetController* this_;
-ArVizWidgetController::ArVizWidgetController()
-{
- widget.setupUi(getWidget());
+ void onSuccess(viz::data::RecordingBatch const& batch)
+ {
+ this_->onGetBatchAsync(batch);
+ }
- replayTimer = new QTimer(this);
- connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&ArVizWidgetController::onReplayTimerTick));
+ void onFailure(Ice::Exception const& ex)
+ {
+ ARMARX_WARNING << "Failed to get batch async.\nReason:"
+ << ex;
+ }
+ };
- connect(widget.layerTree, &QTreeWidget::itemChanged, this, &ArVizWidgetController::layerTreeChanged);
- connect(widget.expandButton, &QPushButton::clicked, this, &ArVizWidgetController::onExpandAll);
- connect(widget.collapseButton, &QPushButton::clicked, this, &ArVizWidgetController::onCollapseAll);
- connect(widget.filterEdit, &QLineEdit::textChanged, this, &ArVizWidgetController::onFilterTextChanged);
- connect(widget.hideAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideAll);
- connect(widget.showAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowAll);
- connect(widget.hideFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideFiltered);
- connect(widget.showFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowFiltered);
+ ArVizWidgetController::ArVizWidgetController()
+ {
+ widget.setupUi(getWidget());
- connect(widget.recordingStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onStartRecording);
- connect(widget.recordingStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onStopRecording);
+ replayTimer = new QTimer(this);
+ connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&ArVizWidgetController::onReplayTimerTick));
- connect(widget.refreshRecordingsButton, &QPushButton::clicked, this, &ArVizWidgetController::onRefreshRecordings);
- connect(widget.recordingList, &QListWidget::currentItemChanged, this, &ArVizWidgetController::onRecordingSelectionChanged);
+ connect(widget.layerTree, &QTreeWidget::itemChanged, this, &ArVizWidgetController::layerTreeChanged);
+ connect(widget.expandButton, &QPushButton::clicked, this, &ArVizWidgetController::onExpandAll);
+ connect(widget.collapseButton, &QPushButton::clicked, this, &ArVizWidgetController::onCollapseAll);
+ connect(widget.filterEdit, &QLineEdit::textChanged, this, &ArVizWidgetController::onFilterTextChanged);
+ connect(widget.hideAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideAll);
+ connect(widget.showAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowAll);
+ connect(widget.hideFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideFiltered);
+ connect(widget.showFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowFiltered);
- connect(widget.replayRevisionSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &ArVizWidgetController::onReplaySpinChanged);
- connect(widget.replayRevisionSlider, QOverload<int>::of(&QSlider::valueChanged), this, &ArVizWidgetController::onReplaySliderChanged);
+ connect(widget.recordingStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onStartRecording);
+ connect(widget.recordingStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onStopRecording);
- connect(widget.replayStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStart);
- connect(widget.replayStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStop);
- connect(widget.replayTimedButton, &QPushButton::toggled, this, &ArVizWidgetController::onReplayTimedStart);
+ connect(widget.refreshRecordingsButton, &QPushButton::clicked, this, &ArVizWidgetController::onRefreshRecordings);
+ connect(widget.recordingList, &QListWidget::currentItemChanged, this, &ArVizWidgetController::onRecordingSelectionChanged);
- connect(this, &ArVizWidgetController::connectGui, this, &ArVizWidgetController::onConnectGui, Qt::QueuedConnection);
- connect(this, &ArVizWidgetController::disconnectGui, this, &ArVizWidgetController::onDisconnectGui, Qt::QueuedConnection);
+ connect(widget.replayRevisionSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &ArVizWidgetController::onReplaySpinChanged);
+ connect(widget.replayRevisionSlider, QOverload<int>::of(&QSlider::valueChanged), this, &ArVizWidgetController::onReplaySliderChanged);
+ connect(widget.replayStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStart);
+ connect(widget.replayStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStop);
+ connect(widget.replayTimedButton, &QPushButton::toggled, this, &ArVizWidgetController::onReplayTimedStart);
- // We need a callback from the visualizer, when the layers have changed
- // So we can update the tree accordingly
- visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const & layers)
- {
- layersChanged(layers);
- };
+ connect(this, &ArVizWidgetController::connectGui, this, &ArVizWidgetController::onConnectGui, Qt::QueuedConnection);
+ connect(this, &ArVizWidgetController::disconnectGui, this, &ArVizWidgetController::onDisconnectGui, Qt::QueuedConnection);
- replayTimer->start(33);
-}
-ArVizWidgetController::~ArVizWidgetController()
-{
+ // We need a callback from the visualizer, when the layers have changed
+ // So we can update the tree accordingly
+ visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const & layers)
+ {
+ layersChanged(layers);
+ };
-}
+ replayTimer->start(33);
+ }
-void ArVizWidgetController::onInitComponent()
-{
- if (storageName.size() > 0)
+ ArVizWidgetController::~ArVizWidgetController()
{
- usingProxy(storageName);
+
}
- ARMARX_IMPORTANT << "OnInit: " << storageName;
- callbackData = new ArVizWidgetBatchCallback();
- callbackData->this_ = this;
- callback = viz::newCallback_StorageInterface_getRecordingBatch(
- callbackData,
- &ArVizWidgetBatchCallback::onSuccess,
- &ArVizWidgetBatchCallback::onFailure);
-}
+ void ArVizWidgetController::onInitComponent()
+ {
+ if (storageName.size() > 0)
+ {
+ usingProxy(storageName);
+ }
+ ARMARX_IMPORTANT << "OnInit: " << storageName;
-void armarx::ArVizWidgetController::onExitComponent()
-{
-}
+ callbackData = new ArVizWidgetBatchCallback();
+ callbackData->this_ = this;
+ callback = viz::newCallback_StorageInterface_getRecordingBatch(
+ callbackData,
+ &ArVizWidgetBatchCallback::onSuccess,
+ &ArVizWidgetBatchCallback::onFailure);
+ }
-void ArVizWidgetController::onConnectComponent()
-{
- getProxy(storage, storageName);
- visualizer.startAsync(storage);
+ void armarx::ArVizWidgetController::onExitComponent()
+ {
+ }
- // Changes to UI elements are only allowed in the GUI thread
- emit connectGui();
-}
+ void ArVizWidgetController::onConnectComponent()
+ {
+ getProxy(storage, storageName);
+ visualizer.startAsync(storage);
-void armarx::ArVizWidgetController::onDisconnectComponent()
-{
- visualizer.stop();
+ // Changes to UI elements are only allowed in the GUI thread
+ emit connectGui();
+ }
- // Changes to UI elements are only allowed in the GUI thread
- emit disconnectGui();
-}
+ void armarx::ArVizWidgetController::onDisconnectComponent()
+ {
+ visualizer.stop();
-void ArVizWidgetController::onConnectGui()
-{
- onRefreshRecordings();
- currentRecordingSelected = false;
- changeMode(ArVizWidgetMode::Live);
-}
+ // Changes to UI elements are only allowed in the GUI thread
+ emit disconnectGui();
+ }
-void ArVizWidgetController::onDisconnectGui()
-{
- changeMode(ArVizWidgetMode::NotConnected);
-}
+ void ArVizWidgetController::onConnectGui()
+ {
+ onRefreshRecordings();
+ currentRecordingSelected = false;
+ changeMode(ArVizWidgetMode::Live);
+ }
-void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
-{
- // Iterate over all items and activate/deactivate layers accordingly
- int componentCount = widget.layerTree->topLevelItemCount();
- for (int compIndex = 0; compIndex < componentCount; ++compIndex)
+ void ArVizWidgetController::onDisconnectGui()
{
- QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
- std::string component = componentItem->text(0).toStdString();
- Qt::CheckState componentCheck = componentItem->checkState(0);
- int layerCount = componentItem->childCount();
+ changeMode(ArVizWidgetMode::NotConnected);
+ }
- if (componentItem == item)
+ void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
+ {
+ // Iterate over all items and activate/deactivate layers accordingly
+ int componentCount = widget.layerTree->topLevelItemCount();
+ for (int compIndex = 0; compIndex < componentCount; ++compIndex)
{
- // The parent was selected or deselected, so all children should be set accordingly
- ARMARX_VERBOSE << "Setting all children of " << component << " to " << (componentCheck == Qt::Checked);
- for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
+ QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
+ std::string component = componentItem->text(0).toStdString();
+ Qt::CheckState componentCheck = componentItem->checkState(0);
+ int layerCount = componentItem->childCount();
+
+ if (componentItem == item)
{
- QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
- if (layerItem->checkState(0) != componentCheck)
+ // The parent was selected or deselected, so all children should be set accordingly
+ ARMARX_VERBOSE << "Setting all children of " << component << " to " << (componentCheck == Qt::Checked);
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
- layerItem->setCheckState(0, componentCheck);
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
+ if (layerItem->checkState(0) != componentCheck)
+ {
+ layerItem->setCheckState(0, componentCheck);
+ }
}
+ return;
}
- return;
- }
- for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
- {
- QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
- std::string layer = layerItem->text(0).toStdString();
- bool layerVisible = (layerItem->checkState(0) == Qt::Checked);
- ARMARX_VERBOSE << "Layer: " << layer << ", Visible: " << layerVisible;
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
+ {
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
+ std::string layer = layerItem->text(0).toStdString();
+ bool layerVisible = (layerItem->checkState(0) == Qt::Checked);
+ ARMARX_VERBOSE << "Layer: " << layer << ", Visible: " << layerVisible;
- viz::CoinLayerID layerID(component, layer);
- visualizer.showLayer(layerID, layerVisible);
+ viz::CoinLayerID layerID(component, layer);
+ visualizer.showLayer(layerID, layerVisible);
+ }
}
}
-}
-void ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
-{
- QTreeWidget* tree = widget.layerTree;
-
- std::map<std::string, QTreeWidgetItem*> currentComponents;
- std::map<viz::CoinLayerID, QTreeWidgetItem*> currentLayers;
- int componentCount = widget.layerTree->topLevelItemCount();
- for (int compIndex = 0; compIndex < componentCount; ++compIndex)
+ void ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
{
- QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
- std::string component = componentItem->text(0).toStdString();
- currentComponents.emplace(component, componentItem);
+ QTreeWidget* tree = widget.layerTree;
- int layerCount = componentItem->childCount();
- for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
+ std::map<std::string, QTreeWidgetItem*> currentComponents;
+ std::map<viz::CoinLayerID, QTreeWidgetItem*> currentLayers;
+ int componentCount = widget.layerTree->topLevelItemCount();
+ for (int compIndex = 0; compIndex < componentCount; ++compIndex)
{
- QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
- std::string layer = layerItem->text(0).toStdString();
-
- viz::CoinLayerID layerID(component, layer);
- currentLayers.emplace(layerID, layerItem);
- }
- }
+ QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
+ std::string component = componentItem->text(0).toStdString();
+ currentComponents.emplace(component, componentItem);
- // We need to determine which layers are new and where to append them
- QTreeWidgetItem* currentItem = nullptr;
- bool componentWasNew = false;
- std::string currentComponent;
- for (auto& entry : layers)
- {
- std::string const& component = entry.first;
- if (component != currentComponent)
- {
- auto iter = currentComponents.find(component);
- if (iter == currentComponents.end())
+ int layerCount = componentItem->childCount();
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
- // Create a new item
- currentItem = new QTreeWidgetItem(tree);
- currentItem->setText(0, QString::fromStdString(component));
- // A new item is visible by default
- currentItem->setCheckState(0, Qt::Checked);
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
+ std::string layer = layerItem->text(0).toStdString();
- componentWasNew = true;
+ viz::CoinLayerID layerID(component, layer);
+ currentLayers.emplace(layerID, layerItem);
}
- else
+ }
+
+ // We need to determine which layers are new and where to append them
+ QTreeWidgetItem* currentItem = nullptr;
+ bool componentWasNew = false;
+ std::string currentComponent;
+ for (auto& entry : layers)
+ {
+ std::string const& component = entry.first;
+ if (component != currentComponent)
{
- // Item exists already
- currentItem = iter->second;
+ auto iter = currentComponents.find(component);
+ if (iter == currentComponents.end())
+ {
+ // Create a new item
+ currentItem = new QTreeWidgetItem(tree);
+ currentItem->setText(0, QString::fromStdString(component));
+ // A new item is visible by default
+ currentItem->setCheckState(0, Qt::Checked);
- componentWasNew = false;
- }
+ componentWasNew = true;
+ }
+ else
+ {
+ // Item exists already
+ currentItem = iter->second;
- currentComponent = component;
- }
+ componentWasNew = false;
+ }
- auto iter = currentLayers.find(entry);
- if (iter == currentLayers.end())
- {
- // Create a new item
- std::string const& layer = entry.second;
- QTreeWidgetItem* layerItem = new QTreeWidgetItem;
- layerItem->setText(0, QString::fromStdString(layer));
- // A new item is visible by default
- layerItem->setCheckState(0, Qt::Checked);
+ currentComponent = component;
+ }
- if (currentItem)
+ auto iter = currentLayers.find(entry);
+ if (iter == currentLayers.end())
{
- currentItem->addChild(layerItem);
+ // Create a new item
+ std::string const& layer = entry.second;
+ QTreeWidgetItem* layerItem = new QTreeWidgetItem;
+ layerItem->setText(0, QString::fromStdString(layer));
+ // A new item is visible by default
+ layerItem->setCheckState(0, Qt::Checked);
+
+ if (currentItem)
+ {
+ currentItem->addChild(layerItem);
- if (componentWasNew)
+ if (componentWasNew)
+ {
+ // Initially expand new items
+ tree->expandItem(currentItem);
+ }
+ }
+ else
{
- // Initially expand new items
- tree->expandItem(currentItem);
+ ARMARX_WARNING << deactivateSpam(10, entry.first + entry.second)
+ << "Invalid component/layer ID: "
+ << entry.first << " / " << entry.second;
}
}
else
{
- ARMARX_WARNING << deactivateSpam(10, entry.first + entry.second)
- << "Invalid component/layer ID: "
- << entry.first << " / " << entry.second;
+ // Item exists already ==> nothing to be done
}
}
- else
- {
- // Item exists already ==> nothing to be done
- }
}
-}
-void ArVizWidgetController::onCollapseAll(bool)
-{
- widget.layerTree->collapseAll();
-}
+ void ArVizWidgetController::onCollapseAll(bool)
+ {
+ widget.layerTree->collapseAll();
+ }
-void ArVizWidgetController::onExpandAll(bool)
-{
- widget.layerTree->expandAll();
-}
+ void ArVizWidgetController::onExpandAll(bool)
+ {
+ widget.layerTree->expandAll();
+ }
-void ArVizWidgetController::onHideAll(bool)
-{
- showAllLayers(false);
-}
+ void ArVizWidgetController::onHideAll(bool)
+ {
+ showAllLayers(false);
+ }
-void ArVizWidgetController::onShowAll(bool)
-{
- showAllLayers(true);
-}
+ void ArVizWidgetController::onShowAll(bool)
+ {
+ showAllLayers(true);
+ }
-void ArVizWidgetController::onHideFiltered(bool)
-{
- showFilteredLayers(false);
-}
+ void ArVizWidgetController::onHideFiltered(bool)
+ {
+ showFilteredLayers(false);
+ }
-void ArVizWidgetController::onShowFiltered(bool)
-{
- showFilteredLayers(true);
-}
+ void ArVizWidgetController::onShowFiltered(bool)
+ {
+ showFilteredLayers(true);
+ }
-void ArVizWidgetController::onFilterTextChanged(const QString& filter)
-{
- // Now we need to show these matches and hide the other items
- // Is there a better way? Probably, via QSortFilterProxyModel...
- QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
- for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
+ void ArVizWidgetController::onFilterTextChanged(const QString& filter)
{
- QTreeWidgetItem* item = *iter;
- QString itemText = item->text(0);
- bool matches = filter.size() == 0 || itemText.contains(rx);
- item->setHidden(!matches);
- if (matches && item->parent())
+ // Now we need to show these matches and hide the other items
+ // Is there a better way? Probably, via QSortFilterProxyModel...
+ QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
+ for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
{
- // Make parent visible if a child is visible
- item->parent()->setHidden(false);
+ QTreeWidgetItem* item = *iter;
+ QString itemText = item->text(0);
+ bool matches = filter.size() == 0 || itemText.contains(rx);
+ item->setHidden(!matches);
+ if (matches && item->parent())
+ {
+ // Make parent visible if a child is visible
+ item->parent()->setHidden(false);
+ }
}
}
-}
-void ArVizWidgetController::showAllLayers(bool visible)
-{
- widget.layerTree->blockSignals(true);
-
- for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
+ void ArVizWidgetController::showAllLayers(bool visible)
{
- QTreeWidgetItem* item = *iter;
- item->setCheckState(0, visible ? Qt::Checked : Qt::Unchecked);
- }
-
- widget.layerTree->blockSignals(false);
-
- // Update shown/hidden layers
- layerTreeChanged(nullptr, 0);
-}
-
-void ArVizWidgetController::showFilteredLayers(bool visible)
-{
- widget.layerTree->blockSignals(true);
-
- QString filter = widget.filterEdit->text();
- QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
+ widget.layerTree->blockSignals(true);
- for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
- {
- QTreeWidgetItem* item = *iter;
- QString itemText = item->text(0);
- bool matches = filter.size() == 0 || itemText.contains(rx);
- if (matches)
+ for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
{
+ QTreeWidgetItem* item = *iter;
item->setCheckState(0, visible ? Qt::Checked : Qt::Unchecked);
}
+
+ widget.layerTree->blockSignals(false);
+
+ // Update shown/hidden layers
+ layerTreeChanged(nullptr, 0);
}
- widget.layerTree->blockSignals(false);
+ void ArVizWidgetController::showFilteredLayers(bool visible)
+ {
+ widget.layerTree->blockSignals(true);
- // Update shown/hidden layers
- layerTreeChanged(nullptr, 0);
-}
+ QString filter = widget.filterEdit->text();
+ QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
-void ArVizWidgetController::onStartRecording()
-{
- std::string recordingID = widget.recordingIdTextBox->text().toStdString();
+ for (QTreeWidgetItemIterator iter(widget.layerTree); *iter; ++iter)
+ {
+ QTreeWidgetItem* item = *iter;
+ QString itemText = item->text(0);
+ bool matches = filter.size() == 0 || itemText.contains(rx);
+ if (matches)
+ {
+ item->setCheckState(0, visible ? Qt::Checked : Qt::Unchecked);
+ }
+ }
- std::string runningID = storage->startRecording(recordingID);
+ widget.layerTree->blockSignals(false);
- changeMode(ArVizWidgetMode::Recording);
+ // Update shown/hidden layers
+ layerTreeChanged(nullptr, 0);
+ }
- if (runningID.size() > 0)
+ void ArVizWidgetController::onStartRecording()
{
- std::string message = "There is already a recording running with ID '"
- + runningID + "'. \n"
- + "You have to stop the running recording first";
- QMessageBox::information(widget.tabWidget,
- "Recording running",
- QString::fromStdString(message));
+ std::string recordingID = widget.recordingIdTextBox->text().toStdString();
+ std::string runningID = storage->startRecording(recordingID);
+ changeMode(ArVizWidgetMode::Recording);
- return;
- }
-}
+ if (runningID.size() > 0)
+ {
+ std::string message = "There is already a recording running with ID '"
+ + runningID + "'. \n"
+ + "You have to stop the running recording first";
+ QMessageBox::information(widget.tabWidget,
+ "Recording running",
+ QString::fromStdString(message));
-void ArVizWidgetController::onStopRecording()
-{
- storage->stopRecording();
- onRefreshRecordings();
- changeMode(ArVizWidgetMode::Live);
-}
-void ArVizWidgetController::onRefreshRecordings()
-{
- allRecordings = storage->getAllRecordings();
- std::sort(allRecordings.begin(), allRecordings.end(),
- [](viz::data::Recording const & lhs, viz::data::Recording const & rhs)
- {
- return lhs.id < rhs.id;
- });
+ return;
+ }
+ }
- std::string currentText;
- QListWidgetItem* currentItem = widget.recordingList->currentItem();
- if (currentItem)
+ void ArVizWidgetController::onStopRecording()
{
- currentText = currentItem->text().toStdString();
+ storage->stopRecording();
+
+ onRefreshRecordings();
+ changeMode(ArVizWidgetMode::Live);
}
- widget.recordingList->clear();
- int currentTextIndex = -1;
- int index = 0;
- for (auto& recording : allRecordings)
+ void ArVizWidgetController::onRefreshRecordings()
{
- if (recording.id == currentText)
+ allRecordings = storage->getAllRecordings();
+ std::sort(allRecordings.begin(), allRecordings.end(),
+ [](viz::data::Recording const & lhs, viz::data::Recording const & rhs)
{
- currentTextIndex = index;
+ return lhs.id < rhs.id;
+ });
+
+ std::string currentText;
+ QListWidgetItem* currentItem = widget.recordingList->currentItem();
+ if (currentItem)
+ {
+ currentText = currentItem->text().toStdString();
}
- widget.recordingList->addItem(QString::fromStdString(recording.id));
- ++index;
- }
- if (currentTextIndex > 0)
- {
- widget.recordingList->setCurrentRow(currentTextIndex);
- }
-}
+ widget.recordingList->clear();
+ int currentTextIndex = -1;
+ int index = 0;
+ for (auto& recording : allRecordings)
+ {
+ if (recording.id == currentText)
+ {
+ currentTextIndex = index;
+ }
+ widget.recordingList->addItem(QString::fromStdString(recording.id));
+ ++index;
+ }
-void ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous)
-{
- if (!current)
- {
- return;
+ if (currentTextIndex > 0)
+ {
+ widget.recordingList->setCurrentRow(currentTextIndex);
+ }
}
- std::string selectedID = current->text().toStdString();
- for (viz::data::Recording const& recording : allRecordings)
+ void ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous)
{
- if (recording.id == selectedID)
+ if (!current)
{
- selectRecording(recording);
- break;
+ return;
+ }
+
+ std::string selectedID = current->text().toStdString();
+ for (viz::data::Recording const& recording : allRecordings)
+ {
+ if (recording.id == selectedID)
+ {
+ selectRecording(recording);
+ break;
+ }
}
}
-}
-static std::string timestampToString(long timestampInMicroSeconds, bool showMS = false)
-{
- IceUtil::Time time = IceUtil::Time::microSeconds(timestampInMicroSeconds);
- std::string timeString = time.toDateTime();
- if (!showMS)
+ static std::string timestampToString(long timestampInMicroSeconds, bool showMS = false)
{
- timeString = timeString.substr(0, timeString.size() - 4);
- };
- return timeString;
-}
+ IceUtil::Time time = IceUtil::Time::microSeconds(timestampInMicroSeconds);
+ std::string timeString = time.toDateTime();
+ if (!showMS)
+ {
+ timeString = timeString.substr(0, timeString.size() - 4);
+ };
+ return timeString;
+ }
-void ArVizWidgetController::onReplaySpinChanged(int newValue)
-{
- widget.replayRevisionSlider->setValue(newValue);
-}
+ void ArVizWidgetController::onReplaySpinChanged(int newValue)
+ {
+ widget.replayRevisionSlider->setValue(newValue);
+ }
-void ArVizWidgetController::onReplaySliderChanged(int newValue)
-{
- if (currentRevision != newValue)
+ void ArVizWidgetController::onReplaySliderChanged(int newValue)
{
- long timestamp = replayToRevision(newValue);
- if (timestamp > 0)
+ if (currentRevision != newValue)
{
- currentRevision = newValue;
- currentTimestamp = timestamp;
- widget.replayRevisionSpinBox->setValue(newValue);
+ long timestamp = replayToRevision(newValue);
+ if (timestamp > 0)
+ {
+ currentRevision = newValue;
+ currentTimestamp = timestamp;
+ widget.replayRevisionSpinBox->setValue(newValue);
- widget.replayTimestampLabel->setText(QString::fromStdString(
- timestampToString(timestamp, true)
- ));
- }
- else
- {
- widget.replayRevisionSlider->setValue(currentRevision);
+ widget.replayTimestampLabel->setText(QString::fromStdString(
+ timestampToString(timestamp, true)
+ ));
+ }
+ else
+ {
+ widget.replayRevisionSlider->setValue(currentRevision);
+ }
}
}
-}
-void ArVizWidgetController::selectRecording(const viz::data::Recording& recording)
-{
- // Update recording description
- widget.recordingIdLabel->setText(QString::fromStdString(recording.id));
-
- widget.recordingRevisionLabel->setText(QString::fromStdString(
- std::to_string(recording.firstRevision) +
- " - " +
- std::to_string(recording.lastRevision)));
-
- std::string firstTimeString = timestampToString(recording.firstTimestampInMicroSeconds);
- std::string lastTimeString = timestampToString(recording.lastTimestampInMicroSeconds);
-
- widget.recordingTimestampLabel->setText(QString::fromStdString(
- firstTimeString +
- " - " +
- lastTimeString));
-
- IceUtil::Time duration = IceUtil::Time::microSeconds(
- recording.lastTimestampInMicroSeconds -
- recording.firstTimestampInMicroSeconds);
- int durationSeconds = duration.toSeconds();
- widget.recordingDurationLabel->setText(QString::fromStdString(
- std::to_string(durationSeconds) + " s"));
-
- widget.recordingBatchesLabel->setText(QString::fromStdString(
- std::to_string(recording.batchHeaders.size())
- ));
-
- // Update replay control
- currentRecording = recording;
- currentRecordingSelected = true;
- enableWidgetAccordingToMode();
-}
-
-void ArVizWidgetController::onReplayStart(bool)
-{
- if (!currentRecordingSelected)
- {
- ARMARX_WARNING << "No recording selected, so no replay can be started";
- return;
- }
+ void ArVizWidgetController::selectRecording(const viz::data::Recording& recording)
{
- std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
- recordingBatchCache.clear();
- }
-
- visualizer.stop();
+ // Update recording description
+ widget.recordingIdLabel->setText(QString::fromStdString(recording.id));
- changeMode(ArVizWidgetMode::ReplayingManual);
+ widget.recordingRevisionLabel->setText(QString::fromStdString(
+ std::to_string(recording.firstRevision) +
+ " - " +
+ std::to_string(recording.lastRevision)));
- widget.replayRevisionSpinBox->blockSignals(true);
- widget.replayRevisionSpinBox->setMinimum(currentRecording.firstRevision);
- widget.replayRevisionSpinBox->setMaximum(currentRecording.lastRevision);
- widget.replayRevisionSpinBox->setValue(currentRecording.firstRevision);
- widget.replayRevisionSpinBox->blockSignals(false);
+ std::string firstTimeString = timestampToString(recording.firstTimestampInMicroSeconds);
+ std::string lastTimeString = timestampToString(recording.lastTimestampInMicroSeconds);
- widget.replayRevisionSlider->blockSignals(true);
- widget.replayRevisionSlider->setMinimum(currentRecording.firstRevision);
- widget.replayRevisionSlider->setMaximum(currentRecording.lastRevision);
- widget.replayRevisionSlider->setValue(currentRecording.firstRevision);
- widget.replayRevisionSlider->blockSignals(false);
+ widget.recordingTimestampLabel->setText(QString::fromStdString(
+ firstTimeString +
+ " - " +
+ lastTimeString));
- currentRevision = -1;
- onReplaySliderChanged(widget.replayRevisionSlider->value());
-}
-
-void ArVizWidgetController::onReplayStop(bool)
-{
- visualizer.startAsync(storage);
+ IceUtil::Time duration = IceUtil::Time::microSeconds(
+ recording.lastTimestampInMicroSeconds -
+ recording.firstTimestampInMicroSeconds);
+ int durationSeconds = duration.toSeconds();
+ widget.recordingDurationLabel->setText(QString::fromStdString(
+ std::to_string(durationSeconds) + " s"));
- changeMode(ArVizWidgetMode::Live);
-}
+ widget.recordingBatchesLabel->setText(QString::fromStdString(
+ std::to_string(recording.batchHeaders.size())
+ ));
-long ArVizWidgetController::replayToRevision(long revision)
-{
- if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
- {
- ARMARX_WARNING << "Cannot call replayToRevision, when not in replaying mode.\n"
- << "Actual mode: " << (int)mode;
- return -1;
+ // Update replay control
+ currentRecording = recording;
+ currentRecordingSelected = true;
+ enableWidgetAccordingToMode();
}
- viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
- for (auto& batchHeader : currentRecording.batchHeaders)
+ void ArVizWidgetController::onReplayStart(bool)
{
- if (batchHeader.firstRevision <= revision &&
- revision <= batchHeader.lastRevision)
+ if (!currentRecordingSelected)
{
- matchingBatchHeader = &batchHeader;
- break;
+ ARMARX_WARNING << "No recording selected, so no replay can be started";
+ return;
+ }
+ {
+ std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
+ recordingBatchCache.clear();
}
- }
- if (matchingBatchHeader == nullptr)
- {
- ARMARX_WARNING << "Could not find batch for revision: " << revision;
- return -1;
- }
-
- viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
- ARMARX_VERBOSE << "Replaying to revision : " << revision
- << "\nGot batch: " << batch.header.firstRevision << " - " << batch.header.lastRevision
- << "\nUpdates: " << batch.updates.size()
- << "\nInitial state: " << batch.initialState.size();
+ visualizer.stop();
+ changeMode(ArVizWidgetMode::ReplayingManual);
- auto revisionLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
- {
- return lhs.revision < rhs.revision;
- };
- viz::data::TimestampedLayerUpdate pivot;
- pivot.revision = revision;
- auto updateBegin = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
- auto updateEnd = std::upper_bound(updateBegin, batch.updates.end(), pivot, revisionLess);
+ widget.replayRevisionSpinBox->blockSignals(true);
+ widget.replayRevisionSpinBox->setMinimum(currentRecording.firstRevision);
+ widget.replayRevisionSpinBox->setMaximum(currentRecording.lastRevision);
+ widget.replayRevisionSpinBox->setValue(currentRecording.firstRevision);
+ widget.replayRevisionSpinBox->blockSignals(false);
- // TODO: Optimize: Only start from the last update position
- std::map<std::string, viz::data::LayerUpdate const*> updates;
+ widget.replayRevisionSlider->blockSignals(true);
+ widget.replayRevisionSlider->setMinimum(currentRecording.firstRevision);
+ widget.replayRevisionSlider->setMaximum(currentRecording.lastRevision);
+ widget.replayRevisionSlider->setValue(currentRecording.firstRevision);
+ widget.replayRevisionSlider->blockSignals(false);
- for (auto& update : batch.initialState)
- {
- updates[update.update.name] = &update.update;
- }
- for (auto updateIter = batch.updates.begin(); updateIter != updateEnd; ++updateIter)
- {
- updates[updateIter->update.name] = &updateIter->update;
+ currentRevision = -1;
+ onReplaySliderChanged(widget.replayRevisionSlider->value());
}
- for (auto& pair : updates)
- {
- visualizer.apply(*pair.second);
- }
-
- return updateBegin->timestampInMicroseconds;
-}
-long ArVizWidgetController::getRevisionForTimestamp(long timestamp)
-{
- if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
+ void ArVizWidgetController::onReplayStop(bool)
{
- ARMARX_WARNING << "Cannot call replayToTimestamp, when not in replaying mode.\n"
- << "Actual mode: " << (int)mode;
- return -1;
- }
+ visualizer.startAsync(storage);
- if (timestamp < currentRecording.firstTimestampInMicroSeconds)
- {
- ARMARX_INFO << "Requested timestamp is earlier than recording: " << timestampToString(timestamp);
- return -1;
+ changeMode(ArVizWidgetMode::Live);
}
- viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
- for (auto& batchHeader : currentRecording.batchHeaders)
+ long ArVizWidgetController::replayToRevision(long revision)
{
- matchingBatchHeader = &batchHeader;
- if (timestamp <= batchHeader.lastTimestampInMicroSeconds)
+ if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
- break;
+ ARMARX_WARNING << "Cannot call replayToRevision, when not in replaying mode.\n"
+ << "Actual mode: " << (int)mode;
+ return -1;
}
- }
- viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
-
- auto timestampLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
- {
- return lhs.timestampInMicroseconds < rhs.timestampInMicroseconds;
- };
- viz::data::TimestampedLayerUpdate pivot;
- pivot.timestampInMicroseconds = timestamp;
- auto updateEnd = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, timestampLess);
- if (updateEnd == batch.updates.end())
- {
- return -2;
- }
- if (updateEnd != batch.updates.begin())
- {
- // lower_bound gives the first entry with a later timestamp then the goal
- // So we should only apply updates before this point
- updateEnd -= 1;
- }
+ viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
+ for (auto& batchHeader : currentRecording.batchHeaders)
+ {
+ if (batchHeader.firstRevision <= revision &&
+ revision <= batchHeader.lastRevision)
+ {
+ matchingBatchHeader = &batchHeader;
+ break;
+ }
+ }
+ if (matchingBatchHeader == nullptr)
+ {
+ ARMARX_WARNING << "Could not find batch for revision: " << revision;
+ return -1;
+ }
- long revisionBeforeTimestamp = updateEnd->revision;
- return revisionBeforeTimestamp;
-}
+ viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
-void ArVizWidgetController::onReplayTimedStart(bool checked)
-{
- if (checked)
- {
+ ARMARX_VERBOSE << "Replaying to revision : " << revision
+ << "\nGot batch: " << batch.header.firstRevision << " - " << batch.header.lastRevision
+ << "\nUpdates: " << batch.updates.size()
+ << "\nInitial state: " << batch.initialState.size();
- changeMode(ArVizWidgetMode::ReplayingTimed);
- replayCurrentTimestamp = currentTimestamp;
- }
- else
- {
- changeMode(ArVizWidgetMode::ReplayingManual);
- }
-}
-void ArVizWidgetController::onReplayTimerTick()
-{
- if (mode != ArVizWidgetMode::ReplayingTimed)
- {
- return;
- }
- double replaySpeed = widget.replaySpeedSpinBox->value();
+ auto revisionLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
+ {
+ return lhs.revision < rhs.revision;
+ };
+ viz::data::TimestampedLayerUpdate pivot;
+ pivot.revision = revision;
+ auto updateBegin = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
+ auto updateEnd = std::upper_bound(updateBegin, batch.updates.end(), pivot, revisionLess);
- replayCurrentTimestamp += 33000 * replaySpeed;
+ // TODO: Optimize: Only start from the last update position
+ std::map<std::string, viz::data::LayerUpdate const*> updates;
- long revision = getRevisionForTimestamp(replayCurrentTimestamp);
- if (revision == -2)
- {
- if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ for (auto& update : batch.initialState)
{
- replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ updates[update.update.name] = &update.update;
}
- else
+ for (auto updateIter = batch.updates.begin(); updateIter != updateEnd; ++updateIter)
{
- revision = currentRecording.lastRevision;
+ updates[updateIter->update.name] = &updateIter->update;
+ }
+ for (auto& pair : updates)
+ {
+ visualizer.apply(*pair.second);
}
- }
- if (revision >= 0)
- {
- widget.replayRevisionSlider->setValue(revision);
- }
-}
-
-void ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
-{
- mode = newMode;
- enableWidgetAccordingToMode();
-}
+ return updateBegin->timestampInMicroseconds;
+ }
-void ArVizWidgetController::enableWidgetAccordingToMode()
-{
- switch (mode)
+ long ArVizWidgetController::getRevisionForTimestamp(long timestamp)
{
- case ArVizWidgetMode::NotConnected:
+ if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
- widget.recordingStartButton->setDisabled(true);
- widget.recordingStopButton->setDisabled(true);
- widget.replayStartButton->setDisabled(true);
- widget.replayStopButton->setDisabled(true);
- widget.replayControlGroupBox->setDisabled(true);
+ ARMARX_WARNING << "Cannot call replayToTimestamp, when not in replaying mode.\n"
+ << "Actual mode: " << (int)mode;
+ return -1;
}
- break;
- case ArVizWidgetMode::Live:
+
+ if (timestamp < currentRecording.firstTimestampInMicroSeconds)
{
- widget.recordingStartButton->setDisabled(false);
- widget.recordingStopButton->setDisabled(true);
- widget.replayStartButton->setDisabled(false);
- widget.replayStopButton->setDisabled(true);
- widget.replayControlGroupBox->setDisabled(true);
- widget.recordingList->setDisabled(false);
+ ARMARX_INFO << "Requested timestamp is earlier than recording: " << timestampToString(timestamp);
+ return -1;
}
- break;
- case ArVizWidgetMode::Recording:
+
+ viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
+ for (auto& batchHeader : currentRecording.batchHeaders)
{
- widget.recordingStartButton->setDisabled(true);
- widget.recordingStopButton->setDisabled(false);
- widget.replayStartButton->setDisabled(true);
- widget.replayStopButton->setDisabled(true);
- widget.replayControlGroupBox->setDisabled(true);
+ matchingBatchHeader = &batchHeader;
+ if (timestamp <= batchHeader.lastTimestampInMicroSeconds)
+ {
+ break;
+ }
}
- break;
- case ArVizWidgetMode::ReplayingManual:
+
+ viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
+
+ auto timestampLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
{
- widget.recordingStartButton->setDisabled(true);
- widget.recordingStopButton->setDisabled(true);
- widget.replayStartButton->setDisabled(true);
- widget.replayStopButton->setDisabled(false);
- widget.replayControlGroupBox->setDisabled(false);
- widget.replayRevisionSlider->setDisabled(false);
- widget.replayRevisionSpinBox->setDisabled(false);
- widget.recordingList->setDisabled(true);
+ return lhs.timestampInMicroseconds < rhs.timestampInMicroseconds;
+ };
+ viz::data::TimestampedLayerUpdate pivot;
+ pivot.timestampInMicroseconds = timestamp;
+ auto updateEnd = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, timestampLess);
+ if (updateEnd == batch.updates.end())
+ {
+ return -2;
}
- break;
- case ArVizWidgetMode::ReplayingTimed:
+ if (updateEnd != batch.updates.begin())
{
- widget.recordingStartButton->setDisabled(true);
- widget.recordingStopButton->setDisabled(true);
- widget.replayStartButton->setDisabled(true);
- widget.replayStopButton->setDisabled(false);
- widget.replayControlGroupBox->setDisabled(false);
- widget.replayRevisionSlider->setDisabled(true);
- widget.replayRevisionSpinBox->setDisabled(true);
- widget.recordingList->setDisabled(true);
+ // lower_bound gives the first entry with a later timestamp then the goal
+ // So we should only apply updates before this point
+ updateEnd -= 1;
}
- break;
+
+ long revisionBeforeTimestamp = updateEnd->revision;
+ return revisionBeforeTimestamp;
}
- if (!currentRecordingSelected)
+ void ArVizWidgetController::onReplayTimedStart(bool checked)
{
- widget.replayStartButton->setDisabled(true);
- widget.replayStopButton->setDisabled(true);
+ if (checked)
+ {
+
+ changeMode(ArVizWidgetMode::ReplayingTimed);
+ replayCurrentTimestamp = currentTimestamp;
+ }
+ else
+ {
+ changeMode(ArVizWidgetMode::ReplayingManual);
+ }
}
-}
-void ArVizWidgetController::onGetBatchAsync(const viz::data::RecordingBatch& batch)
-{
- // We received a batch asynchronously ==> Update the cache
- ARMARX_INFO << "Received async batch: " << batch.header.index;
- std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
+ void ArVizWidgetController::onReplayTimerTick()
+ {
+ if (mode != ArVizWidgetMode::ReplayingTimed)
+ {
+ return;
+ }
+ double replaySpeed = widget.replaySpeedSpinBox->value();
- auto& entry = recordingBatchCache[batch.header.index];
- entry.data = batch;
- entry.lastUsed = IceUtil::Time::now();
-}
+ replayCurrentTimestamp += 33000 * replaySpeed;
-viz::data::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
-{
- IceUtil::Time now = IceUtil::Time::now();
+ long revision = getRevisionForTimestamp(replayCurrentTimestamp);
+ if (revision == -2)
+ {
+ if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ {
+ replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ }
+ else
+ {
+ revision = currentRecording.lastRevision;
+ }
+ }
+ if (revision >= 0)
+ {
+ widget.replayRevisionSlider->setValue(revision);
+ }
+ }
+
+ void ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
+ {
+ mode = newMode;
- std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
+ enableWidgetAccordingToMode();
+ }
- auto iter = recordingBatchCache.find(index);
- if (iter != recordingBatchCache.end())
+ void ArVizWidgetController::enableWidgetAccordingToMode()
{
- // Start prefetching neighbouring batches
- bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
- if (!asyncPrefetchIsRunning)
+ switch (mode)
{
- if (index + 1 < (long)currentRecording.batchHeaders.size()
- && recordingBatchCache.count(index + 1) == 0)
+ case ArVizWidgetMode::NotConnected:
{
- callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
+ widget.recordingStartButton->setDisabled(true);
+ widget.recordingStopButton->setDisabled(true);
+ widget.replayStartButton->setDisabled(true);
+ widget.replayStopButton->setDisabled(true);
+ widget.replayControlGroupBox->setDisabled(true);
}
- else if (index > 0 && recordingBatchCache.count(index - 1) == 0)
+ break;
+ case ArVizWidgetMode::Live:
{
- callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
+ widget.recordingStartButton->setDisabled(false);
+ widget.recordingStopButton->setDisabled(true);
+ widget.replayStartButton->setDisabled(false);
+ widget.replayStopButton->setDisabled(true);
+ widget.replayControlGroupBox->setDisabled(true);
+ widget.recordingList->setDisabled(false);
}
-
+ break;
+ case ArVizWidgetMode::Recording:
+ {
+ widget.recordingStartButton->setDisabled(true);
+ widget.recordingStopButton->setDisabled(false);
+ widget.replayStartButton->setDisabled(true);
+ widget.replayStopButton->setDisabled(true);
+ widget.replayControlGroupBox->setDisabled(true);
+ }
+ break;
+ case ArVizWidgetMode::ReplayingManual:
+ {
+ widget.recordingStartButton->setDisabled(true);
+ widget.recordingStopButton->setDisabled(true);
+ widget.replayStartButton->setDisabled(true);
+ widget.replayStopButton->setDisabled(false);
+ widget.replayControlGroupBox->setDisabled(false);
+ widget.replayRevisionSlider->setDisabled(false);
+ widget.replayRevisionSpinBox->setDisabled(false);
+ widget.recordingList->setDisabled(true);
+ }
+ break;
+ case ArVizWidgetMode::ReplayingTimed:
+ {
+ widget.recordingStartButton->setDisabled(true);
+ widget.recordingStopButton->setDisabled(true);
+ widget.replayStartButton->setDisabled(true);
+ widget.replayStopButton->setDisabled(false);
+ widget.replayControlGroupBox->setDisabled(false);
+ widget.replayRevisionSlider->setDisabled(true);
+ widget.replayRevisionSpinBox->setDisabled(true);
+ widget.recordingList->setDisabled(true);
+ }
+ break;
}
- auto& entry = iter->second;
- entry.lastUsed = now;
- return entry.data;
+ if (!currentRecordingSelected)
+ {
+ widget.replayStartButton->setDisabled(true);
+ widget.replayStopButton->setDisabled(true);
+ }
}
- ARMARX_INFO << "Batch #" << index << " is not in the cache. Getting synchronously, blocking GUI...";
+ void ArVizWidgetController::onGetBatchAsync(const viz::data::RecordingBatch& batch)
+ {
+ // We received a batch asynchronously ==> Update the cache
+ ARMARX_INFO << "Received async batch: " << batch.header.index;
+ std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
- // Entry is not in the cache, we have to get it from ArVizStorage
- auto& newEntry = recordingBatchCache[index];
- newEntry.lastUsed = now;
- newEntry.data = storage->getRecordingBatch(currentRecording.id, index);
+ auto& entry = recordingBatchCache[batch.header.index];
+ entry.data = batch;
+ entry.lastUsed = IceUtil::Time::now();
+ }
- if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
+ viz::data::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
{
- // Remove the entry with the oldest last used timestamp
- auto oldestIter = recordingBatchCache.begin();
- for (auto iter = recordingBatchCache.begin();
- iter != recordingBatchCache.end(); ++iter)
+ IceUtil::Time now = IceUtil::Time::now();
+
+ std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
+
+ auto iter = recordingBatchCache.find(index);
+ if (iter != recordingBatchCache.end())
{
+ // Start prefetching neighbouring batches
+ bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
+ if (!asyncPrefetchIsRunning)
+ {
+ if (index + 1 < (long)currentRecording.batchHeaders.size()
+ && recordingBatchCache.count(index + 1) == 0)
+ {
+ callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
+ }
+ else if (index > 0 && recordingBatchCache.count(index - 1) == 0)
+ {
+ callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
+ }
+
+ }
+
auto& entry = iter->second;
- if (entry.lastUsed < oldestIter->second.lastUsed)
+ entry.lastUsed = now;
+ return entry.data;
+ }
+
+ ARMARX_INFO << "Batch #" << index << " is not in the cache. Getting synchronously, blocking GUI...";
+
+ // Entry is not in the cache, we have to get it from ArVizStorage
+ auto& newEntry = recordingBatchCache[index];
+ newEntry.lastUsed = now;
+ newEntry.data = storage->getRecordingBatch(currentRecording.id, index);
+
+ if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
+ {
+ // Remove the entry with the oldest last used timestamp
+ auto oldestIter = recordingBatchCache.begin();
+ for (auto iter = recordingBatchCache.begin();
+ iter != recordingBatchCache.end(); ++iter)
{
- oldestIter = iter;
+ auto& entry = iter->second;
+ if (entry.lastUsed < oldestIter->second.lastUsed)
+ {
+ oldestIter = iter;
+ }
}
+
+ recordingBatchCache.erase(oldestIter);
}
- recordingBatchCache.erase(oldestIter);
+ return newEntry.data;
}
- return newEntry.data;
-}
-
-SoNode* ArVizWidgetController::getScene()
-{
- return visualizer.root;
-}
+ SoNode* ArVizWidgetController::getScene()
+ {
+ return visualizer.root;
+ }
-const static std::string CONFIG_KEY_STORAGE = "Storage";
+ const static std::string CONFIG_KEY_STORAGE = "Storage";
-void ArVizWidgetController::loadSettings(QSettings* settings)
-{
- storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE),
- "ArVizStorage").toString().toStdString();
-}
+ void ArVizWidgetController::loadSettings(QSettings* settings)
+ {
+ storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE),
+ "ArVizStorage").toString().toStdString();
+ }
-void ArVizWidgetController::saveSettings(QSettings* settings)
-{
- settings->setValue(QString::fromStdString(CONFIG_KEY_STORAGE),
- QString::fromStdString(storageName));
-}
+ void ArVizWidgetController::saveSettings(QSettings* settings)
+ {
+ settings->setValue(QString::fromStdString(CONFIG_KEY_STORAGE),
+ QString::fromStdString(storageName));
+ }
-QPointer<QDialog> armarx::ArVizWidgetController::getConfigDialog(QWidget* parent)
-{
- if (!configDialog)
+ QPointer<QDialog> armarx::ArVizWidgetController::getConfigDialog(QWidget* parent)
{
- configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>({CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
+ if (!configDialog)
+ {
+ configDialog = new SimpleConfigDialog(parent);
+ configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>({CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
+ }
+ return qobject_cast<QDialog*>(configDialog);
}
- return qobject_cast<QDialog*>(configDialog);
-}
-void armarx::ArVizWidgetController::configured()
-{
- if (configDialog)
+ void armarx::ArVizWidgetController::configured()
{
- storageName = configDialog->getProxyName(CONFIG_KEY_STORAGE);
+ if (configDialog)
+ {
+ storageName = configDialog->getProxyName(CONFIG_KEY_STORAGE);
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
index c99e88ada..7180e3d9d 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
@@ -24,9 +24,10 @@
#include "DebugDrawerViewerWidgetController.h"
-using namespace armarx;
-
-DebugDrawerViewerGuiPlugin::DebugDrawerViewerGuiPlugin()
+namespace armarx
{
- addWidget < DebugDrawerViewerWidgetController > ();
+ DebugDrawerViewerGuiPlugin::DebugDrawerViewerGuiPlugin()
+ {
+ addWidget < DebugDrawerViewerWidgetController > ();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index 11d77b7f1..fa70bd098 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -28,263 +28,262 @@
#include <string>
-using namespace armarx;
-
-
-DebugDrawerViewerWidgetController::DebugDrawerViewerWidgetController()
-{
- rootVisu = nullptr;
- widget.setupUi(getWidget());
-
- QTimer* timer = new QTimer(this);
- connect(timer, SIGNAL(timeout()), this, SLOT(updateComboClearLayer()));
- timer->start(100);
-}
-
-
-void DebugDrawerViewerWidgetController::loadSettings(QSettings* settings)
-{
- (void) settings; // unused
-}
-
-void DebugDrawerViewerWidgetController::saveSettings(QSettings* settings)
-{
- (void) settings; // unused
-}
-
-
-void DebugDrawerViewerWidgetController::onInitComponent()
+namespace armarx
{
- rootVisu = new SoSeparator;
- rootVisu->ref();
-
-
- enableMainWidgetAsync(false);
- connect(widget.btnClearAll, SIGNAL(clicked()), this, SLOT(on_btnClearAll_clicked()), Qt::UniqueConnection);
- connect(widget.btnClearLayer, SIGNAL(clicked()), this, SLOT(on_btnClearLayer_clicked()), Qt::UniqueConnection);
-}
-
-
-void DebugDrawerViewerWidgetController::onConnectComponent()
-{
- // create the debugdrawer component
- std::string debugDrawerComponentName = "GUIDebugDrawer_" + getName();
- ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
-
- if (mutex3D)
+ DebugDrawerViewerWidgetController::DebugDrawerViewerWidgetController()
{
- //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
- debugDrawer->setMutex(mutex3D);
- }
- else
- {
- ARMARX_ERROR << " No 3d mutex available...";
+ rootVisu = nullptr;
+ widget.setupUi(getWidget());
+
+ QTimer* timer = new QTimer(this);
+ connect(timer, SIGNAL(timeout()), this, SLOT(updateComboClearLayer()));
+ timer->start(100);
}
- ArmarXManagerPtr m = getArmarXManager();
- m->addObject(debugDrawer, false);
+ void DebugDrawerViewerWidgetController::loadSettings(QSettings* settings)
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
- rootVisu->addChild(debugDrawer->getVisualization());
+ (void) settings; // unused
}
- enableMainWidgetAsync(true);
-}
-void armarx::DebugDrawerViewerWidgetController::onExitComponent()
-{
- if (rootVisu)
+ void DebugDrawerViewerWidgetController::saveSettings(QSettings* settings)
{
- rootVisu->removeAllChildren();
- rootVisu->unref();
- rootVisu = nullptr;
+ (void) settings; // unused
}
-}
-
-SoNode* DebugDrawerViewerWidgetController::getScene()
-{
- return rootVisu;
-}
-void armarx::DebugDrawerViewerWidgetController::on_btnClearAll_clicked()
-{
- if (debugDrawer)
+ void DebugDrawerViewerWidgetController::onInitComponent()
{
- ARMARX_INFO << "Clearing all visualization layers";
- debugDrawer->clearAll();
- }
-}
+ rootVisu = new SoSeparator;
+ rootVisu->ref();
-void DebugDrawerViewerWidgetController::on_btnClearLayer_clicked()
-{
- if (debugDrawer)
- {
- int index = widget.comboClearLayer->currentIndex();
- std::string layerName = widget.comboClearLayer->itemData(index).toString().toStdString();
- if (!layerName.empty())
- {
- ARMARX_INFO << "Clearing layer: '" << layerName << "'";
- debugDrawer->clearLayer(layerName);
- }
+ enableMainWidgetAsync(false);
+ connect(widget.btnClearAll, SIGNAL(clicked()), this, SLOT(on_btnClearAll_clicked()), Qt::UniqueConnection);
+ connect(widget.btnClearLayer, SIGNAL(clicked()), this, SLOT(on_btnClearLayer_clicked()), Qt::UniqueConnection);
}
-}
-
-void DebugDrawerViewerWidgetController::updateComboClearLayer()
-{
- QComboBox* combo = widget.comboClearLayer;
- auto setItalic = [combo](bool italic)
+ void DebugDrawerViewerWidgetController::onConnectComponent()
{
- QFont font = combo->font();
- font.setItalic(italic);
- combo->setFont(font);
- };
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "GUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
- auto disableButton = [combo, this, &setItalic](const std::string & hint)
- {
- QString itemText(hint.c_str());
- QString itemData("");
- setItalic(true);
-
- if (combo->count() != 1)
+ if (mutex3D)
{
- combo->clear();
- combo->insertItem(0, itemText, itemData);
+ //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
+ debugDrawer->setMutex(mutex3D);
}
else
{
- combo->setItemText(0, itemText);
- combo->setItemData(0, itemData);
+ ARMARX_ERROR << " No 3d mutex available...";
}
- this->widget.btnClearLayer->setEnabled(false);
- };
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer, false);
- if (!debugDrawer)
+ {
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ rootVisu->addChild(debugDrawer->getVisualization());
+ }
+ enableMainWidgetAsync(true);
+ }
+
+ void armarx::DebugDrawerViewerWidgetController::onExitComponent()
{
- disableButton("not connected");
- return;
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = nullptr;
+ }
}
- // fetch layer information
- LayerInformationSequence layers = debugDrawer->layerInformation();
- if (layers.empty())
+ SoNode* DebugDrawerViewerWidgetController::getScene()
{
- disableButton("no layers");
- return;
+ return rootVisu;
}
- else
+
+ void armarx::DebugDrawerViewerWidgetController::on_btnClearAll_clicked()
{
- setItalic(false);
- this->widget.btnClearLayer->setEnabled(true);
+ if (debugDrawer)
+ {
+ ARMARX_INFO << "Clearing all visualization layers";
+ debugDrawer->clearAll();
+ }
}
- // sort the layers by name
- std::sort(layers.begin(), layers.end(), [](const LayerInformation & lhs, const LayerInformation & rhs)
+ void DebugDrawerViewerWidgetController::on_btnClearLayer_clicked()
{
- // compare case insensitively
- for (std::size_t i = 0; i < lhs.layerName.size() && i < lhs.layerName.size(); ++i)
+ if (debugDrawer)
{
- auto lhsLow = std::tolower(lhs.layerName[i]);
- auto rhsLow = std::tolower(rhs.layerName[i]);
- if (lhsLow < rhsLow)
- {
- return true;
- }
- else if (lhsLow > rhsLow)
+ int index = widget.comboClearLayer->currentIndex();
+ std::string layerName = widget.comboClearLayer->itemData(index).toString().toStdString();
+
+ if (!layerName.empty())
{
- return false;
+ ARMARX_INFO << "Clearing layer: '" << layerName << "'";
+ debugDrawer->clearLayer(layerName);
}
}
- // if one is a prefix of the other, the shorter one "smaller"
- return lhs.layerName.size() < rhs.layerName.size();
- });
-
+ }
- const int numLayers = static_cast<int>(layers.size());
- for (int i = 0; i < numLayers; ++i)
+ void DebugDrawerViewerWidgetController::updateComboClearLayer()
{
- const LayerInformation& layer = layers[static_cast<std::size_t>(i)];
+ QComboBox* combo = widget.comboClearLayer;
+
+ auto setItalic = [combo](bool italic)
+ {
+ QFont font = combo->font();
+ font.setItalic(italic);
+ combo->setFont(font);
+ };
+
+ auto disableButton = [combo, this, &setItalic](const std::string & hint)
+ {
+ QString itemText(hint.c_str());
+ QString itemData("");
+ setItalic(true);
+
+ if (combo->count() != 1)
+ {
+ combo->clear();
+ combo->insertItem(0, itemText, itemData);
+ }
+ else
+ {
+ combo->setItemText(0, itemText);
+ combo->setItemData(0, itemData);
+ }
+
+ this->widget.btnClearLayer->setEnabled(false);
+ };
+
+ if (!debugDrawer)
+ {
+ disableButton("not connected");
+ return;
+ }
- QString layerName(layer.layerName.c_str());
+ // fetch layer information
+ LayerInformationSequence layers = debugDrawer->layerInformation();
- if (i < combo->count()) // in range
+ if (layers.empty())
{
- QString itemData = combo->itemData(i).toString();
+ disableButton("no layers");
+ return;
+ }
+ else
+ {
+ setItalic(false);
+ this->widget.btnClearLayer->setEnabled(true);
+ }
- // remove deleted layers
- while (itemData.size() != 0 && itemData < layerName)
+ // sort the layers by name
+ std::sort(layers.begin(), layers.end(), [](const LayerInformation & lhs, const LayerInformation & rhs)
+ {
+ // compare case insensitively
+ for (std::size_t i = 0; i < lhs.layerName.size() && i < lhs.layerName.size(); ++i)
{
- // item layer is smaller than next layer
- // => item layer was deleted
- combo->removeItem(i);
- itemData = i < combo->count() ? combo->itemData(i).toString() : "";
+ auto lhsLow = std::tolower(lhs.layerName[i]);
+ auto rhsLow = std::tolower(rhs.layerName[i]);
+ if (lhsLow < rhsLow)
+ {
+ return true;
+ }
+ else if (lhsLow > rhsLow)
+ {
+ return false;
+ }
}
+ // if one is a prefix of the other, the shorter one "smaller"
+ return lhs.layerName.size() < rhs.layerName.size();
+ });
- // update existing layer
- if (itemData == layerName)
+
+ const int numLayers = static_cast<int>(layers.size());
+
+ for (int i = 0; i < numLayers; ++i)
+ {
+ const LayerInformation& layer = layers[static_cast<std::size_t>(i)];
+
+ QString layerName(layer.layerName.c_str());
+
+ if (i < combo->count()) // in range
{
- combo->setItemText(i, makeLayerItemText(layer));
+ QString itemData = combo->itemData(i).toString();
+
+ // remove deleted layers
+ while (itemData.size() != 0 && itemData < layerName)
+ {
+ // item layer is smaller than next layer
+ // => item layer was deleted
+ combo->removeItem(i);
+ itemData = i < combo->count() ? combo->itemData(i).toString() : "";
+ }
+
+ // update existing layer
+ if (itemData == layerName)
+ {
+ combo->setItemText(i, makeLayerItemText(layer));
+ }
+ else // (itemData > layerName)
+ {
+ // item layer is further down than current layer
+ // => insert current layer here
+ combo->insertItem(i, makeLayerItemText(layer), layerName);
+ }
}
- else // (itemData > layerName)
+ else // out of range
{
- // item layer is further down than current layer
- // => insert current layer here
combo->insertItem(i, makeLayerItemText(layer), layerName);
}
+
+ // check invariant
+ ARMARX_CHECK_EQUAL(combo->itemData(i).toString(), layerName);
}
- else // out of range
+
+ // remove excessive items
+ while (combo->count() > numLayers)
{
- combo->insertItem(i, makeLayerItemText(layer), layerName);
+ combo->removeItem(combo->count() - 1);
}
-
- // check invariant
- ARMARX_CHECK_EQUAL(combo->itemData(i).toString(), layerName);
}
- // remove excessive items
- while (combo->count() > numLayers)
- {
- combo->removeItem(combo->count() - 1);
- }
-}
-
-QString DebugDrawerViewerWidgetController::makeLayerItemText(const LayerInformation& layer)
-{
- std::vector<std::string> annotations;
- if (layer.elementCount == 0)
- {
- annotations.push_back("empty");
- }
- else
- {
- annotations.push_back(std::to_string(layer.elementCount));
- }
- if (!layer.visible)
+ QString DebugDrawerViewerWidgetController::makeLayerItemText(const LayerInformation& layer)
{
- annotations.push_back("hidden");
- }
+ std::vector<std::string> annotations;
+ if (layer.elementCount == 0)
+ {
+ annotations.push_back("empty");
+ }
+ else
+ {
+ annotations.push_back(std::to_string(layer.elementCount));
+ }
+ if (!layer.visible)
+ {
+ annotations.push_back("hidden");
+ }
- if (annotations.empty())
- {
- return { layer.layerName.c_str() };
- }
- else
- {
- std::stringstream itemText;
- itemText << layer.layerName
- << " (" << boost::algorithm::join(annotations, ", ") << ")";
- return { itemText.str().c_str() };
+ if (annotations.empty())
+ {
+ return { layer.layerName.c_str() };
+ }
+ else
+ {
+ std::stringstream itemText;
+ itemText << layer.layerName
+ << " (" << boost::algorithm::join(annotations, ", ") << ")";
+ return { itemText.str().c_str() };
+ }
}
}
-
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
index f57189eca..cec6eaaff 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
@@ -24,9 +24,10 @@
#include "GuiHealthClientWidgetController.h"
-using namespace armarx;
-
-GuiHealthClientGuiPlugin::GuiHealthClientGuiPlugin()
+namespace armarx
{
- addWidget < GuiHealthClientWidgetController > ();
+ GuiHealthClientGuiPlugin::GuiHealthClientGuiPlugin()
+ {
+ addWidget < GuiHealthClientWidgetController > ();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index b70f432f2..ca95dd078 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -26,122 +26,123 @@
#include <string>
#include <QPushButton>
-using namespace armarx;
-
-GuiHealthClientWidgetController::GuiHealthClientWidgetController()
+namespace armarx
{
- widget.setupUi(getWidget());
- //ARMARX_IMPORTANT << "ctor start";
-
- healthTimer = new QTimer(this);
- healthTimer->setInterval(10);
- connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb()));
- connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats()));
- connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
- connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
+ GuiHealthClientWidgetController::GuiHealthClientWidgetController()
+ {
+ widget.setupUi(getWidget());
+ //ARMARX_IMPORTANT << "ctor start";
- updateSummaryTimer = new QTimer(this);
- updateSummaryTimer->setInterval(100);
- connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
+ healthTimer = new QTimer(this);
+ healthTimer->setInterval(10);
+ connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb()));
+ connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats()));
+ connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
+ connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
- widget.labelState->setText("idle.");
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
+ updateSummaryTimer = new QTimer(this);
+ updateSummaryTimer->setInterval(100);
+ connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
- //ARMARX_IMPORTANT << "ctor end";
-}
+ widget.labelState->setText("idle.");
+ widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
+ //ARMARX_IMPORTANT << "ctor end";
+ }
-GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
-{
-}
+ GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
+ {
+ }
-void GuiHealthClientWidgetController::loadSettings(QSettings* settings)
-{
-}
+ void GuiHealthClientWidgetController::loadSettings(QSettings* settings)
+ {
-void GuiHealthClientWidgetController::saveSettings(QSettings* settings)
-{
+ }
-}
+ void GuiHealthClientWidgetController::saveSettings(QSettings* settings)
+ {
+ }
-void GuiHealthClientWidgetController::onInitComponent()
-{
- //ARMARX_IMPORTANT << "onInitComponent";
- usingProxy("RobotHealth");
- offeringTopic("RobotHealthTopic");
-}
-void GuiHealthClientWidgetController::healthTimerClb()
-{
- RobotHealthHeartbeatArgs rhha;
- rhha.maximumCycleTimeWarningMS = 250;
- rhha.maximumCycleTimeErrorMS = 500;
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
-}
-void GuiHealthClientWidgetController::updateSummaryTimerClb()
-{
- //ARMARX_IMPORTANT << "updateSummaryTimerClb";
- if (robotHealthComponentPrx)
+ void GuiHealthClientWidgetController::onInitComponent()
{
- //ARMARX_IMPORTANT << "has robotHealthComponentPrx";
- try
- {
- //ARMARX_IMPORTANT << "before set text";
- widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
- //ARMARX_IMPORTANT << "after set text";
- }
- catch (...)
- {}
+ //ARMARX_IMPORTANT << "onInitComponent";
+ usingProxy("RobotHealth");
+ offeringTopic("RobotHealthTopic");
}
-}
-void GuiHealthClientWidgetController::toggleSendOwnHeartbeats()
-{
- if (ownHeartbeatsActive)
+ void GuiHealthClientWidgetController::healthTimerClb()
{
- ownHeartbeatsActive = false;
- healthTimer->stop();
- robotHealthTopicPrx->unregister(getName());
- widget.labelState->setText("idle.");
- //widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
+ RobotHealthHeartbeatArgs rhha;
+ rhha.maximumCycleTimeWarningMS = 250;
+ rhha.maximumCycleTimeErrorMS = 500;
+ robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
}
- else
+ void GuiHealthClientWidgetController::updateSummaryTimerClb()
{
- ownHeartbeatsActive = true;
- healthTimer->start();
- widget.labelState->setText("sending heartbeats...");
- widget.pushButtonToggleOwnHeartbeats->setText("unregister self");
+ //ARMARX_IMPORTANT << "updateSummaryTimerClb";
+ if (robotHealthComponentPrx)
+ {
+ //ARMARX_IMPORTANT << "has robotHealthComponentPrx";
+ try
+ {
+ //ARMARX_IMPORTANT << "before set text";
+ widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
+ //ARMARX_IMPORTANT << "after set text";
+ }
+ catch (...)
+ {}
+ }
}
-}
+ void GuiHealthClientWidgetController::toggleSendOwnHeartbeats()
+ {
+ if (ownHeartbeatsActive)
+ {
+ ownHeartbeatsActive = false;
+ healthTimer->stop();
+ robotHealthTopicPrx->unregister(getName());
+ widget.labelState->setText("idle.");
+ //widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
+ widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
+ }
+ else
+ {
+ ownHeartbeatsActive = true;
+ healthTimer->start();
+ widget.labelState->setText("sending heartbeats...");
+ widget.pushButtonToggleOwnHeartbeats->setText("unregister self");
+ }
+ }
-void GuiHealthClientWidgetController::onConnectComponent()
-{
- //ARMARX_IMPORTANT << "onConnectComponent";
- robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
- robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
- invokeConnectComponentQt();
-}
-void GuiHealthClientWidgetController::onConnectComponentQt()
-{
- //healthTimer->start();
- //ARMARX_IMPORTANT << "updateSummaryTimer->start";
- updateSummaryTimer->start();
-}
+ void GuiHealthClientWidgetController::onConnectComponent()
+ {
+ //ARMARX_IMPORTANT << "onConnectComponent";
+ robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
+ robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
+ invokeConnectComponentQt();
+ }
+ void GuiHealthClientWidgetController::onConnectComponentQt()
+ {
+ //healthTimer->start();
+ //ARMARX_IMPORTANT << "updateSummaryTimer->start";
+ updateSummaryTimer->start();
+ }
-void GuiHealthClientWidgetController::onDisconnectComponent()
-{
- invokeDisconnectComponentQt();
-}
-void GuiHealthClientWidgetController::onDisconnectComponentQt()
-{
- healthTimer->stop();
- updateSummaryTimer->stop();
+
+ void GuiHealthClientWidgetController::onDisconnectComponent()
+ {
+ invokeDisconnectComponentQt();
+ }
+ void GuiHealthClientWidgetController::onDisconnectComponentQt()
+ {
+ healthTimer->stop();
+ updateSummaryTimer->stop();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 17633a623..4ace60472 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -37,247 +37,242 @@
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
-using namespace armarx;
-
-
-HandUnitGuiPlugin::HandUnitGuiPlugin()
+namespace armarx
{
- addWidget<HandUnitWidget>();
-}
-
-
-HandUnitWidget::HandUnitWidget() :
- handName("NOT SET YET"),
- handUnitProxyName(""),
- setJointAnglesFlag(false)
-{
- // init gui
- ui.setupUi(getWidget());
-
- jointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setJointAngles, 50);
- updateInfoTimer = new QTimer(this);
-}
-
+ HandUnitGuiPlugin::HandUnitGuiPlugin()
+ {
+ addWidget<HandUnitWidget>();
+ }
-void HandUnitWidget::onInitComponent()
-{
- usingProxy(handUnitProxyName);
- //usingTopic(handName + "State");
- //ARMARX_WARNING << "Listening on Topic: " << handName + "State";
-}
+ HandUnitWidget::HandUnitWidget() :
+ handName("NOT SET YET"),
+ handUnitProxyName(""),
+ setJointAnglesFlag(false)
+ {
+ // init gui
+ ui.setupUi(getWidget());
-void HandUnitWidget::onConnectComponent()
-{
- connectSlots();
- jointAngleUpdateTask->start();
- updateInfoTimer->start(50);
+ jointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setJointAngles, 50);
+ updateInfoTimer = new QTimer(this);
+ }
- handUnitProxy = getProxy<HandUnitInterfacePrx>(handUnitProxyName);
- handName = handUnitProxy->getHandName();
- // @@@ In simulation hand is called 'Hand L'/'Hand R'. On 3b Hand is called 'TCP R'
- if (handName != "Hand L" && handName != "Hand R" && handName != "TCP L" && handName != "TCP R")
+ void HandUnitWidget::onInitComponent()
{
- //QMessageBox::warning(NULL, "Hand not supported", QString("Hand with name \"") + QString::fromStdString(handName) + " \" is not suppored.");
- ARMARX_WARNING << "Hand with name \"" << handName << "\" is not supported.";
+ usingProxy(handUnitProxyName);
+ //usingTopic(handName + "State");
+ //ARMARX_WARNING << "Listening on Topic: " << handName + "State";
}
- //ui.labelInfo->setText(QString::fromStdString(handUnitProxyName + " :: " + handName));
-
- SingleTypeVariantListPtr preshapeStrings = SingleTypeVariantListPtr::dynamicCast(handUnitProxy->getShapeNames());
- QStringList list;
- int preshapeCount = preshapeStrings->getSize();
-
- for (int i = 0; i < preshapeCount; ++i)
+ void HandUnitWidget::onConnectComponent()
{
- std::string shape = ((preshapeStrings->getVariant(i))->get<std::string>());
- list << QString::fromStdString(shape);
+ connectSlots();
+ jointAngleUpdateTask->start();
+ updateInfoTimer->start(50);
+
+ handUnitProxy = getProxy<HandUnitInterfacePrx>(handUnitProxyName);
+ handName = handUnitProxy->getHandName();
+
+ // @@@ In simulation hand is called 'Hand L'/'Hand R'. On 3b Hand is called 'TCP R'
+ if (handName != "Hand L" && handName != "Hand R" && handName != "TCP L" && handName != "TCP R")
+ {
+ //QMessageBox::warning(NULL, "Hand not supported", QString("Hand with name \"") + QString::fromStdString(handName) + " \" is not suppored.");
+ ARMARX_WARNING << "Hand with name \"" << handName << "\" is not supported.";
+ }
+
+ //ui.labelInfo->setText(QString::fromStdString(handUnitProxyName + " :: " + handName));
+
+ SingleTypeVariantListPtr preshapeStrings = SingleTypeVariantListPtr::dynamicCast(handUnitProxy->getShapeNames());
+ QStringList list;
+ int preshapeCount = preshapeStrings->getSize();
+
+ for (int i = 0; i < preshapeCount; ++i)
+ {
+ std::string shape = ((preshapeStrings->getVariant(i))->get<std::string>());
+ list << QString::fromStdString(shape);
+ }
+
+ ui.comboPreshapes->clear();
+ ui.comboPreshapes->addItems(list);
}
- ui.comboPreshapes->clear();
- ui.comboPreshapes->addItems(list);
-}
-
-void HandUnitWidget::onDisconnectComponent()
-{
- jointAngleUpdateTask->stop();
- updateInfoTimer->stop();
-}
-
-void HandUnitWidget::onExitComponent()
-{
-}
-
-QPointer<QDialog> HandUnitWidget::getConfigDialog(QWidget* parent)
-{
- if (!dialog)
+ void HandUnitWidget::onDisconnectComponent()
{
- dialog = new HandUnitConfigDialog(parent);
- //dialog->ui->editHandUnitName->setText(QString::fromStdString(handUnitProxyName));
- //dialog->ui->editHandName->setText(QString::fromStdString(handName));
+ jointAngleUpdateTask->stop();
+ updateInfoTimer->stop();
}
- return qobject_cast<HandUnitConfigDialog*>(dialog);
-}
-
-void HandUnitWidget::configured()
-{
- handUnitProxyName = dialog->proxyFinder->getSelectedProxyName().toStdString();
-}
-
-void HandUnitWidget::preshapeHand()
-{
- setPreshape(ui.comboPreshapes->currentText().toUtf8().data());
-}
-
-void HandUnitWidget::setJointAngles()
-{
- if (!handUnitProxy)
+ void HandUnitWidget::onExitComponent()
{
- ARMARX_WARNING << "invalid proxy";
- return;
}
- if (!setJointAnglesFlag)
+ QPointer<QDialog> HandUnitWidget::getConfigDialog(QWidget* parent)
{
- return;
+ if (!dialog)
+ {
+ dialog = new HandUnitConfigDialog(parent);
+ //dialog->ui->editHandUnitName->setText(QString::fromStdString(handUnitProxyName));
+ //dialog->ui->editHandName->setText(QString::fromStdString(handName));
+ }
+
+ return qobject_cast<HandUnitConfigDialog*>(dialog);
}
- setJointAnglesFlag = false;
-
- NameValueMap ja;
-
- if (handName == "Hand L" || handName == "TCP L")
+ void HandUnitWidget::configured()
{
- ja["Hand Palm 2 L"] = ui.horizontalSliderPalm->value() * M_PI / 180;
- ja["Index L J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
- ja["Index L J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
- ja["Middle L J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
- ja["Middle L J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
- ja["Thumb L J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
- ja["Thumb L J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
- float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
- ja["Ring L J0"] = rinkyValue;
- ja["Ring L J1"] = rinkyValue;
- ja["Pinky L J0"] = rinkyValue;
- ja["Pinky L J1"] = rinkyValue;
+ handUnitProxyName = dialog->proxyFinder->getSelectedProxyName().toStdString();
}
- else if (handName == "Hand R" || handName == "TCP R")
+
+ void HandUnitWidget::preshapeHand()
{
- ja["Hand Palm 2 R"] = ui.horizontalSliderPalm->value() * M_PI / 180;
- ja["Index R J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
- ja["Index R J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
- ja["Middle R J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
- ja["Middle R J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
- ja["Thumb R J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
- ja["Thumb R J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
- float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
- ja["Ring R J0"] = rinkyValue;
- ja["Ring R J1"] = rinkyValue;
- ja["Pinky R J0"] = rinkyValue;
- ja["Pinky R J1"] = rinkyValue;
+ setPreshape(ui.comboPreshapes->currentText().toUtf8().data());
}
- else
+
+ void HandUnitWidget::setJointAngles()
{
- ARMARX_WARNING << "Hand with name \"" << handName << "\" is not supported.";
+ if (!handUnitProxy)
+ {
+ ARMARX_WARNING << "invalid proxy";
+ return;
+ }
+
+ if (!setJointAnglesFlag)
+ {
+ return;
+ }
+
+ setJointAnglesFlag = false;
+
+ NameValueMap ja;
+
+ if (handName == "Hand L" || handName == "TCP L")
+ {
+ ja["Hand Palm 2 L"] = ui.horizontalSliderPalm->value() * M_PI / 180;
+ ja["Index L J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
+ ja["Index L J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
+ ja["Middle L J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
+ ja["Middle L J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
+ ja["Thumb L J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
+ ja["Thumb L J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
+ float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
+ ja["Ring L J0"] = rinkyValue;
+ ja["Ring L J1"] = rinkyValue;
+ ja["Pinky L J0"] = rinkyValue;
+ ja["Pinky L J1"] = rinkyValue;
+ }
+ else if (handName == "Hand R" || handName == "TCP R")
+ {
+ ja["Hand Palm 2 R"] = ui.horizontalSliderPalm->value() * M_PI / 180;
+ ja["Index R J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
+ ja["Index R J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
+ ja["Middle R J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
+ ja["Middle R J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
+ ja["Thumb R J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
+ ja["Thumb R J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
+ float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
+ ja["Ring R J0"] = rinkyValue;
+ ja["Ring R J1"] = rinkyValue;
+ ja["Pinky R J0"] = rinkyValue;
+ ja["Pinky R J1"] = rinkyValue;
+ }
+ else
+ {
+ ARMARX_WARNING << "Hand with name \"" << handName << "\" is not supported.";
+ }
+
+ handUnitProxy->setJointAngles(ja);
}
- handUnitProxy->setJointAngles(ja);
-}
-
-void HandUnitWidget::requestSetJointAngles()
-{
- setJointAnglesFlag = true;
-}
-
-void HandUnitWidget::openHand()
-{
- setPreshape("Open");
-}
-
-void HandUnitWidget::closeHand()
-{
- setPreshape("Close");
-}
-
-void HandUnitWidget::closeThumb()
-{
- setPreshape("Thumb");
-}
-
-void HandUnitWidget::relaxHand()
-{
- setPreshape("Relax");
-}
+ void HandUnitWidget::requestSetJointAngles()
+ {
+ setJointAnglesFlag = true;
+ }
-void HandUnitWidget::updateInfoLabel()
-{
- ui.labelInfo->setText(QString::fromStdString(handUnitProxyName + " :: " + handName + " State: " + handUnitProxy->describeHandState()));
-}
+ void HandUnitWidget::openHand()
+ {
+ setPreshape("Open");
+ }
-void HandUnitWidget::setPreshape(std::string preshape)
-{
- ARMARX_INFO << "Setting new hand shape: " << preshape;
- handUnitProxy->setShape(preshape);
-}
+ void HandUnitWidget::closeHand()
+ {
+ setPreshape("Close");
+ }
-void HandUnitWidget::loadSettings(QSettings* settings)
-{
- handUnitProxyName = settings->value("handUnitProxyName", QString::fromStdString(handUnitProxyName)).toString().toStdString();
- handName = settings->value("handName", QString::fromStdString(handName)).toString().toStdString();
-}
+ void HandUnitWidget::closeThumb()
+ {
+ setPreshape("Thumb");
+ }
-void HandUnitWidget::saveSettings(QSettings* settings)
-{
- settings->setValue("handUnitProxyName", QString::fromStdString(handUnitProxyName));
- settings->setValue("handName", QString::fromStdString(handName));
-}
+ void HandUnitWidget::relaxHand()
+ {
+ setPreshape("Relax");
+ }
+ void HandUnitWidget::updateInfoLabel()
+ {
+ ui.labelInfo->setText(QString::fromStdString(handUnitProxyName + " :: " + handName + " State: " + handUnitProxy->describeHandState()));
+ }
-void HandUnitWidget::connectSlots()
-{
- connect(ui.buttonPreshape, SIGNAL(clicked()), this, SLOT(preshapeHand()), Qt::UniqueConnection);
- connect(ui.buttonOpenHand, SIGNAL(clicked()), this, SLOT(openHand()), Qt::UniqueConnection);
- connect(ui.buttonCloseHand, SIGNAL(clicked()), this, SLOT(closeHand()), Qt::UniqueConnection);
- connect(ui.buttonCloseThumb, SIGNAL(clicked()), this, SLOT(closeThumb()), Qt::UniqueConnection);
- connect(ui.buttonRelaxHand, SIGNAL(clicked()), this, SLOT(relaxHand()), Qt::UniqueConnection);
- //connect(ui.comboPreshapes, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(selectPreshape(const QString&)), Qt::UniqueConnection);
- connect(ui.buttonSetJointAngles, SIGNAL(clicked()), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderIndexJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderIndexJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderMiddleJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderMiddleJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderRinky, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderPalm, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderThumbJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(ui.horizontalSliderThumbJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
- connect(updateInfoTimer, SIGNAL(timeout()), this, SLOT(updateInfoLabel()));
-}
+ void HandUnitWidget::setPreshape(std::string preshape)
+ {
+ ARMARX_INFO << "Setting new hand shape: " << preshape;
+ handUnitProxy->setShape(preshape);
+ }
+ void HandUnitWidget::loadSettings(QSettings* settings)
+ {
+ handUnitProxyName = settings->value("handUnitProxyName", QString::fromStdString(handUnitProxyName)).toString().toStdString();
+ handName = settings->value("handName", QString::fromStdString(handName)).toString().toStdString();
+ }
+ void HandUnitWidget::saveSettings(QSettings* settings)
+ {
+ settings->setValue("handUnitProxyName", QString::fromStdString(handUnitProxyName));
+ settings->setValue("handName", QString::fromStdString(handName));
+ }
+ void HandUnitWidget::connectSlots()
+ {
+ connect(ui.buttonPreshape, SIGNAL(clicked()), this, SLOT(preshapeHand()), Qt::UniqueConnection);
+ connect(ui.buttonOpenHand, SIGNAL(clicked()), this, SLOT(openHand()), Qt::UniqueConnection);
+ connect(ui.buttonCloseHand, SIGNAL(clicked()), this, SLOT(closeHand()), Qt::UniqueConnection);
+ connect(ui.buttonCloseThumb, SIGNAL(clicked()), this, SLOT(closeThumb()), Qt::UniqueConnection);
+ connect(ui.buttonRelaxHand, SIGNAL(clicked()), this, SLOT(relaxHand()), Qt::UniqueConnection);
+ //connect(ui.comboPreshapes, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(selectPreshape(const QString&)), Qt::UniqueConnection);
+ connect(ui.buttonSetJointAngles, SIGNAL(clicked()), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderIndexJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderIndexJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderMiddleJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderMiddleJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderRinky, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderPalm, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderThumbJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(ui.horizontalSliderThumbJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+ connect(updateInfoTimer, SIGNAL(timeout()), this, SLOT(updateInfoLabel()));
+ }
-void armarx::HandUnitWidget::reportHandShaped(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
-{
- ARMARX_IMPORTANT << handName << ": " << handShapeName;
-}
-void armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
-{
- ARMARX_IMPORTANT << handName << ": " << handShapeName;
-}
-void armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles, const Ice::Current& c)
-{
+ void armarx::HandUnitWidget::reportHandShaped(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
+ {
+ ARMARX_IMPORTANT << handName << ": " << handShapeName;
+ }
-}
+ void armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
+ {
+ ARMARX_IMPORTANT << handName << ": " << handShapeName;
+ }
+ void armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles, const Ice::Current& c)
+ {
+ }
-void armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures, const Ice::Current& c)
-{
+ void armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures, const Ice::Current& c)
+ {
+ }
}
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index aed576720..3d4272f26 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -40,188 +40,188 @@
#include <QComboBox>
#include <QMenu>
-using namespace armarx;
-
-
-HapticUnitGuiPlugin::HapticUnitGuiPlugin()
-{
- addWidget<HapticUnitWidget>();
-}
-
-
-HapticUnitWidget::HapticUnitWidget()
+namespace armarx
{
- // init gui
- ui.setupUi(getWidget());
- //ui.stateOpen->setChecked(true);
- hapticObserverProxyName = "HapticUnitObserver";
- hapticUnitProxyName = "WeissHapticUnit";
+ HapticUnitGuiPlugin::HapticUnitGuiPlugin()
+ {
+ addWidget<HapticUnitWidget>();
+ }
- updateTimer = new QTimer(this);
+ HapticUnitWidget::HapticUnitWidget()
+ {
+ // init gui
+ ui.setupUi(getWidget());
+ //ui.stateOpen->setChecked(true);
+ hapticObserverProxyName = "HapticUnitObserver";
+ hapticUnitProxyName = "WeissHapticUnit";
+ updateTimer = new QTimer(this);
-}
-void HapticUnitWidget::onInitComponent()
-{
- usingProxy(hapticObserverProxyName);
- usingProxy(hapticUnitProxyName);
+ }
-}
+ void HapticUnitWidget::onInitComponent()
+ {
+ usingProxy(hapticObserverProxyName);
+ usingProxy(hapticUnitProxyName);
-void HapticUnitWidget::onConnectComponent()
-{
- hapticObserverProxy = getProxy<ObserverInterfacePrx>(hapticObserverProxyName);
- weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(hapticUnitProxyName);
+ }
- connectSlots();
- createMatrixWidgets();
- updateTimer->start(25); // 50 Hz
-}
+ void HapticUnitWidget::onConnectComponent()
+ {
+ hapticObserverProxy = getProxy<ObserverInterfacePrx>(hapticObserverProxyName);
+ weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(hapticUnitProxyName);
+ connectSlots();
+ createMatrixWidgets();
+ updateTimer->start(25); // 50 Hz
-void HapticUnitWidget::onExitComponent()
-{
- updateTimer->stop();
-}
-
-void HapticUnitWidget::onDisconnectComponent()
-{
- updateTimer->stop();
-}
+ }
-QPointer<QDialog> HapticUnitWidget::getConfigDialog(QWidget* parent)
-{
- if (!dialog)
+ void HapticUnitWidget::onExitComponent()
{
- dialog = new HapticUnitConfigDialog(parent);
+ updateTimer->stop();
}
- return qobject_cast<HapticUnitConfigDialog*>(dialog);
-}
-
-
-void HapticUnitWidget::configured()
-{
-
-}
+ void HapticUnitWidget::onDisconnectComponent()
+ {
+ updateTimer->stop();
+ }
-void HapticUnitWidget::updateData()
-{
- for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ QPointer<QDialog> HapticUnitWidget::getConfigDialog(QWidget* parent)
{
- //MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
- std::string name = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "name"))->getString();
- std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "device"))->getString();
- //float rate = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "rate"))->getFloat();
- TimestampVariantPtr timestamp = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "timestamp")))->get<TimestampVariant>();
- MatrixDatafieldDisplayWidget* matrixDisplay = pair.second;
- matrixDisplay->setInfoOverlay(QString::fromStdString(deviceName) + ": " + QString::fromStdString(name) + "\n" + QString::fromStdString(timestamp->toTime().toDateTime()));
- matrixDisplay->invokeUpdate();
- }
+ if (!dialog)
+ {
+ dialog = new HapticUnitConfigDialog(parent);
+ }
-}
+ return qobject_cast<HapticUnitConfigDialog*>(dialog);
+ }
-void HapticUnitWidget::onContextMenu(QPoint point)
-{
- QMenu* contextMenu = new QMenu(getWidget());
- MatrixDatafieldDisplayWidget* matrixDisplay = qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
- if (!matrixDisplay)
+ void HapticUnitWidget::configured()
{
- return;
+
}
- QAction* setDeviceTag = contextMenu->addAction(tr("Set Device Tag"));
+ void HapticUnitWidget::updateData()
+ {
- QAction* action = contextMenu->exec(matrixDisplay->mapToGlobal(point));
+ for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ {
+ //MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
+ std::string name = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "name"))->getString();
+ std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "device"))->getString();
+ //float rate = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "rate"))->getFloat();
+ TimestampVariantPtr timestamp = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "timestamp")))->get<TimestampVariant>();
+ MatrixDatafieldDisplayWidget* matrixDisplay = pair.second;
+ matrixDisplay->setInfoOverlay(QString::fromStdString(deviceName) + ": " + QString::fromStdString(name) + "\n" + QString::fromStdString(timestamp->toTime().toDateTime()));
+ matrixDisplay->invokeUpdate();
+ }
- std::string channelName = channelNameReverseMap.at(matrixDisplay);
- std::string deviceName = deviceNameReverseMap.at(matrixDisplay);
+ }
- if (action == setDeviceTag)
+ void HapticUnitWidget::onContextMenu(QPoint point)
{
- std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "name"))->getString();
-
- bool ok;
- QString newTag = QInputDialog::getText(getWidget(), tr("Set Device Tag"),
- QString::fromStdString("New Tag for Device '" + deviceName + "'"), QLineEdit::Normal,
- QString::fromStdString(tag), &ok);
+ QMenu* contextMenu = new QMenu(getWidget());
+ MatrixDatafieldDisplayWidget* matrixDisplay = qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
- if (ok && !newTag.isEmpty())
+ if (!matrixDisplay)
{
- ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": " << newTag.toStdString();
- weissHapticUnit->setDeviceTag(deviceName, newTag.toStdString());
+ return;
}
- }
- delete contextMenu;
-}
+ QAction* setDeviceTag = contextMenu->addAction(tr("Set Device Tag"));
+ QAction* action = contextMenu->exec(matrixDisplay->mapToGlobal(point));
-void HapticUnitWidget::loadSettings(QSettings* settings)
-{
-}
+ std::string channelName = channelNameReverseMap.at(matrixDisplay);
+ std::string deviceName = deviceNameReverseMap.at(matrixDisplay);
+ if (action == setDeviceTag)
+ {
+ std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "name"))->getString();
+
+ bool ok;
+ QString newTag = QInputDialog::getText(getWidget(), tr("Set Device Tag"),
+ QString::fromStdString("New Tag for Device '" + deviceName + "'"), QLineEdit::Normal,
+ QString::fromStdString(tag), &ok);
+
+ if (ok && !newTag.isEmpty())
+ {
+ ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": " << newTag.toStdString();
+ weissHapticUnit->setDeviceTag(deviceName, newTag.toStdString());
+ }
+ }
-void HapticUnitWidget::saveSettings(QSettings* settings)
-{
-}
+ delete contextMenu;
+ }
-void HapticUnitWidget::connectSlots()
-{
- connect(this, SIGNAL(doUpdateDisplayWidgets()), SLOT(updateDisplayWidgets()), Qt::QueuedConnection);
- connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateData()));
- connect(ui.checkBoxOffsetFilter, SIGNAL(stateChanged(int)), this, SLOT(onCheckBoxOffsetFilterStateChanged(int)));
-}
+ void HapticUnitWidget::loadSettings(QSettings* settings)
+ {
+ }
-void HapticUnitWidget::createMatrixWidgets()
-{
- //ARMARX_LOG << "HapticUnitWidget::createMatrixWidgets()";
- emit doUpdateDisplayWidgets();
-}
-void HapticUnitWidget::updateDisplayWidgets()
-{
- QLayoutItem* child;
+ void HapticUnitWidget::saveSettings(QSettings* settings)
+ {
+ }
- while ((child = ui.gridLayoutDisplay->takeAt(0)) != 0)
+
+ void HapticUnitWidget::connectSlots()
{
- delete child;
+ connect(this, SIGNAL(doUpdateDisplayWidgets()), SLOT(updateDisplayWidgets()), Qt::QueuedConnection);
+ connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateData()));
+ connect(ui.checkBoxOffsetFilter, SIGNAL(stateChanged(int)), this, SLOT(onCheckBoxOffsetFilterStateChanged(int)));
}
- int i = 0;
- ChannelRegistry channels = hapticObserverProxy->getAvailableChannels(false);
+ void HapticUnitWidget::createMatrixWidgets()
+ {
+ //ARMARX_LOG << "HapticUnitWidget::createMatrixWidgets()";
+ emit doUpdateDisplayWidgets();
+ }
- for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
+ void HapticUnitWidget::updateDisplayWidgets()
{
- std::string channelName = pair.first;
- MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(new DatafieldRef(hapticObserverProxy, channelName, "matrix"), hapticObserverProxy, getWidget());
- matrixDisplay->setRange(0, 4095);
- matrixDisplay->setContextMenuPolicy(Qt::CustomContextMenu);
- connect(matrixDisplay, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(onContextMenu(QPoint)));
- matrixDisplays.insert(std::make_pair(pair.first, matrixDisplay));
- std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "device"))->getString();
- channelNameReverseMap.insert(std::make_pair(matrixDisplay, channelName));
- deviceNameReverseMap.insert(std::make_pair(matrixDisplay, deviceName));
- ui.gridLayoutDisplay->addWidget(matrixDisplay, 0, i);
- i++;
+ QLayoutItem* child;
+
+ while ((child = ui.gridLayoutDisplay->takeAt(0)) != 0)
+ {
+ delete child;
+ }
+
+ int i = 0;
+ ChannelRegistry channels = hapticObserverProxy->getAvailableChannels(false);
+
+ for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
+ {
+ std::string channelName = pair.first;
+ MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(new DatafieldRef(hapticObserverProxy, channelName, "matrix"), hapticObserverProxy, getWidget());
+ matrixDisplay->setRange(0, 4095);
+ matrixDisplay->setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(matrixDisplay, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(onContextMenu(QPoint)));
+ matrixDisplays.insert(std::make_pair(pair.first, matrixDisplay));
+ std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "device"))->getString();
+ channelNameReverseMap.insert(std::make_pair(matrixDisplay, channelName));
+ deviceNameReverseMap.insert(std::make_pair(matrixDisplay, deviceName));
+ ui.gridLayoutDisplay->addWidget(matrixDisplay, 0, i);
+ i++;
+ }
}
-}
-void HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
-{
- //ARMARX_IMPORTANT << "onCheckBoxOffsetFilterToggled: " << state;
- for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ void HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
{
- pair.second->enableOffsetFilter(ui.checkBoxOffsetFilter->isChecked());
+ //ARMARX_IMPORTANT << "onCheckBoxOffsetFilterToggled: " << state;
+ for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ {
+ pair.second->enableOffsetFilter(ui.checkBoxOffsetFilter->isChecked());
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
index 8a05e556a..321e727cd 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
@@ -30,127 +30,128 @@
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
-using namespace armarx;
-
-void MatrixDatafieldDisplayWidget::updateRequested()
+namespace armarx
{
- mtx.lock();
- this->data = matrixDatafieldOffsetFiltered->getDataField()->get<MatrixFloat>()->toEigen();
- this->percentiles = percentilesDatafield->getDataField()->get<MatrixFloat>()->toVector();
- mtx.unlock();
- update();
-}
+ void MatrixDatafieldDisplayWidget::updateRequested()
+ {
+ mtx.lock();
+ this->data = matrixDatafieldOffsetFiltered->getDataField()->get<MatrixFloat>()->toEigen();
+ this->percentiles = percentilesDatafield->getDataField()->get<MatrixFloat>()->toVector();
+ mtx.unlock();
+ update();
+ }
-MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent) :
- QWidget(parent)
-{
- this->matrixDatafield = DatafieldRefPtr::dynamicCast(matrixDatafield);
- this->observer = observer;
- this->min = 0;
- this->max = 1;
- this->data = MatrixXf(1, 1);
- this->data(0, 0) = 0;
- enableOffsetFilter(false);
- QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
- };
- this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
-
- //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
- connect(this, SIGNAL(doUpdate()), SLOT(updateRequested()), Qt::QueuedConnection);
-}
+ MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent) :
+ QWidget(parent)
+ {
+ this->matrixDatafield = DatafieldRefPtr::dynamicCast(matrixDatafield);
+ this->observer = observer;
+ this->min = 0;
+ this->max = 1;
+ this->data = MatrixXf(1, 1);
+ this->data(0, 0) = 0;
+ enableOffsetFilter(false);
+ QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
+ QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
+ };
+ this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
+
+ //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
+ connect(this, SIGNAL(doUpdate()), SLOT(updateRequested()), Qt::QueuedConnection);
+ }
-MatrixDatafieldDisplayWidget::~MatrixDatafieldDisplayWidget()
-{
- //delete ui;
-}
+ MatrixDatafieldDisplayWidget::~MatrixDatafieldDisplayWidget()
+ {
+ //delete ui;
+ }
-void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent*)
-{
- mtx.lock();
+ void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent*)
+ {
+ mtx.lock();
- int paddingBottom = 40;
- QPainter painter(this);
- painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
- int matrixHeight = (height() - paddingBottom) * 8 / 10;
- int percentilesHeight = (height() - paddingBottom) - matrixHeight;
- drawMatrix(QRect(0, 0, width(), matrixHeight), painter);
- drawPercentiles(QRect(0, matrixHeight, width() - 1, percentilesHeight), painter);
+ int paddingBottom = 40;
+ QPainter painter(this);
+ painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
+ int matrixHeight = (height() - paddingBottom) * 8 / 10;
+ int percentilesHeight = (height() - paddingBottom) - matrixHeight;
+ drawMatrix(QRect(0, 0, width(), matrixHeight), painter);
+ drawPercentiles(QRect(0, matrixHeight, width() - 1, percentilesHeight), painter);
- painter.setPen(QColor(Qt::GlobalColor::gray));
- painter.setFont(QFont("Arial", 12));
- painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
+ painter.setPen(QColor(Qt::GlobalColor::gray));
+ painter.setFont(QFont("Arial", 12));
+ painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
- mtx.unlock();
-}
+ mtx.unlock();
+ }
-void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
-{
- if (enabled)
+ void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
{
- this->matrixDatafieldOffsetFiltered = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
+ if (enabled)
+ {
+ this->matrixDatafieldOffsetFiltered = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
+ }
+ else
+ {
+ this->matrixDatafieldOffsetFiltered = this->matrixDatafield;
+ }
+
+ this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)), matrixDatafieldOffsetFiltered));
}
- else
+
+ QColor MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
{
- this->matrixDatafieldOffsetFiltered = this->matrixDatafield;
- }
+ value = (value - min) / (max - min) * (colors.size() - 1);
- this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)), matrixDatafieldOffsetFiltered));
-}
+ if (value < 0)
+ {
+ return colors[0];
+ }
-QColor MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
-{
- value = (value - min) / (max - min) * (colors.size() - 1);
+ if (value >= colors.size() - 1)
+ {
+ return colors[colors.size() - 1];
+ }
- if (value < 0)
- {
- return colors[0];
+ int i = (int)value;
+ float f2 = value - i;
+ float f1 = 1 - f2;
+ QColor c1 = colors[i];
+ QColor c2 = colors[i + 1];
+ return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
}
- if (value >= colors.size() - 1)
+ void MatrixDatafieldDisplayWidget::drawMatrix(const QRect& target, QPainter& painter)
{
- return colors[colors.size() - 1];
- }
+ int pixelSize = std::min(target.width() / data.cols(), target.height() / data.rows());
+ int dx = (target.width() - pixelSize * data.cols()) / 2;
+ int dy = (target.height() - pixelSize * data.rows()) / 2;
+ painter.setFont(QFont("Arial", 8));
+ painter.setPen(QColor(Qt::GlobalColor::gray));
- int i = (int)value;
- float f2 = value - i;
- float f1 = 1 - f2;
- QColor c1 = colors[i];
- QColor c2 = colors[i + 1];
- return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
-}
-
-void MatrixDatafieldDisplayWidget::drawMatrix(const QRect& target, QPainter& painter)
-{
- int pixelSize = std::min(target.width() / data.cols(), target.height() / data.rows());
- int dx = (target.width() - pixelSize * data.cols()) / 2;
- int dy = (target.height() - pixelSize * data.rows()) / 2;
- painter.setFont(QFont("Arial", 8));
- painter.setPen(QColor(Qt::GlobalColor::gray));
-
- for (int x = 0; x < data.cols(); x++)
- {
- for (int y = 0; y < data.rows(); y++)
+ for (int x = 0; x < data.cols(); x++)
{
- QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
- painter.fillRect(target, getColor(data(y, x), min, max));
- painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
+ for (int y = 0; y < data.rows(); y++)
+ {
+ QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
+ painter.fillRect(target, getColor(data(y, x), min, max));
+ painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
+ }
}
}
-}
-void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect& target, QPainter& painter)
-{
- painter.setPen(QColor(Qt::GlobalColor::gray));
- painter.drawRect(target);
- painter.setPen(QColor(Qt::GlobalColor::red));
-
- for (int i = 0; i < (int)percentiles.size() - 1; i++)
+ void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect& target, QPainter& painter)
{
- int x1 = i * target.width() / (percentiles.size() - 1);
- int x2 = (i + 1) * target.width() / (percentiles.size() - 1);
- float y1 = (percentiles.at(i) - min) / (max - min) * target.height();
- float y2 = (percentiles.at(i + 1) - min) / (max - min) * target.height();
- painter.drawLine(target.left() + x1, target.bottom() - (int)y1, target.left() + x2, target.bottom() - (int)y2);
+ painter.setPen(QColor(Qt::GlobalColor::gray));
+ painter.drawRect(target);
+ painter.setPen(QColor(Qt::GlobalColor::red));
+
+ for (int i = 0; i < (int)percentiles.size() - 1; i++)
+ {
+ int x1 = i * target.width() / (percentiles.size() - 1);
+ int x2 = (i + 1) * target.width() / (percentiles.size() - 1);
+ float y1 = (percentiles.at(i) - min) / (max - min) * target.height();
+ float y2 = (percentiles.at(i + 1) - min) / (max - min) * target.height();
+ painter.drawLine(target.left() + x1, target.bottom() - (int)y1, target.left() + x2, target.bottom() - (int)y2);
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
index 8f5bc0d16..72b510ef5 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
@@ -28,79 +28,80 @@
#include <pthread.h>
#include <iostream>
-using namespace armarx;
-
-MatrixDisplayWidget::MatrixDisplayWidget(QWidget* parent) :
- QWidget(parent),
- ui(new Ui::MatrixDisplayWidget)
+namespace armarx
{
- ui->setupUi(this);
- this->min = 0;
- this->max = 1;
- this->data = MatrixXf(1, 1);
- this->data(0, 0) = 0;
- QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
- };
- this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
+ MatrixDisplayWidget::MatrixDisplayWidget(QWidget* parent) :
+ QWidget(parent),
+ ui(new Ui::MatrixDisplayWidget)
+ {
+ ui->setupUi(this);
+ this->min = 0;
+ this->max = 1;
+ this->data = MatrixXf(1, 1);
+ this->data(0, 0) = 0;
+ QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
+ QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
+ };
+ this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
- //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
- connect(this, SIGNAL(doUpdate()), SLOT(update()), Qt::QueuedConnection);
-}
+ //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
+ connect(this, SIGNAL(doUpdate()), SLOT(update()), Qt::QueuedConnection);
+ }
-MatrixDisplayWidget::~MatrixDisplayWidget()
-{
- delete ui;
-}
+ MatrixDisplayWidget::~MatrixDisplayWidget()
+ {
+ delete ui;
+ }
-void MatrixDisplayWidget::paintEvent(QPaintEvent*)
-{
- mtx.lock();
- MatrixXf data = this->data;
+ void MatrixDisplayWidget::paintEvent(QPaintEvent*)
+ {
+ mtx.lock();
+ MatrixXf data = this->data;
- //cout << "[" << pthread_self() << "] MatrixDisplayWidget::paintEvent" << endl;
+ //cout << "[" << pthread_self() << "] MatrixDisplayWidget::paintEvent" << endl;
- int pixelSize = std::min(width() / data.cols(), height() / data.rows());
- int dx = (width() - pixelSize * data.cols()) / 2;
- int dy = (height() - pixelSize * data.rows()) / 2;
- QPainter painter(this);
- painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
- painter.setFont(QFont("Arial", 8));
+ int pixelSize = std::min(width() / data.cols(), height() / data.rows());
+ int dx = (width() - pixelSize * data.cols()) / 2;
+ int dy = (height() - pixelSize * data.rows()) / 2;
+ QPainter painter(this);
+ painter.fillRect(rect(), QColor::fromRgb(0, 0, 0));
+ painter.setFont(QFont("Arial", 8));
- for (int x = 0; x < data.cols(); x++)
- {
- for (int y = 0; y < data.rows(); y++)
+ for (int x = 0; x < data.cols(); x++)
{
- QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
- painter.fillRect(target, getColor(data(y, x), min, max));
- painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
+ for (int y = 0; y < data.rows(); y++)
+ {
+ QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
+ painter.fillRect(target, getColor(data(y, x), min, max));
+ painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
+ }
}
- }
- painter.setFont(QFont("Arial", 12));
- painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
+ painter.setFont(QFont("Arial", 12));
+ painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
- mtx.unlock();
-}
-
-QColor MatrixDisplayWidget::getColor(float value, float min, float max)
-{
- value = (value - min) / (max - min) * (colors.size() - 1);
-
- if (value < 0)
- {
- return colors[0];
+ mtx.unlock();
}
- if (value >= colors.size() - 1)
+ QColor MatrixDisplayWidget::getColor(float value, float min, float max)
{
- return colors[colors.size() - 1];
- }
+ value = (value - min) / (max - min) * (colors.size() - 1);
+
+ if (value < 0)
+ {
+ return colors[0];
+ }
+
+ if (value >= colors.size() - 1)
+ {
+ return colors[colors.size() - 1];
+ }
- int i = (int)value;
- float f2 = value - i;
- float f1 = 1 - f2;
- QColor c1 = colors[i];
- QColor c2 = colors[i + 1];
- return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
+ int i = (int)value;
+ float f2 = value - i;
+ float f1 = 1 - f2;
+ QColor c1 = colors[i];
+ QColor c2 = colors[i + 1];
+ return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
+ }
}
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
index 80e0b9455..8356db80c 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
@@ -24,9 +24,10 @@
#include "HomogeneousMatrixCalculatorWidgetController.h"
-using namespace armarx;
-
-HomogeneousMatrixCalculatorGuiPlugin::HomogeneousMatrixCalculatorGuiPlugin()
+namespace armarx
{
- addWidget < HomogeneousMatrixCalculatorWidgetController > ();
+ HomogeneousMatrixCalculatorGuiPlugin::HomogeneousMatrixCalculatorGuiPlugin()
+ {
+ addWidget < HomogeneousMatrixCalculatorWidgetController > ();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
index ebb7b13b9..97b06a077 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
@@ -8,8 +8,6 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-using namespace armarx;
-
namespace Ui
{
class ViewSelectionConfigDialog;
@@ -45,7 +43,7 @@ private:
Ui::ViewSelectionConfigDialog* ui;
- IceProxyFinderBase* viewSelectionProxyFinder;
+ armarx::IceProxyFinderBase* viewSelectionProxyFinder;
std::string uuid;
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
index 7c436e6ae..da6b2dfc7 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
+++ b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
@@ -23,41 +23,41 @@
#include "KinematicUnitInterfaceStdOverloads.h"
-using namespace armarx;
-
-std::string to_string(const ErrorStatus& status)
+namespace armarx
{
- switch (status)
+ std::string to_string(const ErrorStatus& status)
{
- case armarx::eOk:
- return "Ok";
+ switch (status)
+ {
+ case armarx::eOk:
+ return "Ok";
- case armarx::eWarning:
- return "Warning";
+ case armarx::eWarning:
+ return "Warning";
- case armarx::eError:
- return "Error";
+ case armarx::eError:
+ return "Error";
- default:
- return "?";
+ default:
+ return "?";
+ }
}
-}
-
-std::string to_string(const OperationStatus& status)
-{
- switch (status)
+ std::string to_string(const OperationStatus& status)
{
- case armarx::eOffline:
- return "Offline";
+ switch (status)
+ {
+ case armarx::eOffline:
+ return "Offline";
- case armarx::eOnline:
- return "Online";
+ case armarx::eOnline:
+ return "Online";
- case armarx::eInitialized:
- return "Initialized";
+ case armarx::eInitialized:
+ return "Initialized";
- default:
- return "?";
+ default:
+ return "?";
+ }
}
}
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
index a19b8d051..44494ec0e 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
+++ b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
@@ -29,7 +29,6 @@
namespace armarx
{
- std::string to_string(const armarx::ErrorStatus& status);
- std::string to_string(const armarx::OperationStatus& status);
-
+ std::string to_string(const ErrorStatus& status);
+ std::string to_string(const OperationStatus& status);
}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
index 8ddc9ca8c..3cd107044 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -23,987 +23,988 @@
#include "DSRTBimanualController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
-using namespace armarx;
-
-NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
-
-void DSRTBimanualController::onInitNJointController()
+namespace armarx
{
- ARMARX_INFO << "init ...";
- controllerStopRequested = false;
- controllerRunFinished = false;
- runTask("DSRTBimanualControllerTask", [&]
+ NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
+
+ void DSRTBimanualController::onInitNJointController()
{
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
+ ARMARX_INFO << "init ...";
+ controllerStopRequested = false;
+ controllerRunFinished = false;
+ runTask("DSRTBimanualControllerTask", [&]
{
- ARMARX_INFO << deactivateSpam(1) << "control fct";
- if (isControllerActive())
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
{
- controllerRun();
+ ARMARX_INFO << deactivateSpam(1) << "control fct";
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
}
- c.waitForCycleDuration();
- }
- controllerRunFinished = true;
- ARMARX_INFO << "Control Fct finished";
- });
+ controllerRunFinished = true;
+ ARMARX_INFO << "Control Fct finished";
+ });
-}
+ }
-void DSRTBimanualController::onDisconnectNJointController()
-{
- ARMARX_INFO << "disconnect";
- controllerStopRequested = true;
- while (!controllerRunFinished)
+ void DSRTBimanualController::onDisconnectNJointController()
{
- ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
- usleep(10000);
+ ARMARX_INFO << "disconnect";
+ controllerStopRequested = true;
+ while (!controllerRunFinished)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
+ usleep(10000);
+ }
}
-}
-DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-{
- cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
+ DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
- VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
- VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
+ VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
+ VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
- left_jointNames.clear();
- right_jointNames.clear();
+ left_jointNames.clear();
+ right_jointNames.clear();
- ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm");
- ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm");
+ ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm");
+ ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm");
- // for left arm
- for (size_t i = 0; i < left_ns->getSize(); ++i)
- {
- std::string jointName = left_ns->getNode(i)->getName();
- left_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- left_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
+ // for left arm
+ for (size_t i = 0; i < left_ns->getSize(); ++i)
{
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
+ std::string jointName = left_ns->getNode(i)->getName();
+ left_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ left_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
- left_torqueSensors.push_back(torqueSensor);
- left_gravityTorqueSensors.push_back(gravityTorqueSensor);
- left_velocitySensors.push_back(velocitySensor);
- left_positionSensors.push_back(positionSensor);
- };
+ left_torqueSensors.push_back(torqueSensor);
+ left_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ left_velocitySensors.push_back(velocitySensor);
+ left_positionSensors.push_back(positionSensor);
+ };
- // for right arm
- for (size_t i = 0; i < right_ns->getSize(); ++i)
- {
- std::string jointName = right_ns->getNode(i)->getName();
- right_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- right_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
+ // for right arm
+ for (size_t i = 0; i < right_ns->getSize(); ++i)
{
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
+ std::string jointName = right_ns->getNode(i)->getName();
+ right_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ right_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
- right_torqueSensors.push_back(torqueSensor);
- right_gravityTorqueSensors.push_back(gravityTorqueSensor);
- right_velocitySensors.push_back(velocitySensor);
- right_positionSensors.push_back(positionSensor);
- };
+ right_torqueSensors.push_back(torqueSensor);
+ right_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ right_velocitySensors.push_back(velocitySensor);
+ right_positionSensors.push_back(positionSensor);
+ };
- const SensorValueBase* svlf = useSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = useSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svlf = useSensorValue("FT L");
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = useSensorValue("FT R");
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
- ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
- ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
+ ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
+ ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
- left_arm_tcp = left_ns->getTCP();
- right_arm_tcp = right_ns->getTCP();
+ left_arm_tcp = left_ns->getTCP();
+ right_arm_tcp = right_ns->getTCP();
- left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
- right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
+ left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
+ right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
- left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- // ?? not sure about this
- DSRTBimanualControllerSensorData initSensorData;
+ // ?? not sure about this
+ DSRTBimanualControllerSensorData initSensorData;
- initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
+ initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
- initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
+ initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
- left_oldPosition = left_arm_tcp->getPositionInRootFrame();
- left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ left_oldPosition = left_arm_tcp->getPositionInRootFrame();
+ left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- right_oldPosition = right_arm_tcp->getPositionInRootFrame();
- right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ right_oldPosition = right_arm_tcp->getPositionInRootFrame();
+ right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
- ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
+ std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
- std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
- ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
+ std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
- left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
- left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
- left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
- left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
+ left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
+ left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
+ left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
+ left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
- right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
- right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
- right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
- right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
+ right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
+ right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
+ right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
+ right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
- // set initial joint velocities filter
+ // set initial joint velocities filter
- left_jointVelocity_filtered.resize(left_torque_targets.size());
- left_jointVelocity_filtered.setZero();
- right_jointVelocity_filtered.resize(left_torque_targets.size());
- right_jointVelocity_filtered.setZero();
+ left_jointVelocity_filtered.resize(left_torque_targets.size());
+ left_jointVelocity_filtered.setZero();
+ right_jointVelocity_filtered.resize(left_torque_targets.size());
+ right_jointVelocity_filtered.setZero();
- // do we need to duplicate this?
- DSRTBimanualControllerControlData initData;
- for (size_t i = 0; i < 3; ++i)
- {
- initData.left_tcpDesiredLinearVelocity(i) = 0;
- initData.right_tcpDesiredLinearVelocity(i) = 0;
+ // do we need to duplicate this?
+ DSRTBimanualControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredLinearVelocity(i) = 0;
+ initData.right_tcpDesiredLinearVelocity(i) = 0;
- }
+ }
- for (size_t i = 0; i < 3; ++i)
- {
- initData.left_tcpDesiredAngularError(i) = 0;
- initData.right_tcpDesiredAngularError(i) = 0;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredAngularError(i) = 0;
+ initData.right_tcpDesiredAngularError(i) = 0;
- }
- reinitTripleBuffer(initData);
+ }
+ reinitTripleBuffer(initData);
- // initial filter
- left_desiredTorques_filtered.resize(left_torque_targets.size());
- left_desiredTorques_filtered.setZero();
- right_desiredTorques_filtered.resize(right_torque_targets.size());
- right_desiredTorques_filtered.setZero();
+ // initial filter
+ left_desiredTorques_filtered.resize(left_torque_targets.size());
+ left_desiredTorques_filtered.setZero();
+ right_desiredTorques_filtered.resize(right_torque_targets.size());
+ right_desiredTorques_filtered.setZero();
- left_currentTCPLinearVelocity_filtered.setZero();
- right_currentTCPLinearVelocity_filtered.setZero();
+ left_currentTCPLinearVelocity_filtered.setZero();
+ right_currentTCPLinearVelocity_filtered.setZero();
- left_currentTCPAngularVelocity_filtered.setZero();
- right_currentTCPAngularVelocity_filtered.setZero();
+ left_currentTCPAngularVelocity_filtered.setZero();
+ right_currentTCPAngularVelocity_filtered.setZero();
- left_tcpDesiredTorque_filtered.setZero();
- right_tcpDesiredTorque_filtered.setZero();
+ left_tcpDesiredTorque_filtered.setZero();
+ right_tcpDesiredTorque_filtered.setZero();
- smooth_startup = 0;
+ smooth_startup = 0;
- filterTimeConstant = cfg->filterTimeConstant;
- posiKp = cfg->posiKp;
- v_max = cfg->v_max;
- Damping = cfg->posiDamping;
- Coupling_stiffness = cfg->couplingStiffness;
- Coupling_force_limit = cfg->couplingForceLimit;
- forward_gain = cfg->forwardGain;
- torqueLimit = cfg->torqueLimit;
- null_torqueLimit = cfg->NullTorqueLimit;
- oriKp = cfg->oriKp;
- oriDamping = cfg->oriDamping;
- contactForce = cfg->contactForce;
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ Damping = cfg->posiDamping;
+ Coupling_stiffness = cfg->couplingStiffness;
+ Coupling_force_limit = cfg->couplingForceLimit;
+ forward_gain = cfg->forwardGain;
+ torqueLimit = cfg->torqueLimit;
+ null_torqueLimit = cfg->NullTorqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
+ contactForce = cfg->contactForce;
- left_oriUp.w() = cfg->left_oriUp[0];
- left_oriUp.x() = cfg->left_oriUp[1];
- left_oriUp.y() = cfg->left_oriUp[2];
- left_oriUp.z() = cfg->left_oriUp[3];
+ left_oriUp.w() = cfg->left_oriUp[0];
+ left_oriUp.x() = cfg->left_oriUp[1];
+ left_oriUp.y() = cfg->left_oriUp[2];
+ left_oriUp.z() = cfg->left_oriUp[3];
- left_oriDown.w() = cfg->left_oriDown[0];
- left_oriDown.x() = cfg->left_oriDown[1];
- left_oriDown.y() = cfg->left_oriDown[2];
- left_oriDown.z() = cfg->left_oriDown[3];
+ left_oriDown.w() = cfg->left_oriDown[0];
+ left_oriDown.x() = cfg->left_oriDown[1];
+ left_oriDown.y() = cfg->left_oriDown[2];
+ left_oriDown.z() = cfg->left_oriDown[3];
- right_oriUp.w() = cfg->right_oriUp[0];
- right_oriUp.x() = cfg->right_oriUp[1];
- right_oriUp.y() = cfg->right_oriUp[2];
- right_oriUp.z() = cfg->right_oriUp[3];
+ right_oriUp.w() = cfg->right_oriUp[0];
+ right_oriUp.x() = cfg->right_oriUp[1];
+ right_oriUp.y() = cfg->right_oriUp[2];
+ right_oriUp.z() = cfg->right_oriUp[3];
- right_oriDown.w() = cfg->right_oriDown[0];
- right_oriDown.x() = cfg->right_oriDown[1];
- right_oriDown.y() = cfg->right_oriDown[2];
- right_oriDown.z() = cfg->right_oriDown[3];
+ right_oriDown.w() = cfg->right_oriDown[0];
+ right_oriDown.x() = cfg->right_oriDown[1];
+ right_oriDown.y() = cfg->right_oriDown[2];
+ right_oriDown.z() = cfg->right_oriDown[3];
- guardTargetZUp = cfg->guardTargetZUp;
- guardTargetZDown = cfg->guardTargetZDown;
- guardDesiredZ = guardTargetZUp;
- guard_mounting_correction_z = 0;
+ guardTargetZUp = cfg->guardTargetZUp;
+ guardTargetZDown = cfg->guardTargetZDown;
+ guardDesiredZ = guardTargetZUp;
+ guard_mounting_correction_z = 0;
- guardXYHighFrequency = 0;
- left_force_old.setZero();
- right_force_old.setZero();
+ guardXYHighFrequency = 0;
+ left_force_old.setZero();
+ right_force_old.setZero();
- left_contactForceZ_correction = 0;
- right_contactForceZ_correction = 0;
+ left_contactForceZ_correction = 0;
+ right_contactForceZ_correction = 0;
- std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
- std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
+ std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
+ std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
- left_qnullspace.resize(leftarm_qnullspaceVec.size());
- right_qnullspace.resize(rightarm_qnullspaceVec.size());
+ left_qnullspace.resize(leftarm_qnullspaceVec.size());
+ right_qnullspace.resize(rightarm_qnullspaceVec.size());
- for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
- {
- left_qnullspace(i) = leftarm_qnullspaceVec[i];
- right_qnullspace(i) = rightarm_qnullspaceVec[i];
- }
+ for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
+ {
+ left_qnullspace(i) = leftarm_qnullspaceVec[i];
+ right_qnullspace(i) = rightarm_qnullspaceVec[i];
+ }
- nullspaceKp = cfg->nullspaceKp;
- nullspaceDamping = cfg->nullspaceDamping;
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
- //set GMM parameters ...
- if (cfg->gmmParamsFile == "")
- {
- ARMARX_ERROR << "gmm params file cannot be empty string ...";
- }
- gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
- positionErrorTolerance = cfg->positionErrorTolerance;
- forceFilterCoeff = cfg->forceFilterCoeff;
- for (size_t i = 0 ; i < 3; ++i)
- {
- leftForceOffset[i] = cfg->forceLeftOffset.at(i);
- rightForceOffset[i] = cfg->forceRightOffset.at(i);
+ //set GMM parameters ...
+ if (cfg->gmmParamsFile == "")
+ {
+ ARMARX_ERROR << "gmm params file cannot be empty string ...";
+ }
+ gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
+ positionErrorTolerance = cfg->positionErrorTolerance;
+ forceFilterCoeff = cfg->forceFilterCoeff;
+ for (size_t i = 0 ; i < 3; ++i)
+ {
+ leftForceOffset[i] = cfg->forceLeftOffset.at(i);
+ rightForceOffset[i] = cfg->forceRightOffset.at(i);
+ }
+ isDefaultTarget = false;
+ ARMARX_INFO << "Initialization done";
}
- isDefaultTarget = false;
- ARMARX_INFO << "Initialization done";
-}
-void DSRTBimanualController::controllerRun()
-{
- if (!controllerSensorData.updateReadBuffer())
+ void DSRTBimanualController::controllerRun()
{
- return;
- }
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
- // receive the measurements
- Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
- Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
+ // receive the measurements
+ Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
+ Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
- Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
- Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
- Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
+ Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
+ Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
+ Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
- // float left_force_z = left_force(2);
- // float right_force_z = right_force(2);
+ // float left_force_z = left_force(2);
+ // float right_force_z = right_force(2);
- Eigen::Vector3f left_currentTCPPositionInMeter;
- Eigen::Vector3f right_currentTCPPositionInMeter;
+ Eigen::Vector3f left_currentTCPPositionInMeter;
+ Eigen::Vector3f right_currentTCPPositionInMeter;
- left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
- right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
+ left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
+ right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
- left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
- right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
+ left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
+ right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
- // updating the desired height for the guard based on the interaction forces
- float both_arm_height_z_ave = 0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
+ // updating the desired height for the guard based on the interaction forces
+ float both_arm_height_z_ave = 0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
- float adaptive_ratio = 1;
- float force_error_z = 0;
- float guard_mounting_correction_x = 0;
- float guard_mounting_correction_y = 0;
+ float adaptive_ratio = 1;
+ float force_error_z = 0;
+ float guard_mounting_correction_x = 0;
+ float guard_mounting_correction_y = 0;
- if (cfg->guardDesiredDirection)
- {
- adaptive_ratio = 1;
- force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
+ if (cfg->guardDesiredDirection)
+ {
+ adaptive_ratio = 1;
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
- // meausures the high-frequency components of the interaction forces
- float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
- // diff_norm = deadzone(diff_norm,0.1,2);
- guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
+ // meausures the high-frequency components of the interaction forces
+ float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
+ // diff_norm = deadzone(diff_norm,0.1,2);
+ guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
- guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
+ guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
- left_force_old = left_force;
- right_force_old = right_force;
+ left_force_old = left_force;
+ right_force_old = right_force;
- if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
- {
- guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
- guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
+ if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
+ {
+ guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
+ guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
- guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
- guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
+ guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
+ guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
- guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
- guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
+ guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
+ guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
- }
+ }
- }
- else
- {
- adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
- force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
+ }
+ else
+ {
+ adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
- }
+ }
- force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
- guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
+ force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
+ guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
- guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
- guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
+ guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
+ guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
- if (isDefaultTarget)
- {
- guardDesiredZ = guardTargetZDown;
- }
+ if (isDefaultTarget)
+ {
+ guardDesiredZ = guardTargetZDown;
+ }
- if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
- {
- guard_mounting_correction_y = -0.1;
- }
+ if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
+ {
+ guard_mounting_correction_y = -0.1;
+ }
- // update the desired velocity
- gmmMotionGenerator->updateDesiredVelocity(
- left_currentTCPPositionInMeter,
- right_currentTCPPositionInMeter,
- positionErrorTolerance,
- guardDesiredZ,
- guard_mounting_correction_x,
- guard_mounting_correction_y,
- guard_mounting_correction_z);
+ // update the desired velocity
+ gmmMotionGenerator->updateDesiredVelocity(
+ left_currentTCPPositionInMeter,
+ right_currentTCPPositionInMeter,
+ positionErrorTolerance,
+ guardDesiredZ,
+ guard_mounting_correction_x,
+ guard_mounting_correction_y,
+ guard_mounting_correction_z);
- // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
- // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
- Eigen::Vector3f left_tcpDesiredAngularError;
- Eigen::Vector3f right_tcpDesiredAngularError;
+ Eigen::Vector3f left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError;
- left_tcpDesiredAngularError << 0, 0, 0;
- right_tcpDesiredAngularError << 0, 0, 0;
+ left_tcpDesiredAngularError << 0, 0, 0;
+ right_tcpDesiredAngularError << 0, 0, 0;
- Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
- float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
- float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+ float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+ float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
- lqratio = (lqratio > 1) ? 1 : lqratio;
- lqratio = (lqratio < 0) ? 0 : lqratio;
- rqratio = (rqratio > 1) ? 1 : rqratio;
- rqratio = (rqratio < 0) ? 0 : rqratio;
+ lqratio = (lqratio > 1) ? 1 : lqratio;
+ lqratio = (lqratio < 0) ? 0 : lqratio;
+ rqratio = (rqratio > 1) ? 1 : rqratio;
+ rqratio = (rqratio < 0) ? 0 : rqratio;
- Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
- Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
+ Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
+ Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
- Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
- Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
+ Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
+ Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
- Eigen::Quaternionf left_diffQuaternion(left_orientationError);
- Eigen::Quaternionf right_diffQuaternion(right_orientationError);
+ Eigen::Quaternionf left_diffQuaternion(left_orientationError);
+ Eigen::Quaternionf right_diffQuaternion(right_orientationError);
- Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
- Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
+ Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
+ Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
- left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
- right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
+ left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
+ right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
- // writing to the buffer
- getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
- getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
- getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
+ // writing to the buffer
+ getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
+ getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
+ getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
- getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
- getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
+ getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
+ getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
- writeControlStruct();
- debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
- debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
- debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
- debugCtrlDataInfo.commitWrite();
+ writeControlStruct();
+ debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
+ debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
+ debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
+ debugCtrlDataInfo.commitWrite();
-}
+ }
-void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- // reading the control objectives from the other threads (desired velocities)
- Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
- Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
- Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
- Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
- Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
+ void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ // reading the control objectives from the other threads (desired velocities)
+ Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
+ Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
+ Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
+ Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
- double deltaT = timeSinceLastIteration.toSecondsDouble();
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
- // measure the sesor data for the other threads
+ // measure the sesor data for the other threads
- Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
+ Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
+ Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
- // ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
- // ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
+ // ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
+ // ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
- Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
- Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
- // Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
- // Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
- left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
- right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
+ Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
+ Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
+ // Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
+ // Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
+ left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
+ right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
- Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
- Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
+ Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
+ Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
- Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf left_qpos;
- Eigen::VectorXf left_qvel;
+ Eigen::VectorXf left_qpos;
+ Eigen::VectorXf left_qvel;
- Eigen::VectorXf right_qpos;
- Eigen::VectorXf right_qvel;
+ Eigen::VectorXf right_qpos;
+ Eigen::VectorXf right_qvel;
- left_qpos.resize(left_positionSensors.size());
- left_qvel.resize(left_velocitySensors.size());
+ left_qpos.resize(left_positionSensors.size());
+ left_qvel.resize(left_velocitySensors.size());
- right_qpos.resize(right_positionSensors.size());
- right_qvel.resize(right_velocitySensors.size());
+ right_qpos.resize(right_positionSensors.size());
+ right_qvel.resize(right_velocitySensors.size());
- int jointDim = left_positionSensors.size();
+ int jointDim = left_positionSensors.size();
- for (size_t i = 0; i < left_velocitySensors.size(); ++i)
- {
- left_qpos(i) = left_positionSensors[i]->position;
- left_qvel(i) = left_velocitySensors[i]->velocity;
+ for (size_t i = 0; i < left_velocitySensors.size(); ++i)
+ {
+ left_qpos(i) = left_positionSensors[i]->position;
+ left_qvel(i) = left_velocitySensors[i]->velocity;
- right_qpos(i) = right_positionSensors[i]->position;
- right_qvel(i) = right_velocitySensors[i]->velocity;
- }
+ right_qpos(i) = right_positionSensors[i]->position;
+ right_qvel(i) = right_velocitySensors[i]->velocity;
+ }
- Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
- Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
+ Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
+ Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
- Eigen::Vector3f left_currentTCPLinearVelocity;
- Eigen::Vector3f right_currentTCPLinearVelocity;
- Eigen::Vector3f left_currentTCPAngularVelocity;
- Eigen::Vector3f right_currentTCPAngularVelocity;
- left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
- right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
- left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
- right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
- double filterFactor = deltaT / (filterTimeConstant + deltaT);
- left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
- right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
- left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
- right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
- left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
- right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
+ Eigen::Vector3f left_currentTCPLinearVelocity;
+ Eigen::Vector3f right_currentTCPLinearVelocity;
+ Eigen::Vector3f left_currentTCPAngularVelocity;
+ Eigen::Vector3f right_currentTCPAngularVelocity;
+ left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
+ right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
+ left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
+ right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
+ right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
+ left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
+ right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
+ left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
+ right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
- // writing into the bufffer for other threads
- controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
- controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
- controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
- controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
+ // writing into the bufffer for other threads
+ controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
+ controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
+ controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
+ controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
- // inverse dynamics:
+ // inverse dynamics:
- // reading the tcp orientation
+ // reading the tcp orientation
- // computing the task-specific forces
- Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
- Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
- for (int i = 0; i < 3; ++i)
- {
- left_DS_force(i) = left_DS_force(i) * Damping[i];
- right_DS_force(i) = right_DS_force(i) * Damping[i];
+ // computing the task-specific forces
+ Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
+ Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
+ for (int i = 0; i < 3; ++i)
+ {
+ left_DS_force(i) = left_DS_force(i) * Damping[i];
+ right_DS_force(i) = right_DS_force(i) * Damping[i];
- left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
- right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
+ left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
+ right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
- }
+ }
- // computing coupling forces
- Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
- float coupling_force_norm = coupling_force.norm();
+ // computing coupling forces
+ Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
+ float coupling_force_norm = coupling_force.norm();
- if (coupling_force_norm > Coupling_force_limit)
- {
- coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
- }
+ if (coupling_force_norm > Coupling_force_limit)
+ {
+ coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
+ }
- coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
- coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
- coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
+ coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
+ coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
+ coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
- double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
- float force_contact_switch = 0;
- float left_height = left_currentTCPPose(2, 3) * 0.001;
- float right_height = right_currentTCPPose(2, 3) * 0.001;
+ double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
+ float force_contact_switch = 0;
+ float left_height = left_currentTCPPose(2, 3) * 0.001;
+ float right_height = right_currentTCPPose(2, 3) * 0.001;
- float disTolerance = cfg->contactDistanceTolerance;
- bool isUp = fabs(left_height - guardTargetZUp) < disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
- if (v_both < disTolerance && isUp)
- {
- force_contact_switch = 1;
- }
- else if (v_both < 0.05 && isUp)
- {
- force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
- }
- else if (v_both >= 0.05)
- {
- force_contact_switch = 0;
- }
+ float disTolerance = cfg->contactDistanceTolerance;
+ bool isUp = fabs(left_height - guardTargetZUp) < disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
+ if (v_both < disTolerance && isUp)
+ {
+ force_contact_switch = 1;
+ }
+ else if (v_both < 0.05 && isUp)
+ {
+ force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
+ }
+ else if (v_both >= 0.05)
+ {
+ force_contact_switch = 0;
+ }
- // computing for contact forces
- float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
- float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
+ // computing for contact forces
+ float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
+ float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
- // float left_force_error_limited = left_force_error;
- // float right_force_error_limited = right_force_error;
+ // float left_force_error_limited = left_force_error;
+ // float right_force_error_limited = right_force_error;
- // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
- // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
+ // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
+ // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
- // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
- // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
+ // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
+ // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
- left_force_error = deadzone(left_force_error, 0.5, 2);
- right_force_error = deadzone(right_force_error, 0.5, 2);
+ left_force_error = deadzone(left_force_error, 0.5, 2);
+ right_force_error = deadzone(right_force_error, 0.5, 2);
- left_contactForceZ_correction = left_contactForceZ_correction - forceFilterCoeff * left_force_error;
- right_contactForceZ_correction = right_contactForceZ_correction - forceFilterCoeff * right_force_error;
+ left_contactForceZ_correction = left_contactForceZ_correction - forceFilterCoeff * left_force_error;
+ right_contactForceZ_correction = right_contactForceZ_correction - forceFilterCoeff * right_force_error;
- left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
- right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
+ left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
- left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
- right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
+ left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
- Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
- Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
+ Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
+ Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
- left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
- right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
+ left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
+ right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
- Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
- Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
+ Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
+ Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
- // for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
- // {
- // left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
+ // for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
+ // {
+ // left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
- // }
+ // }
- left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
- right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
+ left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
+ right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
- Eigen::Vector6f left_tcpDesiredWrench;
- Eigen::Vector6f right_tcpDesiredWrench;
+ Eigen::Vector6f left_tcpDesiredWrench;
+ Eigen::Vector6f right_tcpDesiredWrench;
- left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
- right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
+ left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
+ right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
- // calculate the null-spcae torque
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+ // calculate the null-spcae torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
- float lambda = 0.2;
- Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
- Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
+ float lambda = 0.2;
+ Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
+ Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
- Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
- Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
+ Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
+ Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
- for (int i = 0; i < left_nullqerror.size(); ++i)
- {
- if (fabs(left_nullqerror(i)) < 0.09)
+ for (int i = 0; i < left_nullqerror.size(); ++i)
{
- left_nullqerror(i) = 0;
- }
+ if (fabs(left_nullqerror(i)) < 0.09)
+ {
+ left_nullqerror(i) = 0;
+ }
- if (fabs(right_nullqerror(i)) < 0.09)
- {
- right_nullqerror(i) = 0;
+ if (fabs(right_nullqerror(i)) < 0.09)
+ {
+ right_nullqerror(i) = 0;
+ }
}
- }
- Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
- Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
+ Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
+ Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
- Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
- Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
+ Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
+ Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
- float left_nulltorque_norm = left_projectedNullTorque.norm();
- float right_nulltorque_norm = right_projectedNullTorque.norm();
+ float left_nulltorque_norm = left_projectedNullTorque.norm();
+ float right_nulltorque_norm = right_projectedNullTorque.norm();
- if (left_nulltorque_norm > null_torqueLimit)
- {
- left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
- }
+ if (left_nulltorque_norm > null_torqueLimit)
+ {
+ left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
+ }
- if (right_nulltorque_norm > null_torqueLimit)
- {
- right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
- }
+ if (right_nulltorque_norm > null_torqueLimit)
+ {
+ right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
+ }
- Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
- Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
+ Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
+ Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
- right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
+ right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
- for (size_t i = 0; i < left_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+ for (size_t i = 0; i < left_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
- std::string names = left_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
+ std::string names = left_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
- if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
- {
- left_torque_targets.at(i)->torque = 0;
- }
- else
- {
- left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ left_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
}
- }
- for (size_t i = 0; i < right_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+ for (size_t i = 0; i < right_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
- std::string names = right_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
+ std::string names = right_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
- if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
- {
- right_torque_targets.at(i)->torque = 0;
- }
- else
- {
- right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ right_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
}
- }
- smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
- smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
- smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
+ smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
+ smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
+ smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
- debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
- debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
- debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
- debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
- debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
- debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
+ debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
+ debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
- debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
- debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
+ debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
+ debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
- // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
+ // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
- debugDataInfo.commitWrite();
+ debugDataInfo.commitWrite();
- // }
- // else
- // {
- // for (size_t i = 0; i < targets.size(); ++i)
- // {
- // targets.at(i)->torque = 0;
+ // }
+ // else
+ // {
+ // for (size_t i = 0; i < targets.size(); ++i)
+ // {
+ // targets.at(i)->torque = 0;
- // }
- // }
+ // }
+ // }
-}
-
-float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
-{
- if (input > 0)
- {
- input = input - disturbance;
- input = (input < 0) ? 0 : input;
- input = (input > threshold) ? threshold : input;
- }
- else if (input < 0)
- {
- input = input + disturbance;
- input = (input > 0) ? 0 : input;
- input = (input < -threshold) ? -threshold : input;
}
+ float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
+ {
+ if (input > 0)
+ {
+ input = input - disturbance;
+ input = (input < 0) ? 0 : input;
+ input = (input > threshold) ? threshold : input;
+ }
+ else if (input < 0)
+ {
+ input = input + disturbance;
+ input = (input > 0) ? 0 : input;
+ input = (input < -threshold) ? -threshold : input;
+ }
- return input;
-}
-
-Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
-{
- double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
+ return input;
+ }
- Eigen::Quaternionf q1x = q1;
- if (cosHalfTheta < 0)
+ Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
{
- q1x.w() = -q1.w();
- q1x.x() = -q1.x();
- q1x.y() = -q1.y();
- q1x.z() = -q1.z();
- }
+ double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
- if (fabs(cosHalfTheta) >= 1.0)
- {
- return q0;
- }
+ Eigen::Quaternionf q1x = q1;
+ if (cosHalfTheta < 0)
+ {
+ q1x.w() = -q1.w();
+ q1x.x() = -q1.x();
+ q1x.y() = -q1.y();
+ q1x.z() = -q1.z();
+ }
- double halfTheta = acos(cosHalfTheta);
- double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
+ if (fabs(cosHalfTheta) >= 1.0)
+ {
+ return q0;
+ }
- Eigen::Quaternionf result;
- if (fabs(sinHalfTheta) < 0.001)
- {
- result.w() = (1 - t) * q0.w() + t * q1x.w();
- result.x() = (1 - t) * q0.x() + t * q1x.x();
- result.y() = (1 - t) * q0.y() + t * q1x.y();
- result.z() = (1 - t) * q0.z() + t * q1x.z();
+ double halfTheta = acos(cosHalfTheta);
+ double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
- }
- else
- {
- double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
- double ratioB = sin(t * halfTheta) / sinHalfTheta;
- result.w() = ratioA * q0.w() + ratioB * q1x.w();
- result.x() = ratioA * q0.x() + ratioB * q1x.x();
- result.y() = ratioA * q0.y() + ratioB * q1x.y();
- result.z() = ratioA * q0.z() + ratioB * q1x.z();
- }
+ Eigen::Quaternionf result;
+ if (fabs(sinHalfTheta) < 0.001)
+ {
+ result.w() = (1 - t) * q0.w() + t * q1x.w();
+ result.x() = (1 - t) * q0.x() + t * q1x.x();
+ result.y() = (1 - t) * q0.y() + t * q1x.y();
+ result.z() = (1 - t) * q0.z() + t * q1x.z();
- return result;
-}
+ }
+ else
+ {
+ double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
+ double ratioB = sin(t * halfTheta) / sinHalfTheta;
-void DSRTBimanualController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-{
+ result.w() = ratioA * q0.w() + ratioB * q1x.w();
+ result.x() = ratioA * q0.x() + ratioB * q1x.x();
+ result.y() = ratioA * q0.y() + ratioB * q1x.y();
+ result.z() = ratioA * q0.z() + ratioB * q1x.z();
+ }
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
+ return result;
}
- // std::string nameJacobi = "jacobiori";
- // std::string nameJacobipos = "jacobipos";
+ void DSRTBimanualController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
- // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
- // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
- // for (size_t i = 0; i < jacobiVec.size(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string name = nameJacobi + ss.str();
- // datafields[name] = new Variant(jacobiVec[i]);
- // std::string namepos = nameJacobipos + ss.str();
- // datafields[namepos] = new Variant(jacobiPos[i]);
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
- // }
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
- datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
- datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
- datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
- datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
- datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
- datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
+ // }
- datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
- datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
- datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
- datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
- datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
- datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
- datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
- datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
+ datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
+ datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
+ datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
+ datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
+ datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
+ datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
+ datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
+ datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
+ datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
+ datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
+ datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
+ datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
- datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
- datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
- datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
- datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
- datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
- datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
+ datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
+ datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
+ datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
+ datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
+ datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
+ datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
+ datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
+ datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
- // datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- // datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- // datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- // datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- // datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- // datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
- // datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
- // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
- // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+ // datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ // datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ // datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
- // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
- // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+ // datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ // datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ // datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+ // datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
- // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
- // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
- // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
+ // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
- debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
+ // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
-}
-void DSRTBimanualController::rtPreActivateController()
-{
+ debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
-}
+ }
-void DSRTBimanualController::rtPostDeactivateController()
-{
+ void DSRTBimanualController::rtPreActivateController()
+ {
-}
+ }
-void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
-{
- isDefaultTarget = true;
+ void DSRTBimanualController::rtPostDeactivateController()
+ {
+
+ }
+
+ void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
+ {
+ isDefaultTarget = true;
+ }
}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
index efe9bf523..42f3555d8 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
@@ -23,420 +23,421 @@
#include "DSRTController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
-using namespace armarx;
-
-NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
-
-void DSRTController::onInitNJointController()
+namespace armarx
{
- ARMARX_INFO << "init ...";
+ NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
- runTask("DSRTControllerTask", [&]
+ void DSRTController::onInitNJointController()
{
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
+ ARMARX_INFO << "init ...";
+
+ runTask("DSRTControllerTask", [&]
{
- if (isControllerActive())
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
{
- controllerRun();
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
}
- c.waitForCycleDuration();
- }
- });
-
+ });
-}
-void DSRTController::onDisconnectNJointController()
-{
+ }
-}
+ void DSRTController::onDisconnectNJointController()
+ {
+ }
-DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-{
- DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
- jointNames.clear();
- ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
- for (size_t i = 0; i < rns->getSize(); ++i)
+ DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
- std::string jointName = rns->getNode(i)->getName();
- jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
+ DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ jointNames.clear();
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
{
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
+ std::string jointName = rns->getNode(i)->getName();
+ jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
- ARMARX_INFO << "Initialized " << targets.size() << " targets";
- tcp = rns->getTCP();
- ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
+ torqueSensors.push_back(torqueSensor);
+ gravityTorqueSensors.push_back(gravityTorqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+ ARMARX_INFO << "Initialized " << targets.size() << " targets";
+ tcp = rns->getTCP();
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- DSRTControllerSensorData initSensorData;
- initSensorData.tcpPose = tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
+ DSRTControllerSensorData initSensorData;
+ initSensorData.tcpPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
- oldPosition = tcp->getPositionInRootFrame();
- oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ oldPosition = tcp->getPositionInRootFrame();
+ oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- std::vector<float> desiredPositionVec = cfg->desiredPosition;
- for (size_t i = 0; i < 3; ++i)
- {
- desiredPosition(i) = desiredPositionVec[i];
- }
- ARMARX_INFO << "ik reseted ";
+ std::vector<float> desiredPositionVec = cfg->desiredPosition;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ desiredPosition(i) = desiredPositionVec[i];
+ }
+ ARMARX_INFO << "ik reseted ";
- std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
- ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
+ std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
+ ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
- desiredQuaternion.x() = desiredQuaternionVec.at(0);
- desiredQuaternion.y() = desiredQuaternionVec.at(1);
- desiredQuaternion.z() = desiredQuaternionVec.at(2);
- desiredQuaternion.w() = desiredQuaternionVec.at(3);
+ desiredQuaternion.x() = desiredQuaternionVec.at(0);
+ desiredQuaternion.y() = desiredQuaternionVec.at(1);
+ desiredQuaternion.z() = desiredQuaternionVec.at(2);
+ desiredQuaternion.w() = desiredQuaternionVec.at(3);
- DSRTControllerControlData initData;
- for (size_t i = 0; i < 3; ++i)
- {
- initData.tcpDesiredLinearVelocity(i) = 0;
- }
+ DSRTControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredLinearVelocity(i) = 0;
+ }
- for (size_t i = 0; i < 3; ++i)
- {
- initData.tcpDesiredAngularError(i) = 0;
- }
- reinitTripleBuffer(initData);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredAngularError(i) = 0;
+ }
+ reinitTripleBuffer(initData);
- // initial filter
- currentTCPLinearVelocity_filtered.setZero();
- currentTCPAngularVelocity_filtered.setZero();
- filterTimeConstant = cfg->filterTimeConstant;
- posiKp = cfg->posiKp;
- v_max = cfg->v_max;
- posiDamping = cfg->posiDamping;
- torqueLimit = cfg->torqueLimit;
- oriKp = cfg->oriKp;
- oriDamping = cfg->oriDamping;
+ // initial filter
+ currentTCPLinearVelocity_filtered.setZero();
+ currentTCPAngularVelocity_filtered.setZero();
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ posiDamping = cfg->posiDamping;
+ torqueLimit = cfg->torqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
- std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
+ std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
- qnullspace.resize(qnullspaceVec.size());
+ qnullspace.resize(qnullspaceVec.size());
- for (size_t i = 0; i < qnullspaceVec.size(); ++i)
- {
- qnullspace(i) = qnullspaceVec[i];
- }
+ for (size_t i = 0; i < qnullspaceVec.size(); ++i)
+ {
+ qnullspace(i) = qnullspaceVec[i];
+ }
- nullspaceKp = cfg->nullspaceKp;
- nullspaceDamping = cfg->nullspaceDamping;
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
- //set GMM parameters ...
- std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
- if (gmmParamsFiles.size() == 0)
- {
- ARMARX_ERROR << "no gmm found ... ";
- }
+ //set GMM parameters ...
+ std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
+ if (gmmParamsFiles.size() == 0)
+ {
+ ARMARX_ERROR << "no gmm found ... ";
+ }
- gmmMotionGenList.clear();
- float sumBelief = 0;
- for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
- {
- gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
- sumBelief += gmmMotionGenList[i]->belief;
- }
- ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
+ gmmMotionGenList.clear();
+ float sumBelief = 0;
+ for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
+ {
+ gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
+ sumBelief += gmmMotionGenList[i]->belief;
+ }
+ ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
- dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
- positionErrorTolerance = cfg->positionErrorTolerance;
+ dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
+ positionErrorTolerance = cfg->positionErrorTolerance;
- ARMARX_INFO << "Initialization done";
-}
+ ARMARX_INFO << "Initialization done";
+ }
-void DSRTController::controllerRun()
-{
- if (!controllerSensorData.updateReadBuffer())
+ void DSRTController::controllerRun()
{
- return;
- }
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
- Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
- Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
+ Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
+ Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
- Eigen::Vector3f currentTCPPositionInMeter;
- currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
- currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
+ Eigen::Vector3f currentTCPPositionInMeter;
+ currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
- dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
- Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
- dsAdaptorPtr->updateBelief(realVelocity);
+ dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
+ Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
+ dsAdaptorPtr->updateBelief(realVelocity);
- float vecLen = tcpDesiredLinearVelocity.norm();
- if (vecLen > v_max)
- {
+ float vecLen = tcpDesiredLinearVelocity.norm();
+ if (vecLen > v_max)
+ {
- tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
- }
+ tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
+ }
- ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
+ ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
- debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
- debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
- debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
- debugDataInfo.commitWrite();
+ debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
+ debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
+ debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
+ debugDataInfo.commitWrite();
- Eigen::Vector3f tcpDesiredAngularError;
- tcpDesiredAngularError << 0, 0, 0;
+ Eigen::Vector3f tcpDesiredAngularError;
+ tcpDesiredAngularError << 0, 0, 0;
- Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
- Eigen::Quaternionf diffQuaternion(orientationError);
- Eigen::AngleAxisf angleAxis(diffQuaternion);
- tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
+ Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
+ Eigen::Quaternionf diffQuaternion(orientationError);
+ Eigen::AngleAxisf angleAxis(diffQuaternion);
+ tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
- getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
- getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
+ getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
+ getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
- writeControlStruct();
-}
+ writeControlStruct();
+ }
-void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- if (deltaT != 0)
+ void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ if (deltaT != 0)
+ {
- Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
+ Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf qpos;
- Eigen::VectorXf qvel;
- qpos.resize(positionSensors.size());
- qvel.resize(velocitySensors.size());
+ Eigen::VectorXf qpos;
+ Eigen::VectorXf qvel;
+ qpos.resize(positionSensors.size());
+ qvel.resize(velocitySensors.size());
- int jointDim = positionSensors.size();
+ int jointDim = positionSensors.size();
- for (size_t i = 0; i < velocitySensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
- Eigen::VectorXf tcptwist = jacobi * qvel;
+ Eigen::VectorXf tcptwist = jacobi * qvel;
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
- double filterFactor = deltaT / (filterTimeConstant + deltaT);
- currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
- controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
- controllerSensorData.commitWrite();
+ controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
+ controllerSensorData.commitWrite();
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
- // // calculate linear velocity
- // Eigen::Vector3f currentTCPPosition;
- // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
- // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
- // oldPosition = currentTCPPosition;
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+ // // calculate linear velocity
+ // Eigen::Vector3f currentTCPPosition;
+ // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
+ // oldPosition = currentTCPPosition;
- // // calculate angular velocity
- // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
- // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
- // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
- // oldOrientation = currentTCPOrientation;
- // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
+ // // calculate angular velocity
+ // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
+ // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
+ // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
+ // oldOrientation = currentTCPOrientation;
+ // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
- Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
- Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
+ Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
+ Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
- // calculate desired tcp force
- Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
+ // calculate desired tcp force
+ Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
- // calculate desired tcp torque
- Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
+ // calculate desired tcp torque
+ Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
- Eigen::Vector6f tcpDesiredWrench;
- tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+ Eigen::Vector6f tcpDesiredWrench;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
- // calculate desired joint torque
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+ // calculate desired joint torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
- float lambda = 2;
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
+ float lambda = 2;
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
- Eigen::VectorXf nullqerror = qpos - qnullspace;
+ Eigen::VectorXf nullqerror = qpos - qnullspace;
- for (int i = 0; i < nullqerror.rows(); ++i)
- {
- if (fabs(nullqerror(i)) < 0.09)
+ for (int i = 0; i < nullqerror.rows(); ++i)
{
- nullqerror(i) = 0;
+ if (fabs(nullqerror(i)) < 0.09)
+ {
+ nullqerror(i) = 0;
+ }
}
- }
- Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * qvel;
+ Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * qvel;
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
- for (size_t i = 0; i < targets.size(); ++i)
- {
- float desiredTorque = jointDesiredTorques(i);
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ float desiredTorque = jointDesiredTorques(i);
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
- debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
+ debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
- targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
- }
+ targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
+ }
- debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
- debugDataInfo.commitWrite();
+ debugDataInfo.commitWrite();
- }
- else
- {
- for (size_t i = 0; i < targets.size(); ++i)
+ }
+ else
{
- targets.at(i)->torque = 0;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = 0;
+ }
}
- }
-}
-
-void DSRTController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-{
+ }
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
+ void DSRTController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
- datafields[pair.first] = new Variant(pair.second);
- }
- // std::string nameJacobi = "jacobiori";
- // std::string nameJacobipos = "jacobipos";
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
- // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
- // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
- // for (size_t i = 0; i < jacobiVec.size(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string name = nameJacobi + ss.str();
- // datafields[name] = new Variant(jacobiVec[i]);
- // std::string namepos = nameJacobipos + ss.str();
- // datafields[namepos] = new Variant(jacobiPos[i]);
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
- // }
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
+ // }
- datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+ datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
- datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
- datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+ datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
- datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
- datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
- datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+ datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+ datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
- datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
- datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
- datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
- datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
- datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
- datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
+ datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
- debugObs->setDebugChannel("DSControllerOutput", datafields);
+ datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
+ datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
+ datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
-}
+ debugObs->setDebugChannel("DSControllerOutput", datafields);
-void DSRTController::rtPreActivateController()
-{
+ }
-}
+ void DSRTController::rtPreActivateController()
+ {
-void DSRTController::rtPostDeactivateController()
-{
+ }
+ void DSRTController::rtPostDeactivateController()
+ {
+
+ }
}
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
index 200605424..d4383dd55 100644
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
@@ -15,158 +15,176 @@ using namespace std;
/*
Gaussians::Gaussians(GMMs *model)
{
- this->model.nbStates = model->nbStates;
- this->model.nbDim = model->nbDim;
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
- this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
}
*/
-Gaussians::Gaussians(const char *f_mu, const char *f_sigma, const char *f_prio)
+Gaussians::Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio)
{
- int s, i, j;
- int nbStates;
- int nbDim;
-
- Matrix fMatrix;
- fMatrix.Load(f_prio);
- if ( fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0 )
- {
- nbStates = fMatrix.ColumnSize();
- }
- else
- {
- nbStates = fMatrix.ColumnSize() - 1;
- }
-
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Prio = fMatrix(0, s);
- }
-
- fMatrix.Load(f_mu);
- nbDim = fMatrix.RowSize();
- model.nbStates = nbStates;
- model.nbDim = nbDim;
-
-
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim );
- }
-
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Mu = fMatrix.GetColumn(s);
- }
-
- fMatrix.Load(f_sigma);
- j = 0;
- for ( s = 0; s < nbStates; s++ ) {
- for ( i = 0; i < nbDim; i++ ) {
- model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
- j++;
- }
- }
+ int s, i, j;
+ int nbStates;
+ int nbDim;
+
+ Matrix fMatrix;
+ fMatrix.Load(f_prio);
+ if (fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0)
+ {
+ nbStates = fMatrix.ColumnSize();
+ }
+ else
+ {
+ nbStates = fMatrix.ColumnSize() - 1;
+ }
+
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+ fMatrix.Load(f_mu);
+ nbDim = fMatrix.RowSize();
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim);
+ }
+
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
+
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for (s = 0; s < nbStates; s++)
+ {
+ for (i = 0; i < nbDim; i++)
+ {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
}
-Gaussians::Gaussians(int nbStates, int nbDim, const char *f_mu, const char *f_sigma, const char *f_prio)
+Gaussians::Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio)
{
- int s, i, j;
-
- model.nbStates = nbStates;
- model.nbDim = nbDim;
-
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim );
- }
-
- Matrix fMatrix(nbDim, nbStates);
- fMatrix.Load(f_mu);
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Mu = fMatrix.GetColumn(s);
- }
-
- fMatrix.Resize(nbStates * nbDim, nbDim);
- fMatrix.Load(f_sigma);
- j = 0;
- for ( s = 0; s < nbStates; s++ ) {
- for ( i = 0; i < nbDim; i++ ) {
- model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
- j++;
- }
- }
-
- fMatrix.Resize(1, nbStates);
- fMatrix.Load(f_prio);
- Vector fVector(nbStates);
- for ( s = 0; s < nbStates; s++ ) {
- model.States[s].Prio = fMatrix(0, s);
- }
+ int s, i, j;
-}
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
-Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec) {
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim);
+ }
- model.nbStates = nbStates;
- model.nbDim = nbDim;
+ Matrix fMatrix(nbDim, nbStates);
+ fMatrix.Load(f_mu);
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
- for ( int s = 0; s < nbStates; s++ ) {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim );
- }
+ fMatrix.Resize(nbStates * nbDim, nbDim);
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for (s = 0; s < nbStates; s++)
+ {
+ for (i = 0; i < nbDim; i++)
+ {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
- for ( int s = 0; s < nbStates; s++ ) {
- model.States[s].Prio = pri_vec[s];
- }
- // cout << endl << "Printing the constructed Priors" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // cout << model.States[s].Prio << "\t";
- // }
- // cout << endl;
+ fMatrix.Resize(1, nbStates);
+ fMatrix.Load(f_prio);
+ Vector fVector(nbStates);
+ for (s = 0; s < nbStates; s++)
+ {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+}
+
+Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
+{
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
- for ( int s = 0; s < nbStates; s++ ) {
- for (int d = 0; d < nbDim; d++) {
- model.States[s].Mu[d] = mu_vec[s * nbDim + d];
- }
- }
+ for (int s = 0; s < nbStates; s++)
+ {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim);
+ }
+ for (int s = 0; s < nbStates; s++)
+ {
+ model.States[s].Prio = pri_vec[s];
+ }
+ // cout << endl << "Printing the constructed Priors" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // cout << model.States[s].Prio << "\t";
+ // }
+ // cout << endl;
+
+
+ for (int s = 0; s < nbStates; s++)
+ {
+ for (int d = 0; d < nbDim; d++)
+ {
+ model.States[s].Mu[d] = mu_vec[s * nbDim + d];
+ }
+ }
- // cout << endl << "Printing the constructed Mu" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // for (int d = 0; d < nbDim; d++) {
- // cout << model.States[s].Mu[d] << "\t";
- // }
- // cout << endl;
- // }
- for ( int s = 0; s < nbStates; s++ ) {
- for (int row = 0; row < nbDim; row++) {
- for (int col = 0; col < nbDim; col++) {
- int ind = s * nbDim * nbDim + row * nbDim + col;
- model.States[s].Sigma(row, col) = sig_vec[ind];
- }
- }
- }
+ // cout << endl << "Printing the constructed Mu" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int d = 0; d < nbDim; d++) {
+ // cout << model.States[s].Mu[d] << "\t";
+ // }
+ // cout << endl;
+ // }
+
+ for (int s = 0; s < nbStates; s++)
+ {
+ for (int row = 0; row < nbDim; row++)
+ {
+ for (int col = 0; col < nbDim; col++)
+ {
+ int ind = s * nbDim * nbDim + row * nbDim + col;
+ model.States[s].Sigma(row, col) = sig_vec[ind];
+ }
+ }
+ }
- // cout << endl << "Printing the constructed Sigma" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // for (int row = 0; row < nbDim; row++) {
- // for (int col = 0; col < nbDim; col++) {
- // cout << model.States[s].Sigma(row, col) << "\t";
- // }
- // cout <<endl;
- // }
- // cout << endl;
- // }
+ // cout << endl << "Printing the constructed Sigma" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int row = 0; row < nbDim; row++) {
+ // for (int col = 0; col < nbDim; col++) {
+ // cout << model.States[s].Sigma(row, col) << "\t";
+ // }
+ // cout <<endl;
+ // }
+ // cout << endl;
+ // }
@@ -176,158 +194,183 @@ Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> p
-void Gaussians::setGMMs(GMMs *model)
+void Gaussians::setGMMs(GMMs* model)
{
- for (unsigned int s = 0; s < model->nbStates; s++ ) {
- this->model.States[s].Mu = model->States[s].Mu;
- this->model.States[s].Sigma = model->States[s].Sigma;
- this->model.States[s].Prio = model->States[s].Prio;
- }
+ for (unsigned int s = 0; s < model->nbStates; s++)
+ {
+ this->model.States[s].Mu = model->States[s].Mu;
+ this->model.States[s].Sigma = model->States[s].Sigma;
+ this->model.States[s].Prio = model->States[s].Prio;
+ }
}
void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
{
- double det;
- int nbIN = last_inindex - first_inindex + 1;
-
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- gmmpinv[s].MuI.Resize(nbIN);
- gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
- gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
- }
-
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- for ( int i = first_inindex; i <= last_inindex; i++ ) gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
- for ( int i = first_inindex; i <= last_inindex; i++ )
- for ( int j = first_inindex; j <= last_inindex; j++ )
- gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
-
- gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
- //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
- //gmmpinv[s].SigmaII.Inverse(&det);
- if (det < 0) det = 1e-30;
- gmmpinv[s].detSigmaII = det;
-
- }
-
- nbDimI = last_inindex - first_inindex + 1;
- gfDiff.Resize(nbDimI);
- gfDiffp.Resize(nbDimI);
- gDer.Resize(nbDimI);
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ for (int i = first_inindex; i <= last_inindex; i++)
+ {
+ gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+ }
+ for (int i = first_inindex; i <= last_inindex; i++)
+ for (int j = first_inindex; j <= last_inindex; j++)
+ {
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+ }
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
+ //gmmpinv[s].SigmaII.Inverse(&det);
+ if (det < 0)
+ {
+ det = 1e-30;
+ }
+ gmmpinv[s].detSigmaII = det;
+
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
}
double Gaussians::GaussianPDFFast(int state, Vector x)
{
- double p;
- gfDiff = x - gmmpinv[state].MuI;
- gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
- p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * ( gmmpinv[state].detSigmaII + 1e-30));
+ p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * (gmmpinv[state].detSigmaII + 1e-30));
- return p;
+ return p;
}
double Gaussians::GaussianProbFast(Vector x)
{
- double totalP = 0;
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- totalP += model.States[s].Prio * GaussianPDFFast(s, x);
- }
- return totalP;
+ double totalP = 0;
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ totalP += model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return totalP;
}
Vector Gaussians::GaussianDerProbFast(Vector x)
{
- gDer.Zero();
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
- }
- return -gDer;
+ gDer.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return -gDer;
}
void Gaussians::InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
{
- double det;
- int nbIN = last_inindex - first_inindex + 1;
- int nbOUT = last_outindex - first_outindex + 1;
-
- gPdf.Resize(model.nbStates);
-
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- gmmpinv[s].MuI.Resize(nbIN);
- gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
- gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
-
- gmmpinv[s].muO.Resize(nbOUT);
- gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
- gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
- }
-
- for (unsigned int s = 0; s < model.nbStates; s++ ) {
- for ( int i = first_inindex; i <= last_inindex; i++ ) {
- gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
-
- for ( int j = first_inindex; j <= last_inindex; j++ ) {
- gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
- }
- for ( int j = first_outindex; j <= last_outindex; j++ ) {
- gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
- }
- }
-
- for ( int i = first_outindex; i <= last_outindex; i++ ) {
- gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
- }
-
- gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
- if (det < 0) det = 1e-30;
- gmmpinv[s].detSigmaII = det;
- (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
- }
-
- nbDimI = last_inindex - first_inindex + 1;
- gfDiff.Resize(nbDimI);
- gfDiffp.Resize(nbDimI);
- gDer.Resize(nbDimI);
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+ int nbOUT = last_outindex - first_outindex + 1;
+
+ gPdf.Resize(model.nbStates);
+
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+
+ gmmpinv[s].muO.Resize(nbOUT);
+ gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
+ gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ for (int i = first_inindex; i <= last_inindex; i++)
+ {
+ gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+
+ for (int j = first_inindex; j <= last_inindex; j++)
+ {
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+ }
+ for (int j = first_outindex; j <= last_outindex; j++)
+ {
+ gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
+ }
+ }
+
+ for (int i = first_outindex; i <= last_outindex; i++)
+ {
+ gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
+ }
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ if (det < 0)
+ {
+ det = 1e-30;
+ }
+ gmmpinv[s].detSigmaII = det;
+ (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
}
-void Gaussians::Regression(const Vector & indata, Vector & outdata, Matrix & derGMR)
+void Gaussians::Regression(const Vector& indata, Vector& outdata, Matrix& derGMR)
{
- Regression(indata, outdata);
- cout << "derivative is not implemented yet " << endl;
+ Regression(indata, outdata);
+ cout << "derivative is not implemented yet " << endl;
}
-void Gaussians::Regression(const Vector & indata, Vector & outdata)
+void Gaussians::Regression(const Vector& indata, Vector& outdata)
{
- double pdfall;
- Vector h(model.nbStates);
- Vector r_diff(outdata.Size());
-
- for (unsigned int s = 0; s < model.nbStates; s++) {
- gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
- }
- pdfall = gPdf.Sum();
-
- outdata.Zero();
- for (unsigned int s = 0; s < model.nbStates; s++) {
- //h(s) = gPdf(s)/(pdfall + 1e-30 );
- h(s) = gPdf(s) / (pdfall );
- r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
-
- for (unsigned int i = 0; i < r_diff.Size(); i++ ) {
- outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
- }
- }
+ double pdfall;
+ Vector h(model.nbStates);
+ Vector r_diff(outdata.Size());
+
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
+ }
+ pdfall = gPdf.Sum();
+
+ outdata.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++)
+ {
+ //h(s) = gPdf(s)/(pdfall + 1e-30 );
+ h(s) = gPdf(s) / (pdfall);
+ r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
+
+ for (unsigned int i = 0; i < r_diff.Size(); i++)
+ {
+ outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
+ }
+ }
}
-Vector Gaussians::Regression(const Vector & indata)
+Vector Gaussians::Regression(const Vector& indata)
{
- Vector outdata(indata.Size());
- Regression(indata, outdata);
- return outdata;
+ Vector outdata(indata.Size());
+ Regression(indata, outdata);
+ return outdata;
}
@@ -342,177 +385,177 @@ using namespace std;
Gaussians::Gaussians(GMMs *model)
{
- this->model.nbStates = model->nbStates;
- this->model.nbDim = model->nbDim;
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
- this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
}
Gaussians::Gaussians(int nbStates, int nbDim, char *f_mu, char *f_sigma, char *f_prio)
{
- int s, i, j;
-
- model.nbStates = nbStates;
- model.nbDim = nbDim;
- model.States = (GMMState *)malloc(nbStates*sizeof(GMMState) );
-
- for( s=0; s<nbStates; s++ ){
- model.States[s].Mu = zeros<vec>(nbDim);
- model.States[s].Sigma = zeros<mat>(nbDim, nbDim );
- }
-
- // f_mu
- ifstream fin;
- fin.open(f_mu);
- for( i=0; i<nbDim; i++ ){
- for( s=0; s<nbStates; s++ ){
- fin >> model.States[s].Mu(i);
- }
- }
- fin.close();
-
- // f_sigma
- fin.open(f_sigma);
- for( s=0; s<nbStates; s++ ){
- for( i=0; i<nbDim; i++ ){
- for( j=0; j<nbDim; j++ ){
- fin >> model.States[s].Sigma(i,j);
- }
- }
- }
- fin.close();
-
- // f_prio
- fin.open(f_prio);
- for( s=0; s<nbStates; s++ ){
- fin >>model.States[s].Prio;
- }
- fin.close();
+ int s, i, j;
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+ model.States = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for( s=0; s<nbStates; s++ ){
+ model.States[s].Mu = zeros<vec>(nbDim);
+ model.States[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
+
+ // f_mu
+ ifstream fin;
+ fin.open(f_mu);
+ for( i=0; i<nbDim; i++ ){
+ for( s=0; s<nbStates; s++ ){
+ fin >> model.States[s].Mu(i);
+ }
+ }
+ fin.close();
+
+ // f_sigma
+ fin.open(f_sigma);
+ for( s=0; s<nbStates; s++ ){
+ for( i=0; i<nbDim; i++ ){
+ for( j=0; j<nbDim; j++ ){
+ fin >> model.States[s].Sigma(i,j);
+ }
+ }
+ }
+ fin.close();
+
+ // f_prio
+ fin.open(f_prio);
+ for( s=0; s<nbStates; s++ ){
+ fin >>model.States[s].Prio;
+ }
+ fin.close();
}
void Gaussians::setGMMs(GMMs *model)
{
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
}
void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
{
- gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
-
- for(int s=0; s<model.nbStates; s++ ){
- gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
- gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
- gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
- gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
- }
-
- nbDimI = last_inindex - first_inindex +1;
- gfDiff = zeros<vec>(nbDimI);
- gfDiffp = zeros<vec>(nbDimI);
- gDer = zeros<vec>(nbDimI);
+ gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
+
+ for(int s=0; s<model.nbStates; s++ ){
+ gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
+ gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
+ gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
+ gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
+ }
+
+ nbDimI = last_inindex - first_inindex +1;
+ gfDiff = zeros<vec>(nbDimI);
+ gfDiffp = zeros<vec>(nbDimI);
+ gDer = zeros<vec>(nbDimI);
}
double Gaussians::GaussianPDFFast(int state, vec x)
{
- double p;
- gfDiff = x - gmmpinv[state].MuI;
- gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
- p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
+ p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
- return p;
+ return p;
}
double Gaussians::GaussianProbFast(vec x)
{
- double totalP=0;
- for(int s=0; s<model.nbStates; s++ ){
- totalP += model.States[s].Prio*GaussianPDFFast(s,x);
- }
- return totalP;
+ double totalP=0;
+ for(int s=0; s<model.nbStates; s++ ){
+ totalP += model.States[s].Prio*GaussianPDFFast(s,x);
+ }
+ return totalP;
}
vec Gaussians::GaussianDerProbFast(vec x)
{
- gDer.zeros();
- for(int s=0; s<model.nbStates; s++ ){
- gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
- }
- return -gDer;
+ gDer.zeros();
+ for(int s=0; s<model.nbStates; s++ ){
+ gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
+ }
+ return -gDer;
}
//-------------------------------------------------------------------------------------------------------
void AllocateGMMs(GMMs *model, int nbDim, int nbStates)
{
- model->nbDim = nbDim;
- model->nbStates = nbStates;
- model->GMMState = (GMMState *)malloc(nbStates*sizeof(GMMState) );
-
- for(int s=0; s<nbStates; s++ ){
- model->GMMState[s].Mu = zeros<vec>(nbDim);
- model->GMMState[s].Sigma = zeros<mat>(nbDim, nbDim );
- }
+ model->nbDim = nbDim;
+ model->nbStates = nbStates;
+ model->GMMState = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<nbStates; s++ ){
+ model->GMMState[s].Mu = zeros<vec>(nbDim);
+ model->GMMState[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
}
double GaussianPDF(vec x, vec Mu, mat Sigma)
{
- double p;
- vec diff = x - Mu;
- vec diffp = pinv( Sigma ) * diff;
- int nbDim = x.size();
+ double p;
+ vec diff = x - Mu;
+ vec diffp = pinv( Sigma ) * diff;
+ int nbDim = x.size();
- p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
+ p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
if(p < 1e-30){
- return 1e-30;
+ return 1e-30;
+ }
+ else{
+ return p;
}
- else{
- return p;
- }
}
void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut)
{
- int k,l,j;
- int K = modelK->nbStates;
- int L = modelL->nbStates;
- int J = K*L;
-
- //modelOut->nbDim = modelK->nbDim;
- //modelOut->nbStates = J;
- //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
-
- j=0;
- for(k=0; k<K; k++){
- for(l=0; l<L; l++){
- modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
- modelOut->GMMState[j].Mu = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
- modelOut->GMMState[j].Prio = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
- j++;
- }
- }
+ int k,l,j;
+ int K = modelK->nbStates;
+ int L = modelL->nbStates;
+ int J = K*L;
+
+ //modelOut->nbDim = modelK->nbDim;
+ //modelOut->nbStates = J;
+ //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
+
+ j=0;
+ for(k=0; k<K; k++){
+ for(l=0; l<L; l++){
+ modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
+ modelOut->GMMState[j].Mu = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
+ modelOut->GMMState[j].Prio = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
+ j++;
+ }
+ }
}
void GaussianRotate(GMMs *model, arma::vec P, arma::mat R, GMMs *modelOut)
{
- for(int s=0; s<model->nbStates; s++){
- modelOut->GMMState[s].Mu = R*model->GMMState[s].Mu + P;
- modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
- }
- //modelOut->nbDim = model->nbDim;
- //modelOut->nbStates = model->nbStates;
+ for(int s=0; s<model->nbStates; s++){
+ modelOut->GMMState[s].Mu = R*model->GMMState[s].Mu + P;
+ modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
+ }
+ //modelOut->nbDim = model->nbDim;
+ //modelOut->nbStates = model->nbStates;
}
*/
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.h b/source/RobotAPI/libraries/DSControllers/Gaussians.h
index b72890a83..43375b145 100644
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.h
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.h
@@ -12,8 +12,6 @@
#define GAUSSIAN_MAXIMUM_NUMBER 50
-using namespace MathLib;
-
struct GMMState
{
Vector Mu;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
index b118e52c5..d4505ed99 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -16,312 +16,312 @@
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-using namespace armarx;
-
-
-JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
- ActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
- config(positionControllerConfigDataPtr),
- // controller(positionControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
-{
- actorIndex = board->getActorIndex(deviceName);
- sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
- ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
- dataPtr = jointData;
-
- posController.desiredDeceleration = positionControllerConfigDataPtr->maxDecelerationRad;
- posController.desiredJerk = 100;
- posController.maxDt = positionControllerConfigDataPtr->maxDt;
- posController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
- posController.p = 4;
- posController.phase2SwitchDistance = 0.1;
- ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
- // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
- // posController.positionLimitHi = jointData->getSoftLimitHi();
- // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
- // posController.positionLimitLo = jointData->getSoftLimitLo();
- // posController.pControlPosErrorLimit = 0.02f;
- // posController.pid->Kp = posController.calculateProportionalGain();
- // ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
-
- this->isLimitless = jointData->isLimitless();
- pidPosController.reset(new PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
- pidPosController->maxIntegral = positionControllerConfigDataPtr->maxIntegral;
- pidPosController->differentialFilter.reset(new rtfilters::AverageFilter(10));
-
-}
-
-JointPWMPositionController::~JointPWMPositionController() noexcept(true)
+namespace armarx
{
- stopRequested = true;
- try
- {
- threadHandle.join();
- }
- catch (...)
+ JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
+ ActorDataPtr jointData,
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
+ config(positionControllerConfigDataPtr),
+ // controller(positionControllerConfigDataPtr),
+ target(), board(board), deviceName(deviceName)
{
+ actorIndex = board->getActorIndex(deviceName);
+ sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
+ dataPtr = jointData;
+
+ posController.desiredDeceleration = positionControllerConfigDataPtr->maxDecelerationRad;
+ posController.desiredJerk = 100;
+ posController.maxDt = positionControllerConfigDataPtr->maxDt;
+ posController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
+ posController.p = 4;
+ posController.phase2SwitchDistance = 0.1;
+ ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
+ // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
+ // posController.positionLimitHi = jointData->getSoftLimitHi();
+ // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
+ // posController.positionLimitLo = jointData->getSoftLimitLo();
+ // posController.pControlPosErrorLimit = 0.02f;
+ // posController.pid->Kp = posController.calculateProportionalGain();
+ // ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
+
+ this->isLimitless = jointData->isLimitless();
+ pidPosController.reset(new PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
+ pidPosController->maxIntegral = positionControllerConfigDataPtr->maxIntegral;
+ pidPosController->differentialFilter.reset(new rtfilters::AverageFilter(10));
}
-}
-void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- if (target.isValid())
+ JointPWMPositionController::~JointPWMPositionController() noexcept(true)
{
- auto currentPosition = dataPtr->getPosition();
- // float targetPosition = boost::algorithm::clamp(target.position,
- // std::min(currentPosition, posController.positionLimitLo), // lo or current position
- // std::max(currentPosition, posController.positionLimitHi)); // hi or current position
-
- if (isLimitless)
+ stopRequested = true;
+ try
{
- ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
- return;
-
+ threadHandle.join();
}
- else
+ catch (...)
{
- // posController.currentPosition = currentPosition;
+
}
- posController.currentV = lastTargetVelocity;
- posController.dt = timeSinceLastIteration.toSecondsDouble();
- posController.setTargetPosition(target.position);
- // ARMARX_CHECK_EXPRESSION(posController.validParameters());
- auto r = posController.run();
- posController.currentPosition = r.position;
- posController.currentAcc = r.acceleration;
- double newVel = r.velocity;
- // double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
- // newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
- if (std::isnan(newVel))
+ }
+
+ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (target.isValid())
{
- newVel = 0;
- }
- pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
- auto targetPWM = pidPosController->getControlValue();
- // auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
- newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
+ auto currentPosition = dataPtr->getPosition();
+ // float targetPosition = boost::algorithm::clamp(target.position,
+ // std::min(currentPosition, posController.positionLimitLo), // lo or current position
+ // std::max(currentPosition, posController.positionLimitHi)); // hi or current position
+
+ if (isLimitless)
+ {
+ ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
+ return;
+
+ }
+ else
+ {
+ // posController.currentPosition = currentPosition;
+ }
+ posController.currentV = lastTargetVelocity;
+ posController.dt = timeSinceLastIteration.toSecondsDouble();
+ posController.setTargetPosition(target.position);
+ // ARMARX_CHECK_EXPRESSION(posController.validParameters());
+ auto r = posController.run();
+ posController.currentPosition = r.position;
+ posController.currentAcc = r.acceleration;
+ double newVel = r.velocity;
+ // double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
+ // newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ }
+ pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
+ auto targetPWM = pidPosController->getControlValue();
+ // auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+ newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
- float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
- targetPWM += torqueFF;
- targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
+ float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
+ targetPWM += torqueFF;
+ targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
- // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
- // << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
- // << VAROUT(torqueFF);
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
+ // << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
+ // << VAROUT(torqueFF);
- if (std::isnan(targetPWM))
- {
- ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
- targetPWM = 0.0f;
- }
- float updateRatio = 0.3;
- this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
- float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
- // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
- if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
- {
- // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
- dataPtr->setTargetPWM(targetPWM);
- }
+ if (std::isnan(targetPWM))
+ {
+ ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
+ targetPWM = 0.0f;
+ }
+ float updateRatio = 0.3;
+ this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
+ float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
+ // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
+ if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
+ {
+ // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
+ dataPtr->setTargetPWM(targetPWM);
+ }
- this->targetPWM = targetPWM;
- lastTargetVelocity = newVel;
- // auto name = getParent().getDeviceName().c_str();
- // ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
- // currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
+ this->targetPWM = targetPWM;
+ lastTargetVelocity = newVel;
+ // auto name = getParent().getDeviceName().c_str();
+ // ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
+ // currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
+ }
+ else
+ {
+ ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ }
}
- else
+
+ ControlTargetBase* JointPWMPositionController::getControlTarget()
{
- ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ return ⌖
}
-}
-
-ControlTargetBase* JointPWMPositionController::getControlTarget()
-{
- return ⌖
-}
-
-void JointPWMPositionController::rtPreActivateController()
-{
- targetPWM = 0.0f;
- lastTargetVelocity = dataPtr->getVelocity();
- posController.currentAcc = dataPtr->getAcceleration();
- posController.currentPosition = dataPtr->getPosition();
- posController.currentV = dataPtr->getVelocity();
- pidPosController->reset();
- // controller.reset(dataPtr->getVelocity());
-}
-void JointPWMPositionController::rtPostDeactivateController()
-{
- // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- // dataPtr->setTargetPWM(0);
-}
+ void JointPWMPositionController::rtPreActivateController()
+ {
+ targetPWM = 0.0f;
+ lastTargetVelocity = dataPtr->getVelocity();
+ posController.currentAcc = dataPtr->getAcceleration();
+ posController.currentPosition = dataPtr->getPosition();
+ posController.currentV = dataPtr->getVelocity();
+ pidPosController->reset();
+ // controller.reset(dataPtr->getVelocity());
+ }
-StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-{
+ void JointPWMPositionController::rtPostDeactivateController()
+ {
+ // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ // dataPtr->setTargetPWM(0);
+ }
- if (!remoteGui)
+ StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
{
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+
+ if (!remoteGui)
{
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
{
- ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
{
- continue;
+ object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
+ catch (...)
+ {
+ sleep(1);
+ }
+
}
- catch (...)
+ if (remoteGui)
{
- sleep(1);
- }
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace RemoteGui;
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace RemoteGui;
+ auto vLayout = makeVBoxLayout();
- auto vLayout = makeVBoxLayout();
-
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
- WidgetPtr KpSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(pidPosController->Kp * 5)
- .value(pidPosController->Kp);
- WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider, KpLabelValue});
+ WidgetPtr KpSlider = makeFloatSlider("KpSlider")
+ .min(0.0f).max(pidPosController->Kp * 5)
+ .value(pidPosController->Kp);
+ WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
+ WidgetPtr line = makeHBoxLayout()
+ .children({KpLabel, KpSlider, KpLabelValue});
- vLayout.addChild(line);
+ vLayout.addChild(line);
- }
+ }
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(pidPosController->Ki * 5)
- .value(pidPosController->Ki);
- WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f).max(pidPosController->Ki * 5)
+ .value(pidPosController->Ki);
+ WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider, KiLabelValue});
+ WidgetPtr line = makeHBoxLayout()
+ .children({KiLabel, KiSlider, KiLabelValue});
- vLayout.addChild(line);
+ vLayout.addChild(line);
- }
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
- .steps(1000)
- .value(pidPosController->Kd);
- WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
-
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider, KdLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
+ .steps(1000)
+ .value(pidPosController->Kd);
+ WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
+
+ WidgetPtr line = makeHBoxLayout()
+ .children({KdLabel, KdSlider, KdLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox")
+ .label("Group")
+ .child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- pidPosController->Kp = tab.getValue<float>("KpSlider").get();
- pidPosController->Ki = tab.getValue<float>("KiSlider").get();
- pidPosController->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
+ while (!stopRequested)
+ {
+ RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ pidPosController->Kp = tab.getValue<float>("KpSlider").get();
+ pidPosController->Ki = tab.getValue<float>("KiSlider").get();
+ pidPosController->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
}
- }
- });
+ });
+ }
+ return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ {"targetPosition", new Variant(posController.currentPosition)}, // position of profile generator is target position
+ {"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
+ {"pidError", new Variant(pidPosController->previousError)},
+ // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ {"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
+ {"pidIntegral", new Variant(pidPosController->integral)},
+ {"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
+ {"pidDiffCV", new Variant(pidPosController->derivative * pidPosController->Kd)},
+ // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
+ // {"pospidIntegral", new Variant(posController.pid->integral)},
+ // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
+ // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
+ // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
+ {"desiredPWM", new Variant(targetPWM.load())}
+
+
+ };
}
- return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- {"targetPosition", new Variant(posController.currentPosition)}, // position of profile generator is target position
- {"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
- {"pidError", new Variant(pidPosController->previousError)},
- // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
- {"pidIntegral", new Variant(pidPosController->integral)},
- {"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
- {"pidDiffCV", new Variant(pidPosController->derivative * pidPosController->Kd)},
- // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
- // {"pospidIntegral", new Variant(posController.pid->integral)},
- // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
- // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
- // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
- {"desiredPWM", new Variant(targetPWM.load())}
-
-
- };
-}
-PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
-{
- PWMPositionControllerConfiguration configData;
-
- configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
- configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
- configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
- configData.maxDt = node.first_node("maxDt").value_as_float();
- configData.p = node.first_node("p").value_as_float();
- configData.i = node.first_node("i").value_as_float();
- configData.d = node.first_node("d").value_as_float();
- configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
- configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
- configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
- configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
- configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
- configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
- configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
-
- return std::make_shared<PWMPositionControllerConfiguration>(configData);
+ PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
+ {
+ PWMPositionControllerConfiguration configData;
+
+ configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
+ configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
+ configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
+ configData.maxDt = node.first_node("maxDt").value_as_float();
+ configData.p = node.first_node("p").value_as_float();
+ configData.i = node.first_node("i").value_as_float();
+ configData.d = node.first_node("d").value_as_float();
+ configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
+ configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
+ configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
+ configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
+ configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
+ configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
+
+ return std::make_shared<PWMPositionControllerConfiguration>(configData);
+ }
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
index a0a45a955..7451e76c6 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
@@ -14,244 +14,239 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-using namespace armarx;
-
-
-JointPWMVelocityController::JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
- config(velocityControllerConfigDataPtr),
- controller(velocityControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
-{
- actorIndex = board->getActorIndex(deviceName);
- sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
- ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
- dataPtr = jointData;
-
- // velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
- velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
- velController.jerk = 30;
- velController.maxDt = velocityControllerConfigDataPtr->maxDt;
- velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
- velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
- ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
- // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
- velController.positionLimitHiSoft = jointData->getSoftLimitHi();
- // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
- velController.positionLimitLoSoft = jointData->getSoftLimitLo();
- this->isLimitless = jointData->isLimitless();
-}
-
-JointPWMVelocityController::~JointPWMVelocityController() noexcept(true)
+namespace armarx
{
- stopRequested = true;
- try
+ JointPWMVelocityController::JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
+ config(velocityControllerConfigDataPtr),
+ controller(velocityControllerConfigDataPtr),
+ target(), board(board), deviceName(deviceName)
{
- threadHandle.join();
+ actorIndex = board->getActorIndex(deviceName);
+ sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
+ dataPtr = jointData;
+
+ // velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ velController.jerk = 30;
+ velController.maxDt = velocityControllerConfigDataPtr->maxDt;
+ velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
+ velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
+ ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
+ // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
+ velController.positionLimitHiSoft = jointData->getSoftLimitHi();
+ // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
+ velController.positionLimitLoSoft = jointData->getSoftLimitLo();
+ this->isLimitless = jointData->isLimitless();
}
- catch (...)
+
+ JointPWMVelocityController::~JointPWMVelocityController() noexcept(true)
{
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+ }
}
-}
-void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- if (target.isValid())
+ void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
-
+ if (target.isValid())
{
- auto currentPosition = dataPtr->getPosition();
- if (isLimitless)
- {
- velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
- // ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
- }
- else
+
{
- velController.currentPosition = currentPosition;
- }
- velController.currentV = lastTargetVelocity;
- velController.currentAcc = lastTargetAcceleration;
- velController.dt = timeSinceLastIteration.toSecondsDouble();
- velController.targetV = target.velocity;
- auto r = velController.run();
- double newVel = r.velocity;
- double newAcc = r.acceleration;
+ auto currentPosition = dataPtr->getPosition();
+ if (isLimitless)
+ {
+ velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
+ // ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
+ }
+ else
+ {
+ velController.currentPosition = currentPosition;
+ }
+ velController.currentV = lastTargetVelocity;
+ velController.currentAcc = lastTargetAcceleration;
+ velController.dt = timeSinceLastIteration.toSecondsDouble();
+ velController.targetV = target.velocity;
+ auto r = velController.run();
+ double newVel = r.velocity;
+ double newAcc = r.acceleration;
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
- if (std::isnan(newVel))
- {
- newVel = 0;
- newAcc = 0;
- }
- // float newVel = target.velocity;
- if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
- || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
- {
- newVel = 0;
- newAcc = 0;
- ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
- }
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ newAcc = 0;
+ }
+ // float newVel = target.velocity;
+ if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
+ || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
+ {
+ newVel = 0;
+ newAcc = 0;
+ ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
+ }
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
- dataPtr->setTargetPWM(targetPWM);
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
+ dataPtr->setTargetPWM(targetPWM);
- lastTargetVelocity = newVel;
- lastTargetAcceleration = newAcc;
+ lastTargetVelocity = newVel;
+ lastTargetAcceleration = newAcc;
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+ }
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
}
}
- else
+
+ ControlTargetBase* JointPWMVelocityController::getControlTarget()
{
- ARMARX_ERROR << "invalid target set for actor";
+ return ⌖
}
-}
-
-ControlTargetBase* JointPWMVelocityController::getControlTarget()
-{
- return ⌖
-}
-void JointPWMVelocityController::rtPreActivateController()
-{
- lastTargetVelocity = dataPtr->getVelocity();
- lastTargetAcceleration = dataPtr->getAcceleration();
- controller.reset(dataPtr->getVelocity());
-}
-
-void JointPWMVelocityController::rtPostDeactivateController()
-{
- // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- // dataPtr->setTargetPWM(0);
-}
+ void JointPWMVelocityController::rtPreActivateController()
+ {
+ lastTargetVelocity = dataPtr->getVelocity();
+ lastTargetAcceleration = dataPtr->getAcceleration();
+ controller.reset(dataPtr->getVelocity());
+ }
-StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-{
+ void JointPWMVelocityController::rtPostDeactivateController()
+ {
+ // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ // dataPtr->setTargetPWM(0);
+ }
- if (!remoteGui && !threadHandle.isValid())
+ StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
{
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+
+ if (!remoteGui && !threadHandle.isValid())
{
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
{
- ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
+ {
+ object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ return;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
{
- return;
+ handleExceptions();
+ sleep(1);
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
+
}
- catch (...)
+ if (remoteGui)
{
- handleExceptions();
- sleep(1);
- }
-
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace RemoteGui;
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace RemoteGui;
- auto vLayout = makeVBoxLayout();
+ auto vLayout = makeVBoxLayout();
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
- WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(5000.0f)
- .value(config->p);
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KiSlider});
+ WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ .min(0.0f).max(5000.0f)
+ .value(config->p);
+ WidgetPtr line = makeHBoxLayout()
+ .children({KpLabel, KiSlider});
- vLayout.addChild(line);
+ vLayout.addChild(line);
- }
+ }
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(50000.0f)
- .value(config->i);
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f).max(50000.0f)
+ .value(config->i);
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
+ WidgetPtr line = makeHBoxLayout()
+ .children({KiLabel, KiSlider});
- vLayout.addChild(line);
+ vLayout.addChild(line);
- }
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(0.0f).max(50.0f)
- .steps(100)
- .value(config->d);
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(0.0f).max(50.0f)
+ .steps(100)
+ .value(config->d);
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
+ WidgetPtr line = makeHBoxLayout()
+ .children({KdLabel, KdSlider});
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox")
+ .label("Group")
+ .child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
+ while (!stopRequested)
+ {
+ RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
}
- }
- });
+ });
+ }
+ return
+ {
+ {"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ {"lastTargetAcceleration", new Variant(lastTargetAcceleration.load())},
+ {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
+ {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
+ {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
+
+ };
}
- return
- {
- {"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- {"lastTargetAcceleration", new Variant(lastTargetAcceleration.load())},
- {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
- {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
-
- };
}
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
index c55b17723..f6345d0a5 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
@@ -14,205 +14,198 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-using namespace armarx;
-
-
-PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(DefaultRapidXmlReaderNode node)
+namespace armarx
{
- PWMZeroTorqueControllerConfiguration configData;
-
- configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
- configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
+ PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(DefaultRapidXmlReaderNode node)
+ {
+ PWMZeroTorqueControllerConfiguration configData;
+ configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
- return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
-}
+ return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
-JointPWMZeroTorqueController::JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
- config(config), target(), board(board), deviceName(deviceName)
-{
- actorIndex = board->getActorIndex(deviceName);
- dataPtr = jointData;
+ }
+ JointPWMZeroTorqueController::JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
+ config(config), target(), board(board), deviceName(deviceName)
+ {
+ actorIndex = board->getActorIndex(deviceName);
+ dataPtr = jointData;
- this->isLimitless = jointData->isLimitless();
-}
+ this->isLimitless = jointData->isLimitless();
-JointPWMZeroTorqueController::~JointPWMZeroTorqueController() noexcept(true)
-{
- stopRequested = true;
- try
- {
- threadHandle.join();
}
- catch (...)
+
+ JointPWMZeroTorqueController::~JointPWMZeroTorqueController() noexcept(true)
{
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+ }
}
-}
-void JointPWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- if (target.isValid())
+ void JointPWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
- targetPWM += math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
- // targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
- dataPtr->setTargetPWM(targetPWM);
+ if (target.isValid())
+ {
+ float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
+ targetPWM += math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
+ // targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
+ dataPtr->setTargetPWM(targetPWM);
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
+ }
}
- else
+
+ ControlTargetBase* JointPWMZeroTorqueController::getControlTarget()
{
- ARMARX_ERROR << "invalid target set for actor";
+ return ⌖
}
-}
-ControlTargetBase* JointPWMZeroTorqueController::getControlTarget()
-{
- return ⌖
-}
-
-void JointPWMZeroTorqueController::rtPreActivateController()
-{
- lastTargetVelocity = dataPtr->getVelocity();
- // controller.reset(dataPtr->getVelocity());
-}
-
-void JointPWMZeroTorqueController::rtPostDeactivateController()
-{
- ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- dataPtr->setTargetPWM(0);
-}
+ void JointPWMZeroTorqueController::rtPreActivateController()
+ {
+ lastTargetVelocity = dataPtr->getVelocity();
+ // controller.reset(dataPtr->getVelocity());
+ }
-StringVariantBaseMap JointPWMZeroTorqueController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
-{
+ void JointPWMZeroTorqueController::rtPostDeactivateController()
+ {
+ ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ dataPtr->setTargetPWM(0);
+ }
- if (!remoteGui && !threadHandle.isValid())
+ StringVariantBaseMap JointPWMZeroTorqueController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
{
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+
+ if (!remoteGui && !threadHandle.isValid())
{
- return;
- // std::string guiTabName;
- // while (!stopRequested)
- // {
- // ManagedIceObjectPtr object;
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- // try
- // {
- // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- // ARMARX_CHECK_EXPRESSION(object);
- // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- // if (!remoteGui)
- // {
- // return;
- // }
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- // guiTabName = getParent().getDeviceName() + getControlMode();
- // break;
- // }
- // catch (...)
- // {
- // handleExceptions();
- // sleep(1);
- // }
-
- // }
- // if (remoteGui)
- // {
- // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- // using namespace RemoteGui;
-
-
-
- // // auto vLayout = makeVBoxLayout();
-
- // // {
- // // WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- // // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- // // .min(0.0f).max(5000.0f)
- // // .value(config->p);
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KpLabel, KiSlider});
-
- // // vLayout.addChild(line);
-
- // // }
-
-
- // // {
- // // WidgetPtr KiLabel = makeTextLabel("Ki: ");
- // // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- // // .min(0.0f).max(50000.0f)
- // // .value(config->i);
-
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KiLabel, KiSlider});
-
- // // vLayout.addChild(line);
-
- // // }
-
- // // {
- // // WidgetPtr KdLabel = makeTextLabel("Kd: ");
- // // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- // // .min(0.0f).max(50.0f)
- // // .steps(100)
- // // .value(config->d);
-
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KdLabel, KdSlider});
-
- // // vLayout.addChild(line);
- // // vLayout.addChild(new VSpacer);
- // // }
-
- // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // // .min(0.0f).max(2.0f)
- // // .steps(20).decimals(2)
- // // .value(0.4f);
-
-
-
-
- // WidgetPtr groupBox = makeGroupBox("GroupBox")
- // .label("Group")
- // .child(vLayout);
-
- // remoteGui->createTab(guiTabName, groupBox);
-
- // while (!stopRequested)
- // {
- // RemoteGui::TabProxy tab(remoteGui, guiTabName);
- // tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- // usleep(100000);
- // }
- // }
-
- });
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ {
+ return;
+ // std::string guiTabName;
+ // while (!stopRequested)
+ // {
+ // ManagedIceObjectPtr object;
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ // try
+ // {
+ // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ // ARMARX_CHECK_EXPRESSION(object);
+ // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ // if (!remoteGui)
+ // {
+ // return;
+ // }
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ // guiTabName = getParent().getDeviceName() + getControlMode();
+ // break;
+ // }
+ // catch (...)
+ // {
+ // handleExceptions();
+ // sleep(1);
+ // }
+
+ // }
+ // if (remoteGui)
+ // {
+ // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ // using namespace RemoteGui;
+
+
+
+ // // auto vLayout = makeVBoxLayout();
+
+ // // {
+ // // WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ // // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ // // .min(0.0f).max(5000.0f)
+ // // .value(config->p);
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KpLabel, KiSlider});
+
+ // // vLayout.addChild(line);
+
+ // // }
+
+
+ // // {
+ // // WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ // // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ // // .min(0.0f).max(50000.0f)
+ // // .value(config->i);
+
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KiLabel, KiSlider});
+
+ // // vLayout.addChild(line);
+
+ // // }
+
+ // // {
+ // // WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ // // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ // // .min(0.0f).max(50.0f)
+ // // .steps(100)
+ // // .value(config->d);
+
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KdLabel, KdSlider});
+
+ // // vLayout.addChild(line);
+ // // vLayout.addChild(new VSpacer);
+ // // }
+
+ // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // // .min(0.0f).max(2.0f)
+ // // .steps(20).decimals(2)
+ // // .value(0.4f);
+
+
+
+
+ // WidgetPtr groupBox = makeGroupBox("GroupBox")
+ // .label("Group")
+ // .child(vLayout);
+
+ // remoteGui->createTab(guiTabName, groupBox);
+
+ // while (!stopRequested)
+ // {
+ // RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ // tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ // usleep(100000);
+ // }
+ // }
+
+ });
+ }
+ return {};
+ // return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ // {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
+ // {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
+ // {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
}
- return {};
- // return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- // {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
- // {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- // {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
}
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
index 24acd8297..f2f558638 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
@@ -15,51 +15,40 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-using namespace armarx;
-
-
-ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
- ActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
- JointPWMPositionController(deviceName, board, jointData, positionControllerConfigDataPtr)
+namespace armarx
{
- linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-}
+ ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
+ ActorDataPtr jointData,
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
+ JointPWMPositionController(deviceName, board, jointData, positionControllerConfigDataPtr)
+ {
+ linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
+ }
-ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
-{
+ ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
+ {
-}
+ }
-void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- if (target.isValid())
+ void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- float linkedPositionFactor = 2.0 / 3.0;
- target.position += (linkedDataPtr->getRelativePosition() * linkedPositionFactor);
- ARMARX_RT_LOGF_INFO("target.position %.2f, relative partner pos: %.2f", target.position, linkedDataPtr->getRelativePosition()).deactivateSpam(0.5);
- JointPWMPositionController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ if (target.isValid())
+ {
+ float linkedPositionFactor = 2.0 / 3.0;
+ target.position += (linkedDataPtr->getRelativePosition() * linkedPositionFactor);
+ ARMARX_RT_LOGF_INFO("target.position %.2f, relative partner pos: %.2f", target.position, linkedDataPtr->getRelativePosition()).deactivateSpam(0.5);
+ JointPWMPositionController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ else
+ {
+ ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ }
}
- else
+
+ void ParallelGripperPositionController::rtPreActivateController()
{
- ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
+ ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
+ JointPWMPositionController::rtPreActivateController();
}
}
-
-void ParallelGripperPositionController::rtPreActivateController()
-{
- linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
- ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
- JointPWMPositionController::rtPreActivateController();
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
index a4473c5b2..3ed5f029b 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
@@ -14,42 +14,42 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-using namespace armarx;
-
-
-ParallelGripperVelocityController::ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
- JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
-{
- this->linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-
-}
-
-ParallelGripperVelocityController::~ParallelGripperVelocityController() noexcept(true)
+namespace armarx
{
+ ParallelGripperVelocityController::ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
+ {
+ this->linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-}
+ }
-void ParallelGripperVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- if (target.isValid())
+ ParallelGripperVelocityController::~ParallelGripperVelocityController() noexcept(true)
{
- float linkedVelocityFactor = 2.0f / 3.0f;
- target.velocity += linkedVelocityFactor * linkedDataPtr->getVelocity();
- JointPWMVelocityController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+
}
- else
+
+ void ParallelGripperVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- ARMARX_ERROR << "invalid target set for actor";
+ if (target.isValid())
+ {
+ float linkedVelocityFactor = 2.0f / 3.0f;
+ target.velocity += linkedVelocityFactor * linkedDataPtr->getVelocity();
+ JointPWMVelocityController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
+ }
}
-}
-void ParallelGripperVelocityController::rtPreActivateController()
-{
- linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
- ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
- JointPWMVelocityController::rtPreActivateController();
+ void ParallelGripperVelocityController::rtPreActivateController()
+ {
+ linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
+ ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
+ JointPWMVelocityController::rtPreActivateController();
+ }
}
@@ -58,4 +58,3 @@ void ParallelGripperVelocityController::rtPreActivateController()
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
index 7966df46e..0a38422a6 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
@@ -96,7 +96,7 @@ namespace armarx
class KITGripperBasisBoardSlave :
- public AbstractSlaveWithInputOutput<KITGripperBasisBoardIN_t, KITGripperBasisBoardOUT_t>
+ public AbstractSlaveWithInputOutput<KITGripperBasisBoardIN_t, KITGripperBasisBoardOUT_t>
{
public:
KITGripperBasisBoardSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
index 7e1949ebe..cd07e4be8 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
@@ -31,20 +31,6 @@ namespace armarx
Eigen::VectorXf targetTSVel;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointAdaptiveWipingController class
* @ingroup Library-RobotUnit-NJointControllers
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
index 55c7e160b..495d9ba47 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -30,20 +30,6 @@ namespace armarx
double canVal;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointAnomalyDetectionAdaptiveWipingController class
* @ingroup Library-RobotUnit-NJointControllers
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index 2bc75223f..71939269f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -17,16 +17,12 @@
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointBimanualCCDMPControllerControlData
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
index a0b05fe04..6d5a4e275 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
@@ -19,15 +19,12 @@
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <dmp/general/simoxhelpers.h>
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityController);
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointBimanualCCDMPVelocityControllerControlData
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
index 63269b3c1..7c80df635 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
@@ -16,16 +16,12 @@
#include <RobotAPI/libraries/core/PIDController.h>
-
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointBimanualCCDMPControllerControlData
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
index 7d1a8f2c0..34ee1edba 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
@@ -380,7 +380,7 @@ namespace armarx
}
- void NJointBimanualForceMPController::learnDMPFromFiles(const string& whichDMP, const Ice::StringSeq& fileNames, const Ice::Current&)
+ void NJointBimanualForceMPController::learnDMPFromFiles(const std::string& whichDMP, const Ice::StringSeq& fileNames, const Ice::Current&)
{
ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
if (whichDMP == "Left")
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
index 29f765ded..96e0b7649 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
@@ -15,13 +15,12 @@
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-using namespace DMP;
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointBimanualForceMPControllerControlData
{
@@ -50,7 +49,7 @@ namespace armarx
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
// NJointBimanualForceMPControllerInterface interface
- void learnDMPFromFiles(const string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&) override;
+ void learnDMPFromFiles(const std::string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&) override;
bool isFinished(const Ice::Current&) override
{
return finished;
@@ -63,7 +62,7 @@ namespace armarx
return canVal;
}
- void setViaPoints(const string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&) override;
+ void setViaPoints(const std::string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&) override;
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index c9fa7a03a..837c624db 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -56,7 +56,7 @@ namespace armarx
dmpTypes = cfg->dmpTypes;
for (size_t i = 0; i < dmpPtrList.size(); ++i)
{
- dmpPtrList[i].reset(new UMITSMP(cfg->kernelSize, 2, cfg->tau));
+ dmpPtrList[i].reset(new DMP::UMITSMP(cfg->kernelSize, 2, cfg->tau));
canVals[i] = timeDurations[i];
}
finished = false;
@@ -227,7 +227,9 @@ namespace armarx
dmpQ.x() = currentStates[i][4].pos;
dmpQ.y() = currentStates[i][5].pos;
dmpQ.z() = currentStates[i][6].pos;
- Eigen::Quaterniond angularVel0 = 2 * quatVel0 * dmpQ.inverse();
+ //Eigen::Quaterniond angularVel0 = 2.0 * quatVel0 * dmpQ.inverse();
+ const Eigen::Quaterniond aVtmp0 = quatVel0 * dmpQ.inverse();
+ const Eigen::Quaterniond angularVel0{2 * aVtmp0.w(), 2 * aVtmp0.x(), 2 * aVtmp0.y(), 2 * aVtmp0.z()};
Eigen::Vector3f angularVelVec;
@@ -267,8 +269,14 @@ namespace armarx
Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
- Eigen::Quaterniond quatVel1 = phaseKpOri * diffQ;
- Eigen::Quaterniond angularVel1 = 2 * quatVel1 * currentQ.inverse();
+ const Eigen::Quaterniond quatVel1
+ {
+ phaseKpOri * diffQ.w(), phaseKpOri * diffQ.x(),
+ phaseKpOri * diffQ.y(), phaseKpOri * diffQ.z()
+ };
+ //Eigen::Quaterniond angularVel1 = 2.0 * quatVel1 * currentQ.inverse();
+ const Eigen::Quaterniond aVtmp1 = quatVel1 * currentQ.inverse();
+ const Eigen::Quaterniond angularVel1{2 * aVtmp1.w(), 2 * aVtmp1.x(), 2 * aVtmp1.y(), 2 * aVtmp1.z()};
targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index 2fd838d91..132e6bda5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -12,17 +12,12 @@
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointCCDMPControllerControlData
{
@@ -36,20 +31,6 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointCCDMPController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -58,6 +39,19 @@ namespace armarx
public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
public NJointCCDMPControllerInterface
{
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
public:
using ConfigPtrT = NJointCCDMPControllerConfigPtr;
NJointCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
@@ -130,14 +124,14 @@ namespace armarx
std::string nodeSetName;
// dmp parameters
- std::vector<UMITSMPPtr > dmpPtrList;
+ std::vector<DMP::UMITSMPPtr > dmpPtrList;
std::vector<double> canVals;
std::vector<double> timeDurations;
std::vector<std::string > dmpTypes;
std::vector<double> amplitudes;
- std::vector<Vec<DMPState > > currentStates;
- std::vector<DVec > targetSubStates;
+ std::vector<DMP::Vec<DMP::DMPState > > currentStates;
+ std::vector<DMP::DVec > targetSubStates;
bool finished;
double tau;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 8b59a9687..053113604 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -29,20 +29,6 @@ namespace armarx
Eigen::VectorXf targetTSVel;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointPeriodicTSDMPCompliantController class
* @ingroup Library-RobotUnit-NJointControllers
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
index 351a9451c..907bf3866 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
@@ -29,20 +29,6 @@ namespace armarx
Eigen::Matrix4f targetPose;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointPeriodicTSDMPForwardVelController class
* @ingroup Library-RobotUnit-NJointControllers
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index bdc0ef800..7cc9b8042 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -14,16 +14,12 @@
#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointTSDMPController);
TYPEDEF_PTRS_HANDLE(NJointTSDMPControllerControlData);
- using ViaPoint = std::pair<double, DVec >;
+ using ViaPoint = std::pair<double, DMP::DVec >;
using ViaPointsSet = std::vector<ViaPoint >;
class NJointTSDMPControllerControlData
{
@@ -35,20 +31,6 @@ namespace armarx
float avoidJointLimitsKp = 0;
};
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
/**
* @brief The NJointTSDMPController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -57,6 +39,19 @@ namespace armarx
public NJointControllerWithTripleBuffer<NJointTSDMPControllerControlData>,
public NJointTaskSpaceDMPControllerInterface
{
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
public:
using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
NJointTSDMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 49a104b54..25f6792ca 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -12,16 +12,11 @@
#include <dmp/representation/dmp/umidmp.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-
-using namespace DMP;
namespace armarx
{
-
-
TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPController);
TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPControllerControlData);
-
class NJointTaskSpaceImpedanceDMPControllerControlData
{
public:
diff --git a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
index 8660ad028..e95378770 100644
--- a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
+++ b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
@@ -34,438 +34,433 @@
#include <RobotAPI/interface/core/OrientedPoint.h>
#include <RobotAPI/interface/core/PoseBase.h>
-namespace armarx
+namespace armarx::detail
{
- namespace detail
- {
- RobotAPIVariantWidgetDummySymbol::RobotAPIVariantWidgetDummySymbol(int) {}
- }
+ RobotAPIVariantWidgetDummySymbol::RobotAPIVariantWidgetDummySymbol(int) {}
+}
- namespace VariantDataWidgets
+namespace armarx::VariantDataWidgets
+{
+ class Vector2BaseWidget: public VariantDataWidgetBase
{
- class Vector2BaseWidget: public VariantDataWidgetBase
+ public:
+ Vector2BaseWidget(const VariantDataPtr& v)
{
- public:
- Vector2BaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelX = new QLabel;
- labelY = new QLabel;
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- Vector2BasePtr v = Vector2BasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelX->setText(QString::number(v->x));
- labelY->setText(QString::number(v->y));
- }
- private:
- QLabel* labelX;
- QLabel* labelY;
- };
- VariantDataWidgetFactoryRegistration<Vector2BaseWidget> registerVector2BaseWidget {Vector2Base::ice_staticId()};
-
- class Vector3BaseWidget: public VariantDataWidgetBase
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelX = new QLabel;
+ labelY = new QLabel;
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
{
- public:
+ Vector2BasePtr v = Vector2BasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelX->setText(QString::number(v->x));
+ labelY->setText(QString::number(v->y));
+ }
+ private:
+ QLabel* labelX;
+ QLabel* labelY;
+ };
+ VariantDataWidgetFactoryRegistration<Vector2BaseWidget> registerVector2BaseWidget {Vector2Base::ice_staticId()};
- Vector3BaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelX = new QLabel;
- labelY = new QLabel;
- labelZ = new QLabel;
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- l->addRow("Z", labelZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- Vector3BasePtr v = Vector3BasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelX->setText(QString::number(v->x));
- labelY->setText(QString::number(v->y));
- labelZ->setText(QString::number(v->z));
- }
- private:
- QLabel* labelX;
- QLabel* labelY;
- QLabel* labelZ;
- };
- VariantDataWidgetFactoryRegistration<Vector3BaseWidget> registerVector3BaseWidget {Vector3Base::ice_staticId()};
+ class Vector3BaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class FramedPositionBaseWidget: public VariantDataWidgetBase
+ Vector3BaseWidget(const VariantDataPtr& v)
+ {
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelX = new QLabel;
+ labelY = new QLabel;
+ labelZ = new QLabel;
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ l->addRow("Z", labelZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
{
- public:
+ Vector3BasePtr v = Vector3BasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelX->setText(QString::number(v->x));
+ labelY->setText(QString::number(v->y));
+ labelZ->setText(QString::number(v->z));
+ }
+ private:
+ QLabel* labelX;
+ QLabel* labelY;
+ QLabel* labelZ;
+ };
+ VariantDataWidgetFactoryRegistration<Vector3BaseWidget> registerVector3BaseWidget {Vector3Base::ice_staticId()};
- FramedPositionBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelAg = new QLabel;
- labelFr = new QLabel;
- labelX = new QLabel;
- labelY = new QLabel;
- labelZ = new QLabel;
- l->addRow("Agent", labelAg);
- l->addRow("Frame", labelFr);
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- l->addRow("Z", labelZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- FramedPositionBasePtr v = FramedPositionBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelAg->setText(QString::fromStdString(v->agent));
- labelFr->setText(QString::fromStdString(v->frame));
- labelX->setText(QString::number(v->x));
- labelY->setText(QString::number(v->y));
- labelZ->setText(QString::number(v->z));
- }
- private:
- QLabel* labelAg;
- QLabel* labelFr;
- QLabel* labelX;
- QLabel* labelY;
- QLabel* labelZ;
- };
- VariantDataWidgetFactoryRegistration<FramedPositionBaseWidget> registerFramedPositionBaseWidget {FramedPositionBase::ice_staticId()};
+ class FramedPositionBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class FramedDirectionBaseWidget: public VariantDataWidgetBase
+ FramedPositionBaseWidget(const VariantDataPtr& v)
{
- public:
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelAg = new QLabel;
+ labelFr = new QLabel;
+ labelX = new QLabel;
+ labelY = new QLabel;
+ labelZ = new QLabel;
+ l->addRow("Agent", labelAg);
+ l->addRow("Frame", labelFr);
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ l->addRow("Z", labelZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ FramedPositionBasePtr v = FramedPositionBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelAg->setText(QString::fromStdString(v->agent));
+ labelFr->setText(QString::fromStdString(v->frame));
+ labelX->setText(QString::number(v->x));
+ labelY->setText(QString::number(v->y));
+ labelZ->setText(QString::number(v->z));
+ }
+ private:
+ QLabel* labelAg;
+ QLabel* labelFr;
+ QLabel* labelX;
+ QLabel* labelY;
+ QLabel* labelZ;
+ };
+ VariantDataWidgetFactoryRegistration<FramedPositionBaseWidget> registerFramedPositionBaseWidget {FramedPositionBase::ice_staticId()};
- FramedDirectionBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelAg = new QLabel;
- labelFr = new QLabel;
- labelX = new QLabel;
- labelY = new QLabel;
- labelZ = new QLabel;
- l->addRow("Agent", labelAg);
- l->addRow("Frame", labelFr);
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- l->addRow("Z", labelZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- FramedDirectionBasePtr v = FramedDirectionBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelAg->setText(QString::fromStdString(v->agent));
- labelFr->setText(QString::fromStdString(v->frame));
- labelX->setText(QString::number(v->x));
- labelY->setText(QString::number(v->y));
- labelZ->setText(QString::number(v->z));
- }
- private:
- QLabel* labelAg;
- QLabel* labelFr;
- QLabel* labelX;
- QLabel* labelY;
- QLabel* labelZ;
- };
- VariantDataWidgetFactoryRegistration<FramedDirectionBaseWidget> registerFramedDirectionBaseWidget {FramedDirectionBase::ice_staticId()};
+ class FramedDirectionBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class OrientedPointBaseWidget: public VariantDataWidgetBase
+ FramedDirectionBaseWidget(const VariantDataPtr& v)
+ {
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelAg = new QLabel;
+ labelFr = new QLabel;
+ labelX = new QLabel;
+ labelY = new QLabel;
+ labelZ = new QLabel;
+ l->addRow("Agent", labelAg);
+ l->addRow("Frame", labelFr);
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ l->addRow("Z", labelZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
{
- public:
+ FramedDirectionBasePtr v = FramedDirectionBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelAg->setText(QString::fromStdString(v->agent));
+ labelFr->setText(QString::fromStdString(v->frame));
+ labelX->setText(QString::number(v->x));
+ labelY->setText(QString::number(v->y));
+ labelZ->setText(QString::number(v->z));
+ }
+ private:
+ QLabel* labelAg;
+ QLabel* labelFr;
+ QLabel* labelX;
+ QLabel* labelY;
+ QLabel* labelZ;
+ };
+ VariantDataWidgetFactoryRegistration<FramedDirectionBaseWidget> registerFramedDirectionBaseWidget {FramedDirectionBase::ice_staticId()};
- OrientedPointBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelPX = new QLabel;
- labelPY = new QLabel;
- labelPZ = new QLabel;
- labelNX = new QLabel;
- labelNY = new QLabel;
- labelNZ = new QLabel;
- l->addRow("PX", labelPX);
- l->addRow("PY", labelPY);
- l->addRow("PZ", labelPZ);
- l->addRow("NX", labelNX);
- l->addRow("NY", labelNY);
- l->addRow("NZ", labelNZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- OrientedPointBasePtr v = OrientedPointBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelPX->setText(QString::number(v->px));
- labelPY->setText(QString::number(v->py));
- labelPZ->setText(QString::number(v->pz));
- labelNX->setText(QString::number(v->nx));
- labelNY->setText(QString::number(v->ny));
- labelNZ->setText(QString::number(v->nz));
- }
- private:
- QLabel* labelPX;
- QLabel* labelPY;
- QLabel* labelPZ;
- QLabel* labelNX;
- QLabel* labelNY;
- QLabel* labelNZ;
- };
- VariantDataWidgetFactoryRegistration<OrientedPointBaseWidget> registerOrientedPointBaseWidget {OrientedPointBase::ice_staticId()};
+ class OrientedPointBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class FramedOrientedPointBaseWidget: public VariantDataWidgetBase
+ OrientedPointBaseWidget(const VariantDataPtr& v)
+ {
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelPX = new QLabel;
+ labelPY = new QLabel;
+ labelPZ = new QLabel;
+ labelNX = new QLabel;
+ labelNY = new QLabel;
+ labelNZ = new QLabel;
+ l->addRow("PX", labelPX);
+ l->addRow("PY", labelPY);
+ l->addRow("PZ", labelPZ);
+ l->addRow("NX", labelNX);
+ l->addRow("NY", labelNY);
+ l->addRow("NZ", labelNZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
{
- public:
+ OrientedPointBasePtr v = OrientedPointBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelPX->setText(QString::number(v->px));
+ labelPY->setText(QString::number(v->py));
+ labelPZ->setText(QString::number(v->pz));
+ labelNX->setText(QString::number(v->nx));
+ labelNY->setText(QString::number(v->ny));
+ labelNZ->setText(QString::number(v->nz));
+ }
+ private:
+ QLabel* labelPX;
+ QLabel* labelPY;
+ QLabel* labelPZ;
+ QLabel* labelNX;
+ QLabel* labelNY;
+ QLabel* labelNZ;
+ };
+ VariantDataWidgetFactoryRegistration<OrientedPointBaseWidget> registerOrientedPointBaseWidget {OrientedPointBase::ice_staticId()};
- FramedOrientedPointBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelAg = new QLabel;
- labelFr = new QLabel;
- labelPX = new QLabel;
- labelPY = new QLabel;
- labelPZ = new QLabel;
- labelNX = new QLabel;
- labelNY = new QLabel;
- labelNZ = new QLabel;
- l->addRow("Agent", labelAg);
- l->addRow("Frame", labelFr);
- l->addRow("PX", labelPX);
- l->addRow("PY", labelPY);
- l->addRow("PZ", labelPZ);
- l->addRow("NX", labelNX);
- l->addRow("NY", labelNY);
- l->addRow("NZ", labelNZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- FramedOrientedPointBasePtr v = FramedOrientedPointBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelAg->setText(QString::fromStdString(v->agent));
- labelFr->setText(QString::fromStdString(v->frame));
- labelPX->setText(QString::number(v->px));
- labelPY->setText(QString::number(v->py));
- labelPZ->setText(QString::number(v->pz));
- labelNX->setText(QString::number(v->nx));
- labelNY->setText(QString::number(v->ny));
- labelNZ->setText(QString::number(v->nz));
- }
- private:
- QLabel* labelAg;
- QLabel* labelFr;
- QLabel* labelPX;
- QLabel* labelPY;
- QLabel* labelPZ;
- QLabel* labelNX;
- QLabel* labelNY;
- QLabel* labelNZ;
- };
- VariantDataWidgetFactoryRegistration<FramedOrientedPointBaseWidget> registerFramedOrientedPointBaseWidget {FramedOrientedPointBase::ice_staticId()};
+ class FramedOrientedPointBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class QuaternionBaseWidget: public VariantDataWidgetBase
+ FramedOrientedPointBaseWidget(const VariantDataPtr& v)
{
- public:
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelAg = new QLabel;
+ labelFr = new QLabel;
+ labelPX = new QLabel;
+ labelPY = new QLabel;
+ labelPZ = new QLabel;
+ labelNX = new QLabel;
+ labelNY = new QLabel;
+ labelNZ = new QLabel;
+ l->addRow("Agent", labelAg);
+ l->addRow("Frame", labelFr);
+ l->addRow("PX", labelPX);
+ l->addRow("PY", labelPY);
+ l->addRow("PZ", labelPZ);
+ l->addRow("NX", labelNX);
+ l->addRow("NY", labelNY);
+ l->addRow("NZ", labelNZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ FramedOrientedPointBasePtr v = FramedOrientedPointBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelAg->setText(QString::fromStdString(v->agent));
+ labelFr->setText(QString::fromStdString(v->frame));
+ labelPX->setText(QString::number(v->px));
+ labelPY->setText(QString::number(v->py));
+ labelPZ->setText(QString::number(v->pz));
+ labelNX->setText(QString::number(v->nx));
+ labelNY->setText(QString::number(v->ny));
+ labelNZ->setText(QString::number(v->nz));
+ }
+ private:
+ QLabel* labelAg;
+ QLabel* labelFr;
+ QLabel* labelPX;
+ QLabel* labelPY;
+ QLabel* labelPZ;
+ QLabel* labelNX;
+ QLabel* labelNY;
+ QLabel* labelNZ;
+ };
+ VariantDataWidgetFactoryRegistration<FramedOrientedPointBaseWidget> registerFramedOrientedPointBaseWidget {FramedOrientedPointBase::ice_staticId()};
- QuaternionBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelQW = new QLabel;
- labelQX = new QLabel;
- labelQY = new QLabel;
- labelQZ = new QLabel;
- l->addRow("QW", labelQW);
- l->addRow("QX", labelQX);
- l->addRow("QY", labelQY);
- l->addRow("QZ", labelQZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- QuaternionBasePtr v = QuaternionBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelQW->setText(QString::number(v->qw));
- labelQX->setText(QString::number(v->qx));
- labelQY->setText(QString::number(v->qy));
- labelQZ->setText(QString::number(v->qz));
- }
- private:
- QLabel* labelQW;
- QLabel* labelQX;
- QLabel* labelQY;
- QLabel* labelQZ;
- };
- VariantDataWidgetFactoryRegistration<QuaternionBaseWidget> registerQuaternionBaseWidget {QuaternionBase::ice_staticId()};
+ class QuaternionBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class FramedOrientationBaseWidget: public VariantDataWidgetBase
+ QuaternionBaseWidget(const VariantDataPtr& v)
{
- public:
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelQW = new QLabel;
+ labelQX = new QLabel;
+ labelQY = new QLabel;
+ labelQZ = new QLabel;
+ l->addRow("QW", labelQW);
+ l->addRow("QX", labelQX);
+ l->addRow("QY", labelQY);
+ l->addRow("QZ", labelQZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ QuaternionBasePtr v = QuaternionBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelQW->setText(QString::number(v->qw));
+ labelQX->setText(QString::number(v->qx));
+ labelQY->setText(QString::number(v->qy));
+ labelQZ->setText(QString::number(v->qz));
+ }
+ private:
+ QLabel* labelQW;
+ QLabel* labelQX;
+ QLabel* labelQY;
+ QLabel* labelQZ;
+ };
+ VariantDataWidgetFactoryRegistration<QuaternionBaseWidget> registerQuaternionBaseWidget {QuaternionBase::ice_staticId()};
- FramedOrientationBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelAg = new QLabel;
- labelFr = new QLabel;
- labelQW = new QLabel;
- labelQX = new QLabel;
- labelQY = new QLabel;
- labelQZ = new QLabel;
- l->addRow("Agent", labelAg);
- l->addRow("Frame", labelFr);
- l->addRow("QW", labelQW);
- l->addRow("QX", labelQX);
- l->addRow("QY", labelQY);
- l->addRow("QZ", labelQZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- FramedOrientationBasePtr v = FramedOrientationBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- labelAg->setText(QString::fromStdString(v->agent));
- labelFr->setText(QString::fromStdString(v->frame));
- labelQW->setText(QString::number(v->qw));
- labelQX->setText(QString::number(v->qx));
- labelQY->setText(QString::number(v->qy));
- labelQZ->setText(QString::number(v->qz));
- }
- private:
- QLabel* labelAg;
- QLabel* labelFr;
- QLabel* labelQW;
- QLabel* labelQX;
- QLabel* labelQY;
- QLabel* labelQZ;
- };
- VariantDataWidgetFactoryRegistration<FramedOrientationBaseWidget> registerFramedOrientationBaseWidget {FramedOrientationBase::ice_staticId()};
+ class FramedOrientationBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class PoseBaseWidget: public VariantDataWidgetBase
+ FramedOrientationBaseWidget(const VariantDataPtr& v)
{
- public:
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelAg = new QLabel;
+ labelFr = new QLabel;
+ labelQW = new QLabel;
+ labelQX = new QLabel;
+ labelQY = new QLabel;
+ labelQZ = new QLabel;
+ l->addRow("Agent", labelAg);
+ l->addRow("Frame", labelFr);
+ l->addRow("QW", labelQW);
+ l->addRow("QX", labelQX);
+ l->addRow("QY", labelQY);
+ l->addRow("QZ", labelQZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ FramedOrientationBasePtr v = FramedOrientationBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ labelAg->setText(QString::fromStdString(v->agent));
+ labelFr->setText(QString::fromStdString(v->frame));
+ labelQW->setText(QString::number(v->qw));
+ labelQX->setText(QString::number(v->qx));
+ labelQY->setText(QString::number(v->qy));
+ labelQZ->setText(QString::number(v->qz));
+ }
+ private:
+ QLabel* labelAg;
+ QLabel* labelFr;
+ QLabel* labelQW;
+ QLabel* labelQX;
+ QLabel* labelQY;
+ QLabel* labelQZ;
+ };
+ VariantDataWidgetFactoryRegistration<FramedOrientationBaseWidget> registerFramedOrientationBaseWidget {FramedOrientationBase::ice_staticId()};
- PoseBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelX = new QLabel;
- labelY = new QLabel;
- labelZ = new QLabel;
- labelQW = new QLabel;
- labelQX = new QLabel;
- labelQY = new QLabel;
- labelQZ = new QLabel;
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- l->addRow("Z", labelZ);
- l->addRow("QW", labelQW);
- l->addRow("QX", labelQX);
- l->addRow("QY", labelQY);
- l->addRow("QZ", labelQZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- PoseBasePtr v = PoseBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- ARMARX_CHECK_EXPRESSION(v->position);
- ARMARX_CHECK_EXPRESSION(v->orientation);
- labelX->setText(QString::number(v->position->x));
- labelY->setText(QString::number(v->position->y));
- labelZ->setText(QString::number(v->position->z));
- labelQW->setText(QString::number(v->orientation->qw));
- labelQX->setText(QString::number(v->orientation->qx));
- labelQY->setText(QString::number(v->orientation->qy));
- labelQZ->setText(QString::number(v->orientation->qz));
- }
- private:
- QLabel* labelX;
- QLabel* labelY;
- QLabel* labelZ;
- QLabel* labelQW;
- QLabel* labelQX;
- QLabel* labelQY;
- QLabel* labelQZ;
- };
- VariantDataWidgetFactoryRegistration<PoseBaseWidget> registerPoseBaseWidget {PoseBase::ice_staticId()};
+ class PoseBaseWidget: public VariantDataWidgetBase
+ {
+ public:
- class FramedPoseBaseWidget: public VariantDataWidgetBase
+ PoseBaseWidget(const VariantDataPtr& v)
{
- public:
-
- FramedPoseBaseWidget(const VariantDataPtr& v)
- {
- auto l = new QFormLayout;
- l->setContentsMargins(0, 0, 0, 0);
- setLayout(l);
- labelAg = new QLabel;
- labelFr = new QLabel;
- labelX = new QLabel;
- labelY = new QLabel;
- labelZ = new QLabel;
- labelQW = new QLabel;
- labelQX = new QLabel;
- labelQY = new QLabel;
- labelQZ = new QLabel;
- l->addRow("Agent", labelAg);
- l->addRow("Frame", labelFr);
- l->addRow("X", labelX);
- l->addRow("Y", labelY);
- l->addRow("Z", labelZ);
- l->addRow("QW", labelQW);
- l->addRow("QX", labelQX);
- l->addRow("QY", labelQY);
- l->addRow("QZ", labelQZ);
- update(v);
- }
- void update(const VariantDataPtr& p) override
- {
- FramedPoseBasePtr v = FramedPoseBasePtr::dynamicCast(p);
- ARMARX_CHECK_EXPRESSION(v);
- ARMARX_CHECK_EXPRESSION(v->position);
- ARMARX_CHECK_EXPRESSION(v->orientation);
- labelAg->setText(QString::fromStdString(v->agent));
- labelFr->setText(QString::fromStdString(v->frame));
- labelX->setText(QString::number(v->position->x));
- labelY->setText(QString::number(v->position->y));
- labelZ->setText(QString::number(v->position->z));
- labelQW->setText(QString::number(v->orientation->qw));
- labelQX->setText(QString::number(v->orientation->qx));
- labelQY->setText(QString::number(v->orientation->qy));
- labelQZ->setText(QString::number(v->orientation->qz));
- }
- private:
- QLabel* labelAg;
- QLabel* labelFr;
- QLabel* labelX;
- QLabel* labelY;
- QLabel* labelZ;
- QLabel* labelQW;
- QLabel* labelQX;
- QLabel* labelQY;
- QLabel* labelQZ;
- };
- VariantDataWidgetFactoryRegistration<FramedPoseBaseWidget> registerFramedPoseBaseWidget {FramedPoseBase::ice_staticId()};
- }
-}
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelX = new QLabel;
+ labelY = new QLabel;
+ labelZ = new QLabel;
+ labelQW = new QLabel;
+ labelQX = new QLabel;
+ labelQY = new QLabel;
+ labelQZ = new QLabel;
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ l->addRow("Z", labelZ);
+ l->addRow("QW", labelQW);
+ l->addRow("QX", labelQX);
+ l->addRow("QY", labelQY);
+ l->addRow("QZ", labelQZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ PoseBasePtr v = PoseBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ ARMARX_CHECK_EXPRESSION(v->position);
+ ARMARX_CHECK_EXPRESSION(v->orientation);
+ labelX->setText(QString::number(v->position->x));
+ labelY->setText(QString::number(v->position->y));
+ labelZ->setText(QString::number(v->position->z));
+ labelQW->setText(QString::number(v->orientation->qw));
+ labelQX->setText(QString::number(v->orientation->qx));
+ labelQY->setText(QString::number(v->orientation->qy));
+ labelQZ->setText(QString::number(v->orientation->qz));
+ }
+ private:
+ QLabel* labelX;
+ QLabel* labelY;
+ QLabel* labelZ;
+ QLabel* labelQW;
+ QLabel* labelQX;
+ QLabel* labelQY;
+ QLabel* labelQZ;
+ };
+ VariantDataWidgetFactoryRegistration<PoseBaseWidget> registerPoseBaseWidget {PoseBase::ice_staticId()};
+ class FramedPoseBaseWidget: public VariantDataWidgetBase
+ {
+ public:
+ FramedPoseBaseWidget(const VariantDataPtr& v)
+ {
+ auto l = new QFormLayout;
+ l->setContentsMargins(0, 0, 0, 0);
+ setLayout(l);
+ labelAg = new QLabel;
+ labelFr = new QLabel;
+ labelX = new QLabel;
+ labelY = new QLabel;
+ labelZ = new QLabel;
+ labelQW = new QLabel;
+ labelQX = new QLabel;
+ labelQY = new QLabel;
+ labelQZ = new QLabel;
+ l->addRow("Agent", labelAg);
+ l->addRow("Frame", labelFr);
+ l->addRow("X", labelX);
+ l->addRow("Y", labelY);
+ l->addRow("Z", labelZ);
+ l->addRow("QW", labelQW);
+ l->addRow("QX", labelQX);
+ l->addRow("QY", labelQY);
+ l->addRow("QZ", labelQZ);
+ update(v);
+ }
+ void update(const VariantDataPtr& p) override
+ {
+ FramedPoseBasePtr v = FramedPoseBasePtr::dynamicCast(p);
+ ARMARX_CHECK_EXPRESSION(v);
+ ARMARX_CHECK_EXPRESSION(v->position);
+ ARMARX_CHECK_EXPRESSION(v->orientation);
+ labelAg->setText(QString::fromStdString(v->agent));
+ labelFr->setText(QString::fromStdString(v->frame));
+ labelX->setText(QString::number(v->position->x));
+ labelY->setText(QString::number(v->position->y));
+ labelZ->setText(QString::number(v->position->z));
+ labelQW->setText(QString::number(v->orientation->qw));
+ labelQX->setText(QString::number(v->orientation->qx));
+ labelQY->setText(QString::number(v->orientation->qy));
+ labelQZ->setText(QString::number(v->orientation->qz));
+ }
+ private:
+ QLabel* labelAg;
+ QLabel* labelFr;
+ QLabel* labelX;
+ QLabel* labelY;
+ QLabel* labelZ;
+ QLabel* labelQW;
+ QLabel* labelQX;
+ QLabel* labelQY;
+ QLabel* labelQZ;
+ };
+ VariantDataWidgetFactoryRegistration<FramedPoseBaseWidget> registerFramedPoseBaseWidget {FramedPoseBase::ice_staticId()};
+}
diff --git a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
index 40b616f95..55819d35d 100644
--- a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
+++ b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
@@ -24,18 +24,16 @@
#include <ArmarXGui/libraries/VariantWidget/VariantWidget.h>
-namespace armarx
+namespace armarx::detail
{
- namespace detail
+ struct RobotAPIVariantWidgetDummySymbol
{
- struct RobotAPIVariantWidgetDummySymbol
- {
- RobotAPIVariantWidgetDummySymbol(int);
- };
- const RobotAPIVariantWidgetDummySymbol robotAPIVariantWidgetDummySymbolInstanceGlobal(1);
- inline std::size_t robotAPIVariantWidgetDummySymbolFunction()
- {
- return sizeof(robotAPIVariantWidgetDummySymbolInstanceGlobal);
- }
+ RobotAPIVariantWidgetDummySymbol(int);
+ };
+ const RobotAPIVariantWidgetDummySymbol robotAPIVariantWidgetDummySymbolInstanceGlobal(1);
+ inline std::size_t robotAPIVariantWidgetDummySymbolFunction()
+ {
+ return sizeof(robotAPIVariantWidgetDummySymbolInstanceGlobal);
}
}
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
index a170c9391..b6cf85960 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
@@ -26,27 +26,28 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
-using namespace armarx;
-
-ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName)
+namespace armarx
{
- forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
- torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
- setZero();
-}
+ ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName)
+ {
+ forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
+ torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
+ setZero();
+ }
-Eigen::Vector3f ForceTorqueHelper::getForce()
-{
- return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
-}
+ Eigen::Vector3f ForceTorqueHelper::getForce()
+ {
+ return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
+ }
-Eigen::Vector3f ForceTorqueHelper::getTorque()
-{
- return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
-}
+ Eigen::Vector3f ForceTorqueHelper::getTorque()
+ {
+ return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
+ }
-void ForceTorqueHelper::setZero()
-{
- initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
- initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+ void ForceTorqueHelper::setZero()
+ {
+ initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
+ initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+ }
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
index b0482a8db..cf01c7caf 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
@@ -23,39 +23,39 @@
#include "KinematicUnitHelper.h"
-using namespace armarx;
-
-KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit)
- : kinUnit(kinUnit)
+namespace armarx
{
-}
+ KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit)
+ : kinUnit(kinUnit)
+ {
+ }
-void KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
-{
- kinUnit->switchControlMode(MakeControlModes(jointAngles, ePositionControl));
- kinUnit->setJointAngles(jointAngles);
-}
+ void KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
+ {
+ kinUnit->switchControlMode(MakeControlModes(jointAngles, ePositionControl));
+ kinUnit->setJointAngles(jointAngles);
+ }
-void KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
-{
- kinUnit->switchControlMode(MakeControlModes(jointVelocities, eVelocityControl));
- kinUnit->setJointVelocities(jointVelocities);
-}
+ void KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
+ {
+ kinUnit->switchControlMode(MakeControlModes(jointVelocities, eVelocityControl));
+ kinUnit->setJointVelocities(jointVelocities);
+ }
-void KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
-{
- kinUnit->switchControlMode(MakeControlModes(jointTorques, eTorqueControl));
- kinUnit->setJointTorques(jointTorques);
-}
+ void KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
+ {
+ kinUnit->switchControlMode(MakeControlModes(jointTorques, eTorqueControl));
+ kinUnit->setJointTorques(jointTorques);
+ }
-NameControlModeMap KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
-{
- NameControlModeMap cm;
- for (const auto& pair : jointValues)
+ NameControlModeMap KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
{
- cm[pair.first] = controlMode;
+ NameControlModeMap cm;
+ for (const auto& pair : jointValues)
+ {
+ cm[pair.first] = controlMode;
+ }
+ return cm;
}
- return cm;
}
-
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index a54284c3a..8443046e1 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -29,246 +29,247 @@
#include <VirtualRobot/math/Helpers.h>
-using namespace armarx;
-
-PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+namespace armarx
{
- waypoints.push_back(target);
-}
+ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+ {
+ waypoints.push_back(target);
+ }
-PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
-{
-}
+ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
+ {
+ }
-PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
-{
- for (const PosePtr& pose : waypoints)
+ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
{
- this->waypoints.push_back(pose->toEigen());
+ for (const PosePtr& pose : waypoints)
+ {
+ this->waypoints.push_back(pose->toEigen());
+ }
}
-}
-void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
-{
- thresholdOrientationNear = config->thresholdOrientationNear;
- thresholdOrientationReached = config->thresholdOrientationReached;
- thresholdPositionNear = config->thresholdPositionNear;
- thresholdPositionReached = config->thresholdPositionReached;
- posController.KpOri = config->KpOri;
- posController.KpPos = config->KpPos;
- posController.maxOriVel = config->maxOriVel;
- posController.maxPosVel = config->maxPosVel;
-}
+ void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
+ {
+ thresholdOrientationNear = config->thresholdOrientationNear;
+ thresholdOrientationReached = config->thresholdOrientationReached;
+ thresholdPositionNear = config->thresholdPositionNear;
+ thresholdPositionReached = config->thresholdPositionReached;
+ posController.KpOri = config->KpOri;
+ posController.KpPos = config->KpPos;
+ posController.maxOriVel = config->maxOriVel;
+ posController.maxPosVel = config->maxPosVel;
+ }
-void PositionControllerHelper::update()
-{
- updateRead();
- updateWrite();
-}
+ void PositionControllerHelper::update()
+ {
+ updateRead();
+ updateWrite();
+ }
-void PositionControllerHelper::updateRead()
-{
- if (!isLastWaypoint() && isCurrentTargetNear())
+ void PositionControllerHelper::updateRead()
{
- currentWaypointIndex++;
+ if (!isLastWaypoint() && isCurrentTargetNear())
+ {
+ currentWaypointIndex++;
+ }
}
-}
-void PositionControllerHelper::updateWrite()
-{
- Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
- velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
- if (autoClearFeedForward)
+ void PositionControllerHelper::updateWrite()
{
- clearFeedForwardVelocity();
+ Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+ velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
+ if (autoClearFeedForward)
+ {
+ clearFeedForwardVelocity();
+ }
}
-}
-void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
-{
- this->waypoints = waypoints;
- currentWaypointIndex = 0;
-}
+ void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+ {
+ this->waypoints = waypoints;
+ currentWaypointIndex = 0;
+ }
-void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
-{
- this->waypoints.clear();
- for (const PosePtr& pose : waypoints)
+ void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
{
- this->waypoints.push_back(pose->toEigen());
+ this->waypoints.clear();
+ for (const PosePtr& pose : waypoints)
+ {
+ this->waypoints.push_back(pose->toEigen());
+ }
+ currentWaypointIndex = 0;
}
- currentWaypointIndex = 0;
-}
-void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
-{
- this->waypoints.push_back(waypoint);
-}
+ void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
+ {
+ this->waypoints.push_back(waypoint);
+ }
-void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
-{
- waypoints.clear();
- waypoints.push_back(target);
- currentWaypointIndex = 0;
-}
+ void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
+ {
+ waypoints.clear();
+ waypoints.push_back(target);
+ currentWaypointIndex = 0;
+ }
-void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
-{
- this->feedForwardVelocity = feedForwardVelocity;
-}
+ void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ {
+ this->feedForwardVelocity = feedForwardVelocity;
+ }
-void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
-{
- feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
- feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
-}
+ void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ {
+ feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+ feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+ }
-void PositionControllerHelper::clearFeedForwardVelocity()
-{
- feedForwardVelocity = Eigen::Vector6f::Zero();
-}
+ void PositionControllerHelper::clearFeedForwardVelocity()
+ {
+ feedForwardVelocity = Eigen::Vector6f::Zero();
+ }
-void PositionControllerHelper::immediateHardStop(bool clearTargets)
-{
- velocityControllerHelper->controller->immediateHardStop();
- if (clearTargets)
+ void PositionControllerHelper::immediateHardStop(bool clearTargets)
{
- setTarget(posController.getTcp()->getPoseInRootFrame());
+ velocityControllerHelper->controller->immediateHardStop();
+ if (clearTargets)
+ {
+ setTarget(posController.getTcp()->getPoseInRootFrame());
+ }
}
-}
-float PositionControllerHelper::getPositionError() const
-{
- return posController.getPositionError(getCurrentTarget());
-}
+ float PositionControllerHelper::getPositionError() const
+ {
+ return posController.getPositionError(getCurrentTarget());
+ }
-float PositionControllerHelper::getOrientationError() const
-{
- return posController.getOrientationError(getCurrentTarget());
-}
+ float PositionControllerHelper::getOrientationError() const
+ {
+ return posController.getOrientationError(getCurrentTarget());
+ }
-bool PositionControllerHelper::isCurrentTargetReached() const
-{
- return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
-}
+ bool PositionControllerHelper::isCurrentTargetReached() const
+ {
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ }
-bool PositionControllerHelper::isCurrentTargetNear() const
-{
- return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
-}
+ bool PositionControllerHelper::isCurrentTargetNear() const
+ {
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ }
-bool PositionControllerHelper::isFinalTargetReached() const
-{
- return isLastWaypoint() && isCurrentTargetReached();
-}
+ bool PositionControllerHelper::isFinalTargetReached() const
+ {
+ return isLastWaypoint() && isCurrentTargetReached();
+ }
-bool PositionControllerHelper::isFinalTargetNear() const
-{
- return isLastWaypoint() && isCurrentTargetNear();
-}
+ bool PositionControllerHelper::isFinalTargetNear() const
+ {
+ return isLastWaypoint() && isCurrentTargetNear();
+ }
-bool PositionControllerHelper::isLastWaypoint() const
-{
- return currentWaypointIndex + 1 >= waypoints.size();
-}
+ bool PositionControllerHelper::isLastWaypoint() const
+ {
+ return currentWaypointIndex + 1 >= waypoints.size();
+ }
-const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
-{
- return waypoints.at(currentWaypointIndex);
-}
+ const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
+ {
+ return waypoints.at(currentWaypointIndex);
+ }
-const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
-{
- return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
-}
+ const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
+ {
+ return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+ }
-size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
-{
- float dist = FLT_MAX;
- size_t minIndex = 0;
- for (size_t i = 0; i < waypoints.size(); i++)
+ size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
{
- float posErr = posController.getPositionError(waypoints.at(i));
- float oriErr = posController.getOrientationError(waypoints.at(i));
- float d = posErr + oriErr * rad2mmFactor;
- if (d < dist)
+ float dist = FLT_MAX;
+ size_t minIndex = 0;
+ for (size_t i = 0; i < waypoints.size(); i++)
{
- minIndex = i;
- dist = d;
+ float posErr = posController.getPositionError(waypoints.at(i));
+ float oriErr = posController.getOrientationError(waypoints.at(i));
+ float d = posErr + oriErr * rad2mmFactor;
+ if (d < dist)
+ {
+ minIndex = i;
+ dist = d;
+ }
}
+ currentWaypointIndex = minIndex;
+ return currentWaypointIndex;
}
- currentWaypointIndex = minIndex;
- return currentWaypointIndex;
-}
-void PositionControllerHelper::setNullSpaceControl(bool enabled)
-{
- velocityControllerHelper->setNullSpaceControl(enabled);
-}
-
-std::string PositionControllerHelper::getStatusText()
-{
- std::stringstream ss;
- ss.precision(2);
- ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
- return ss.str();
-}
+ void PositionControllerHelper::setNullSpaceControl(bool enabled)
+ {
+ velocityControllerHelper->setNullSpaceControl(enabled);
+ }
-bool PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
-{
- CartesianPositionController posHelper(tcp);
+ std::string PositionControllerHelper::getStatusText()
+ {
+ std::stringstream ss;
+ ss.precision(2);
+ ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ return ss.str();
+ }
- CartesianVelocityControllerPtr cartesianVelocityController;
- cartesianVelocityController.reset(new CartesianVelocityController(rns, tcp, args.invJacMethod));
+ bool PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
+ {
+ CartesianPositionController posHelper(tcp);
- float errorOriInitial = posHelper.getOrientationError(target);
- float errorPosInitial = posHelper.getPositionError(target);
+ CartesianVelocityControllerPtr cartesianVelocityController;
+ cartesianVelocityController.reset(new CartesianVelocityController(rns, tcp, args.invJacMethod));
- float stepLength = args.stepLength;
- float eps = args.eps;
+ float errorOriInitial = posHelper.getOrientationError(target);
+ float errorPosInitial = posHelper.getPositionError(target);
- std::vector<float> initialJointAngles = rns->getJointValues();
+ float stepLength = args.stepLength;
+ float eps = args.eps;
- for (int i = 0; i < args.loops; i++)
- {
- Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
- Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
- float nullspaceLen = nullspaceVel.norm();
- if (nullspaceLen > stepLength)
- {
- nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
- }
- Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
+ std::vector<float> initialJointAngles = rns->getJointValues();
- //jointVelocities = jointVelocities * stepLength;
- float len = jointVelocities.norm();
- if (len > stepLength)
+ for (int i = 0; i < args.loops; i++)
{
- jointVelocities = jointVelocities / len * stepLength;
+ Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
+ Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
+ float nullspaceLen = nullspaceVel.norm();
+ if (nullspaceLen > stepLength)
+ {
+ nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
+ }
+ Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
+
+ //jointVelocities = jointVelocities * stepLength;
+ float len = jointVelocities.norm();
+ if (len > stepLength)
+ {
+ jointVelocities = jointVelocities / len * stepLength;
+ }
+
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
+ }
+ if (len < eps)
+ {
+ break;
+ }
}
- for (size_t n = 0; n < rns->getSize(); n++)
+ float errorOriAfter = posHelper.getOrientationError(target);
+ float errorPosAfter = posHelper.getPositionError(target);
+ if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
{
- rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
+ return true;
}
- if (len < eps)
+ else
{
- break;
+ rns->setJointValues(initialJointAngles);
+ return false;
}
}
-
- float errorOriAfter = posHelper.getOrientationError(target);
- float errorPosAfter = posHelper.getPositionError(target);
- if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
- {
- return true;
- }
- else
- {
- rns->setJointValues(initialJointAngles);
- return false;
- }
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 9d0551330..b4d16c5e2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -24,212 +24,212 @@
#include "RobotNameHelper.h"
#include <ArmarXCore/core/util/StringHelpers.h>
-using namespace armarx;
-
-const std::string RobotNameHelper::LocationLeft = "Left";
-const std::string RobotNameHelper::LocationRight = "Right";
-
-RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
- : root(Node(robotInfo))
+namespace armarx
{
- StatechartProfilePtr p = profile;
- while (p && !p->isRoot())
+ const std::string RobotNameHelper::LocationLeft = "Left";
+ const std::string RobotNameHelper::LocationRight = "Right";
+
+ RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+ : root(Node(robotInfo))
{
- profiles.emplace_back(p->getName());
- p = p->getParent();
- }
- profiles.emplace_back(""); // for matching the default/root
+ StatechartProfilePtr p = profile;
+ while (p && !p->isRoot())
+ {
+ profiles.emplace_back(p->getName());
+ p = p->getParent();
+ }
+ profiles.emplace_back(""); // for matching the default/root
-}
+ }
-std::string RobotNameHelper::select(const std::string& path) const
-{
- Node node = root;
- for (const std::string& p : Split(path, "/"))
+ std::string RobotNameHelper::select(const std::string& path) const
{
- node = node.select(p, profiles);
+ Node node = root;
+ for (const std::string& p : Split(path, "/"))
+ {
+ node = node.select(p, profiles);
+ }
+ if (!node.isValid())
+ {
+ throw std::runtime_error("RobotNameHelper::select: path " + path + " not found");
+ }
+ return node.value();
}
- if (!node.isValid())
+
+ RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
{
- throw std::runtime_error("RobotNameHelper::select: path " + path + " not found");
+ return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
}
- return node.value();
-}
-
-RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
-{
- return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
-}
-RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side)
-{
- return Arm(shared_from_this(), side);
-}
-
-RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot)
-{
- return RobotArm(Arm(shared_from_this(), side), robot);
-}
+ RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side)
+ {
+ return Arm(shared_from_this(), side);
+ }
-RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
- : robotInfo(robotInfo)
-{
+ RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot)
+ {
+ return RobotArm(Arm(shared_from_this(), side), robot);
+ }
-}
+ RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
+ : robotInfo(robotInfo)
+ {
-std::string RobotNameHelper::Node::value()
-{
- checkValid();
- return robotInfo->value;
-}
+ }
-RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
-{
- if (!isValid())
+ std::string RobotNameHelper::Node::value()
{
- return Node(nullptr);
+ checkValid();
+ return robotInfo->value;
}
- std::map<std::string, RobotInfoNodePtr> matches;
- for (const RobotInfoNodePtr& child : robotInfo->children)
+
+ RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
{
- if (child->name == name)
+ if (!isValid())
{
- matches[child->profile] = child;
+ return Node(nullptr);
}
- }
- for (const std::string& p : profiles)
- {
- if (matches.count(p))
+ std::map<std::string, RobotInfoNodePtr> matches;
+ for (const RobotInfoNodePtr& child : robotInfo->children)
+ {
+ if (child->name == name)
+ {
+ matches[child->profile] = child;
+ }
+ }
+ for (const std::string& p : profiles)
{
- return matches.at(p);
+ if (matches.count(p))
+ {
+ return matches.at(p);
+ }
}
+ return Node(nullptr);
}
- return Node(nullptr);
-}
-bool RobotNameHelper::Node::isValid()
-{
- return robotInfo ? true : false;
-}
+ bool RobotNameHelper::Node::isValid()
+ {
+ return robotInfo ? true : false;
+ }
-void RobotNameHelper::Node::checkValid()
-{
- if (!isValid())
+ void RobotNameHelper::Node::checkValid()
{
- throw std::runtime_error("RobotNameHelper::Node nullptr exception");
+ if (!isValid())
+ {
+ throw std::runtime_error("RobotNameHelper::Node nullptr exception");
+ }
}
-}
-std::string RobotNameHelper::Arm::getSide() const
-{
- return side;
-}
+ std::string RobotNameHelper::Arm::getSide() const
+ {
+ return side;
+ }
-std::string RobotNameHelper::Arm::getKinematicChain() const
-{
- return select("KinematicChain");
-}
+ std::string RobotNameHelper::Arm::getKinematicChain() const
+ {
+ return select("KinematicChain");
+ }
-std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
-{
- return select("TorsoKinematicChain");
-}
+ std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
+ {
+ return select("TorsoKinematicChain");
+ }
-std::string RobotNameHelper::Arm::getTCP() const
-{
- return select("TCP");
-}
+ std::string RobotNameHelper::Arm::getTCP() const
+ {
+ return select("TCP");
+ }
-std::string RobotNameHelper::Arm::getForceTorqueSensor() const
-{
- return select("ForceTorqueSensor");
-}
+ std::string RobotNameHelper::Arm::getForceTorqueSensor() const
+ {
+ return select("ForceTorqueSensor");
+ }
-std::string RobotNameHelper::Arm::getEndEffector() const
-{
- return select("EndEffector");
-}
+ std::string RobotNameHelper::Arm::getEndEffector() const
+ {
+ return select("EndEffector");
+ }
-std::string RobotNameHelper::Arm::getMemoryHandName() const
-{
- return select("MemoryHandName");
-}
+ std::string RobotNameHelper::Arm::getMemoryHandName() const
+ {
+ return select("MemoryHandName");
+ }
-std::string RobotNameHelper::Arm::getHandControllerName() const
-{
- return select("HandControllerName");
-}
+ std::string RobotNameHelper::Arm::getHandControllerName() const
+ {
+ return select("HandControllerName");
+ }
-std::string RobotNameHelper::Arm::getHandRootNode() const
-{
- return select("HandRootNode");
-}
+ std::string RobotNameHelper::Arm::getHandRootNode() const
+ {
+ return select("HandRootNode");
+ }
-std::string RobotNameHelper::Arm::getHandModelPath() const
-{
- return select("HandModelPath");
-}
+ std::string RobotNameHelper::Arm::getHandModelPath() const
+ {
+ return select("HandModelPath");
+ }
-std::string RobotNameHelper::Arm::getHandModelPackage() const
-{
- return select("HandModelPackage");
-}
+ std::string RobotNameHelper::Arm::getHandModelPackage() const
+ {
+ return select("HandModelPackage");
+ }
-RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
-{
- return RobotArm(*this, robot);
-}
+ RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+ {
+ return RobotArm(*this, robot);
+ }
-RobotNameHelper::Arm::Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side)
- : rnh(rnh), side(side)
-{
+ RobotNameHelper::Arm::Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side)
+ : rnh(rnh), side(side)
+ {
-}
+ }
-std::string RobotNameHelper::Arm::select(const std::string& path) const
-{
- return rnh->select(side + "Arm/" + path);
-}
+ std::string RobotNameHelper::Arm::select(const std::string& path) const
+ {
+ return rnh->select(side + "Arm/" + path);
+ }
-std::string RobotNameHelper::RobotArm::getSide() const
-{
- return arm.getSide();
-}
+ std::string RobotNameHelper::RobotArm::getSide() const
+ {
+ return arm.getSide();
+ }
-VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
-{
- return robot->getRobotNodeSet(arm.getKinematicChain());
-}
+ VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
+ {
+ return robot->getRobotNodeSet(arm.getKinematicChain());
+ }
-VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
-{
- return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
-}
+ VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
+ {
+ return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
+ }
-VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
-{
- return robot->getRobotNode(arm.getTCP());
-}
+ VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
+ {
+ return robot->getRobotNode(arm.getTCP());
+ }
-VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
-{
- return robot->getRobotNode(arm.getHandRootNode());
-}
+ VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
+ {
+ return robot->getRobotNode(arm.getHandRootNode());
+ }
-Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
-{
- return getTCP()->getPoseInRootFrame().inverse() * getHandRootNode()->getPoseInRootFrame();
-}
+ Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
+ {
+ return getTCP()->getPoseInRootFrame().inverse() * getHandRootNode()->getPoseInRootFrame();
+ }
-RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
-{
- return arm;
-}
+ RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
+ {
+ return arm;
+ }
-RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
- : arm(arm), robot(robot)
-{
+ RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
+ : arm(arm), robot(robot)
+ {
+ }
}
-
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
index 720addebc..295251d4e 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
@@ -21,8 +21,3 @@
*/
#include "RobotStatechartHelpers.h"
-
-
-using namespace armarx;
-
-
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index c1a2369f9..d4d9a8846 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -25,68 +25,68 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-using namespace armarx;
-
-
-VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName)
- : robotUnit(robotUnit), controllerName(controllerName)
-{
- config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
- init();
-}
-
-VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName)
- : robotUnit(robotUnit), controllerName(controllerName)
+namespace armarx
{
- this->config = config;
- init();
-}
-
-void VelocityControllerHelper::init()
-{
- NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
- if (ctrl)
+ VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName)
+ : robotUnit(robotUnit), controllerName(controllerName)
{
- controllerCreated = false;
+ config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
+ init();
}
- else
+
+ VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName)
+ : robotUnit(robotUnit), controllerName(controllerName)
{
- ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
- controllerCreated = true;
+ this->config = config;
+ init();
}
- controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
- controller->activateController();
-}
-void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
-{
- controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
-}
-
-void VelocityControllerHelper::setNullSpaceControl(bool enabled)
-{
- if (enabled)
+ void VelocityControllerHelper::init()
{
- controller->setJointLimitAvoidanceScale(config->jointLimitAvoidanceScale);
- controller->setKpJointLimitAvoidance(config->KpJointLimitAvoidance);
+ NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+ if (ctrl)
+ {
+ controllerCreated = false;
+ }
+ else
+ {
+ ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+ controllerCreated = true;
+ }
+ controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
+ controller->activateController();
}
- else
+
+ void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
{
- controller->setJointLimitAvoidanceScale(0);
- controller->setKpJointLimitAvoidance(0);
+ controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
}
-}
-void VelocityControllerHelper::cleanup()
-{
- if (controllerCreated)
+ void VelocityControllerHelper::setNullSpaceControl(bool enabled)
{
- // delete controller only if it was created
- controller->deactivateAndDeleteController();
+ if (enabled)
+ {
+ controller->setJointLimitAvoidanceScale(config->jointLimitAvoidanceScale);
+ controller->setKpJointLimitAvoidance(config->KpJointLimitAvoidance);
+ }
+ else
+ {
+ controller->setJointLimitAvoidanceScale(0);
+ controller->setKpJointLimitAvoidance(0);
+ }
}
- else
+
+ void VelocityControllerHelper::cleanup()
{
- // if the controller existed, only deactivate it
- controller->deactivateController();
+ if (controllerCreated)
+ {
+ // delete controller only if it was created
+ controller->deactivateAndDeleteController();
+ }
+ else
+ {
+ // if the controller existed, only deactivate it
+ controller->deactivateController();
+ }
}
}
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
index 696acf03e..11336c50b 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
@@ -23,44 +23,44 @@
#include "SimpleJsonLogger.h"
-using namespace armarx;
-
-SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append)
- : autoflush(true)
-{
- file.open(filename.c_str(), append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
-}
-
-void SimpleJsonLogger::log(JsonDataPtr entry)
+namespace armarx
{
- file << entry->toJsonString() << "\n";
- if (autoflush)
+ SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append)
+ : autoflush(true)
{
- file.flush();
+ file.open(filename.c_str(), append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
}
-}
-void SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
-{
- log(entry.obj);
-}
+ void SimpleJsonLogger::log(JsonDataPtr entry)
+ {
+ file << entry->toJsonString() << "\n";
+ if (autoflush)
+ {
+ file.flush();
+ }
+ }
-void SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
-{
- log(entry->obj);
-}
+ void SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
+ {
+ log(entry.obj);
+ }
-void SimpleJsonLogger::close()
-{
- file.flush();
- file.close();
-}
+ void SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
+ {
+ log(entry->obj);
+ }
-SimpleJsonLogger::~SimpleJsonLogger()
-{
- if (file.is_open())
+ void SimpleJsonLogger::close()
{
+ file.flush();
file.close();
}
-}
+ SimpleJsonLogger::~SimpleJsonLogger()
+ {
+ if (file.is_open())
+ {
+ file.close();
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
index fde6f96c9..75096ffed 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -24,151 +24,152 @@
#include "SimpleJsonLoggerEntry.h"
#include <IceUtil/Time.h>
-using namespace armarx;
-
-SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
- : obj(new JsonObject)
+namespace armarx
{
-}
+ SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
+ : obj(new JsonObject)
+ {
+ }
-void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f& vec)
-{
- obj->add(key, ToArr((Eigen::VectorXf)vec));
-}
+ void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f& vec)
+ {
+ obj->add(key, ToArr((Eigen::VectorXf)vec));
+ }
-void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf& vec)
-{
- obj->add(key, ToArr(vec));
-}
+ void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf& vec)
+ {
+ obj->add(key, ToArr(vec));
+ }
-void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f& vec)
-{
- obj->add(key, ToObj(vec));
-}
+ void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f& vec)
+ {
+ obj->add(key, ToObj(vec));
+ }
-void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f& mat)
-{
- obj->add(key, ToArr(mat));
-}
+ void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f& mat)
+ {
+ obj->add(key, ToArr(mat));
+ }
-void SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
-{
- obj->add(key, MatrixToArr(mat));
-}
+ void SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
+ {
+ obj->add(key, MatrixToArr(mat));
+ }
-void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
-{
- obj->add(key, JsonValue::Create(value));
-}
+ void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
+ {
+ obj->add(key, JsonValue::Create(value));
+ }
-void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
-{
- obj->add(key, JsonValue::Create(value));
-}
+ void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
+ {
+ obj->add(key, JsonValue::Create(value));
+ }
-void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
-{
- obj->add(key, ToArr(value));
-}
+ void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
+ {
+ obj->add(key, ToArr(value));
+ }
-void SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
-{
- obj->add(key, ToObj(value));
-}
+ void SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
+ {
+ obj->add(key, ToObj(value));
+ }
-JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
-{
- JsonArrayPtr arr(new JsonArray);
- for (int i = 0; i < vec.rows(); i++)
+ JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
{
- arr->add(JsonValue::Create(vec(i)));
+ JsonArrayPtr arr(new JsonArray);
+ for (int i = 0; i < vec.rows(); i++)
+ {
+ arr->add(JsonValue::Create(vec(i)));
+ }
+ return arr;
}
- return arr;
-}
-JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
-{
- JsonArrayPtr arr(new JsonArray);
- for (float v : vec)
+ JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
{
- arr->add(JsonValue::Create(v));
+ JsonArrayPtr arr(new JsonArray);
+ for (float v : vec)
+ {
+ arr->add(JsonValue::Create(v));
+ }
+ return arr;
}
- return arr;
-}
-JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
-{
- JsonObjectPtr obj(new JsonObject);
- obj->add("x", JsonValue::Create(vec(0)));
- obj->add("y", JsonValue::Create(vec(1)));
- obj->add("z", JsonValue::Create(vec(2)));
- return obj;
-}
+ JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
+ {
+ JsonObjectPtr obj(new JsonObject);
+ obj->add("x", JsonValue::Create(vec(0)));
+ obj->add("y", JsonValue::Create(vec(1)));
+ obj->add("z", JsonValue::Create(vec(2)));
+ return obj;
+ }
-JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
-{
- JsonObjectPtr obj(new JsonObject);
- for (const std::pair<std::string, float>& pair : value)
+ JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
{
- obj->add(pair.first, JsonValue::Create(pair.second));
+ JsonObjectPtr obj(new JsonObject);
+ for (const std::pair<std::string, float>& pair : value)
+ {
+ obj->add(pair.first, JsonValue::Create(pair.second));
+ }
+ return obj;
}
- return obj;
-}
-JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
-{
- JsonArrayPtr arr(new JsonArray);
- JsonArrayPtr row1(new JsonArray);
- JsonArrayPtr row2(new JsonArray);
- JsonArrayPtr row3(new JsonArray);
- JsonArrayPtr row4(new JsonArray);
-
- row1->add(JsonValue::Create(mat(0, 0)));
- row1->add(JsonValue::Create(mat(0, 1)));
- row1->add(JsonValue::Create(mat(0, 2)));
- row1->add(JsonValue::Create(mat(0, 3)));
-
- row2->add(JsonValue::Create(mat(1, 0)));
- row2->add(JsonValue::Create(mat(1, 1)));
- row2->add(JsonValue::Create(mat(1, 2)));
- row2->add(JsonValue::Create(mat(1, 3)));
-
- row3->add(JsonValue::Create(mat(2, 0)));
- row3->add(JsonValue::Create(mat(2, 1)));
- row3->add(JsonValue::Create(mat(2, 2)));
- row3->add(JsonValue::Create(mat(2, 3)));
-
- row4->add(JsonValue::Create(mat(3, 0)));
- row4->add(JsonValue::Create(mat(3, 1)));
- row4->add(JsonValue::Create(mat(3, 2)));
- row4->add(JsonValue::Create(mat(3, 3)));
-
- arr->add(row1);
- arr->add(row2);
- arr->add(row3);
- arr->add(row4);
-
- return arr;
-}
+ JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
+ {
+ JsonArrayPtr arr(new JsonArray);
+ JsonArrayPtr row1(new JsonArray);
+ JsonArrayPtr row2(new JsonArray);
+ JsonArrayPtr row3(new JsonArray);
+ JsonArrayPtr row4(new JsonArray);
+
+ row1->add(JsonValue::Create(mat(0, 0)));
+ row1->add(JsonValue::Create(mat(0, 1)));
+ row1->add(JsonValue::Create(mat(0, 2)));
+ row1->add(JsonValue::Create(mat(0, 3)));
+
+ row2->add(JsonValue::Create(mat(1, 0)));
+ row2->add(JsonValue::Create(mat(1, 1)));
+ row2->add(JsonValue::Create(mat(1, 2)));
+ row2->add(JsonValue::Create(mat(1, 3)));
+
+ row3->add(JsonValue::Create(mat(2, 0)));
+ row3->add(JsonValue::Create(mat(2, 1)));
+ row3->add(JsonValue::Create(mat(2, 2)));
+ row3->add(JsonValue::Create(mat(2, 3)));
+
+ row4->add(JsonValue::Create(mat(3, 0)));
+ row4->add(JsonValue::Create(mat(3, 1)));
+ row4->add(JsonValue::Create(mat(3, 2)));
+ row4->add(JsonValue::Create(mat(3, 3)));
+
+ arr->add(row1);
+ arr->add(row2);
+ arr->add(row3);
+ arr->add(row4);
+
+ return arr;
+ }
-JsonArrayPtr SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
-{
- JsonArrayPtr arr(new JsonArray);
- for (int i = 0; i < mat.rows(); i++)
+ JsonArrayPtr SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
{
- JsonArrayPtr row(new JsonArray);
- for (int j = 0; j < mat.cols(); j++)
+ JsonArrayPtr arr(new JsonArray);
+ for (int i = 0; i < mat.rows(); i++)
{
- row->add(JsonValue::Create(mat(i, j)));
+ JsonArrayPtr row(new JsonArray);
+ for (int j = 0; j < mat.cols(); j++)
+ {
+ row->add(JsonValue::Create(mat(i, j)));
+ }
+ arr->add(row);
}
- arr->add(row);
+ return arr;
}
- return arr;
-}
-void SimpleJsonLoggerEntry::AddTimestamp()
-{
- IceUtil::Time now = IceUtil::Time::now();
- obj->add("timestamp", JsonValue::Create(now.toDateTime()));
+ void SimpleJsonLoggerEntry::AddTimestamp()
+ {
+ IceUtil::Time now = IceUtil::Time::now();
+ obj->add("timestamp", JsonValue::Create(now.toDateTime()));
+ }
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
index 5e3db572f..0a1bbdbc9 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
@@ -1,51 +1,51 @@
#include "exceptions.h"
-using namespace armarx::trajectory::error;
-
-
-TrajectoryException::TrajectoryException(const std::string& msg) : std::logic_error(msg)
-{}
-
-
-InterpolateDifferentTypesError::InterpolateDifferentTypesError() :
- TrajectoryException("Interpolating between two different types.")
-{}
-
-
-NoTrackWithID::NoTrackWithID(const TrackID& id) : TrajectoryException(makeMsg(id))
-{}
-
-std::string NoTrackWithID::makeMsg(const TrackID& id)
-{
- std::stringstream ss;
- ss << "No track with ID '" << id << "'. \n"
- << "Add a track with ID '" << id << "' before before adding keyframes.";
- return ss.str();
-}
-
-
-EmptyTrack::EmptyTrack(const TrackID& id) : TrajectoryException(makeMsg(id))
-{}
-
-std::string EmptyTrack::makeMsg(const TrackID& id)
-{
- std::stringstream ss;
- ss << "Track with ID '" << id << "' is empty. \n"
- "Add a keyframe to track '" << id << "' before updating.";
- return ss.str();
-}
-
-WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(
- const TrackID& trackID, const std::type_info& type, const std::type_info& expected) :
- TrajectoryException(makeMsg(trackID, type, expected))
-{}
-
-std::string WrongValueTypeInKeyframe::makeMsg(
- const TrackID& id, const std::type_info& type, const std::type_info& expected)
+namespace armarx::trajectory::error
{
- std::stringstream ss;
- ss << "Tried to add keyframe with value type '" << type.name() << "' to non-empty track '"
- << id << "' containing values of type '" << expected.name() << "'. \n"
- << "Only one value type per track is allowed.";
- return ss.str();
+ TrajectoryException::TrajectoryException(const std::string& msg) : std::logic_error(msg)
+ {}
+
+
+ InterpolateDifferentTypesError::InterpolateDifferentTypesError() :
+ TrajectoryException("Interpolating between two different types.")
+ {}
+
+
+ NoTrackWithID::NoTrackWithID(const TrackID& id) : TrajectoryException(makeMsg(id))
+ {}
+
+ std::string NoTrackWithID::makeMsg(const TrackID& id)
+ {
+ std::stringstream ss;
+ ss << "No track with ID '" << id << "'. \n"
+ << "Add a track with ID '" << id << "' before before adding keyframes.";
+ return ss.str();
+ }
+
+
+ EmptyTrack::EmptyTrack(const TrackID& id) : TrajectoryException(makeMsg(id))
+ {}
+
+ std::string EmptyTrack::makeMsg(const TrackID& id)
+ {
+ std::stringstream ss;
+ ss << "Track with ID '" << id << "' is empty. \n"
+ "Add a keyframe to track '" << id << "' before updating.";
+ return ss.str();
+ }
+
+ WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(
+ const TrackID& trackID, const std::type_info& type, const std::type_info& expected) :
+ TrajectoryException(makeMsg(trackID, type, expected))
+ {}
+
+ std::string WrongValueTypeInKeyframe::makeMsg(
+ const TrackID& id, const std::type_info& type, const std::type_info& expected)
+ {
+ std::stringstream ss;
+ ss << "Tried to add keyframe with value type '" << type.name() << "' to non-empty track '"
+ << id << "' containing values of type '" << expected.name() << "'. \n"
+ << "Only one value type per track is allowed.";
+ return ss.str();
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
index dedc54059..18f347139 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
@@ -24,87 +24,88 @@
#include "CartesianFeedForwardPositionController.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
-using namespace armarx;
-
-CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp)
- : tcp(tcp)
+namespace armarx
{
+ CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp)
+ : tcp(tcp)
+ {
-}
-
-Eigen::VectorXf CartesianFeedForwardPositionController::calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode)
-{
- int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
- int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
- Eigen::VectorXf cartesianVel(posLen + oriLen);
+ }
- if (posLen)
+ Eigen::VectorXf CartesianFeedForwardPositionController::calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode)
{
- Eigen::Vector3f targetPos = trajectory->GetPosition(t);
- Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
- Eigen::Vector3f posVel = errPos * KpPos;
- if (enableFeedForward)
+ int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+ int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ Eigen::VectorXf cartesianVel(posLen + oriLen);
+
+ if (posLen)
{
- posVel += trajectory->GetPositionDerivative(t);
+ Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+ Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (enableFeedForward)
+ {
+ posVel += trajectory->GetPositionDerivative(t);
+ }
+
+ if (maxPosVel > 0)
+ {
+ posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
}
- if (maxPosVel > 0)
+ if (oriLen)
{
- posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ Eigen::Vector3f errOri = aa.axis() * aa.angle();
+ Eigen::Vector3f oriVel = errOri * KpOri;
+ if (enableFeedForward)
+ {
+ oriVel += trajectory->GetOrientationDerivative(t);
+ }
+
+ if (maxOriVel > 0)
+ {
+ oriVel = math::MathUtils::LimitTo(oriVel, maxOriVel);
+ }
+ cartesianVel.block<3, 1>(posLen, 0) = oriVel;
}
- cartesianVel.block<3, 1>(0, 0) = posVel;
+ return cartesianVel;
+ }
+
+ float CartesianFeedForwardPositionController::getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ {
+ Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+ Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+ return errPos.norm();
}
- if (oriLen)
+ float CartesianFeedForwardPositionController::getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
{
Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
Eigen::AngleAxisf aa(oriDir);
- Eigen::Vector3f errOri = aa.axis() * aa.angle();
- Eigen::Vector3f oriVel = errOri * KpOri;
- if (enableFeedForward)
- {
- oriVel += trajectory->GetOrientationDerivative(t);
- }
-
- if (maxOriVel > 0)
- {
- oriVel = math::MathUtils::LimitTo(oriVel, maxOriVel);
- }
- cartesianVel.block<3, 1>(posLen, 0) = oriVel;
+ return aa.angle();
}
- return cartesianVel;
-}
-
-float CartesianFeedForwardPositionController::getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
-{
- Eigen::Vector3f targetPos = trajectory->GetPosition(t);
- Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
- return errPos.norm();
-}
-float CartesianFeedForwardPositionController::getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
-{
- Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
- Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
- Eigen::AngleAxisf aa(oriDir);
- return aa.angle();
-}
-
-Eigen::Vector3f CartesianFeedForwardPositionController::getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
-{
- Eigen::Vector3f targetPos = trajectory->GetPosition(t);
- return targetPos - tcp->getPositionInRootFrame();
+ Eigen::Vector3f CartesianFeedForwardPositionController::getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ {
+ Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+ return targetPos - tcp->getPositionInRootFrame();
-}
+ }
-Eigen::Vector3f CartesianFeedForwardPositionController::getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
-{
- Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
- Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
- Eigen::AngleAxisf aa(oriDir);
- return aa.axis() * aa.angle();
+ Eigen::Vector3f CartesianFeedForwardPositionController::getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ {
+ Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ return aa.axis() * aa.angle();
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
index bec043def..4c2704aca 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
@@ -38,7 +38,7 @@ namespace armarx
{
public:
CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp);
- Eigen::VectorXf calculate(const math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode);
float getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
float getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 60c62092f..9ce26dd28 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -25,86 +25,86 @@
#include <RobotAPI/libraries/core/math/MathUtils.h>
-using namespace armarx;
-
-CartesianPositionController::CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp)
- : tcp(tcp)
-{
-}
-
-Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
+namespace armarx
{
- int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
- int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
- Eigen::VectorXf cartesianVel(posLen + oriLen);
+ CartesianPositionController::CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp)
+ : tcp(tcp)
+ {
+ }
- if (posLen)
+ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
- Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
- Eigen::Vector3f errPos = targetPos - currentPos;
- Eigen::Vector3f posVel = errPos * KpPos;
- if (maxPosVel > 0)
+ int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+ int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ Eigen::VectorXf cartesianVel(posLen + oriLen);
+
+ if (posLen)
{
- posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (maxPosVel > 0)
+ {
+ posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
}
- cartesianVel.block<3, 1>(0, 0) = posVel;
+
+ if (oriLen)
+ {
+ Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ Eigen::Vector3f errOri = aa.axis() * aa.angle();
+ Eigen::Vector3f oriVel = errOri * KpOri;
+
+ if (maxOriVel > 0)
+ {
+ oriVel = math::MathUtils::LimitTo(oriVel, maxOriVel);
+ }
+ cartesianVel.block<3, 1>(posLen, 0) = oriVel;
+ }
+ return cartesianVel;
+ }
+
+ float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
+ {
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+ return errPos.norm();
}
- if (oriLen)
+ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
{
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
Eigen::AngleAxisf aa(oriDir);
- Eigen::Vector3f errOri = aa.axis() * aa.angle();
- Eigen::Vector3f oriVel = errOri * KpOri;
-
- if (maxOriVel > 0)
- {
- oriVel = math::MathUtils::LimitTo(oriVel, maxOriVel);
- }
- cartesianVel.block<3, 1>(posLen, 0) = oriVel;
+ return aa.angle();
}
- return cartesianVel;
-}
-
-float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
-{
- Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
- return errPos.norm();
-}
-float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
-{
- Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
- Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
- Eigen::AngleAxisf aa(oriDir);
- return aa.angle();
-}
-
-Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
-{
- Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
- return targetPos - tcp->getPositionInRootFrame();
+ Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
+ {
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ return targetPos - tcp->getPositionInRootFrame();
-}
+ }
-Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
-{
- Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
- Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
- Eigen::AngleAxisf aa(oriDir);
- return aa.axis() * aa.angle();
-}
+ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
+ {
+ Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ return aa.axis() * aa.angle();
+ }
-VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
-{
- return tcp;
-}
+ VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+ {
+ return tcp;
+ }
@@ -112,52 +112,53 @@ VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
#define SET_FLT(x) obj->setFloat(#x, x)
#define GET_FLT(x) x = obj->getFloat(#x);
-CartesianPositionControllerConfig::CartesianPositionControllerConfig()
-{
-}
+ CartesianPositionControllerConfig::CartesianPositionControllerConfig()
+ {
+ }
-std::string CartesianPositionControllerConfig::output(const Ice::Current& c) const
-{
- std::stringstream ss;
-
- SS_OUT(KpPos);
- SS_OUT(KpOri);
- SS_OUT(maxPosVel);
- SS_OUT(maxOriVel);
- SS_OUT(thresholdOrientationNear);
- SS_OUT(thresholdOrientationReached);
- SS_OUT(thresholdPositionNear);
- SS_OUT(thresholdPositionReached);
-
- return ss.str();
-}
+ std::string CartesianPositionControllerConfig::output(const Ice::Current& c) const
+ {
+ std::stringstream ss;
+
+ SS_OUT(KpPos);
+ SS_OUT(KpOri);
+ SS_OUT(maxPosVel);
+ SS_OUT(maxOriVel);
+ SS_OUT(thresholdOrientationNear);
+ SS_OUT(thresholdOrientationReached);
+ SS_OUT(thresholdPositionNear);
+ SS_OUT(thresholdPositionReached);
+
+ return ss.str();
+ }
-void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
-{
- AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
- SET_FLT(KpPos);
- SET_FLT(KpOri);
- SET_FLT(maxPosVel);
- SET_FLT(maxOriVel);
- SET_FLT(thresholdOrientationNear);
- SET_FLT(thresholdOrientationReached);
- SET_FLT(thresholdPositionNear);
- SET_FLT(thresholdPositionReached);
+ SET_FLT(KpPos);
+ SET_FLT(KpOri);
+ SET_FLT(maxPosVel);
+ SET_FLT(maxOriVel);
+ SET_FLT(thresholdOrientationNear);
+ SET_FLT(thresholdOrientationReached);
+ SET_FLT(thresholdPositionNear);
+ SET_FLT(thresholdPositionReached);
-}
+ }
-void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
-{
- AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
- GET_FLT(KpPos);
- GET_FLT(KpOri);
- GET_FLT(maxPosVel);
- GET_FLT(maxOriVel);
- GET_FLT(thresholdOrientationNear);
- GET_FLT(thresholdOrientationReached);
- GET_FLT(thresholdPositionNear);
- GET_FLT(thresholdPositionReached);
+ GET_FLT(KpPos);
+ GET_FLT(KpOri);
+ GET_FLT(maxPosVel);
+ GET_FLT(maxOriVel);
+ GET_FLT(thresholdOrientationNear);
+ GET_FLT(thresholdOrientationReached);
+ GET_FLT(thresholdPositionNear);
+ GET_FLT(thresholdPositionReached);
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index e5f2c832e..c3fab807b 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -63,19 +63,20 @@ namespace armarx
private:
VirtualRobot::RobotNodePtr tcp;
};
+}
+namespace armarx::VariantType
+{
+ // variant types
+ const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
+}
- namespace VariantType
- {
- // variant types
- const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
- }
-
+namespace armarx
+{
class CartesianPositionControllerConfig : virtual public CartesianPositionControllerConfigBase
{
public:
CartesianPositionControllerConfig();
-
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override
{
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 0f84e0a0a..6bb30d988 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -30,191 +30,192 @@
#include <VirtualRobot/math/Helpers.h>
-using namespace armarx;
-
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
- : rns(rns),
- considerJointLimits(considerJointLimits)
+namespace armarx
{
- //ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
- this->tcp = tcp ? tcp : rns->getTCP();
+ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
+ : rns(rns),
+ considerJointLimits(considerJointLimits)
+ {
+ //ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
+ this->tcp = tcp ? tcp : rns->getTCP();
- this->cartesianMMRegularization = 100;
- this->cartesianRadianRegularization = 1;
-}
+ this->cartesianMMRegularization = 100;
+ this->cartesianRadianRegularization = 1;
+ }
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
-{
- Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
- return calculate(cartesianVel, nullspaceVel, mode);
-}
+ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+ Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
+ return calculate(cartesianVel, nullspaceVel, mode);
+ }
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
-{
- //ARMARX_IMPORTANT << VAROUT(rns.get());
- //ARMARX_IMPORTANT << VAROUT(tcp.get());
- //ARMARX_IMPORTANT << VAROUT(ik.get());
- jacobi = ik->getJacobianMatrix(tcp, mode);
+ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+ //ARMARX_IMPORTANT << VAROUT(rns.get());
+ //ARMARX_IMPORTANT << VAROUT(tcp.get());
+ //ARMARX_IMPORTANT << VAROUT(ik.get());
+ jacobi = ik->getJacobianMatrix(tcp, mode);
- // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
- //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
- //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+ //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
+ //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
- // Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+ // Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
- {
- float squaredNorm = nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+ for (int i = 0; i < nullspace.cols(); i++)
{
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
}
- }
- // Eigen::VectorXf nsv = nullspace * nullspaceVel;
+ // Eigen::VectorXf nsv = nullspace * nullspaceVel;
- //nsv /= kernel.cols();
+ //nsv /= kernel.cols();
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
- if (considerJointLimits)
- {
- adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
- }
+ if (considerJointLimits)
+ {
+ adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ }
- Eigen::VectorXf jointVel = inv * cartesianVel;
- jointVel += nsv;
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ jointVel += nsv;
- if (maximumJointVelocities.rows() > 0)
- {
- jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
- }
+ if (maximumJointVelocities.rows() > 0)
+ {
+ jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
+ }
- return jointVel;
-}
+ return jointVel;
+ }
-Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
-{
- Eigen::VectorXf r(rns->getSize());
- for (size_t i = 0; i < rns->getSize(); i++)
+ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
{
- VirtualRobot::RobotNodePtr rn = rns->getNode(i);
- if (rn->isLimitless())
- {
- r(i) = 0;
- }
- else
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
{
- float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
- r(i) = cos(f * M_PI);
- //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ r(i) = cos(f * M_PI);
+ //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
+ }
}
+ return r;
}
- return r;
-}
-
-Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode)
-{
- Eigen::VectorXf regularization = calculateRegularization(mode);
- Eigen::VectorXf vel = cartesianVel * regularization;
- return KpScale * vel.norm() * calculateJointLimitAvoidance();
-
-}
-
-void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
-{
- this->cartesianMMRegularization = cartesianMMRegularization;
- this->cartesianRadianRegularization = cartesianRadianRegularization;
-}
-
-Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
-{
- Eigen::VectorXf regularization(6);
-
- int i = 0;
-
- if (mode & VirtualRobot::IKSolver::X)
+ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode)
{
- regularization(i++) = 1 / cartesianMMRegularization;
- }
+ Eigen::VectorXf regularization = calculateRegularization(mode);
+ Eigen::VectorXf vel = cartesianVel * regularization;
+
+ return KpScale * vel.norm() * calculateJointLimitAvoidance();
- if (mode & VirtualRobot::IKSolver::Y)
- {
- regularization(i++) = 1 / cartesianMMRegularization;
}
- if (mode & VirtualRobot::IKSolver::Z)
+ void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ this->cartesianMMRegularization = cartesianMMRegularization;
+ this->cartesianRadianRegularization = cartesianRadianRegularization;
}
- if (mode & VirtualRobot::IKSolver::Orientation)
+ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
{
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
- }
- return regularization.topRows(i);
-}
+ Eigen::VectorXf regularization(6);
-bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
-{
+ int i = 0;
- bool modifiedJacobi = false;
+ if (mode & VirtualRobot::IKSolver::X)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
- Eigen::VectorXf jointVel = inv * cartesianVel;
- size_t size = rns->getSize();
+ if (mode & VirtualRobot::IKSolver::Y)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
- for (size_t i = 0; i < size; ++i)
- {
- auto& node = rns->getNode(static_cast<int>(i));
+ if (mode & VirtualRobot::IKSolver::Z)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
- if (node->isLimitless() || // limitless joint cannot be out of limits
- std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
- )
+ if (mode & VirtualRobot::IKSolver::Orientation)
{
- continue;
+ regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / cartesianRadianRegularization;
}
+ return regularization.topRows(i);
+ }
+
+ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+ {
- if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
- || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
+ bool modifiedJacobi = false;
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ size_t size = rns->getSize();
+
+ for (size_t i = 0; i < size; ++i)
{
- for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
+ auto& node = rns->getNode(static_cast<int>(i));
+
+ if (node->isLimitless() || // limitless joint cannot be out of limits
+ std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
+ )
+ {
+ continue;
+ }
+
+ if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
+ || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
{
- jacobi(k, i) = 0.0f;
+ for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
+ {
+ jacobi(k, i) = 0.0f;
+ }
+ modifiedJacobi = true;
}
- modifiedJacobi = true;
}
+
+ if (modifiedJacobi)
+ {
+ // calculate inverse jacobi again since atleast one joint would be driven into the limit
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ }
+
+ return modifiedJacobi;
}
- if (modifiedJacobi)
+ bool CartesianVelocityController::getConsiderJointLimits() const
{
- // calculate inverse jacobi again since atleast one joint would be driven into the limit
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ return considerJointLimits;
}
- return modifiedJacobi;
-}
-
-bool CartesianVelocityController::getConsiderJointLimits() const
-{
- return considerJointLimits;
-}
-
-void CartesianVelocityController::setConsiderJointLimits(bool value)
-{
- considerJointLimits = value;
+ void CartesianVelocityController::setConsiderJointLimits(bool value)
+ {
+ considerJointLimits = value;
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 775d29544..3c9f18f9b 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -25,67 +25,68 @@
#include <ArmarXCore/core/logging/Logging.h>
-using namespace armarx;
-
-CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
- float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
- : controller(rns, tcp), mode(mode)
+namespace armarx
{
- setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
+ float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
+ : controller(rns, tcp), mode(mode)
+ {
+ setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
- setCurrentJointVelocity(currentJointVelocity);
+ setCurrentJointVelocity(currentJointVelocity);
#pragma GCC diagnostic pop
-}
+ }
-void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
-{
- Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
- Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+ void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
+ {
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+ Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
- {
- nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
- }
- cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
- nullSpaceVelocityRamp.setState(nsv);
+ for (int i = 0; i < nullspace.cols(); i++)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+ }
+ cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
+ nullSpaceVelocityRamp.setState(nsv);
-}
+ }
-Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
-{
- Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
- return calculate(cartesianVel, nullspaceVel, dt);
-}
+ Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
+ {
+ Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
+ return calculate(cartesianVel, nullspaceVel, dt);
+ }
-Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
-{
- return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
-}
+ Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
+ {
+ return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
+ }
-void CartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration)
-{
- cartesianVelocityRamp.setMaxOrientationAcceleration(maxOrientationAcceleration);
- cartesianVelocityRamp.setMaxPositionAcceleration(maxPositionAcceleration);
- nullSpaceVelocityRamp.setMaxAccelaration(maxNullspaceAcceleration);
-}
+ void CartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration)
+ {
+ cartesianVelocityRamp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+ cartesianVelocityRamp.setMaxPositionAcceleration(maxPositionAcceleration);
+ nullSpaceVelocityRamp.setMaxAccelaration(maxNullspaceAcceleration);
+ }
-float CartesianVelocityControllerWithRamp::getMaxOrientationAcceleration()
-{
- return cartesianVelocityRamp.getMaxOrientationAcceleration();
-}
+ float CartesianVelocityControllerWithRamp::getMaxOrientationAcceleration()
+ {
+ return cartesianVelocityRamp.getMaxOrientationAcceleration();
+ }
-float CartesianVelocityControllerWithRamp::getMaxPositionAcceleration()
-{
- return cartesianVelocityRamp.getMaxPositionAcceleration();
-}
+ float CartesianVelocityControllerWithRamp::getMaxPositionAcceleration()
+ {
+ return cartesianVelocityRamp.getMaxPositionAcceleration();
+ }
-float CartesianVelocityControllerWithRamp::getMaxNullspaceAcceleration()
-{
- return nullSpaceVelocityRamp.getMaxAccelaration();
+ float CartesianVelocityControllerWithRamp::getMaxNullspaceAcceleration()
+ {
+ return nullSpaceVelocityRamp.getMaxAccelaration();
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index 21cc73a78..e904576b7 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -25,65 +25,67 @@
#include <ArmarXCore/core/logging/Logging.h>
-using namespace armarx;
-
-CartesianVelocityRamp::CartesianVelocityRamp()
-{ }
-
-void CartesianVelocityRamp::setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode)
+namespace armarx
{
- this->state = state;
- this->mode = mode;
-}
+ CartesianVelocityRamp::CartesianVelocityRamp()
+ { }
-Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
-{
- if (dt <= 0)
+ void CartesianVelocityRamp::setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode)
{
- return state;
+ this->state = state;
+ this->mode = mode;
}
- Eigen::VectorXf delta = target - state;
- int i = 0;
- float factor = 1 ;
- if (mode & VirtualRobot::IKSolver::Position)
+ Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
{
- Eigen::Vector3f posDelta = delta.block<3, 1>(i, 0);
- float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
- factor = std::max(factor, posFactor);
- i += 3;
- }
+ if (dt <= 0)
+ {
+ return state;
+ }
+ Eigen::VectorXf delta = target - state;
+ int i = 0;
+ float factor = 1 ;
- if (mode & VirtualRobot::IKSolver::Orientation)
- {
- Eigen::Vector3f oriDelta = delta.block<3, 1>(i, 0);
- float oriFactor = oriDelta.norm() / maxOrientationAcceleration / dt;
- factor = std::max(factor, oriFactor);
+ if (mode & VirtualRobot::IKSolver::Position)
+ {
+ Eigen::Vector3f posDelta = delta.block<3, 1>(i, 0);
+ float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
+ factor = std::max(factor, posFactor);
+ i += 3;
+ }
+
+ if (mode & VirtualRobot::IKSolver::Orientation)
+ {
+ Eigen::Vector3f oriDelta = delta.block<3, 1>(i, 0);
+ float oriFactor = oriDelta.norm() / maxOrientationAcceleration / dt;
+ factor = std::max(factor, oriFactor);
+ }
+
+ state += delta / factor;
+ // ARMARX_IMPORTANT << "CartRamp state " << state;
+ return state;
}
- state += delta / factor;
- // ARMARX_IMPORTANT << "CartRamp state " << state;
- return state;
-}
+ void CartesianVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+ {
+ this->maxPositionAcceleration = maxPositionAcceleration;
-void CartesianVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
-{
- this->maxPositionAcceleration = maxPositionAcceleration;
+ }
-}
+ void CartesianVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+ {
+ this->maxOrientationAcceleration = maxOrientationAcceleration;
-void CartesianVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
-{
- this->maxOrientationAcceleration = maxOrientationAcceleration;
+ }
-}
+ float CartesianVelocityRamp::getMaxPositionAcceleration()
+ {
+ return maxPositionAcceleration;
+ }
-float CartesianVelocityRamp::getMaxPositionAcceleration()
-{
- return maxPositionAcceleration;
+ float CartesianVelocityRamp::getMaxOrientationAcceleration()
+ {
+ return maxOrientationAcceleration;
+ }
}
-float CartesianVelocityRamp::getMaxOrientationAcceleration()
-{
- return maxOrientationAcceleration;
-}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index 675dc505f..af8d731a9 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -174,7 +174,7 @@ namespace armarx
const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
{
- return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+ return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
index 1494cd9e3..fa5ea9fe7 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
@@ -27,162 +27,160 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-using namespace armarx;
-
-
-
-FramedOrientedPoint::FramedOrientedPoint()
- = default;
-
-FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
- IceUtil::Shared(source),
- OrientedPointBase(source),
- FramedOrientedPointBase(source),
- OrientedPoint(source)
+namespace armarx
{
-}
+ FramedOrientedPoint::FramedOrientedPoint()
+ = default;
-FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent) :
- OrientedPoint(position, normal)
-{
- this->frame = frame;
- this->agent = agent;
+ FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
+ IceUtil::Shared(source),
+ OrientedPointBase(source),
+ FramedOrientedPointBase(source),
+ OrientedPoint(source)
+ {
+ }
-}
+ FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent) :
+ OrientedPoint(position, normal)
+ {
+ this->frame = frame;
+ this->agent = agent;
-FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent) :
- FramedOrientedPoint(pointWithNormal.positionToEigen(), pointWithNormal.normalToEigen(), frame, agent)
-{
-}
+ }
-FramedOrientedPoint::FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent) :
- OrientedPoint(px, py, pz, nx, ny, nz)
-{
- this->frame = frame;
- this->agent = agent;
-}
+ FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent) :
+ FramedOrientedPoint(pointWithNormal.positionToEigen(), pointWithNormal.normalToEigen(), frame, agent)
+ {
+ }
-std::string FramedOrientedPoint::getFrame() const
-{
- return frame;
-}
+ FramedOrientedPoint::FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent) :
+ OrientedPoint(px, py, pz, nx, ny, nz)
+ {
+ this->frame = frame;
+ this->agent = agent;
+ }
+ std::string FramedOrientedPoint::getFrame() const
+ {
+ return frame;
+ }
-void FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
-{
- FramedPosition framedPos(positionToEigen(), frame, agent);
- FramedDirection framedDir(normalToEigen(), frame, agent);
-
- framedPos.changeFrame(robot, newFrame);
- framedDir.changeFrame(robot, newFrame);
- this->px = framedPos.x;
- this->py = framedPos.y;
- this->pz = framedPos.z;
-
- this->nx = framedDir.x;
- this->ny = framedDir.y;
- this->nz = framedDir.z;
-}
-void FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
-{
- VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
- changeFrame(sharedRobot, GlobalFrame);
-}
+ void FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
+ {
+ FramedPosition framedPos(positionToEigen(), frame, agent);
+ FramedDirection framedDir(normalToEigen(), frame, agent);
+
+ framedPos.changeFrame(robot, newFrame);
+ framedDir.changeFrame(robot, newFrame);
+ this->px = framedPos.x;
+ this->py = framedPos.y;
+ this->pz = framedPos.z;
+
+ this->nx = framedDir.x;
+ this->ny = framedDir.y;
+ this->nz = framedDir.z;
+ }
-void FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
-{
- changeFrame(referenceRobot, GlobalFrame);
-}
+ void FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ changeFrame(sharedRobot, GlobalFrame);
+ }
-Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
-{
- return getFramedPosition().toGlobalEigen(referenceRobot);
-}
+ void FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ {
+ changeFrame(referenceRobot, GlobalFrame);
+ }
-Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
-{
- return getFramedNormal().toGlobalEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ return getFramedPosition().toGlobalEigen(referenceRobot);
+ }
-Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
-{
- return getFramedPosition().toGlobalEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ return getFramedNormal().toGlobalEigen(referenceRobot);
+ }
-Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
-{
- return getFramedNormal().toGlobalEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ return getFramedPosition().toGlobalEigen(referenceRobot);
+ }
+ Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ return getFramedNormal().toGlobalEigen(referenceRobot);
+ }
-FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
-{
- return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
-}
-FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
-{
- return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
-}
+ FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
+ }
-Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
-{
- return getFramedPosition().toRootEigen(referenceRobot);
-}
+ FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
+ }
-Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
-{
- return getFramedNormal().toRootEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ return getFramedPosition().toRootEigen(referenceRobot);
+ }
-Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
-{
- return getFramedPosition().toRootEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ return getFramedNormal().toRootEigen(referenceRobot);
+ }
-Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
-{
- return getFramedNormal().toRootEigen(referenceRobot);
-}
+ Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ return getFramedPosition().toRootEigen(referenceRobot);
+ }
-FramedPosition FramedOrientedPoint::getFramedPosition() const
-{
- return FramedPosition(positionToEigen(), frame, agent);
-}
+ Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ return getFramedNormal().toRootEigen(referenceRobot);
+ }
-FramedDirection FramedOrientedPoint::getFramedNormal() const
-{
- return FramedDirection(normalToEigen(), frame, agent);
-}
+ FramedPosition FramedOrientedPoint::getFramedPosition() const
+ {
+ return FramedPosition(positionToEigen(), frame, agent);
+ }
+ FramedDirection FramedOrientedPoint::getFramedNormal() const
+ {
+ return FramedDirection(normalToEigen(), frame, agent);
+ }
-std::string FramedOrientedPoint::output(const Ice::Current& c) const
-{
- std::stringstream s;
- s << OrientedPoint::output(c) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
- return s.str();
-}
+ std::string FramedOrientedPoint::output(const Ice::Current& c) const
+ {
+ std::stringstream s;
+ s << OrientedPoint::output(c) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ return s.str();
+ }
-void FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
-{
- AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
- OrientedPoint::serialize(serializer);
- obj->setString("frame", frame);
- obj->setString("agent", agent);
-}
+ void FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ OrientedPoint::serialize(serializer);
+ obj->setString("frame", frame);
+ obj->setString("agent", agent);
-void FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
-{
- AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
- OrientedPoint::deserialize(serializer);
- frame = obj->getString("frame");
+ }
- if (obj->hasElement("agent"))
+ void FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
- agent = obj->getString("agent");
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ OrientedPoint::deserialize(serializer);
+ frame = obj->getString("frame");
+
+ if (obj->hasElement("agent"))
+ {
+ agent = obj->getString("agent");
+ }
}
}
-
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.h b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
index 7f2231c17..3504cad3a 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.h
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
@@ -31,14 +31,14 @@
#include <RobotAPI/interface/core/RobotState.h>
#include "FramedPose.h"
-namespace armarx
+namespace armarx::VariantType
{
- namespace VariantType
- {
- // variant types
- const VariantTypeId FramedOrientedPoint = Variant::addTypeName("::armarx::FramedOrientedPointBase");
- }
+ // variant types
+ const VariantTypeId FramedOrientedPoint = Variant::addTypeName("::armarx::FramedOrientedPointBase");
+}
+namespace armarx
+{
class FramedOrientedPoint;
using FramedOrientedPointPtr = IceInternal::Handle<FramedOrientedPoint>;
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 390396d22..0880da76b 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -45,16 +45,17 @@ namespace VirtualRobot
class LinkedCoordinate;
}
+namespace armarx::VariantType
+{
+ // variant types
+ const VariantTypeId FramedPose = Variant::addTypeName("::armarx::FramedPoseBase");
+ const VariantTypeId FramedDirection = Variant::addTypeName("::armarx::FramedDirectionBase");
+ const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
+ const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
+}
+
namespace armarx
{
- namespace VariantType
- {
- // variant types
- const VariantTypeId FramedPose = Variant::addTypeName("::armarx::FramedPoseBase");
- const VariantTypeId FramedDirection = Variant::addTypeName("::armarx::FramedDirectionBase");
- const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
- const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
- }
/**
* @ingroup RobotAPI-FramedPose
* Variable of the global coordinate system. use this if you are specifying a global pose.
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
index 33128cf1f..3619a4fc6 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -24,43 +24,42 @@
#include "JointVelocityRamp.h"
#include <ArmarXCore/core/logging/Logging.h>
-using namespace armarx;
-
-
-JointVelocityRamp::JointVelocityRamp()
-{ }
-
-void JointVelocityRamp::setState(const Eigen::VectorXf& state)
+namespace armarx
{
- this->state = state;
-}
+ JointVelocityRamp::JointVelocityRamp()
+ { }
-Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
-{
- if (dt <= 0)
+ void JointVelocityRamp::setState(const Eigen::VectorXf& state)
{
- return state;
+ this->state = state;
}
- Eigen::VectorXf delta = target - state;
+ Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+ {
+ if (dt <= 0)
+ {
+ return state;
+ }
- float factor = delta.norm() / maxAcceleration / dt;
- factor = std::max(factor, 1.f);
+ Eigen::VectorXf delta = target - state;
- state += delta / factor;
- // ARMARX_IMPORTANT << "JointRamp state " << state;
- return state;
+ float factor = delta.norm() / maxAcceleration / dt;
+ factor = std::max(factor, 1.f);
-}
+ state += delta / factor;
+ // ARMARX_IMPORTANT << "JointRamp state " << state;
+ return state;
-void JointVelocityRamp::setMaxAccelaration(float maxAcceleration)
-{
- this->maxAcceleration = maxAcceleration;
+ }
-}
+ void JointVelocityRamp::setMaxAccelaration(float maxAcceleration)
+ {
+ this->maxAcceleration = maxAcceleration;
-float JointVelocityRamp::getMaxAccelaration()
-{
- return maxAcceleration;
-}
+ }
+ float JointVelocityRamp::getMaxAccelaration()
+ {
+ return maxAcceleration;
+ }
+}
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index 414309200..c0cceefce 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -37,22 +37,20 @@
#include <sstream>
-
-namespace armarx
+namespace armarx::VariantType
{
- namespace VariantType
+ // variant types
+ const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
+ const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
+ inline void suppressWarningUnusedVariableForLinkedPoseAndDirection()
{
- // variant types
- const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
- const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
- inline void suppressWarningUnusedVariableForLinkedPoseAndDirection()
- {
- ARMARX_DEBUG_S << VAROUT(LinkedPose);
- ARMARX_DEBUG_S << VAROUT(LinkedDirection);
- }
+ ARMARX_DEBUG_S << VAROUT(LinkedPose);
+ ARMARX_DEBUG_S << VAROUT(LinkedDirection);
}
+}
-
+namespace armarx
+{
class LinkedPose;
using LinkedPosePtr = IceInternal::Handle<LinkedPose>;
diff --git a/source/RobotAPI/libraries/core/OrientedPoint.h b/source/RobotAPI/libraries/core/OrientedPoint.h
index 4bb8955e8..a347e4954 100644
--- a/source/RobotAPI/libraries/core/OrientedPoint.h
+++ b/source/RobotAPI/libraries/core/OrientedPoint.h
@@ -30,14 +30,14 @@
#include <sstream>
-namespace armarx
+namespace armarx::VariantType
{
- namespace VariantType
- {
- // variant types
- const VariantTypeId OrientedPoint = Variant::addTypeName("::armarx::OrientedPointBase");
- }
+ // variant types
+ const VariantTypeId OrientedPoint = Variant::addTypeName("::armarx::OrientedPointBase");
+}
+namespace armarx
+{
class OrientedPoint:
public virtual OrientedPointBase
{
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 5a4700dc5..684a62747 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -30,157 +30,157 @@
#include <memory>
-using namespace armarx;
-
-
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
- Kp(Kp),
- Ki(Ki),
- Kd(Kd),
- integral(0),
- derivative(0),
- previousError(0),
- processValue(0),
- target(0),
- controlValue(0),
- controlValueDerivation(0),
- maxControlValue(maxControlValue),
- maxDerivation(maxDerivation),
- limitless(limitless),
- threadSafe(threadSafe)
+namespace armarx
{
- reset();
-}
-
-void PIDController::reset()
-{
- ScopedRecursiveLockPtr lock = getLock();
- firstRun = true;
- previousError = 0;
- integral = 0;
- lastUpdateTime = TimeUtil::GetTime();
- if (pdOutputFilter)
- {
- pdOutputFilter->reset();
- }
- if (differentialFilter)
+ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
+ Kp(Kp),
+ Ki(Ki),
+ Kd(Kd),
+ integral(0),
+ derivative(0),
+ previousError(0),
+ processValue(0),
+ target(0),
+ controlValue(0),
+ controlValueDerivation(0),
+ maxControlValue(maxControlValue),
+ maxDerivation(maxDerivation),
+ limitless(limitless),
+ threadSafe(threadSafe)
{
- differentialFilter->reset();
+ reset();
}
-}
-ScopedRecursiveLockPtr PIDController::getLock() const
-{
- if (threadSafe)
- {
- return std::make_shared<ScopedRecursiveLock>(mutex);
- }
- else
+ void PIDController::reset()
{
- return ScopedRecursiveLockPtr();
+ ScopedRecursiveLockPtr lock = getLock();
+ firstRun = true;
+ previousError = 0;
+ integral = 0;
+ lastUpdateTime = TimeUtil::GetTime();
+ if (pdOutputFilter)
+ {
+ pdOutputFilter->reset();
+ }
+ if (differentialFilter)
+ {
+ differentialFilter->reset();
+ }
}
-}
-void PIDController::update(double measuredValue, double targetValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- IceUtil::Time now = TimeUtil::GetTime();
-
- if (firstRun)
+ ScopedRecursiveLockPtr PIDController::getLock() const
{
- lastUpdateTime = TimeUtil::GetTime();
+ if (threadSafe)
+ {
+ return std::make_shared<ScopedRecursiveLock>(mutex);
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
}
- double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt, measuredValue, targetValue);
- lastUpdateTime = now;
-}
-
+ void PIDController::update(double measuredValue, double targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ IceUtil::Time now = TimeUtil::GetTime();
-void PIDController::update(double measuredValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- update(measuredValue, target);
-}
+ if (firstRun)
+ {
+ lastUpdateTime = TimeUtil::GetTime();
+ }
-void PIDController::setTarget(double newTarget)
-{
- ScopedRecursiveLockPtr lock = getLock();
- target = newTarget;
-}
+ double dt = (now - lastUpdateTime).toSecondsDouble();
+ update(dt, measuredValue, targetValue);
+ lastUpdateTime = now;
+ }
-void PIDController::setMaxControlValue(double newMax)
-{
- ScopedRecursiveLockPtr lock = getLock();
- maxControlValue = newMax;
-}
-void PIDController::update(double deltaSec, double measuredValue, double targetValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- processValue = measuredValue;
- target = targetValue;
+ void PIDController::update(double measuredValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ update(measuredValue, target);
+ }
+ void PIDController::setTarget(double newTarget)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ target = newTarget;
+ }
- double error = target - processValue;
- //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
- if (limitless)
+ void PIDController::setMaxControlValue(double newMax)
{
- //ARMARX_INFO << VAROUT(error);
- error = math::MathUtils::angleModPI(error);
- //ARMARX_INFO << VAROUT(error);
- //error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
- //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
+ ScopedRecursiveLockPtr lock = getLock();
+ maxControlValue = newMax;
}
- //double dt = (now - lastUpdateTime).toSecondsDouble();
- // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if (!firstRun)
+ void PIDController::update(double deltaSec, double measuredValue, double targetValue)
{
- if (std::abs(error) < conditionalIntegralErrorTreshold)
+ ScopedRecursiveLockPtr lock = getLock();
+ processValue = measuredValue;
+ target = targetValue;
+
+
+ double error = target - processValue;
+ //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
+ if (limitless)
{
- integral += error * deltaSec;
- integral = math::MathUtils::LimitTo(integral, maxIntegral);
+ //ARMARX_INFO << VAROUT(error);
+ error = math::MathUtils::angleModPI(error);
+ //ARMARX_INFO << VAROUT(error);
+ //error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
+ //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
}
- if (deltaSec > 0.0)
+
+ //double dt = (now - lastUpdateTime).toSecondsDouble();
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
{
- derivative = (error - previousError) / deltaSec;
- if (differentialFilter)
+ if (std::abs(error) < conditionalIntegralErrorTreshold)
{
- derivative = differentialFilter->update(deltaSec, derivative);
+ integral += error * deltaSec;
+ integral = math::MathUtils::LimitTo(integral, maxIntegral);
+ }
+ if (deltaSec > 0.0)
+ {
+ derivative = (error - previousError) / deltaSec;
+ if (differentialFilter)
+ {
+ derivative = differentialFilter->update(deltaSec, derivative);
+ }
}
}
- }
- firstRun = false;
- double oldControlValue = controlValue;
- double pdControlValue = Kp * error + Kd * derivative;
- if (pdOutputFilter)
- {
- pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue);
- }
- controlValue = pdControlValue + Ki * integral;
- if (deltaSec > 0.0)
- {
- double deriv = (controlValue - oldControlValue) / deltaSec;
- if (fabs(deriv) > maxDerivation)
+ firstRun = false;
+ double oldControlValue = controlValue;
+ double pdControlValue = Kp * error + Kd * derivative;
+ if (pdOutputFilter)
{
- controlValueDerivation = maxDerivation * deltaSec * math::MathUtils::Sign(deriv);
- controlValue = oldControlValue + controlValueDerivation;
+ pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue);
}
- }
- controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+ controlValue = pdControlValue + Ki * integral;
+ if (deltaSec > 0.0)
+ {
+ double deriv = (controlValue - oldControlValue) / deltaSec;
+ if (fabs(deriv) > maxDerivation)
+ {
+ controlValueDerivation = maxDerivation * deltaSec * math::MathUtils::Sign(deriv);
+ controlValue = oldControlValue + controlValueDerivation;
+ }
+ }
+ controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
- previousError = error;
- lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);
+ previousError = error;
+ lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);
-}
+ }
-double PIDController::getControlValue() const
-{
- ScopedRecursiveLockPtr lock = getLock();
- return controlValue;
+ double PIDController::getControlValue() const
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ return controlValue;
+ }
}
@@ -196,4 +196,3 @@ double PIDController::getControlValue() const
-
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 400b2839c..7ec5b2f98 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -31,32 +31,20 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-using namespace Eigen;
-
template class ::IceInternal::Handle<::armarx::Pose>;
template class ::IceInternal::Handle<::armarx::Vector2>;
template class ::IceInternal::Handle<::armarx::Vector3>;
template class ::IceInternal::Handle<::armarx::Quaternion>;
-namespace Eigen
-{
- template class Matrix<float, 3, 1>;
- template class Matrix<float, 3, 3>;
- template class Matrix<float, 4, 4>;
-}
-
-
namespace armarx
{
-
Vector2::Vector2()
{
x = 0;
y = 0;
}
- Vector2::Vector2(const Vector2f& v)
+ Vector2::Vector2(const Eigen::Vector2f& v)
{
x = v(0);
y = v(1);
@@ -68,9 +56,9 @@ namespace armarx
this->y = y;
}
- Vector2f Vector2::toEigen() const
+ Eigen::Vector2f Vector2::toEigen() const
{
- Vector2f v;
+ Eigen::Vector2f v;
v << this->x, this->y;
return v;
}
@@ -111,14 +99,14 @@ namespace armarx
z = 0;
}
- Vector3::Vector3(const Vector3f& v)
+ Vector3::Vector3(const Eigen::Vector3f& v)
{
x = v(0);
y = v(1);
z = v(2);
}
- Vector3::Vector3(const Matrix4f& m)
+ Vector3::Vector3(const Eigen::Matrix4f& m)
{
x = m(0, 3);
y = m(1, 3);
@@ -132,9 +120,9 @@ namespace armarx
this->z = z;
}
- Vector3f Vector3::toEigen() const
+ Eigen::Vector3f Vector3::toEigen() const
{
- Vector3f v;
+ Eigen::Vector3f v;
v << this->x, this->y, this->z;
return v;
}
@@ -175,13 +163,13 @@ namespace armarx
Quaternion::Quaternion()
= default;
- Quaternion::Quaternion(const Matrix4f& m4)
+ Quaternion::Quaternion(const Eigen::Matrix4f& m4)
{
- Matrix3f m3 = m4.block<3, 3>(0, 0);
+ Eigen::Matrix3f m3 = m4.block<3, 3>(0, 0);
this->init(m3);
}
- Quaternion::Quaternion(const Matrix3f& m3)
+ Quaternion::Quaternion(const Eigen::Matrix3f& m3)
{
this->init(m3);
}
@@ -199,19 +187,19 @@ namespace armarx
this->qz = qz;
}
- Matrix3f Quaternion::toEigen() const
+ Eigen::Matrix3f Quaternion::toEigen() const
{
return toEigenQuaternion().toRotationMatrix();
}
Eigen::Quaternionf Quaternion::toEigenQuaternion() const
{
- return Quaternionf(this->qw, this->qx, this->qy, this->qz);
+ return {this->qw, this->qx, this->qy, this->qz};
}
- void Quaternion::init(const Matrix3f& m3)
+ void Quaternion::init(const Eigen::Matrix3f& m3)
{
- Quaternionf q;
+ Eigen::Quaternionf q;
q = m3;
init(q);
}
@@ -224,15 +212,15 @@ namespace armarx
this->qz = q.z();
}
- Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
+ Eigen::Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
{
return Quaternion::slerp(alpha, this->toEigen(), m);
}
- Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
+ Eigen::Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
{
- Matrix3f result;
- Quaternionf q1, q2;
+ Eigen::Matrix3f result;
+ Eigen::Quaternionf q1, q2;
q1 = m1;
q2 = m2;
result = q1.slerp(alpha, q2);
@@ -296,23 +284,23 @@ namespace armarx
init();
}
- Pose::Pose(const Matrix4f& m)
+ Pose::Pose(const Eigen::Matrix4f& m)
{
this->orientation = new Quaternion(m);
this->position = new Vector3(m);
init();
}
- void Pose::operator=(const Matrix4f& matrix)
+ void Pose::operator=(const Eigen::Matrix4f& matrix)
{
this->orientation = new Quaternion(matrix);
this->position = new Vector3(matrix);
init();
}
- Matrix4f Pose::toEigen() const
+ Eigen::Matrix4f Pose::toEigen() const
{
- Matrix4f m = Matrix4f::Identity();
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
ARMARX_CHECK_EXPRESSION(c_orientation);
ARMARX_CHECK_EXPRESSION(c_position);
m.block<3, 3>(0, 0) = c_orientation->toEigen();
@@ -348,11 +336,8 @@ namespace armarx
this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
}
-
void Pose::ice_postUnmarshal()
{
init();
}
-
-
}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index acd23ad09..4c9669d0c 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -34,21 +34,19 @@
#include <sstream>
+namespace armarx::VariantType
+{
+ // variant types
+ const VariantTypeId Vector2 = Variant::addTypeName("::armarx::Vector2Base");
+ const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
+ const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
+ const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
+}
namespace armarx
{
- namespace VariantType
- {
- // variant types
- const VariantTypeId Vector2 = Variant::addTypeName("::armarx::Vector2Base");
- const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
- const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
- const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
- }
-
const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
-
/**
* @brief The Vector2 class
* @ingroup VariantsGrp
@@ -278,10 +276,3 @@ extern template class ::IceInternal::Handle< ::armarx::Pose>;
extern template class ::IceInternal::Handle< ::armarx::Vector2>;
extern template class ::IceInternal::Handle< ::armarx::Vector3>;
extern template class ::IceInternal::Handle< ::armarx::Quaternion>;
-
-namespace Eigen
-{
- extern template class Matrix<float, 3, 1>;
- extern template class Matrix<float, 3, 3>;
- extern template class Matrix<float, 4, 4>;
-}
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index 17ca6912b..4bdf585bb 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -24,23 +24,17 @@
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-
-namespace armarx
+namespace armarx::ObjectFactories
{
- namespace ObjectFactories
- {
-
- /**
- * @class RobotAPIObjectFactories
- */
- class RobotAPIObjectFactories : public FactoryCollectionBase
- {
- public:
- ObjectFactoryMap getFactories() override;
- };
- const FactoryCollectionBaseCleanUp RobotAPIObjectFactoriesVar = FactoryCollectionBase::addToPreregistration(new RobotAPIObjectFactories());
- }
+ /**
+ * @class RobotAPIObjectFactories
+ */
+ class RobotAPIObjectFactories : public FactoryCollectionBase
+ {
+ public:
+ ObjectFactoryMap getFactories() override;
+ };
+ const FactoryCollectionBaseCleanUp RobotAPIObjectFactoriesVar = FactoryCollectionBase::addToPreregistration(new RobotAPIObjectFactories());
}
-
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
index 27a2f4ed2..04139e259 100644
--- a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
@@ -25,77 +25,76 @@
#include "CartesianVelocityController.h"
#include "SimpleDiffIK.h"
-using namespace armarx;
+namespace armarx
+{
+ SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
+ {
+ tcp = tcp ? tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+ Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
-SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
-{
- tcp = tcp ? tcp : rns->getTCP();
- CartesianVelocityController velocityController(rns);
- CartesianPositionController positionController(tcp);
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
- Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
- for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
- {
- Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
- Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
- //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
- Eigen::VectorXf cartesialVel(6);
- cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+ Eigen::VectorXf newJointValues = currentJointValues + jv;
+ rns->setJointValues(newJointValues);
+ currentJointValues = newJointValues;
+ }
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posDiff = positionController.getPositionDiff(targetPose);
+ result.oriDiff = positionController.getOrientationDiff(targetPose);
+ result.posError = positionController.getPositionError(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
- float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
- jv = jv * stepLength;
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
- Eigen::VectorXf newJointValues = currentJointValues + jv;
- rns->setJointValues(newJointValues);
- currentJointValues = newJointValues;
+ return result;
}
- Result result;
- result.jointValues = rns->getJointValuesEigen();
- result.posDiff = positionController.getPositionDiff(targetPose);
- result.oriDiff = positionController.getOrientationDiff(targetPose);
- result.posError = positionController.getPositionError(targetPose);
- result.oriError = positionController.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
-
- result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
- result.minimumJointLimitMargin = FLT_MAX;
- for (size_t i = 0; i < rns->getSize(); i++)
+ SimpleDiffIK::Reachability SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
{
- VirtualRobot::RobotNodePtr rn = rns->getNode(i);
- if (rn->isLimitless())
+ Reachability r;
+ rns->setJointValues(initialJV);
+ for (const Eigen::Matrix4f& target : targets)
{
- result.jointLimitMargins(i) = M_PI;
+ Result res = CalculateDiffIK(target, rns, tcp, params);
+ r.aggregate(res);
}
- else
+ if (!r.reachable)
{
- result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
- result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ r.minimumJointLimitMargin = -1;
}
+ return r;
}
-
- return result;
-}
-
-SimpleDiffIK::Reachability SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
-{
- Reachability r;
- rns->setJointValues(initialJV);
- for (const Eigen::Matrix4f& target : targets)
- {
- Result res = CalculateDiffIK(target, rns, tcp, params);
- r.aggregate(res);
- }
- if (!r.reachable)
- {
- r.minimumJointLimitMargin = -1;
- }
- return r;
}
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
index c13ea798a..c8b8ec74e 100644
--- a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
@@ -1,44 +1,44 @@
#include "CartesianVelocityController.h"
#include "SimpleGridReachability.h"
-using namespace armarx;
-
-
-SimpleGridReachability::Result SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
+namespace armarx
{
- VirtualRobot::RobotNodePtr rns = params.nodeSet;
- VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
- CartesianVelocityController velocityController(rns);
- CartesianPositionController positionController(tcp);
-
- Eigen::VectorXf initialJV = rns->getJointValuesEigen();
- for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ SimpleGridReachability::Result SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
{
- Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
- Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+ VirtualRobot::RobotNodePtr rns = params.nodeSet;
+ VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+
+ Eigen::VectorXf initialJV = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
- //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
- Eigen::VectorXf cartesialVel(6);
- cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
- float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
- jv = jv * stepLength;
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
- for (size_t n = 0; n < rns->getSize(); n++)
- {
- rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ }
}
- }
- Result result;
- result.jointValues = rns->getJointValuesEigen();
- result.posError = positionController.getPositionDiff(targetPose);
- result.oriError = positionController.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posError = positionController.getPositionDiff(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
- return result;
+ return result;
+ }
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 7f6a00fbb..cea227464 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -30,6 +30,7 @@
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
#include <memory>
+#include <cmath>
namespace armarx
{
@@ -1568,16 +1569,6 @@ namespace armarx
CopyData(filteredTraj, *this);
}
-
-
-
-
-
- // gauss macro
-#define g(x) exp(-double((x)*(x))/(2*sigma*sigma)) / ( sigma * sqrt(2*3.14159265))
-#define round(x) int(0.5+x)
-
-
double
Trajectory::__gaussianFilter(double filterRadiusInTime, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
{
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index f4e02ebe1..318c66bef 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -42,16 +42,14 @@
#include <RobotAPI/interface/core/Trajectory.h>
-namespace armarx
+namespace armarx::VariantType
{
+ // Variant types
+ const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
+}
- namespace VariantType
- {
- // Variant types
- const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
- }
-
-
+namespace armarx
+{
using DoubleSeqPtr = std::shared_ptr<Ice::DoubleSeq>;
class Trajectory;
diff --git a/source/RobotAPI/libraries/core/math/ColorUtils.h b/source/RobotAPI/libraries/core/math/ColorUtils.h
index 6bbb60e30..ab333c362 100644
--- a/source/RobotAPI/libraries/core/math/ColorUtils.h
+++ b/source/RobotAPI/libraries/core/math/ColorUtils.h
@@ -25,160 +25,156 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include "MathUtils.h"
-namespace armarx
+namespace armarx::colorutils
{
- namespace colorutils
- {
- DrawColor24Bit HsvToRgb(const HsvColor& in)
+ DrawColor24Bit HsvToRgb(const HsvColor& in)
+ {
+ double hh, p, q, t, ff;
+ long i;
+ double r, g, b;
+ if (in.s <= 0.0) // < is bogus, just shuts up warnings
+ {
+ r = in.v;
+ g = in.v;
+ b = in.v;
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
+ }
+ hh = in.h;
+ if (hh >= 360.0)
{
- double hh, p, q, t, ff;
- long i;
- double r, g, b;
- if (in.s <= 0.0) // < is bogus, just shuts up warnings
- {
+ hh = 0.0;
+ }
+ hh /= 60.0;
+ i = (long)hh;
+ ff = hh - i;
+ p = in.v * (1.0 - in.s);
+ q = in.v * (1.0 - (in.s * ff));
+ t = in.v * (1.0 - (in.s * (1.0 - ff)));
+
+ switch (i)
+ {
+ case 0:
r = in.v;
+ g = t;
+ b = p;
+ break;
+ case 1:
+ r = q;
g = in.v;
- b = in.v;
- return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
- }
- hh = in.h;
- if (hh >= 360.0)
- {
- hh = 0.0;
- }
- hh /= 60.0;
- i = (long)hh;
- ff = hh - i;
- p = in.v * (1.0 - in.s);
- q = in.v * (1.0 - (in.s * ff));
- t = in.v * (1.0 - (in.s * (1.0 - ff)));
-
- switch (i)
- {
- case 0:
- r = in.v;
- g = t;
- b = p;
- break;
- case 1:
- r = q;
- g = in.v;
- b = p;
- break;
- case 2:
- r = p;
- g = in.v;
- b = t;
- break;
-
- case 3:
- r = p;
- g = q;
- b = in.v;
- break;
- case 4:
- r = t;
- g = p;
- b = in.v;
- break;
- case 5:
- default:
- r = in.v;
- g = p;
- b = q;
- break;
- }
+ b = p;
+ break;
+ case 2:
+ r = p;
+ g = in.v;
+ b = t;
+ break;
- return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
+ case 3:
+ r = p;
+ g = q;
+ b = in.v;
+ break;
+ case 4:
+ r = t;
+ g = p;
+ b = in.v;
+ break;
+ case 5:
+ default:
+ r = in.v;
+ g = p;
+ b = q;
+ break;
}
- HsvColor RgbToHsv(const DrawColor24Bit& in)
- {
- double r = 0.00392156862 * in.r;
- double g = 0.00392156862 * in.g;
- double b = 0.00392156862 * in.b;
- HsvColor out;
- double min, max, delta;
-
- min = r < g ? r : g;
- min = min < b ? min : b;
-
- max = r > g ? r : g;
- max = max > b ? max : b;
-
- out.v = max; // v
- delta = max - min;
- if (delta < 0.00001)
- {
- out.s = 0;
- out.h = 0; // undefined, maybe nan?
- return out;
- }
- if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
- {
- out.s = (delta / max); // s
- }
- else
- {
- // if max is 0, then r = g = b = 0
- // s = 0, h is undefined
- out.s = 0.0;
- out.h = 0; // its now undefined
- return out;
- }
- if (r >= max) // > is bogus, just keeps compilor happy
- {
- out.h = (g - b) / delta; // between yellow & magenta
- }
- else if (g >= max)
- {
- out.h = 2.0 + (b - r) / delta; // between cyan & yellow
- }
- else
- {
- out.h = 4.0 + (r - g) / delta; // between magenta & cyan
- }
-
- out.h *= 60.0; // degrees
-
- if (out.h < 0.0)
- {
- out.h += 360.0;
- }
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
+ }
- return out;
+ HsvColor RgbToHsv(const DrawColor24Bit& in)
+ {
+ double r = 0.00392156862 * in.r;
+ double g = 0.00392156862 * in.g;
+ double b = 0.00392156862 * in.b;
+ HsvColor out;
+ double min, max, delta;
+ min = r < g ? r : g;
+ min = min < b ? min : b;
- }
+ max = r > g ? r : g;
+ max = max > b ? max : b;
- /**
- * @brief HeatMapColor calculates the color of a value between 0 and 1 on a heat map.
- * @param percentage value between 0..1
- * @return color on a heatmap corresponding to parameter. 0 -> blue, 1 -> red. Color has full (255) saturation and value.
- */
- HsvColor HeatMapColor(float percentage)
+ out.v = max; // v
+ delta = max - min;
+ if (delta < 0.00001)
{
- percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
-
- return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
+ out.s = 0;
+ out.h = 0; // undefined, maybe nan?
+ return out;
}
-
- DrawColor24Bit HeatMapRGBColor(float percentage)
+ if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
{
- return HsvToRgb(HeatMapColor(percentage));
+ out.s = (delta / max); // s
}
+ else
+ {
+ // if max is 0, then r = g = b = 0
+ // s = 0, h is undefined
+ out.s = 0.0;
+ out.h = 0; // its now undefined
+ return out;
+ }
+ if (r >= max) // > is bogus, just keeps compilor happy
+ {
+ out.h = (g - b) / delta; // between yellow & magenta
+ }
+ else if (g >= max)
+ {
+ out.h = 2.0 + (b - r) / delta; // between cyan & yellow
+ }
+ else
+ {
+ out.h = 4.0 + (r - g) / delta; // between magenta & cyan
+ }
+
+ out.h *= 60.0; // degrees
- DrawColor HeatMapRGBAColor(float percentage)
+ if (out.h < 0.0)
{
- auto color = HsvToRgb(HeatMapColor(percentage));
- return DrawColor {0.0039215686f * color.r, //divide by 255
- 0.0039215686f * color.g,
- 0.0039215686f * color.b,
- 1.0
- };
+ out.h += 360.0;
}
+ return out;
+
+
+ }
+
+ /**
+ * @brief HeatMapColor calculates the color of a value between 0 and 1 on a heat map.
+ * @param percentage value between 0..1
+ * @return color on a heatmap corresponding to parameter. 0 -> blue, 1 -> red. Color has full (255) saturation and value.
+ */
+ HsvColor HeatMapColor(float percentage)
+ {
+ percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
+
+ return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
+ }
+
+ DrawColor24Bit HeatMapRGBColor(float percentage)
+ {
+ return HsvToRgb(HeatMapColor(percentage));
+ }
+
+ DrawColor HeatMapRGBAColor(float percentage)
+ {
+ auto color = HsvToRgb(HeatMapColor(percentage));
+ return DrawColor {0.0039215686f * color.r, //divide by 255
+ 0.0039215686f * color.g,
+ 0.0039215686f * color.b,
+ 1.0
+ };
}
}
diff --git a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
index 81a649e7a..f50d96328 100644
--- a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
+++ b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
@@ -26,29 +26,25 @@
#include <vector>
#include <memory>
-namespace armarx
+namespace armarx::math
{
- namespace math
- {
- class LinearizeAngularTrajectory;
- using LinearizedAngularTrajectoryPtr = std::shared_ptr<LinearizeAngularTrajectory>;
+ class LinearizeAngularTrajectory;
+ using LinearizedAngularTrajectoryPtr = std::shared_ptr<LinearizeAngularTrajectory>;
- class LinearizeAngularTrajectory
- {
- public:
- LinearizeAngularTrajectory(float initialLinearValue);
+ class LinearizeAngularTrajectory
+ {
+ public:
+ LinearizeAngularTrajectory(float initialLinearValue);
- float update(float angle);
- float getLinearValue();
+ float update(float angle);
+ float getLinearValue();
- static std::vector<float> Linearize(const std::vector<float>& data);
- static void LinearizeRef(std::vector<float>& data);
- static std::vector<float> Angularize(const std::vector<float>& data, float center = 0);
- static void AngularizeRef(std::vector<float>& data, float center = 0);
+ static std::vector<float> Linearize(const std::vector<float>& data);
+ static void LinearizeRef(std::vector<float>& data);
+ static std::vector<float> Angularize(const std::vector<float>& data, float center = 0);
+ static void AngularizeRef(std::vector<float>& data, float center = 0);
- private:
- float linearValue;
- };
- }
+ private:
+ float linearValue;
+ };
}
-
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index 0607aa0d9..ce3299509 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -26,156 +26,152 @@
#include <Eigen/Eigen>
#include <vector>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class MathUtils
{
- class MathUtils
+ public:
+ static int Sign(double value)
{
- public:
- static int Sign(double value)
- {
- return value >= 0 ? 1 : -1;
- }
+ return value >= 0 ? 1 : -1;
+ }
- static int LimitMinMax(int min, int max, int value)
- {
- return value < min ? min : (value > max ? max : value);
- }
- static float LimitMinMax(float min, float max, float value)
- {
- return value < min ? min : (value > max ? max : value);
- }
- static double LimitMinMax(double min, double max, double value)
- {
- return value < min ? min : (value > max ? max : value);
- }
- static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
- {
- return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
- }
-
- static double LimitTo(double value, double absThreshold)
- {
- return LimitMinMax(-absThreshold, absThreshold, value);
- //int sign = (value >= 0) ? 1 : -1;
- //return sign * std::min<double>(fabs(value), absThreshold);
- }
+ static int LimitMinMax(int min, int max, int value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static float LimitMinMax(float min, float max, float value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static double LimitMinMax(double min, double max, double value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
+ {
+ return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
+ }
- static Eigen::Vector3f LimitTo(const Eigen::Vector3f& val, float maxNorm)
- {
- float norm = val.norm();
- if (norm > maxNorm)
- {
- return val / norm * maxNorm;
- }
- return val;
- }
+ static double LimitTo(double value, double absThreshold)
+ {
+ return LimitMinMax(-absThreshold, absThreshold, value);
+ //int sign = (value >= 0) ? 1 : -1;
+ //return sign * std::min<double>(fabs(value), absThreshold);
+ }
- static bool CheckMinMax(int min, int max, int value)
- {
- return value >= min && value <= max;
- }
- static bool CheckMinMax(float min, float max, float value)
- {
- return value >= min && value <= max;
- }
- static bool CheckMinMax(double min, double max, double value)
- {
- return value >= min && value <= max;
- }
- static bool CheckMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
+ static Eigen::Vector3f LimitTo(const Eigen::Vector3f& val, float maxNorm)
+ {
+ float norm = val.norm();
+ if (norm > maxNorm)
{
- return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
+ return val / norm * maxNorm;
}
+ return val;
+ }
- static std::vector<float> VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
- {
- std::vector<float> result;
+ static bool CheckMinMax(int min, int max, int value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(float min, float max, float value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(double min, double max, double value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
+ {
+ return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
+ }
- for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
- {
- result.push_back(v1.at(i) - v2.at(i));
- }
+ static std::vector<float> VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
+ {
+ std::vector<float> result;
- return result;
- }
- static std::vector<float> VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
+ for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
{
- std::vector<float> result;
-
- for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
- {
- result.push_back(std::fabs(v1.at(i) - v2.at(i)));
- }
-
- return result;
+ result.push_back(v1.at(i) - v2.at(i));
}
- static float VectorMax(const std::vector<float>& vec)
+ return result;
+ }
+ static std::vector<float> VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
+ {
+ std::vector<float> result;
+
+ for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
{
- float max = vec.at(0);
+ result.push_back(std::fabs(v1.at(i) - v2.at(i)));
+ }
- for (size_t i = 1; i < vec.size(); i++)
- {
- max = std::max(max, vec.at(i));
- }
+ return result;
+ }
- return max;
- }
+ static float VectorMax(const std::vector<float>& vec)
+ {
+ float max = vec.at(0);
- static float fmod(float value, float boundLow, float boundHigh)
+ for (size_t i = 1; i < vec.size(); i++)
{
- value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
- if (value < boundLow)
- {
- value += boundHigh - boundLow;
- }
- return value;
+ max = std::max(max, vec.at(i));
}
- static float angleMod2PI(float value)
- {
- return fmod(value, 0, 2 * M_PI);
- }
+ return max;
+ }
- static float angleModPI(float value)
+ static float fmod(float value, float boundLow, float boundHigh)
+ {
+ value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+ if (value < boundLow)
{
- return angleMod2PI(value + M_PI) - M_PI;
+ value += boundHigh - boundLow;
}
+ return value;
+ }
- static float angleModX(float value, float center)
- {
- return angleMod2PI(value + M_PI - center) - M_PI + center;
- }
+ static float angleMod2PI(float value)
+ {
+ return fmod(value, 0, 2 * M_PI);
+ }
- static float Lerp(float a, float b, float f)
- {
- return a * (1 - f) + b * f;
- }
+ static float angleModPI(float value)
+ {
+ return angleMod2PI(value + M_PI) - M_PI;
+ }
- static float LerpClamp(float a, float b, float f)
- {
- return Lerp(a, b, LimitMinMax(0.f, 1.f, f));
- }
+ static float angleModX(float value, float center)
+ {
+ return angleMod2PI(value + M_PI - center) - M_PI + center;
+ }
- static float ILerp(float a, float b, float f)
- {
- return (f - a) / (b - a);
- }
+ static float Lerp(float a, float b, float f)
+ {
+ return a * (1 - f) + b * f;
+ }
- static float AngleLerp(float a, float b, float f)
- {
- b = fmod(b, a - M_PI, a + M_PI);
- return Lerp(a, b, f);
- }
+ static float LerpClamp(float a, float b, float f)
+ {
+ return Lerp(a, b, LimitMinMax(0.f, 1.f, f));
+ }
- static float AngleDelta(float angle1, float angle2)
- {
- return angleModPI(angle2 - angle1);
- }
+ static float ILerp(float a, float b, float f)
+ {
+ return (f - a) / (b - a);
+ }
- };
- }
-}
+ static float AngleLerp(float a, float b, float f)
+ {
+ b = fmod(b, a - M_PI, a + M_PI);
+ return Lerp(a, b, f);
+ }
+
+ static float AngleDelta(float angle1, float angle2)
+ {
+ return angleModPI(angle2 - angle1);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/core/math/MatrixHelpers.h b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
index 6a2d1c086..80b8fba70 100644
--- a/source/RobotAPI/libraries/core/math/MatrixHelpers.h
+++ b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
@@ -25,46 +25,44 @@
#include <math.h>
#include <Eigen/Eigen>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class MatrixHelpers
{
- class MatrixHelpers
+ public:
+ static void SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
{
- public:
- static void SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
+ for (int i = 0; i < matrix.cols(); i++)
{
- for (int i = 0; i < matrix.cols(); i++)
- {
- matrix(rowNr, i) = value;
- }
+ matrix(rowNr, i) = value;
}
+ }
- static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf& points)
- {
- Eigen::Vector3f sum(0, 0, 0);
-
- for (int i = 0; i < points.cols(); i++)
- {
- sum += points.block<3, 1>(0, i);
- }
+ static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf& points)
+ {
+ Eigen::Vector3f sum(0, 0, 0);
- return sum / points.cols();
+ for (int i = 0; i < points.cols(); i++)
+ {
+ sum += points.block<3, 1>(0, i);
}
- static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
- {
- Eigen::MatrixXf matrix(3, points.cols());
+ return sum / points.cols();
+ }
- for (int i = 0; i < points.cols(); i++)
- {
- matrix.block<3, 1>(0, i) = points.block<3, 1>(0, i) - vec;
- }
+ static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
+ {
+ Eigen::MatrixXf matrix(3, points.cols());
- return matrix;
+ for (int i = 0; i < points.cols(); i++)
+ {
+ matrix.block<3, 1>(0, i) = points.block<3, 1>(0, i) - vec;
}
- };
- }
+ return matrix;
+ }
+
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/math/SVD.h b/source/RobotAPI/libraries/core/math/SVD.h
index 54262287b..4160bc058 100644
--- a/source/RobotAPI/libraries/core/math/SVD.h
+++ b/source/RobotAPI/libraries/core/math/SVD.h
@@ -25,36 +25,34 @@
#include <math.h>
#include <Eigen/Eigen>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class SVD
{
- class SVD
+ private:
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd;
+ public:
+ Eigen::MatrixXf matrixU;
+ Eigen::MatrixXf matrixV;
+ Eigen::VectorXf singularValues;
+ SVD(Eigen::MatrixXf matrix)
+ : svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV)
{
- private:
- Eigen::JacobiSVD<Eigen::MatrixXf> svd;
- public:
- Eigen::MatrixXf matrixU;
- Eigen::MatrixXf matrixV;
- Eigen::VectorXf singularValues;
- SVD(Eigen::MatrixXf matrix)
- : svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV)
- {
- matrixU = svd.matrixU();
- matrixV = svd.matrixV();
- singularValues = svd.singularValues();
- }
+ matrixU = svd.matrixU();
+ matrixV = svd.matrixV();
+ singularValues = svd.singularValues();
+ }
- Eigen::Vector3f getLeftSingularVector3D(int nr)
- {
- return matrixU.block<3, 1>(0, nr);
- }
- float getLeftSingularValue(int nr)
- {
- return singularValues(nr);
- }
+ Eigen::Vector3f getLeftSingularVector3D(int nr)
+ {
+ return matrixU.block<3, 1>(0, nr);
+ }
+ float getLeftSingularValue(int nr)
+ {
+ return singularValues(nr);
+ }
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
index ca2ad0feb..17f9c0a70 100644
--- a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
+++ b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
@@ -29,69 +29,67 @@
#include <ArmarXCore/core/exceptions/Exception.h>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class SlidingWindowVectorMedian;
+ using SlidingWindowVectorMedianPtr = std::shared_ptr<SlidingWindowVectorMedian>;
+
+ class SlidingWindowVectorMedian
{
- class SlidingWindowVectorMedian;
- using SlidingWindowVectorMedianPtr = std::shared_ptr<SlidingWindowVectorMedian>;
+ private:
+ size_t windowSize;
+ size_t vectorSize;
+ std::vector<float> data;
+ size_t currentIndex;
+ bool fullCycle;
+
+ public:
+ SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
+ : windowSize(windowSize),
+ vectorSize(vectorSize),
+ data(vectorSize * windowSize, 0), // initialize all data to 0
+ currentIndex(0),
+ fullCycle(false)
+ {
+ }
- class SlidingWindowVectorMedian
+ void addEntry(const std::vector<float>& entry)
{
- private:
- size_t windowSize;
- size_t vectorSize;
- std::vector<float> data;
- size_t currentIndex;
- bool fullCycle;
-
- public:
- SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
- : windowSize(windowSize),
- vectorSize(vectorSize),
- data(vectorSize * windowSize, 0), // initialize all data to 0
- currentIndex(0),
- fullCycle(false)
+ if (entry.size() != vectorSize)
{
+ throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
}
- void addEntry(const std::vector<float>& entry)
+ for (size_t i = 0; i < entry.size(); i++)
{
- if (entry.size() != vectorSize)
- {
- throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
- }
+ data.at(i + currentIndex * vectorSize) = entry.at(i);
+ }
- for (size_t i = 0; i < entry.size(); i++)
- {
- data.at(i + currentIndex * vectorSize) = entry.at(i);
- }
+ currentIndex = (currentIndex + 1) % windowSize;
+ fullCycle = fullCycle || currentIndex == 0;
+ }
- currentIndex = (currentIndex + 1) % windowSize;
- fullCycle = fullCycle || currentIndex == 0;
- }
+ std::vector<float> getMedian()
+ {
+ std::vector<float> median;
- std::vector<float> getMedian()
+ for (size_t i = 0; i < vectorSize; i++)
{
- std::vector<float> median;
+ std::vector<float> samples;
- for (size_t i = 0; i < vectorSize; i++)
+ for (size_t n = 0; n < windowSize; n++)
{
- std::vector<float> samples;
-
- for (size_t n = 0; n < windowSize; n++)
- {
- samples.push_back(data.at(i + n * vectorSize));
- }
-
- std::sort(samples.begin(), samples.end());
- median.push_back(StatUtils::GetMedian(samples));
+ samples.push_back(data.at(i + n * vectorSize));
}
- return median;
+ std::sort(samples.begin(), samples.end());
+ median.push_back(StatUtils::GetMedian(samples));
}
- };
- }
+ return median;
+ }
+
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
index f32c3d56c..d2245928e 100644
--- a/source/RobotAPI/libraries/core/math/StatUtils.h
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -25,50 +25,48 @@
#include <math.h>
#include <vector>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class StatUtils
{
- class StatUtils
+ public:
+ static float GetPercentile(const std::vector<float>& sortedData, float percentile)
{
- public:
- static float GetPercentile(const std::vector<float>& sortedData, float percentile)
+ if (sortedData.size() == 0)
{
- if (sortedData.size() == 0)
- {
- throw LocalException("GetPercentile not possible for empty vector");
- }
-
- float indexf = (sortedData.size() - 1) * percentile;
- indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
- int index = (int)indexf;
- float f = indexf - index;
+ throw LocalException("GetPercentile not possible for empty vector");
+ }
- if (index == (int)sortedData.size() - 1)
- {
- return sortedData.at(sortedData.size() - 1);
- }
+ float indexf = (sortedData.size() - 1) * percentile;
+ indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
+ int index = (int)indexf;
+ float f = indexf - index;
- return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
- }
- static std::vector<float> GetPercentiles(const std::vector<float>& sortedData, int percentiles)
+ if (index == (int)sortedData.size() - 1)
{
- std::vector<float> result;
- result.push_back(sortedData.at(0));
+ return sortedData.at(sortedData.size() - 1);
+ }
- for (int i = 1; i < percentiles; i++)
- {
- result.push_back(GetPercentile(sortedData, 1.f / percentiles * i));
- }
+ return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
+ }
+ static std::vector<float> GetPercentiles(const std::vector<float>& sortedData, int percentiles)
+ {
+ std::vector<float> result;
+ result.push_back(sortedData.at(0));
- result.push_back(sortedData.at(sortedData.size() - 1));
- return result;
- }
- static float GetMedian(const std::vector<float>& sortedData)
+ for (int i = 1; i < percentiles; i++)
{
- return GetPercentile(sortedData, 0.5f);
+ result.push_back(GetPercentile(sortedData, 1.f / percentiles * i));
}
- };
- }
+
+ result.push_back(sortedData.at(sortedData.size() - 1));
+ return result;
+ }
+ static float GetMedian(const std::vector<float>& sortedData)
+ {
+ return GetPercentile(sortedData, 0.5f);
+ }
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
index 0eef86b42..d56d2f656 100644
--- a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
+++ b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
@@ -25,31 +25,29 @@
#include <memory>
-namespace armarx
+namespace armarx::math
{
- namespace math
- {
- class TimeSeriesUtils;
- using TimeSeriesUtilsPtr = std::shared_ptr<TimeSeriesUtils>;
+ class TimeSeriesUtils;
+ using TimeSeriesUtilsPtr = std::shared_ptr<TimeSeriesUtils>;
- class TimeSeriesUtils
+ class TimeSeriesUtils
+ {
+ public:
+ enum class BorderMode
{
- public:
- enum class BorderMode
- {
- Nearest
- };
- TimeSeriesUtils();
-
- static std::vector<float> Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps);
- static std::vector<float> ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode);
- static std::vector<float> ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode);
- static std::vector<float> CreateGaussianFilter(const float sigma, float sampleTime, float truncate = 4);
- static std::vector<float> MakeTimestamps(float start, float end, size_t count);
-
- private:
+ Nearest
};
- }
+ TimeSeriesUtils();
+
+ static std::vector<float> Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps);
+ static std::vector<float> ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode);
+ static std::vector<float> ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode);
+ static std::vector<float> CreateGaussianFilter(const float sigma, float sampleTime, float truncate = 4);
+ static std::vector<float> MakeTimestamps(float start, float end, size_t count);
+
+ private:
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/math/Trigonometry.h b/source/RobotAPI/libraries/core/math/Trigonometry.h
index f9ef05646..1fd7ad174 100644
--- a/source/RobotAPI/libraries/core/math/Trigonometry.h
+++ b/source/RobotAPI/libraries/core/math/Trigonometry.h
@@ -24,35 +24,33 @@
#include <math.h>
-namespace armarx
+namespace armarx::math
{
- namespace math
+ class Trigonometry
{
- class Trigonometry
+ public:
+ static float Deg2RadF(const float angle)
{
- public:
- static float Deg2RadF(const float angle)
- {
- return angle / 180.0f * M_PI;
- }
- static double Deg2RadD(const double angle)
- {
- return angle / 180.0 * M_PI;
- }
- static float Rad2DegF(const float rad)
- {
- return rad / M_PI * 180.0f;
- }
- static double Rad2DegD(const float rad)
- {
- return rad / M_PI * 180.0;
- }
+ return angle / 180.0f * M_PI;
+ }
+ static double Deg2RadD(const double angle)
+ {
+ return angle / 180.0 * M_PI;
+ }
+ static float Rad2DegF(const float rad)
+ {
+ return rad / M_PI * 180.0f;
+ }
+ static double Rad2DegD(const float rad)
+ {
+ return rad / M_PI * 180.0;
+ }
- static double GetAngleFromVectorXY(const Eigen::Vector3f& vector)
- {
- return atan2(vector(1), vector(0));
- }
- };
- }
+ static double GetAngleFromVectorXY(const Eigen::Vector3f& vector)
+ {
+ return atan2(vector(1), vector(0));
+ }
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index 17407c34d..cdb7046db 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -27,283 +27,281 @@
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <algorithm>
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ class MatrixMaxFilter :
+ public MatrixMaxFilterBase,
+ public DatafieldFilter
{
+ public:
+ MatrixMaxFilter()
+ {
+ this->windowFilterSize = 1;
+ }
- class MatrixMaxFilter :
- public MatrixMaxFilterBase,
- public DatafieldFilter
+ VariantBasePtr calculate(const Ice::Current&) const override
{
- public:
- MatrixMaxFilter()
+ ScopedLock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
{
- this->windowFilterSize = 1;
+ return new Variant(new MatrixFloat(1, 1));
}
- VariantBasePtr calculate(const Ice::Current&) const override
- {
- ScopedLock lock(historyMutex);
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ return new Variant(matrix->toEigen().maxCoeff());
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ };
+
+ class MatrixMinFilter :
+ public MatrixMinFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixMinFilter()
+ {
+ this->windowFilterSize = 1;
+ }
- if (dataHistory.size() == 0)
- {
- return new Variant(new MatrixFloat(1, 1));
- }
+ VariantBasePtr calculate(const Ice::Current&) const override
+ {
+ ScopedLock lock(historyMutex);
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- return new Variant(matrix->toEigen().maxCoeff());
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ if (dataHistory.size() == 0)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ return new Variant(new MatrixFloat(1, 1));
}
- };
- class MatrixMinFilter :
- public MatrixMinFilterBase,
- public DatafieldFilter
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ return new Variant(matrix->toEigen().minCoeff());
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
{
- public:
- MatrixMinFilter()
- {
- this->windowFilterSize = 1;
- }
-
- VariantBasePtr calculate(const Ice::Current&) const override
- {
- ScopedLock lock(historyMutex);
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ };
+
+ class MatrixAvgFilter :
+ public MatrixAvgFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixAvgFilter()
+ {
+ this->windowFilterSize = 1;
+ }
- if (dataHistory.size() == 0)
- {
- return new Variant(new MatrixFloat(1, 1));
- }
+ VariantBasePtr calculate(const Ice::Current&) const override
+ {
+ ScopedLock lock(historyMutex);
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- return new Variant(matrix->toEigen().minCoeff());
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ if (dataHistory.size() == 0)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ return new Variant(new MatrixFloat(1, 1));
}
- };
- class MatrixAvgFilter :
- public MatrixAvgFilterBase,
- public DatafieldFilter
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ return new Variant(matrix->toEigen().mean());
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
{
- public:
- MatrixAvgFilter()
- {
- this->windowFilterSize = 1;
- }
-
- VariantBasePtr calculate(const Ice::Current&) const override
- {
- ScopedLock lock(historyMutex);
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ };
+
+ class MatrixPercentileFilter :
+ public MatrixPercentileFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixPercentileFilter()
+ {
+ this->windowFilterSize = 1;
+ }
+ MatrixPercentileFilter(float percentile)
+ {
+ this->percentile = percentile;
+ this->windowFilterSize = 1;
+ }
- if (dataHistory.size() == 0)
- {
- return new Variant(new MatrixFloat(1, 1));
- }
+ VariantBasePtr calculate(const Ice::Current&) const override
+ {
+ ScopedLock lock(historyMutex);
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- return new Variant(matrix->toEigen().mean());
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ if (dataHistory.size() == 0)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ return new Variant(new MatrixFloat(1, 1));
}
- };
- class MatrixPercentileFilter :
- public MatrixPercentileFilterBase,
- public DatafieldFilter
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ return new Variant(GetPercentile(vector, percentile));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
{
- public:
- MatrixPercentileFilter()
- {
- this->windowFilterSize = 1;
- }
- MatrixPercentileFilter(float percentile)
- {
- this->percentile = percentile;
- this->windowFilterSize = 1;
- }
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
- VariantBasePtr calculate(const Ice::Current&) const override
+ static float GetPercentile(const std::vector<float>& sortedData, float percentile)
+ {
+ if (sortedData.size() == 0)
{
- ScopedLock lock(historyMutex);
+ throw LocalException("GetPercentile not possible for empty vector");
+ }
- if (dataHistory.size() == 0)
- {
- return new Variant(new MatrixFloat(1, 1));
- }
+ float indexf = (sortedData.size() - 1) * percentile;
+ indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
+ int index = (int)indexf;
+ float f = indexf - index;
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- std::vector<float> vector = matrix->toVector();
- std::sort(vector.begin(), vector.end());
- return new Variant(GetPercentile(vector, percentile));
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ if (index == (int)sortedData.size() - 1)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ return sortedData.at(sortedData.size() - 1);
}
- static float GetPercentile(const std::vector<float>& sortedData, float percentile)
- {
- if (sortedData.size() == 0)
- {
- throw LocalException("GetPercentile not possible for empty vector");
- }
-
- float indexf = (sortedData.size() - 1) * percentile;
- indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
- int index = (int)indexf;
- float f = indexf - index;
+ return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
+ }
+ };
- if (index == (int)sortedData.size() - 1)
- {
- return sortedData.at(sortedData.size() - 1);
- }
-
- return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
- }
- };
+ class MatrixPercentilesFilter :
+ public MatrixPercentilesFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixPercentilesFilter()
+ {
+ this->windowFilterSize = 1;
+ this->percentiles = 10;
+ }
+ MatrixPercentilesFilter(int percentiles)
+ {
+ this->percentiles = percentiles;
+ this->windowFilterSize = 1;
+ }
- class MatrixPercentilesFilter :
- public MatrixPercentilesFilterBase,
- public DatafieldFilter
+ VariantBasePtr calculate(const Ice::Current&) const override
{
- public:
- MatrixPercentilesFilter()
- {
- this->windowFilterSize = 1;
- this->percentiles = 10;
- }
- MatrixPercentilesFilter(int percentiles)
- {
- this->percentiles = percentiles;
- this->windowFilterSize = 1;
- }
+ ScopedLock lock(historyMutex);
- VariantBasePtr calculate(const Ice::Current&) const override
+ if (dataHistory.size() == 0)
{
- ScopedLock lock(historyMutex);
-
- if (dataHistory.size() == 0)
- {
- ARMARX_IMPORTANT_S << "no data";
- return new Variant(new MatrixFloat(1, 1));
- }
+ ARMARX_IMPORTANT_S << "no data";
+ return new Variant(new MatrixFloat(1, 1));
+ }
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- std::vector<float> vector = matrix->toVector();
- std::sort(vector.begin(), vector.end());
- std::vector<float> result;
- result.push_back(vector.at(0));
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ std::vector<float> result;
+ result.push_back(vector.at(0));
- for (int i = 1; i < percentiles; i++)
- {
- result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
- }
-
- result.push_back(vector.at(vector.size() - 1));
- return new Variant(new MatrixFloat(1, result.size(), result));
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ for (int i = 1; i < percentiles; i++)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
}
- };
- class MatrixCumulativeFrequencyFilter :
- public MatrixCumulativeFrequencyFilterBase,
- public DatafieldFilter
+ result.push_back(vector.at(vector.size() - 1));
+ return new Variant(new MatrixFloat(1, result.size(), result));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
{
- public:
- MatrixCumulativeFrequencyFilter()
- {
- this->windowFilterSize = 1;
- }
- MatrixCumulativeFrequencyFilter(float min, float max, int bins)
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ };
+
+ class MatrixCumulativeFrequencyFilter :
+ public MatrixCumulativeFrequencyFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixCumulativeFrequencyFilter()
+ {
+ this->windowFilterSize = 1;
+ }
+ MatrixCumulativeFrequencyFilter(float min, float max, int bins)
+ {
+ this->min = min;
+ this->max = max;
+ this->bins = bins;
+ this->windowFilterSize = 1;
+ }
+ VariantBasePtr calculate(const Ice::Current&) const override
+ {
+ ScopedLock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
{
- this->min = min;
- this->max = max;
- this->bins = bins;
- this->windowFilterSize = 1;
+ return new Variant(new MatrixFloat(1, 1));
}
- VariantBasePtr calculate(const Ice::Current&) const override
- {
- ScopedLock lock(historyMutex);
- if (dataHistory.size() == 0)
- {
- return new Variant(new MatrixFloat(1, 1));
- }
-
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- std::vector<float> vector = matrix->toVector();
- std::sort(vector.begin(), vector.end());
- std::vector<int> result = Calculate(vector, min, max, bins);
- std::vector<float> resultF;
-
- for (int v : result)
- {
- resultF.push_back(v);
- }
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ std::vector<int> result = Calculate(vector, min, max, bins);
+ std::vector<float> resultF;
- return new Variant(new MatrixFloat(1, resultF.size(), resultF));
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ for (int v : result)
{
- ParameterTypeList result;
- result.push_back(VariantType::MatrixFloat);
- return result;
+ resultF.push_back(v);
}
- static std::vector<int> Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
- {
- std::vector<int> result;
- float val = min;
- int nr = 0;
- int lastCount = 0;
- for (size_t i = 0; i < sortedData.size(); i++)
- {
- if (sortedData.at(i) > val && nr < bins)
- {
- result.push_back(i);
- nr++;
- val = min + (max - min) * nr / bins;
- lastCount = i;
- }
- }
+ return new Variant(new MatrixFloat(1, resultF.size(), resultF));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ static std::vector<int> Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
+ {
+ std::vector<int> result;
+ float val = min;
+ int nr = 0;
+ int lastCount = 0;
- while ((int)result.size() < bins)
+ for (size_t i = 0; i < sortedData.size(); i++)
+ {
+ if (sortedData.at(i) > val && nr < bins)
{
- result.push_back(lastCount);
+ result.push_back(i);
+ nr++;
+ val = min + (max - min) * nr / bins;
+ lastCount = i;
}
+ }
- return result;
+ while ((int)result.size() < bins)
+ {
+ result.push_back(lastCount);
}
- };
- }
+ return result;
+ }
+
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
index 0c9178cab..c7881099c 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
@@ -28,50 +28,46 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ /**
+ * @class PoseMedianOffsetFilter
+ * @ingroup ObserverFilters
+ * @brief The MedianFilter class provides an implementation
+ * for a median for datafields of type float, int and double.
+ */
+ class MedianDerivativeFilterV3 :
+ public ::armarx::MedianDerivativeFilterV3Base,
+ public MedianFilter
{
+ public:
+ MedianDerivativeFilterV3(std::size_t offsetWindowSize = 1001, std::size_t windowSize = 11);
+
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c) const override;
/**
- * @class PoseMedianOffsetFilter
- * @ingroup ObserverFilters
- * @brief The MedianFilter class provides an implementation
- * for a median for datafields of type float, int and double.
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+ * @return List of VariantTypes
*/
- class MedianDerivativeFilterV3 :
- public ::armarx::MedianDerivativeFilterV3Base,
- public MedianFilter
- {
- public:
- MedianDerivativeFilterV3(std::size_t offsetWindowSize = 1001, std::size_t windowSize = 11);
-
- // DatafieldFilterBase interface
- public:
- VariantBasePtr calculate(const Ice::Current& c) const override;
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
- /**
- * @brief This filter supports: Vector3, FramedDirection, FramedPosition
- * @return List of VariantTypes
- */
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
+ private:
+ Eigen::Vector3f currentValue;
+ Eigen::Vector3f currentOffset;
+ std::vector<Eigen::Vector3f> valueData;
+ std::vector<Eigen::Vector3f> offsetData;
+ std::size_t valueIndex;
+ std::size_t offsetIndex;
- private:
- Eigen::Vector3f currentValue;
- Eigen::Vector3f currentOffset;
- std::vector<Eigen::Vector3f> valueData;
- std::vector<Eigen::Vector3f> offsetData;
- std::size_t valueIndex;
- std::size_t offsetIndex;
+ float median(std::vector<float>& values);
+ Eigen::Vector3f calculateMedian(std::vector<Eigen::Vector3f> const& values);
- float median(std::vector<float>& values);
- Eigen::Vector3f calculateMedian(std::vector<Eigen::Vector3f> const& values);
+ public:
+ void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
- public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
+ };
- };
-
- } // namespace Filters
}
-
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index b5f668bae..d161d4ca3 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -27,128 +27,126 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ /**
+ * @class OffsetFilter
+ * @ingroup ObserverFilters
+ * @brief The OffsetFilter class returns values
+ * relative to value from the first call of the filter.
+ * E.g. this is useful for Forces which should be nulled
+ * at a specific moment.
+ */
+ class OffsetFilter :
+ public ::armarx::OffsetFilterBase,
+ public DatafieldFilter
{
+ public:
+ OffsetFilter()
+ {
+ firstRun = true;
+ windowFilterSize = 1;
+ }
- /**
- * @class OffsetFilter
- * @ingroup ObserverFilters
- * @brief The OffsetFilter class returns values
- * relative to value from the first call of the filter.
- * E.g. this is useful for Forces which should be nulled
- * at a specific moment.
- */
- class OffsetFilter :
- public ::armarx::OffsetFilterBase,
- public DatafieldFilter
+
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
{
- public:
- OffsetFilter()
- {
- firstRun = true;
- windowFilterSize = 1;
- }
+ ScopedLock lock(historyMutex);
+
+ VariantPtr newVariant;
- // DatafieldFilterBase interface
- public:
- VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
+ if (initialValue && dataHistory.size() > 0)
{
+ VariantTypeId type = dataHistory.begin()->second->getType();
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
- ScopedLock lock(historyMutex);
+ if (currentValue->getType() != initialValue->getType())
+ {
+ ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
+ return NULL;
+ }
- VariantPtr newVariant;
+ if (type == VariantType::Int)
+ {
+ int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::Float)
+ {
+ float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::Double)
+ {
+ double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
+ FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
+ Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
- if (initialValue && dataHistory.size() > 0)
+ newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
+ FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
+ Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
+ newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
+ }
+ else if (type == VariantType::MatrixFloat)
{
- VariantTypeId type = dataHistory.begin()->second->getType();
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
-
- if (currentValue->getType() != initialValue->getType())
- {
- ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
- return NULL;
- }
-
- if (type == VariantType::Int)
- {
- int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::Float)
- {
- float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::Double)
- {
- double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::FramedDirection)
- {
- FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
- FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
- Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
-
- newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
- FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
- Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
- newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
- }
- else if (type == VariantType::MatrixFloat)
- {
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
- Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
- newVariant = new Variant(new MatrixFloat(newMatrix));
- }
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
+ Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
+ newVariant = new Variant(new MatrixFloat(newMatrix));
}
+ }
- return newVariant;
+ return newVariant;
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
- {
- ParameterTypeList result;
- result.push_back(VariantType::Int);
- result.push_back(VariantType::Float);
- result.push_back(VariantType::Double);
- result.push_back(VariantType::FramedDirection);
- result.push_back(VariantType::FramedPosition);
- result.push_back(VariantType::MatrixFloat);
- return result;
- }
- private:
- bool firstRun;
- VariantPtr initialValue;
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::Int);
+ result.push_back(VariantType::Float);
+ result.push_back(VariantType::Double);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ private:
+ bool firstRun;
+ VariantPtr initialValue;
+
+ // DatafieldFilterBase interface
+ public:
+ void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override
+ {
+ DatafieldFilter::update(timestamp, value, c);
- // DatafieldFilterBase interface
- public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override
+ if (firstRun)
{
- DatafieldFilter::update(timestamp, value, c);
+ ScopedLock lock(historyMutex);
- if (firstRun)
+ if (dataHistory.size() == 0)
{
- ScopedLock lock(historyMutex);
-
- if (dataHistory.size() == 0)
- {
- return;
- }
-
- initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
- firstRun = false;
+ return;
}
+
+ initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
+ firstRun = false;
}
- };
- }
+ }
+ };
}
+
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
index e7a3eb89f..5b0cf4dbe 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -23,94 +23,91 @@
*/
#include "PoseAverageFilter.h"
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
{
+ ScopedLock lock(historyMutex);
- VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
+ if (dataHistory.size() == 0)
{
- ScopedLock lock(historyMutex);
-
- if (dataHistory.size() == 0)
- {
- return NULL;
- }
+ return NULL;
+ }
- VariantTypeId type = dataHistory.begin()->second->getType();
+ VariantTypeId type = dataHistory.begin()->second->getType();
- if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
- {
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
+ {
- Eigen::Vector3f vec;
- vec.setZero();
- std::string frame = "";
- std::string agent = "";
- VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+ Eigen::Vector3f vec;
+ vec.setZero();
+ std::string frame = "";
+ std::string agent = "";
+ VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
- if (type == VariantType::FramedDirection)
- {
- FramedDirectionPtr p = var->get<FramedDirection>();
- frame = p->frame;
- agent = p->agent;
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr p = var->get<FramedPosition>();
- frame = p->frame;
- agent = p->agent;
- }
- Eigen::MatrixXf keypointPositions(dataHistory.size(), 3);
- int i = 0;
- for (auto& v : dataHistory)
- {
- VariantPtr v2 = VariantPtr::dynamicCast(v.second);
- Eigen::Vector3f value = v2->get<Vector3>()->toEigen();
- keypointPositions(i, 0) = value(0);
- keypointPositions(i, 1) = value(1);
- keypointPositions(i, 2) = value(2);
- i++;
- }
- vec = keypointPositions.colwise().mean();
+ if (type == VariantType::FramedDirection)
+ {
+ FramedDirectionPtr p = var->get<FramedDirection>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr p = var->get<FramedPosition>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ Eigen::MatrixXf keypointPositions(dataHistory.size(), 3);
+ int i = 0;
+ for (auto& v : dataHistory)
+ {
+ VariantPtr v2 = VariantPtr::dynamicCast(v.second);
+ Eigen::Vector3f value = v2->get<Vector3>()->toEigen();
+ keypointPositions(i, 0) = value(0);
+ keypointPositions(i, 1) = value(1);
+ keypointPositions(i, 2) = value(2);
+ i++;
+ }
+ vec = keypointPositions.colwise().mean();
- if (type == VariantType::Vector3)
- {
- Vector3Ptr vecVar = new Vector3(vec);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedDirection)
- {
+ if (type == VariantType::Vector3)
+ {
+ Vector3Ptr vecVar = new Vector3(vec);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
- FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
- return new Variant(vecVar);
- }
- else
- {
- ARMARX_WARNING_S << "Implementation missing here";
- return NULL;
- }
+ FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
+ return new Variant(vecVar);
}
- else if (type == VariantType::Double)
+ else if (type == VariantType::FramedPosition)
{
- // auto values = SortVariants<double>(dataHistory);
- // return new Variant(values.at(values.size()/2));
+ FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
+ return new Variant(vecVar);
}
- else if (type == VariantType::Int)
+ else
{
- // auto values = SortVariants<int>(dataHistory);
- // return new Variant(values.at(values.size()/2));
+ ARMARX_WARNING_S << "Implementation missing here";
+ return NULL;
}
-
- return AverageFilter::calculate(c);
}
+ else if (type == VariantType::Double)
+ {
+ // auto values = SortVariants<double>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+ else if (type == VariantType::Int)
+ {
+ // auto values = SortVariants<int>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+
+ return AverageFilter::calculate(c);
+ }
- } // namespace filters
-} // namespace armarx
+}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
index 34dae88a9..6ba6fa1e5 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
@@ -29,40 +29,37 @@
-namespace armarx
+namespace armarx::filters
{
- namespace filters
- {
- class PoseAverageFilter :
- public ::armarx::PoseAverageFilterBase,
- public AverageFilter
+ class PoseAverageFilter :
+ public ::armarx::PoseAverageFilterBase,
+ public AverageFilter
+ {
+ public:
+ PoseAverageFilter(int windowSize = 11)
{
- public:
- PoseAverageFilter(int windowSize = 11)
- {
- this->windowFilterSize = windowSize;
- }
+ this->windowFilterSize = windowSize;
+ }
- // DatafieldFilterBase interface
- public:
- VariantBasePtr calculate(const Ice::Current& c) const override;
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c) const override;
- /**
- * @brief This filter supports: Vector3, FramedDirection, FramedPosition
- * @return List of VariantTypes
- */
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
- {
- ParameterTypeList result = AverageFilter::getSupportedTypes(c);
- result.push_back(VariantType::Vector3);
- result.push_back(VariantType::FramedDirection);
- result.push_back(VariantType::FramedPosition);
- return result;
- }
- };
+ /**
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+ * @return List of VariantTypes
+ */
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+ {
+ ParameterTypeList result = AverageFilter::getSupportedTypes(c);
+ result.push_back(VariantType::Vector3);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ return result;
+ }
+ };
- } // namespace
-} // namespace
+}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index be995fa04..8b43dd548 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -27,127 +27,125 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ /**
+ * @class PoseMedianFilter
+ * @ingroup ObserverFilters
+ * @brief The MedianFilter class provides an implementation
+ * for a median for datafields of type float, int and double.
+ */
+ class PoseMedianFilter :
+ public ::armarx::PoseMedianFilterBase,
+ public MedianFilter
{
+ public:
+ PoseMedianFilter(int windowSize = 11)
+ {
+ this->windowFilterSize = windowSize;
+ }
- /**
- * @class PoseMedianFilter
- * @ingroup ObserverFilters
- * @brief The MedianFilter class provides an implementation
- * for a median for datafields of type float, int and double.
- */
- class PoseMedianFilter :
- public ::armarx::PoseMedianFilterBase,
- public MedianFilter
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c) const override
{
- public:
- PoseMedianFilter(int windowSize = 11)
+ ScopedLock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
{
- this->windowFilterSize = windowSize;
+ return NULL;
}
- // DatafieldFilterBase interface
- public:
- VariantBasePtr calculate(const Ice::Current& c) const override
+ VariantTypeId type = dataHistory.begin()->second->getType();
+
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
{
- ScopedLock lock(historyMutex);
- if (dataHistory.size() == 0)
+ Eigen::Vector3f vec;
+ vec.setZero();
+ std::string frame = "";
+ std::string agent = "";
+ VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+
+ if (type == VariantType::FramedDirection)
{
- return NULL;
+ FramedDirectionPtr p = var->get<FramedDirection>();
+ frame = p->frame;
+ agent = p->agent;
}
-
- VariantTypeId type = dataHistory.begin()->second->getType();
-
- if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
+ else if (type == VariantType::FramedPosition)
{
+ FramedPositionPtr p = var->get<FramedPosition>();
+ frame = p->frame;
+ agent = p->agent;
+ }
- Eigen::Vector3f vec;
- vec.setZero();
- std::string frame = "";
- std::string agent = "";
- VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+ for (int i = 0; i < 3; ++i)
+ {
+ std::vector<double> values;
- if (type == VariantType::FramedDirection)
- {
- FramedDirectionPtr p = var->get<FramedDirection>();
- frame = p->frame;
- agent = p->agent;
- }
- else if (type == VariantType::FramedPosition)
+ for (auto v : dataHistory)
{
- FramedPositionPtr p = var->get<FramedPosition>();
- frame = p->frame;
- agent = p->agent;
+ VariantPtr v2 = VariantPtr::dynamicCast(v.second);
+ values.push_back(v2->get<Vector3>()->toEigen()[i]);
}
- for (int i = 0; i < 3; ++i)
- {
- std::vector<double> values;
-
- for (auto v : dataHistory)
- {
- VariantPtr v2 = VariantPtr::dynamicCast(v.second);
- values.push_back(v2->get<Vector3>()->toEigen()[i]);
- }
-
- std::sort(values.begin(), values.end());
- vec[i] = values.at(values.size() / 2);
- }
+ std::sort(values.begin(), values.end());
+ vec[i] = values.at(values.size() / 2);
+ }
- if (type == VariantType::Vector3)
- {
- Vector3Ptr vecVar = new Vector3(vec);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedDirection)
- {
+ if (type == VariantType::Vector3)
+ {
+ Vector3Ptr vecVar = new Vector3(vec);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
- FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
- return new Variant(vecVar);
- }
- else
- {
- ARMARX_WARNING_S << "Implementation missing here";
- return NULL;
- }
+ FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
+ return new Variant(vecVar);
}
- else if (type == VariantType::Double)
+ else if (type == VariantType::FramedPosition)
{
- // auto values = SortVariants<double>(dataHistory);
- // return new Variant(values.at(values.size()/2));
+ FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
+ return new Variant(vecVar);
}
- else if (type == VariantType::Int)
+ else
{
- // auto values = SortVariants<int>(dataHistory);
- // return new Variant(values.at(values.size()/2));
+ ARMARX_WARNING_S << "Implementation missing here";
+ return NULL;
}
-
- return MedianFilter::calculate(c);
}
-
- /**
- * @brief This filter supports: Vector3, FramedDirection, FramedPosition
- * @return List of VariantTypes
- */
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+ else if (type == VariantType::Double)
{
- ParameterTypeList result = MedianFilter::getSupportedTypes(c);
- result.push_back(VariantType::Vector3);
- result.push_back(VariantType::FramedDirection);
- result.push_back(VariantType::FramedPosition);
- return result;
+ // auto values = SortVariants<double>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
}
+ else if (type == VariantType::Int)
+ {
+ // auto values = SortVariants<int>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+
+ return MedianFilter::calculate(c);
+ }
+
+ /**
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+ * @return List of VariantTypes
+ */
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+ {
+ ParameterTypeList result = MedianFilter::getSupportedTypes(c);
+ result.push_back(VariantType::Vector3);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ return result;
+ }
+
+ };
- };
+} // namespace Filters
- } // namespace Filters
-}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
index 584b51e38..73d79140c 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
@@ -28,48 +28,46 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
-namespace armarx
+namespace armarx::filters
{
- namespace filters
+
+ /**
+ * @class PoseMedianOffsetFilter
+ * @ingroup ObserverFilters
+ * @brief The MedianFilter class provides an implementation
+ * for a median for datafields of type float, int and double.
+ */
+ class PoseMedianOffsetFilter :
+ public ::armarx::PoseMedianOffsetFilterBase,
+ public MedianFilter
{
+ public:
+ PoseMedianOffsetFilter(int windowSize = 11);
+
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c) const override;
/**
- * @class PoseMedianOffsetFilter
- * @ingroup ObserverFilters
- * @brief The MedianFilter class provides an implementation
- * for a median for datafields of type float, int and double.
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+ * @return List of VariantTypes
*/
- class PoseMedianOffsetFilter :
- public ::armarx::PoseMedianOffsetFilterBase,
- public MedianFilter
- {
- public:
- PoseMedianOffsetFilter(int windowSize = 11);
-
- // DatafieldFilterBase interface
- public:
- VariantBasePtr calculate(const Ice::Current& c) const override;
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
- /**
- * @brief This filter supports: Vector3, FramedDirection, FramedPosition
- * @return List of VariantTypes
- */
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
+ private:
+ Eigen::Vector3f offset;
+ Eigen::Vector3f currentValue;
+ std::vector<Eigen::Vector3f> data;
+ int dataIndex;
- private:
- Eigen::Vector3f offset;
- Eigen::Vector3f currentValue;
- std::vector<Eigen::Vector3f> data;
- int dataIndex;
+ float median(std::vector<float>& values);
+ Eigen::Vector3f calculateMedian();
- float median(std::vector<float>& values);
- Eigen::Vector3f calculateMedian();
+ public:
+ void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
- public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
+ };
- };
+} // namespace Filters
- } // namespace Filters
-}
diff --git a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
index 0c7f49a96..6454d71cf 100644
--- a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
+
#define BOOST_TEST_MODULE RobotAPI::FramedPose::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
index a18af95c2..bd36e5307 100644
--- a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -66,16 +66,37 @@ public:
PointCloud(const VectorT& points) : points(points) {}
// Container methods.
- std::size_t size() const { return points.size(); }
+ std::size_t size() const
+ {
+ return points.size();
+ }
- PointT& operator[](std::size_t i) { return points[i]; }
- const PointT& operator[](std::size_t i) const { return points[i]; }
+ PointT& operator[](std::size_t i)
+ {
+ return points[i];
+ }
+ const PointT& operator[](std::size_t i) const
+ {
+ return points[i];
+ }
// Iterators.
- typename VectorT::iterator begin() { return points.begin(); }
- typename VectorT::const_iterator begin() const { return points.begin(); }
- typename VectorT::iterator end() { return points.end(); }
- typename VectorT::const_iterator end() const { return points.end(); }
+ typename VectorT::iterator begin()
+ {
+ return points.begin();
+ }
+ typename VectorT::const_iterator begin() const
+ {
+ return points.begin();
+ }
+ typename VectorT::iterator end()
+ {
+ return points.end();
+ }
+ typename VectorT::const_iterator end() const
+ {
+ return points.end();
+ }
/// The points.
@@ -112,7 +133,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZ, Fixture<PointXYZ>)
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZ&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+ [](const PointXYZ&)
+ {
+ return DrawColor{0, 0.5, 1, 1};
+ }, pointSize);
}
@@ -122,7 +146,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBA, Fixture<PointXYZRGBA>)
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZRGBA&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+ [](const PointXYZRGBA&)
+ {
+ return DrawColor{0, 0.5, 1, 1};
+ }, pointSize);
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
@@ -134,7 +161,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBL, Fixture<PointXYZRGBL>)
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZRGBL&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+ [](const PointXYZRGBL&)
+ {
+ return DrawColor{0, 0.5, 1, 1};
+ }, pointSize);
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index ca6e0b439..e1b3adfc8 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -191,7 +191,7 @@ namespace armarx
void DebugDrawerTopic::drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
const DrawColor& color, bool ignoreLengthScale)
{
- drawBox(id, math::Helpers::Position(pose), Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+ drawBox(id, ::math::Helpers::Position(pose), Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
extents, color, ignoreLengthScale);
}
@@ -207,8 +207,8 @@ namespace armarx
const DrawColor& color, bool ignoreLengthScale)
{
const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
- drawBox(id, math::Helpers::TransformPosition(pose, center),
- Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+ drawBox(id, ::math::Helpers::TransformPosition(pose, center),
+ Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
boundingBox.getMax() - boundingBox.getMin(), color, ignoreLengthScale);
}
@@ -226,7 +226,7 @@ namespace armarx
const Eigen::Vector3f& extents,
float width, const DrawColor& color, bool ignoreLengthScale)
{
- drawBoxEdges(id, math::Helpers::Pose(position, orientation), extents, width, color, ignoreLengthScale);
+ drawBoxEdges(id, ::math::Helpers::Pose(position, orientation), extents, width, color, ignoreLengthScale);
}
void DebugDrawerTopic::drawBoxEdges(
@@ -250,8 +250,8 @@ namespace armarx
Eigen::Vector3f start = { bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z() };
Eigen::Vector3f end = { bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z() };
- start = math::Helpers::TransformPosition(pose, start);
- end = math::Helpers::TransformPosition(pose, end);
+ start = ::math::Helpers::TransformPosition(pose, start);
+ end = ::math::Helpers::TransformPosition(pose, end);
points.push_back(start);
points.push_back(end);
@@ -298,8 +298,8 @@ namespace armarx
}
void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id, const Eigen::Matrix32f& aabb,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ const DebugDrawerTopic::VisuID& id, const Eigen::Matrix32f& aabb,
+ float width, const DrawColor& color, bool ignoreLengthScale)
{
drawBoxEdges(id, aabb, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
}
@@ -311,8 +311,8 @@ namespace armarx
{
const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
- drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
- math::Helpers::Orientation(pose)),
+ drawBoxEdges(id, ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
+ ::math::Helpers::Orientation(pose)),
boundingBox.getMax() - boundingBox.getMin(),
width, color, ignoreLengthScale);
}
@@ -324,8 +324,8 @@ namespace armarx
{
const Eigen::Vector3f center = 0.5 * (aabb.col(0) + aabb.col(1));
- drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
- math::Helpers::Orientation(pose)),
+ drawBoxEdges(id, ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
+ ::math::Helpers::Orientation(pose)),
aabb.col(1) - aabb.col(0),
width, color, ignoreLengthScale);
}
@@ -593,7 +593,7 @@ namespace armarx
const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
bool ignoreLengthScale)
{
- drawPose(id, math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
+ drawPose(id, ::math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
}
@@ -622,7 +622,7 @@ namespace armarx
const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
float scale, bool ignoreLengthScale)
{
- drawPose(id, math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
+ drawPose(id, ::math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
}
@@ -661,7 +661,7 @@ namespace armarx
const DebugDrawerTopic::VisuID& id,
const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, bool ignoreScale)
{
- updateRobotPose(id, math::Helpers::Pose(pos, ori), ignoreScale);
+ updateRobotPose(id, ::math::Helpers::Pose(pos, ori), ignoreScale);
}
@@ -794,7 +794,7 @@ namespace armarx
auto toVector3 = [&scale, &isIdentity, &pose](const Eigen::Vector3f & v)
{
- return scaled(scale, isIdentity ? v : math::Helpers::TransformPosition(pose, v));
+ return scaled(scale, isIdentity ? v : ::math::Helpers::TransformPosition(pose, v));
};
ARMARX_INFO << "Drawing trimesh as polygons";
@@ -961,7 +961,7 @@ namespace armarx
else
{
Eigen::Matrix4f out = pose;
- math::Helpers::Position(out) *= scale;
+ ::math::Helpers::Position(out) *= scale;
return new Pose(out);
}
}
diff --git a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
index 249afb6e1..8737c606c 100644
--- a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
+++ b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
@@ -55,7 +55,7 @@ DebugLayerControlWidget::~DebugLayerControlWidget()
delete ui;
}
-void DebugLayerControlWidget::setEntityDrawer(DebugDrawerComponentPtr entityDrawer)
+void DebugLayerControlWidget::setEntityDrawer(armarx::DebugDrawerComponentPtr entityDrawer)
{
this->entityDrawer = entityDrawer;
}
diff --git a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
index 607a6b0fe..619e936e8 100644
--- a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
+++ b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
@@ -45,7 +45,6 @@
/* System headers */
#include <string>
-using namespace armarx;
namespace Ui
{
@@ -59,7 +58,7 @@ class DebugLayerControlWidget : public QWidget
public:
DebugLayerControlWidget(QWidget* parent = 0);
~DebugLayerControlWidget() override;
- void setEntityDrawer(DebugDrawerComponentPtr entityDrawer);
+ void setEntityDrawer(armarx::DebugDrawerComponentPtr entityDrawer);
public slots:
void updateLayers();
@@ -67,7 +66,7 @@ public slots:
void layerToggleVisibility(QString layerName);
protected:
- DebugDrawerComponentPtr entityDrawer;
+ armarx::DebugDrawerComponentPtr entityDrawer;
SoTimerSensor* timerSensor;
/**
* @brief Maps events to toggle a layer's visibility from checkboxes contained in the table to layerToggleVisibility.
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
index 2ac702f87..de0a5494a 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
@@ -23,33 +23,31 @@
#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.generated.h>
-namespace armarx
+namespace armarx::ForceTorqueUtility
{
- namespace ForceTorqueUtility
+ class DetectForceFlank :
+ public DetectForceFlankGeneratedBase < DetectForceFlank >
{
- class DetectForceFlank :
- public DetectForceFlankGeneratedBase < DetectForceFlank >
+ public:
+ DetectForceFlank(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < DetectForceFlank > (stateData), DetectForceFlankGeneratedBase < DetectForceFlank > (stateData)
{
- public:
- DetectForceFlank(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < DetectForceFlank > (stateData), DetectForceFlankGeneratedBase < DetectForceFlank > (stateData)
- {
- }
-
- // inherited from StateBase
- void onEnter() override {}
- void run() override;
- void onExit() override {}
-
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
-
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ }
+
+ // inherited from StateBase
+ void onEnter() override {}
+ void run() override;
+ void onExit() override {}
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
index 03bd9170b..9ee5ce9e2 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
@@ -23,33 +23,28 @@
#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.generated.h>
-namespace armarx
+namespace armarx::ForceTorqueUtility
{
- namespace ForceTorqueUtility
+ class DetectForceSpike :
+ public DetectForceSpikeGeneratedBase < DetectForceSpike >
{
- class DetectForceSpike :
- public DetectForceSpikeGeneratedBase < DetectForceSpike >
+ public:
+ DetectForceSpike(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < DetectForceSpike > (stateData), DetectForceSpikeGeneratedBase < DetectForceSpike > (stateData)
{
- public:
- DetectForceSpike(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < DetectForceSpike > (stateData), DetectForceSpikeGeneratedBase < DetectForceSpike > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override {}
- void run() override;
- void onExit() override {}
+ // inherited from StateBase
+ void onEnter() override {}
+ void run() override;
+ void onExit() override {}
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
index e1299f985..8fe4acb8b 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
@@ -25,28 +25,24 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "ForceTorqueUtilityStatechartContext.generated.h"
-namespace armarx
+namespace armarx::ForceTorqueUtility
{
- namespace ForceTorqueUtility
+ class ForceTorqueUtilityRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class ForceTorqueUtilityRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
-
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
index 92ea07c58..6d132d9bc 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
@@ -25,28 +25,24 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "OrientedTactileSensorGroupStatechartContext.generated.h"
-namespace armarx
+namespace armarx::OrientedTactileSensorGroup
{
- namespace OrientedTactileSensorGroup
+ class OrientedTactileSensorGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class OrientedTactileSensorGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
-
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
index 690c8756d..b9d5a2c02 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
@@ -24,33 +24,29 @@
#include <RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.generated.h>
-namespace armarx
+namespace armarx::OrientedTactileSensorGroup
{
- namespace OrientedTactileSensorGroup
+ class OrientedTactileSensorTest :
+ public OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest>
{
- class OrientedTactileSensorTest :
- public OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest>
+ public:
+ OrientedTactileSensorTest(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate <OrientedTactileSensorTest> (stateData), OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest> (stateData)
{
- public:
- OrientedTactileSensorTest(const XMLStateConstructorParams& stateData):
- XMLStateTemplate <OrientedTactileSensorTest> (stateData), OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest> (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- // void run();
- // void onBreak();
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ // void run();
+ // void onBreak();
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
index cf1bcd9ab..f49336429 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
@@ -98,10 +98,10 @@ void CyberGloveProsthesisControl::run()
// should check constantly whether isRunningTaskStopped() returns true
// get a private kinematic instance for this state of the robot (tick "Robot State Component" proxy checkbox in statechart group)
-// VirtualRobot::RobotPtr robot = getLocalRobot();
+ // VirtualRobot::RobotPtr robot = getLocalRobot();
-// DebugDrawerHelper debugDrawerHelper(getDebugDrawerTopic(), "RemoteTeachIn", robot);
-// ProsthesisPositionHelperPtr prosthesisPositionHelper;
+ // DebugDrawerHelper debugDrawerHelper(getDebugDrawerTopic(), "RemoteTeachIn", robot);
+ // ProsthesisPositionHelperPtr prosthesisPositionHelper;
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
rootLayoutBuilder
@@ -147,7 +147,7 @@ void CyberGloveProsthesisControl::run()
std::chrono::time_point<std::chrono::system_clock> start, end;
-// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
+ // uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
while (!isRunningTaskStopped()) // stop run function if returning true
{
// do your calculations
@@ -274,32 +274,32 @@ void CyberGloveProsthesisControl::run()
//SimpleJsonLoggerEntry e;
shapeHand(indexValue, thumbValue);
-// e.AddTimestamp();
+ // e.AddTimestamp();
NameValueMap handMotorValues = getHandUnit()->getCurrentJointValues();
-// e.Add("indexValueTarget", indexValue);
-// e.Add("thumbValueTarget", thumbValue);
-// e.Add("indexValueActual", handMotorValues["Fingers"]);
-// e.Add("thumbValueActual", handMotorValues["Thumb"]);
+ // e.Add("indexValueTarget", indexValue);
+ // e.Add("thumbValueTarget", thumbValue);
+ // e.Add("indexValueActual", handMotorValues["Fingers"]);
+ // e.Add("thumbValueActual", handMotorValues["Thumb"]);
if (record)
{
end = std::chrono::system_clock::now();
double elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>
- (end-start).count();
+ (end - start).count();
logger << elapsed_milliseconds << "," << indexValue << "," << thumbValue << "," << handMotorValues["Fingers"] << "," << handMotorValues["Thumb"] << std::endl;
-// if (storeCalibrationValuesForCyberGlove)
-// {
-// ARMARX_IMPORTANT << "Recording raw joint values for calibration.";
-// e = getRawJointValuesForCalibration(openValues, closedValues);
-// storeCalibrationValuesForCyberGlove = false;
-// }
-// else
-// {
-// addCurrentRawJointValues(currentValues, e);
-// }
-// logger->log(e);
+ // if (storeCalibrationValuesForCyberGlove)
+ // {
+ // ARMARX_IMPORTANT << "Recording raw joint values for calibration.";
+ // e = getRawJointValuesForCalibration(openValues, closedValues);
+ // storeCalibrationValuesForCyberGlove = false;
+ // }
+ // else
+ // {
+ // addCurrentRawJointValues(currentValues, e);
+ // }
+ // logger->log(e);
}
}
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
index 522338b6a..dccc99d23 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
@@ -24,49 +24,47 @@
#include <RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.generated.h>
//#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-namespace armarx
+namespace armarx::ProsthesisKinestheticTeachIn
{
- namespace ProsthesisKinestheticTeachIn
+ class CyberGloveProsthesisControl :
+ public CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl >
{
- class CyberGloveProsthesisControl :
- public CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl >
+ public:
+ CyberGloveProsthesisControl(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < CyberGloveProsthesisControl > (stateData), CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl > (stateData)
{
- public:
- CyberGloveProsthesisControl(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < CyberGloveProsthesisControl > (stateData), CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl > (stateData)
- {
- }
+ }
- enum class PhaseType
- {
- CalibOpen,
- CalibClose,
- StartPose,
- TeachIn
- };
+ enum class PhaseType
+ {
+ CalibOpen,
+ CalibClose,
+ StartPose,
+ TeachIn
+ };
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
- private:
- void shapeHand(float fingers, float thumb);
- //SimpleJsonLoggerEntry getRawJointValuesForCalibration(CyberGloveValues& openValues, CyberGloveValues& closedValues);
- //void addCurrentRawJointValues(CyberGloveValues& currentValues, SimpleJsonLoggerEntry& e);
+ private:
+ void shapeHand(float fingers, float thumb);
+ //SimpleJsonLoggerEntry getRawJointValuesForCalibration(CyberGloveValues& openValues, CyberGloveValues& closedValues);
+ //void addCurrentRawJointValues(CyberGloveValues& currentValues, SimpleJsonLoggerEntry& e);
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
index 916c56613..2a6b6114c 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
@@ -25,28 +25,26 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "ProsthesisKinestheticTeachInStatechartContext.generated.h"
-namespace armarx
+namespace armarx::ProsthesisKinestheticTeachIn
{
- namespace ProsthesisKinestheticTeachIn
+ class ProsthesisKinestheticTeachInRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < ProsthesisKinestheticTeachInStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class ProsthesisKinestheticTeachInRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < ProsthesisKinestheticTeachInStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- ProsthesisKinestheticTeachInRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ ProsthesisKinestheticTeachInRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
index dccee7089..3553f8e4b 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
@@ -25,28 +25,26 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "RobotNameHelperTestGroupStatechartContext.generated.h"
-namespace armarx
+namespace armarx::RobotNameHelperTestGroup
{
- namespace RobotNameHelperTestGroup
+ class RobotNameHelperTestGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class RobotNameHelperTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
index 84b0ac8e9..a273d15b5 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
@@ -23,34 +23,32 @@
#include <RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.generated.h>
-namespace armarx
+namespace armarx::RobotNameHelperTestGroup
{
- namespace RobotNameHelperTestGroup
+ class TestGetNames :
+ public TestGetNamesGeneratedBase < TestGetNames >
{
- class TestGetNames :
- public TestGetNamesGeneratedBase < TestGetNames >
+ public:
+ TestGetNames(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < TestGetNames > (stateData), TestGetNamesGeneratedBase < TestGetNames > (stateData)
{
- public:
- TestGetNames(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestGetNames > (stateData), TestGetNamesGeneratedBase < TestGetNames > (stateData)
- {
- }
-
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- // void onBreak() override;
- void onExit() override;
-
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
-
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ }
+
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
+
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
index d3891271c..cc843a739 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
@@ -25,28 +25,26 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "SpeechObserverTestGroupStatechartContext.generated.h"
-namespace armarx
+namespace armarx::SpeechObserverTestGroup
{
- namespace SpeechObserverTestGroup
+ class SpeechObserverTestGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < SpeechObserverTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class SpeechObserverTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < SpeechObserverTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- SpeechObserverTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ SpeechObserverTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
index 5911942a0..8488ecc11 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
@@ -23,35 +23,30 @@
#include <RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.generated.h>
-namespace armarx
+namespace armarx::SpeechObserverTestGroup
{
- namespace SpeechObserverTestGroup
+ class TestTextToSpeech :
+ public TestTextToSpeechGeneratedBase < TestTextToSpeech >
{
- class TestTextToSpeech :
- public TestTextToSpeechGeneratedBase < TestTextToSpeech >
+ public:
+ TestTextToSpeech(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < TestTextToSpeech > (stateData), TestTextToSpeechGeneratedBase < TestTextToSpeech > (stateData)
{
- public:
- TestTextToSpeech(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestTextToSpeech > (stateData), TestTextToSpeechGeneratedBase < TestTextToSpeech > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- void waitForSpeechFinished();
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ void waitForSpeechFinished();
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecution.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecution.h
index 6d410eb07..a9e3ec9b5 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecution.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecution.h
@@ -23,34 +23,29 @@
#include <RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecution.generated.h>
-namespace armarx
+namespace armarx::StatechartExecutionGroup
{
- namespace StatechartExecutionGroup
+ class StatechartExecution :
+ public StatechartExecutionGeneratedBase < StatechartExecution >
{
- class StatechartExecution :
- public StatechartExecutionGeneratedBase < StatechartExecution >
+ public:
+ StatechartExecution(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < StatechartExecution > (stateData), StatechartExecutionGeneratedBase < StatechartExecution > (stateData)
{
- public:
- StatechartExecution(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < StatechartExecution > (stateData), StatechartExecutionGeneratedBase < StatechartExecution > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- // void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ // void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
index 3ffadd3fe..030cac2fe 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
@@ -25,28 +25,24 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "StatechartExecutionGroupStatechartContext.generated.h"
-namespace armarx
+namespace armarx::StatechartExecutionGroup
{
- namespace StatechartExecutionGroup
+ class StatechartExecutionGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < StatechartExecutionGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class StatechartExecutionGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < StatechartExecutionGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- StatechartExecutionGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ StatechartExecutionGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/Executor.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/Executor.h
index 73eaf61e8..5735c14d9 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/Executor.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/Executor.h
@@ -26,32 +26,30 @@
namespace armarx
{
DEFINEEVENT(EvAbort)
- namespace StatechartExecutionGroup
+}
+namespace armarx::StatechartExecutionGroup
+{
+ class Executor :
+ public ExecutorGeneratedBase < Executor >
{
- class Executor :
- public ExecutorGeneratedBase < Executor >
+ public:
+ Executor(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < Executor > (stateData), ExecutorGeneratedBase < Executor > (stateData)
{
- public:
- Executor(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < Executor > (stateData), ExecutorGeneratedBase < Executor > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- // void run() override;
- void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ // void run() override;
+ void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/PrepareNext.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/PrepareNext.h
index 7b0831d25..922c8c159 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/PrepareNext.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/PrepareNext.h
@@ -23,34 +23,29 @@
#include <RobotAPI/statecharts/StatechartExecutionGroup/PrepareNext.generated.h>
-namespace armarx
+namespace armarx::StatechartExecutionGroup
{
- namespace StatechartExecutionGroup
+ class PrepareNext :
+ public PrepareNextGeneratedBase < PrepareNext >
{
- class PrepareNext :
- public PrepareNextGeneratedBase < PrepareNext >
+ public:
+ PrepareNext(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < PrepareNext > (stateData), PrepareNextGeneratedBase < PrepareNext > (stateData)
{
- public:
- PrepareNext(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < PrepareNext > (stateData), PrepareNextGeneratedBase < PrepareNext > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- // void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ // void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/WaitForNext.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/WaitForNext.h
index b98d39ef1..de648b883 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/WaitForNext.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Substates/WaitForNext.h
@@ -26,32 +26,30 @@
namespace armarx
{
DEFINEEVENT(EvNextStatechartAvailable)
- namespace StatechartExecutionGroup
+}
+namespace armarx::StatechartExecutionGroup
+{
+ class WaitForNext :
+ public WaitForNextGeneratedBase < WaitForNext >
{
- class WaitForNext :
- public WaitForNextGeneratedBase < WaitForNext >
+ public:
+ WaitForNext(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < WaitForNext > (stateData), WaitForNextGeneratedBase < WaitForNext > (stateData)
{
- public:
- WaitForNext(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < WaitForNext > (stateData), WaitForNextGeneratedBase < WaitForNext > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- // void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ // void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
index 09cd6baf8..7bee11d15 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
@@ -23,34 +23,29 @@
#include <RobotAPI/statecharts/StatechartExecutionGroup/TestStateForStatechartExecution.generated.h>
-namespace armarx
+namespace armarx::StatechartExecutionGroup
{
- namespace StatechartExecutionGroup
+ class TestStateForStatechartExecution :
+ public TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution >
{
- class TestStateForStatechartExecution :
- public TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution >
+ public:
+ TestStateForStatechartExecution(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < TestStateForStatechartExecution > (stateData), TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution > (stateData)
{
- public:
- TestStateForStatechartExecution(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestStateForStatechartExecution > (stateData), TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution > (stateData)
- {
- }
+ }
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- // void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
-
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
index 73f191c9a..40d04d844 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
@@ -27,28 +27,24 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include <RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupStatechartContext.generated.h>
-namespace armarx
+namespace armarx::StatechartProfilesTestGroup
{
- namespace StatechartProfilesTestGroup
+ class StatechartProfilesTestGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < StatechartProfilesTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class StatechartProfilesTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < StatechartProfilesTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- StatechartProfilesTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ StatechartProfilesTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
-
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
index 1737f94b0..adad16ef6 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
@@ -26,30 +26,26 @@
#include "TestState.generated.h"
-namespace armarx
+namespace armarx::StatechartProfilesTestGroup
{
- namespace StatechartProfilesTestGroup
+ class TestState :
+ public TestStateGeneratedBase<TestState>
{
- class TestState :
- public TestStateGeneratedBase<TestState>
- {
- public:
- TestState(const XMLStateConstructorParams& stateData);
+ public:
+ TestState(const XMLStateConstructorParams& stateData);
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ void onBreak() override;
+ void onExit() override;
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
-
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
index afc11e6ea..d28777d5f 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
@@ -24,33 +24,31 @@
#include <RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.generated.h>
-namespace armarx
+namespace armarx::TrajectoryExecutionCode
{
- namespace TrajectoryExecutionCode
+ class PlayTrajectory :
+ public PlayTrajectoryGeneratedBase < PlayTrajectory >
{
- class PlayTrajectory :
- public PlayTrajectoryGeneratedBase < PlayTrajectory >
+ public:
+ PlayTrajectory(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < PlayTrajectory > (stateData), PlayTrajectoryGeneratedBase < PlayTrajectory > (stateData)
{
- public:
- PlayTrajectory(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < PlayTrajectory > (stateData), PlayTrajectoryGeneratedBase < PlayTrajectory > (stateData)
- {
- }
-
- // inherited from StateBase
- void onEnter() override;
- // void run();
- // void onBreak();
- void onExit() override;
-
- // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
-
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ }
+
+ // inherited from StateBase
+ void onEnter() override;
+ // void run();
+ // void onBreak();
+ void onExit() override;
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
+
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
index 155861772..46df35ec3 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
@@ -25,28 +25,26 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "TrajectoryExecutionCodeStatechartContext.generated.h"
-namespace armarx
+namespace armarx::TrajectoryExecutionCode
{
- namespace TrajectoryExecutionCode
+ class TrajectoryExecutionCodeRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < TrajectoryExecutionCodeStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class TrajectoryExecutionCodeRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < TrajectoryExecutionCodeStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- TrajectoryExecutionCodeRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ TrajectoryExecutionCodeRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
index 9fd3d4457..33ab41528 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
@@ -27,28 +27,26 @@
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "WeissHapticGroupStatechartContext.generated.h"
-namespace armarx
+namespace armarx::WeissHapticGroup
{
- namespace WeissHapticGroup
+ class WeissHapticGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
- class WeissHapticGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
- {
- public:
- WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+ public:
+ WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
- // inherited from RemoteStateOfferer
- void onInitXMLRemoteStateOfferer() override;
- void onConnectXMLRemoteStateOfferer() override;
- void onExitXMLRemoteStateOfferer() override;
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
- // static functions for AbstractFactory Method
- static std::string GetName();
- static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
- static SubClassRegistry Registry;
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
- };
- }
+ };
}
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
index 247302b36..209f76d76 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
@@ -25,31 +25,29 @@
#include <RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h>
-namespace armarx
+namespace armarx::WeissHapticGroup
{
- namespace WeissHapticGroup
+ class WeissHapticSensorTest
+ :
+ public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
{
- class WeissHapticSensorTest
- :
- public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
- {
- public:
- WeissHapticSensorTest(const XMLStateConstructorParams& stateData);
+ public:
+ WeissHapticSensorTest(const XMLStateConstructorParams& stateData);
- // inherited from StateBase
- void onEnter() override;
- void run() override;
- void onBreak() override;
- void onExit() override;
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ void onBreak() override;
+ void onExit() override;
- // // static functions for AbstractFactory Method
- static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
- static SubClassRegistry Registry;
+ // // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
- // DO NOT INSERT ANY CLASS MEMBERS,
- // use stateparameters instead,
- // if classmember are neccessary nonetheless, reset them in onEnter
- };
- }
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
}
+
--
GitLab