diff --git a/scenarios/SpeechObserver/config/DummyTextToSpeechApp.cfg b/scenarios/SpeechObserver/config/DummyTextToSpeechApp.cfg
index 4b7302497882ef33de9eba5411327ed574f9e787..3c85bf5a6253068d42ac8313f9d638292365fba6 100644
--- a/scenarios/SpeechObserver/config/DummyTextToSpeechApp.cfg
+++ b/scenarios/SpeechObserver/config/DummyTextToSpeechApp.cfg
@@ -162,11 +162,6 @@
# ArmarX.Verbosity = Info
-# Ice.Config: Custom Property
-# Attributes:
-# - Default: ::NOT_DEFINED::
-# - Case sensitivity: no
-# - Required: no
-# Ice.Config = ::NOT_DEFINED::
+
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 4746e98cf0360d1f54c1394a9ef31bcc35374869..503c8589ec95a34d0e27d2798ecf950f59d79ef8 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -1002,6 +1002,8 @@ namespace armarx
face.idNormal3 = f.vertex3.normalID;
face.idColor3 = f.vertex3.colorID;
+ face.normal = Eigen::Vector3f(f.normal.x, f.normal.y, f.normal.z);
+
triMesh->addFace(face);
}
diff --git a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
index 76406efc20632032ff41568f8d36088c0c180483..c5a80a8326d5e71a220078758aa2f7f7c4ec29de 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
+++ b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
@@ -55,24 +55,27 @@ void DummyTextToSpeech::onExitComponent()
armarx::PropertyDefinitionsPtr DummyTextToSpeech::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new DummyTextToSpeechPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
-void armarx::DummyTextToSpeech::reportText(const std::string&text, const Ice::Current&)
+void armarx::DummyTextToSpeech::reportText(const std::string& text, const Ice::Current&)
{
- ARMARX_IMPORTANT << "reportState";
+ //ARMARX_IMPORTANT << "reportState";
textToSpeechStateTopicPrx->reportState(eStartedSpeaking);
- ARMARX_IMPORTANT << "sleep";
+ ARMARX_IMPORTANT << "DummyTTS StartedSpeaking: " << text;
+
+ //ARMARX_IMPORTANT << "sleep";
sleep(1);
TimeUtil::SleepMS(text.length() * 10);
- ARMARX_IMPORTANT << "reportState";
+ //ARMARX_IMPORTANT << "reportState";
+ ARMARX_IMPORTANT << "DummyTTS FinishedSpeaking";
textToSpeechStateTopicPrx->reportState(eFinishedSpeaking);
}
-void armarx::DummyTextToSpeech::reportTextWithParams(const std::string&text, const Ice::StringSeq¶ms, const Ice::Current&)
+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/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 4a9aa61ee61ebc005c2abd9e9c1550f010f7edbe..9de7dca530084c57728842fa1f213afc5a710ef1 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -69,7 +69,7 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
- localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eFull);
+ localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eStructure);
// localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
@@ -300,17 +300,17 @@ namespace armarx
RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
+ auto tcpNode = kinematicChain->getTCP();
+ VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
- VirtualRobot::RobotNodePrismaticPtr virtualJoint;
- for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
+ virtualPrismaticJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+ if (!virtualPrismaticJoint)
{
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
- {
- virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
- }
-
+ ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
+ continue;
}
+
// set other not-used joints to 0
for (auto& nodeName : possiblyInvolvedJointNames)
{
@@ -322,8 +322,8 @@ namespace armarx
}
}
- ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName());
- VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
+ ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualPrismaticJoint->getName()) << " " << VAROUT(kinematicChain->getName());
+ VirtualRobot::GazeIK ikSolver(kinematicChain, virtualPrismaticJoint);
ikSolver.enableJointLimitAvoidance(true);
ikSolver.setup(10, 30, 20);
//ikSolver.setVerbose(true);
@@ -373,10 +373,10 @@ namespace armarx
DrawColor {0, 1, 1, 0.2},
17);
}
-
+ auto tcpNode = kinematicChain->getTCP();
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+ if (kinematicChain->getNode(i) != tcpNode)
{
targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index c988cec09e2d59ad93e63eb38dde3d7eeb3d5306..2706e2c4524cfc6e473f13226fb54a714bed4cf1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -76,9 +76,10 @@ namespace armarx
for (size_t i = 0; i < dimNames.size(); ++i)
{
const auto& jointName = dimNames.at(i);
- if (targets.count(jointName) == 1)
+ auto it = targets.find(jointName);
+ if (it != targets.end())
{
- targets[jointName]->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ it->second->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 5679160c7e1e8035e2bc0804ae5c86a95d008a5e..1abf7cecd98e67e872adf7fc735c36d83fa801ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -1070,13 +1070,16 @@ namespace armarx
auto start = MicroNow();
nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
auto duration = MicroNow() - start;
- if (duration.count() > 500)
+ if (checkControllerExecutionTimings)
{
- ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
- }
- else if (duration.count() > 50)
- {
- ARMARX_RT_LOGF_WARN("An NJointController took %d µs to run!", duration.count()).deactivateSpam(5);
+ if (duration.count() > 500)
+ {
+ ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+ }
+ else if (duration.count() > 50)
+ {
+ ARMARX_RT_LOGF_WARN("The NJointController with ID %d took %d µs to run!", nJointCtrl->getId(), duration.count()).deactivateSpam(5);
+ }
}
}
}
@@ -2827,9 +2830,8 @@ namespace armarx
}
- void armarx::RobotUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+ void armarx::RobotUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
-
if (changedProperties.count("ObserverPublishSensorValues"))
{
ObserverPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
@@ -2860,6 +2862,8 @@ namespace armarx
ObserverPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
ObserverPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ checkControllerExecutionTimings = getProperty<bool>("CheckControllerExecutionTimings");
+
//load robot
{
robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 006fcf0cdf65580fab63759bd811faafd138a8f6..38ba178628e48b1f584f566661786c6e7b988767 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -240,6 +240,14 @@ namespace armarx
defineOptionalProperty<std::string>(
"TrajectoryControllerUnitName", "TrajectoryPlayer",
"Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
+
+ // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////////////////////////// Misc. Properties ///////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+
+ defineOptionalProperty<bool>(
+ "CheckControllerExecutionTimings", false,
+ "Check controller execution timings and print a warning if execution takes long.");
}
};
@@ -593,6 +601,7 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
// util
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ bool checkControllerExecutionTimings = false;
protected:
template<class ExceptT = LogicError>
inline void throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn = false) const;
@@ -626,7 +635,7 @@ namespace armarx
static constexpr std::size_t IndexSentinel();
public:
- void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
protected:
virtual void onInitRobotUnit();
virtual void onConnectRobotUnit();
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index b6b0c4bcc356832158afe1a55dd48e62f5f14d84..244ebd5457cf3201e9e3d44136ca67b52620215b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -185,8 +185,7 @@ bool TCPControllerSubUnit::isRequested(const Ice::Current&)
return false;
}
-
-void armarx::TCPControllerSubUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index 9f591b3d430bc480efa896cd39c473de7c36b79f..b8911b7e5a2b05c85d0591322ffda56e6da454a8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -108,6 +108,6 @@ namespace armarx
// PropertyUser interface
public:
- void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 7ea102ce2202e4f8c274af36574bafd7c2c5bf9f..85ab576eeb6c5b6231076ff16fa562fe8053e13c 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -168,7 +168,14 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
return;
}
- this->jointTraj = this->jointTraj->normalize(0, *this->jointTraj->getTimestamps().rbegin() - *this->jointTraj->getTimestamps().begin());
+ if (this->jointTraj->size() == 0)
+ {
+ ARMARX_ERROR << "Error when loading TrajectoryPlayer: trajectory is empty !!!";
+ return;
+ }
+
+ auto startTime = this->jointTraj->begin()->getTimestamp();
+ this->jointTraj->shiftTime(-startTime);
usedJoints = this->jointTraj->getDimensionNames();
ARMARX_INFO << VAROUT(usedJoints);
@@ -370,7 +377,7 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
config->jointNames = jointNames;
config->considerConstraints = considerConstraintsForTrajectoryOptimization;
config->isPreview = isPreview;
-
+ controllerName = this->getName() + "_" + "JointTrajectory" + IceUtil::generateUUID();
trajController = NJointTrajectoryControllerPtr::dynamicCast(
robotUnit->createNJointController(
"NJointTrajectoryController", controllerName,
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 872dd12816d6f7b9fb76f4021678b29ab5e3c0bd..530c49adf230b29eb40f87e778e195927ed856fb 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -246,6 +246,12 @@ void RobotViewerWidgetController::onConnectComponent()
ui.kinematicChainComboBox->addItem(QString::fromStdString(nodeSetName));
}
ui.kinematicChainComboBox->setCurrentIndex(0);
+ ui.tcpComboBox->addItem("<default>");
+ for (auto& node : sharedRobot->getRobotNodes())
+ {
+ ui.tcpComboBox->addItem(QString::fromStdString(node));
+ }
+ ui.tcpComboBox->setCurrentIndex(0);
ui.frameComboBox->addItem("<Global>");
robotNodeNames = sharedRobot->getRobotNodes();
@@ -574,11 +580,12 @@ static std::string writeJointConfigurationToJson(VirtualRobot::Robot& robot,
}
template <typename FrameType>
-static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRobot::RobotNodeSetPtr const& nodeSet, std::string const& frameName)
+static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRobot::RobotNodeSetPtr const& nodeSet, std::string const& frameName, const std::string& tcpName)
{
+ auto tcp = tcpName.empty() ? nodeSet->getTCP() : robot->getRobotNode(tcpName);
if (nodeSet)
{
- Eigen::Matrix4f tcpMatrix = nodeSet->getTCP()->getPoseInRootFrame();
+ Eigen::Matrix4f tcpMatrix = tcp->getPoseInRootFrame();
IceInternal::Handle<FrameType> position = new FrameType(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
position->changeFrame(robot, frameName);
@@ -621,7 +628,7 @@ void RobotViewerWidgetController::updateState()
// Set the locale so that the floats get converted correctly
const char* oldLocale = std::setlocale(LC_ALL, "en_US.UTF-8");
-
+ std::string tcpName = ui.tcpComboBox->currentIndex() == 0 ? "" : ui.tcpComboBox->currentText().toStdString();
std::string output;
switch (outputType)
{
@@ -630,15 +637,15 @@ void RobotViewerWidgetController::updateState()
break;
case eFramedPositionTCP:
- output = writeFramedTCP<FramedPosition>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedPosition>(robot, robotNodeSet, frameName, tcpName);
break;
case eFramedOrientationTCP:
- output = writeFramedTCP<FramedOrientation>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedOrientation>(robot, robotNodeSet, frameName, tcpName);
break;
case eFramedPoseTCP:
- output = writeFramedTCP<FramedPose>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedPose>(robot, robotNodeSet, frameName, tcpName);
break;
default:
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
index a2421fb314a664223e56f7372b0f23c3ca8fe01b..8450a4ec69529c37deb14cc1a9518a10978cfd3e 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
@@ -28,6 +28,13 @@
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout_3">
+ <item row="5" column="2">
+ <widget class="QLabel" name="label_collisionModelInflationValue">
+ <property name="text">
+ <string>0 mm</string>
+ </property>
+ </widget>
+ </item>
<item row="5" column="0">
<widget class="QLabel" name="label_collisionModelInflation">
<property name="text">
@@ -35,13 +42,10 @@
</property>
</widget>
</item>
- <item row="8" column="0" colspan="3">
- <widget class="QComboBox" name="kinematicChainComboBox"/>
- </item>
- <item row="0" column="2">
- <widget class="QCheckBox" name="cbDebugLayer">
+ <item row="1" column="2">
+ <widget class="QCheckBox" name="cbRobot">
<property name="text">
- <string>show debug layer</string>
+ <string>show robot</string>
</property>
<property name="checked">
<bool>true</bool>
@@ -49,70 +53,66 @@
</widget>
</item>
<item row="10" column="0" colspan="3">
- <widget class="QComboBox" name="frameComboBox"/>
- </item>
- <item row="7" column="0" colspan="3">
- <widget class="QLabel" name="chooseKinematicChainLabel">
+ <widget class="QLabel" name="chooseFrameLabel">
<property name="text">
- <string>Choose the kinematic chain:</string>
+ <string>Choose a frame for the coordinates:</string>
</property>
</widget>
</item>
- <item row="13" column="2">
- <widget class="QPushButton" name="copyToClipboardButton">
- <property name="text">
- <string>Copy to Clipboard</string>
- </property>
- </widget>
+ <item row="13" column="0" colspan="3">
+ <widget class="QComboBox" name="outputTypeComboBox"/>
</item>
- <item row="2" column="2">
- <widget class="QCheckBox" name="cbRoot">
- <property name="text">
- <string>show root</string>
+ <item row="15" column="0" colspan="3">
+ <widget class="QPlainTextEdit" name="previewTextBox"/>
+ </item>
+ <item row="5" column="1">
+ <widget class="QSlider" name="horizontalSliderCollisionModelInflation">
+ <property name="enabled">
+ <bool>false</bool>
</property>
- <property name="checked">
- <bool>true</bool>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBelow</enum>
</property>
</widget>
</item>
- <item row="9" column="0" colspan="3">
- <widget class="QLabel" name="chooseFrameLabel">
+ <item row="14" column="0">
+ <widget class="QLabel" name="previewLabel">
<property name="text">
- <string>Choose a frame for the coordinates:</string>
+ <string>Preview:</string>
</property>
</widget>
</item>
- <item row="14" column="0" colspan="3">
- <widget class="QPlainTextEdit" name="previewTextBox"/>
- </item>
- <item row="11" column="0" colspan="3">
- <widget class="QLabel" name="chooseOutputTypeLabel">
+ <item row="0" column="2">
+ <widget class="QCheckBox" name="cbDebugLayer">
<property name="text">
- <string>Choose the output type:</string>
+ <string>show debug layer</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="13" column="0">
- <widget class="QLabel" name="previewLabel">
+ <item row="12" column="0" colspan="3">
+ <widget class="QLabel" name="chooseOutputTypeLabel">
<property name="text">
- <string>Preview:</string>
+ <string>Choose the output type:</string>
</property>
</widget>
</item>
- <item row="12" column="0" colspan="3">
- <widget class="QComboBox" name="outputTypeComboBox"/>
- </item>
- <item row="1" column="2">
- <widget class="QCheckBox" name="cbRobot">
+ <item row="2" column="2">
+ <widget class="QCheckBox" name="cbRoot">
<property name="text">
- <string>show robot</string>
+ <string>show root</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
- <item row="13" column="1">
+ <item row="14" column="1">
<widget class="QCheckBox" name="autoUpdateCheckBox">
<property name="text">
<string>Auto Update</string>
@@ -122,25 +122,19 @@
</property>
</widget>
</item>
- <item row="0" column="0" colspan="2">
- <layout class="QVBoxLayout" name="verticalLayout">
- <property name="sizeConstraint">
- <enum>QLayout::SetMaximumSize</enum>
+ <item row="2" column="0" colspan="2">
+ <widget class="QLineEdit" name="leRobotInfo">
+ <property name="enabled">
+ <bool>false</bool>
</property>
- <item>
- <widget class="QLabel" name="labelRobotViewerName">
- <property name="font">
- <font>
- <family>AlArabiya</family>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>RobotViewer</string>
- </property>
- </widget>
- </item>
- </layout>
+ </widget>
+ </item>
+ <item row="14" column="2">
+ <widget class="QPushButton" name="copyToClipboardButton">
+ <property name="text">
+ <string>Copy to Clipboard</string>
+ </property>
+ </widget>
</item>
<item row="1" column="0" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout">
@@ -163,33 +157,49 @@
</item>
</layout>
</item>
- <item row="2" column="0" colspan="2">
- <widget class="QLineEdit" name="leRobotInfo">
- <property name="enabled">
- <bool>false</bool>
+ <item row="0" column="0" colspan="2">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetMaximumSize</enum>
</property>
- </widget>
+ <item>
+ <widget class="QLabel" name="labelRobotViewerName">
+ <property name="font">
+ <font>
+ <family>AlArabiya</family>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>RobotViewer</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</item>
- <item row="5" column="1">
- <widget class="QSlider" name="horizontalSliderCollisionModelInflation">
- <property name="enabled">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBelow</enum>
+ <item row="11" column="0" colspan="3">
+ <widget class="QComboBox" name="frameComboBox"/>
+ </item>
+ <item row="8" column="0" colspan="2">
+ <widget class="QLabel" name="chooseKinematicChainLabel">
+ <property name="text">
+ <string>Choose the kinematic chain:</string>
</property>
</widget>
</item>
- <item row="5" column="2">
- <widget class="QLabel" name="label_collisionModelInflationValue">
+ <item row="8" column="2">
+ <widget class="QLabel" name="label">
<property name="text">
- <string>0 mm</string>
+ <string>Choose the TCP:</string>
</property>
</widget>
</item>
+ <item row="9" column="0" colspan="2">
+ <widget class="QComboBox" name="kinematicChainComboBox"/>
+ </item>
+ <item row="9" column="2">
+ <widget class="QComboBox" name="tcpComboBox"/>
+ </item>
</layout>
</item>
</layout>
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 1a342947e2210b180d2416b14e3bc78deedd72e8..d07c6eb97eeb1dd8624823d36cea7065451a868f 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -113,6 +113,13 @@ module armarx
int normalID;
int colorID;
};
+
+ struct DebugDrawerNormal
+ {
+ float x;
+ float y;
+ float z;
+ };
/*!
* \brief A triangle face consisting of 3 position-normal-color-vertices.
@@ -122,6 +129,7 @@ module armarx
DebugDrawerVertexID vertex1;
DebugDrawerVertexID vertex2;
DebugDrawerVertexID vertex3;
+ DebugDrawerNormal normal;
};
sequence<DebugDrawerFace> DebugDrawerFaceSequence;
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index b0a1a80cab68dc9f10e178cc00da1e1b65b49c94..12b46048fadfc49819e2bcf46d6bc8f18d916bb5 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -6,3 +6,5 @@ add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotAPINJointControllers)
add_subdirectory(DMPController)
+
+add_subdirectory(RobotStatechartHelpers)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a7d4a2e9e15e6ab016dc614dd2c04b381fb5bcd6
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
@@ -0,0 +1,41 @@
+set(LIB_NAME RobotStatechartHelpers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR}
+ ${Simox_INCLUDE_DIRS}
+ )
+endif()
+
+set(COMPONENT_LIBS
+ ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+ ArmarXCore ArmarXCoreObservers
+)
+
+set(LIB_FILES
+#./RobotStatechartHelpers.cpp
+VelocityControllerHelper.cpp
+PositionControllerHelper.cpp
+ForceTorqueHelper.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+#./RobotStatechartHelpers.h
+VelocityControllerHelper.h
+PositionControllerHelper.h
+ForceTorqueHelper.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..26156ee74a83b61aba5dd57bc6ac3dd38bd9128d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ForceTorqueHelper.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+using namespace armarx;
+
+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::getTorque()
+{
+ return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
+}
+
+void ForceTorqueHelper::setZero()
+{
+ initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
+ initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e3a2d9e065d0ab90c0f0c2f9dca1e2fd66b5f40
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class DatafieldRef;
+ typedef IceInternal::Handle<DatafieldRef> DatafieldRefPtr;
+
+ class ForceTorqueHelper;
+ typedef boost::shared_ptr<ForceTorqueHelper> ForceTorqueHelperPtr;
+
+ class ForceTorqueHelper
+ {
+ public:
+ ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName);
+
+ Eigen::Vector3f getForce();
+ Eigen::Vector3f getTorque();
+ void setZero();
+
+ DatafieldRefPtr forceDf;
+ DatafieldRefPtr torqueDf;
+ Eigen::Vector3f initialForce;
+ Eigen::Vector3f initialTorque;
+ };
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..229077ce661f1c3d49341d1ec296eb2e5b3d5134
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -0,0 +1,113 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PositionControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr &tcp, const VelocityControllerHelperPtr &velocityControllerHelper, const Eigen::Matrix4f &target)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper)
+{
+ waypoints.push_back(target);
+ currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::update()
+{
+ if(!isLastWaypoint() && isTargetNear())
+ {
+ currentWaypointIndex++;
+ }
+ Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+ velocityControllerHelper->setTargetVelocity(cv);
+}
+
+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)
+ {
+ this->waypoints.push_back(pose->toEigen());
+ }
+ currentWaypointIndex = 0;
+}
+
+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;
+}
+
+float PositionControllerHelper::getPositionError() const
+{
+ return posController.getPositionError(getCurrentTarget());
+}
+
+float PositionControllerHelper::getOrientationError() const
+{
+ return posController.getOrientationError(getCurrentTarget());
+}
+
+bool PositionControllerHelper::isTargetReached() const
+{
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+}
+
+bool PositionControllerHelper::isTargetNear() const
+{
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+}
+
+bool PositionControllerHelper::isFinalTargetReached() const
+{
+ return isLastWaypoint() && isTargetReached();
+}
+
+bool PositionControllerHelper::isFinalTargetNear() const
+{
+ return isLastWaypoint() && isTargetNear();
+}
+
+bool PositionControllerHelper::isLastWaypoint() const
+{
+ return currentWaypointIndex + 1 >= waypoints.size();
+}
+
+const Eigen::Matrix4f &PositionControllerHelper::getCurrentTarget() const
+{
+ return waypoints.at(currentWaypointIndex);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..cedc9b5405773a6712fa0fc6aed4e099cd3be2ce
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -0,0 +1,78 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <Eigen/Dense>
+
+#include "VelocityControllerHelper.h"
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+ class PositionControllerHelper;
+ typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
+
+ class PositionControllerHelper
+ {
+ public:
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
+
+ void update();
+
+ void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+ void setNewWaypoints(const std::vector<PosePtr>& waypoints);
+ void addWaypoint(const Eigen::Matrix4f& waypoint);
+ void setTarget(const Eigen::Matrix4f& target);
+
+ float getPositionError() const;
+
+ float getOrientationError() const;
+
+ bool isTargetReached() const;
+ bool isTargetNear() const;
+ bool isFinalTargetReached() const;
+ bool isFinalTargetNear() const;
+
+ bool isLastWaypoint() const;
+
+ const Eigen::Matrix4f& getCurrentTarget() const;
+
+ CartesianPositionController posController;
+ VelocityControllerHelperPtr velocityControllerHelper;
+
+ std::vector<Eigen::Matrix4f> waypoints;
+ size_t currentWaypointIndex = 0;
+
+ float thresholdPositionReached = 0;
+ float thresholdOrientationReached = 0;
+ float thresholdPositionNear = 0;
+ float thresholdOrientationNear = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..720addebc816a025b5ca6fef106dedb2df718f6f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
@@ -0,0 +1,28 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatechartHelpers
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStatechartHelpers.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..c476160c84e8ea782ff55c10f4ca890560abe550
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
@@ -0,0 +1,45 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatechartHelpers
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-RobotStatechartHelpers RobotStatechartHelpers
+ * @ingroup RobotAPI
+ * A description of the library RobotStatechartHelpers.
+ *
+ * @class RobotStatechartHelpers
+ * @ingroup Library-RobotStatechartHelpers
+ * @brief Brief description of class RobotStatechartHelpers.
+ *
+ * Detailed description of class RobotStatechartHelpers.
+ */
+ class RobotStatechartHelpers
+ {
+ public:
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..22d7b732fcb966d797cf93184aad3b862e22abe1
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "VelocityControllerHelper.h"
+
+#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)
+{
+ this->config = config;
+ init();
+}
+
+void VelocityControllerHelper::init()
+{
+ NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+ if (ctrl)
+ {
+ controllerCreated = false;
+ }
+ else
+ {
+ ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+ controllerCreated = true;
+ }
+ 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::cleanup()
+{
+ controller->deactivateController();
+ if (controllerCreated)
+ {
+ while (controller->isControllerActive())
+ {
+ TimeUtil::SleepMS(1);
+ }
+ controller->deleteController();
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..07876699976dd9445d4405a3efe559cbdb2b33bc
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class VelocityControllerHelper;
+ typedef boost::shared_ptr<VelocityControllerHelper> VelocityControllerHelperPtr;
+
+ class VelocityControllerHelper
+ {
+ public:
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName);
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName);
+
+ void init();
+
+ void setTargetVelocity(const Eigen::VectorXf& cv);
+
+ void cleanup();
+
+ NJointCartesianVelocityControllerWithRampConfigPtr config;
+ NJointCartesianVelocityControllerWithRampInterfacePrx controller;
+ RobotUnitInterfacePrx robotUnit;
+ std::string controllerName;
+ bool controllerCreated = false;
+ };
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 1c49e462dee3383b6ac73cdeca27d5f16a4388bc..3ffd160461bef2710fc8041f66cf3d8588819fd4 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -32,7 +32,7 @@ CartesianPositionController::CartesianPositionController(const VirtualRobot::Rob
{
}
-Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
@@ -68,14 +68,14 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
return cartesianVel;
}
-float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose)
+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)
+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);
@@ -84,14 +84,14 @@ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& ta
return aa.angle();
}
-Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose)
+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)
+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);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 9d6843006f86c38460153a67ad44b7d4055cc15d..e0c4ec59905a5672a93e19bb11f4dc0eb7cd4338 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -40,12 +40,12 @@ namespace armarx
public:
CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp);
- Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
- float getPositionError(const Eigen::Matrix4f& targetPose);
- float getOrientationError(const Eigen::Matrix4f& targetPose);
- Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose);
- Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose);
+ float getPositionError(const Eigen::Matrix4f& targetPose) const;
+ float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
//CartesianVelocityController velocityController;
float KpPos = 1;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index a9054c4a45d06e6bce222695f69697c9bf9856f8..62e5740e433786088c18dd6a1a6b9d7c6fcc63cf 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -30,11 +30,11 @@
using namespace armarx;
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp)
+CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod)
: rns(rns)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
this->tcp = tcp ? tcp : rns->getTCP();
this->cartesianMMRegularization = 100;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 66fbcf4bb8ddc7462e2cc087a811cbb5971ff076..9c8c32d8836b15ca259280b6ab3ee4e766375f9c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -39,7 +39,8 @@ namespace armarx
class CartesianVelocityController
{
public:
- CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr);
+ CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
+ const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 0cd53f085c58c3359d439fbfeacd2b50ba77560a..ff6bcc0ed75847d058ee63d1abc49ca8e614c511 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -220,7 +220,7 @@ namespace armarx
string FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -318,7 +318,8 @@ namespace armarx
string FramedPose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, 4, " ", "\n", "", "");
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -658,7 +659,7 @@ namespace armarx
string FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -728,7 +729,7 @@ namespace armarx
string FramedOrientation::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen()/*.format(ArmarXEigenFormat)*/ << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index f6e8963d3ac67555c690e60df458bcf66595b4cf..cf2ef51c61f126f2e7dd25a79d68937fb91b505d 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -147,7 +147,7 @@ namespace armarx
string Vector3::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen().format(ArmarXEigenFormat);
return s.str();
}
@@ -247,7 +247,7 @@ namespace armarx
string Quaternion::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen()/*.format(ArmarXEigenFormat)*/;
return s.str();
}
@@ -328,7 +328,7 @@ namespace armarx
string Pose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen()/*.format(ArmarXEigenFormat)*/;
return s.str();
}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index a3c79e75050b949ff974922122f96e5c1c852ef5..6911f787073c25528b707b632444c638a55e766e 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -47,6 +47,9 @@ namespace armarx
const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
}
+ const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
+
+
/**
* @brief The Vector2 class
* @ingroup VariantsGrp
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index a3cf6245cea50b9a1745480fd01d05b59b9e95bc..ffd7288aae77878a2b49853aaa14fb5c808d7e51 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -211,38 +211,53 @@ namespace armarx
typename timestamp_view::iterator it = dataMap.find(t);
- if (it == dataMap.end() || !it->data.at(dim))
+ if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
{
+ // ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
__fillBaseDataAtTimestamp(t);// interpolates and retrieves
it = dataMap.find(t);
}
+ if (it->data.size() <= dim)
+ {
+ // ARMARX_ERROR << "FAILED!";
+ // ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
+ throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
+ }
+
if (!it->data.at(dim))
// it->data.at(dim).reset(new Ice::DoubleSeq());
{
throw LocalException() << "std::vector ptr is NULL!?";
}
- std::vector<DoubleSeqPtr>& vec = it->data;
+
+ std::vector<DoubleSeqPtr>& vec = it->data;
+ ARMARX_CHECK_GREATER(vec.size(), dim);
if (derivation != 0 && vec.at(dim)->size() <= derivation)
{
//resize and calculate derivations
- size_t curDeriv = it->data.at(dim)->size();
- it->data.at(dim)->resize(derivation + 1);
+ ARMARX_CHECK_GREATER(vec.size(), dim);
+ ARMARX_CHECK_EXPRESSION(vec.at(dim));
+
+ size_t curDeriv = vec.at(dim)->size();
+ // ARMARX_INFO << VAROUT(curDeriv) << VAROUT(dim);
+ vec.at(dim)->resize(derivation + 1);
while (curDeriv <= derivation)
{
double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
checkValue(curDeriv);
- it->data.at(dim)->at(curDeriv) = derivValue;
+ vec.at(dim)->at(curDeriv) = derivValue;
curDeriv++;
}
}
+ ARMARX_CHECK_GREATER_W_HINT(vec.at(dim)->size(), derivation, VAROUT(t) << VAROUT(dim) << VAROUT(*this));
// std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
- double result = it->data.at(dim)->at(derivation);
+ double result = vec.at(dim)->at(derivation);
// checkValue(result);
return result;
}
@@ -795,6 +810,7 @@ namespace armarx
std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
{
+ // ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
// typename timestamp_view::const_iterator it = dataMap.find(t);
// if(it != dataMap.end())
// return it->data;
@@ -831,20 +847,22 @@ namespace armarx
{
typename timestamp_view::const_iterator it = dataMap.find(t);
- if (it != dataMap.end())
+ if (it != dataMap.end() && it->data.size() == dim())
{
bool foundEmpty = false;
for (size_t i = 0; i < it->data.size(); i++)
{
- if (!it->data.at(i))
+ if (!it->data.at(i) || it->data.at(i)->empty())
{
foundEmpty = true;
+ break;
}
}
if (!foundEmpty)
{
+ // ARMARX_INFO << "Was not empty for " << t;
return it;
}
}
@@ -855,7 +873,7 @@ namespace armarx
dataMap.insert(entry);
it = dataMap.find(t);
- assert(it != dataMap.end());
+ ARMARX_CHECK_EXPRESSION(it != dataMap.end());
// const ordered_view& ordv = dataMap.get<TagOrdered>();
// typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
it->data = __calcBaseDataAtTimestamp(t);
@@ -1252,7 +1270,7 @@ namespace armarx
if (itNext == itPrev)
{
- throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size());
+ throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
}
// double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
@@ -1452,24 +1470,28 @@ namespace armarx
double next = 0;
// find previous SystemState that exists for that dimension
- while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL /*|| itPrev->data[dimension]->size() <= derivation*/))
+ while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL || itPrev->data.at(dimension)->size() <= derivation))
{
itPrev--;
}
if (itPrev != ordView.end())
{
+ // ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
+ ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itPrev->timestamp, VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this));
previous = getState(itPrev->timestamp, dimension, derivation);
}
// find next SystemState that exists for that dimension
- while (itNext != ordView.end() && (itNext->data.at(dimension) == NULL /*|| itNext->data[dimension]->size() <= derivation*/))
+ while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
{
itNext++;
}
if (itNext != ordView.end())
{
+ // ARMARX_INFO << "Found next state at " << itNext->timestamp;
+ ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itNext->timestamp, VAROUT(t) << VAROUT(itNext->timestamp));
next = getState(itNext->timestamp, dimension, derivation);
}
@@ -1482,10 +1504,12 @@ namespace armarx
if (itNext == ordView.end())
{
+ // ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
}
else if (itPrev == ordView.end())
{
+ // ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
}
else
@@ -1870,7 +1894,8 @@ namespace armarx
for (; itMap != ordv.end(); itMap++)
{
- //itMap->timestamp -= shift;
+ // since all values are shifted this operation is OK
+ *const_cast<double*>(&(itMap->timestamp)) += shift;
}
}
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 063bef2f10e5ccfb62fdf15bdcb3f6c8d42bb783..0884d4cff46f75a3d58e873e8b05a76998f08771 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -47,12 +47,12 @@ namespace armarx
veloctities.resize(traj->dim(), 1);
}
- Eigen::VectorXf TrajectoryController::update(double deltaT, const Eigen::VectorXf currentPosition)
+ const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
{
ARMARX_CHECK_EXPRESSION(pid);
ARMARX_CHECK_EXPRESSION(traj);
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
- int dim = traj->dim();
+ size_t dim = traj->dim();
currentTimestamp = currentTimestamp + deltaT;
if (currentTimestamp < 0.0)
@@ -60,7 +60,7 @@ namespace armarx
currentTimestamp = 0.0;
}
- for (int i = 0; i < dim; ++i)
+ for (size_t i = 0; i < dim; ++i)
{
positions(i) = traj->getState(currentTimestamp, i, 0);
veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index a12d592f4f9fdefcf7e96bc6fc0967c09f95ac24..f715d29aed2fccf9b077be81bf27207fd5dab555 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -33,7 +33,7 @@ namespace armarx
{
public:
TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
- Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
+ const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
//const MultiDimPIDControllerPtr& getPid() const;
//void setPid(const MultiDimPIDControllerPtr& value);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 96f3271324461e52a93cef33b81fcc814da03b2c..24e7f8467554d87c8797809169cfb701e80bc7f8 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -22,6 +22,7 @@
#include "RobotStateObserver.h"
#include <RobotAPI/interface/core/RobotState.h>
+#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
@@ -35,6 +36,7 @@
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <boost/algorithm/string/trim.hpp>
@@ -206,53 +208,38 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
tcpPoses[tcpName] = new FramedPose(currentPose, GlobalFrame, "");
}
- IceUtil::Time currentTimestamp = IceUtil::Time::microSeconds(timestampMicroSeconds);
- double tDelta = 0.0f;
- if (lastVelocityUpdate.toMicroSeconds() > 0)
- {
- tDelta = (lastVelocityUpdate - currentTimestamp).toMilliSecondsDouble();
- }
-
- for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
- {
- NameValueMap::const_iterator itSrc = jointVel.find(it->first);
-
- if (itSrc != jointVel.end())
- {
- it->second += itSrc->second * tDelta;
- }
- }
velocityReportRobot->setJointValues(tempJointAngles);
velocityReportRobot->setGlobalPose(robot->getGlobalPose());
- // ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
- // start = IceUtil::Time::now();
+
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
- for (unsigned int i = 0; i < nodesToReport.size(); i++)
+ auto keys = armarx::getMapKeys(jointVel);
+ Eigen::VectorXf jointVelocities(jointVel.size());
+ auto rootFrameName = velocityReportRobot->getRootNode()->getName();
+ RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
{
- RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
- const std::string& tcpName = node->getName();
- const Eigen::Matrix4f& currentPose = node->getGlobalPose();
-
+ jointVelocities(i) = jointVel.at(rns->getNode(i)->getName());
+ }
+ DifferentialIKPtr j(new DifferentialIK(rns));
- FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, GlobalFrame, "");
- FramedDirectionPtr::dynamicCast(tcpTranslationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
- mat = currentPose * lastPose->toEigen().inverse();
+ auto robotName = velocityReportRobot->getName();
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
+ {
+ RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
+ Eigen::MatrixXf jac = j->getJacobianMatrix(node, IKSolver::All);
+ Eigen::VectorXf nodeVel = jac * jointVelocities;
- VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
+ const std::string tcpName = node->getName();
+ tcpTranslationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
+ tcpOrientationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
- tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, GlobalFrame, "");
- FramedDirectionPtr::dynamicCast(tcpOrientationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
}
-
- // ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
- // ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index bec92a7fbfc5c74145d2cd1615376f161047f059..57d792ac7cef38364c95c55bdb988e6ee64a8978 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -6,4 +6,5 @@ add_subdirectory(StatechartProfilesTestGroup)
add_subdirectory(OrientedTactileSensorGroup)
add_subdirectory(TrajectoryExecutionCode)
-add_subdirectory(SpeechObserverTestGroup)
\ No newline at end of file
+add_subdirectory(SpeechObserverTestGroup)
+add_subdirectory(ForceTorqueUtility)
\ No newline at end of file
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/CMakeLists.txt b/source/RobotAPI/statecharts/ForceTorqueUtility/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..de39626430545225ec015175d53796e5ea837687
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/CMakeLists.txt
@@ -0,0 +1,45 @@
+armarx_component_set_name("ForceTorqueUtility")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+#find_package(Eigen3 QUIET)
+#find_package(Simox QUIET)
+
+#
+#armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+#armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+#
+#if (Eigen3_FOUND AND Simox_FOUND)
+# include_directories(
+# ${Eigen3_INCLUDE_DIR}
+# ${Simox_INCLUDE_DIRS}
+# )
+#endif()
+
+set(COMPONENT_LIBS
+ ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+ ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+
+# Sources
+
+set(SOURCES
+ForceTorqueUtilityRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+ForceTorqueUtilityRemoteStateOfferer.h
+ForceTorqueUtility.scgxml
+)
+
+# adds all existing state headers and sources to CMake
+armarx_generate_statechart_cmake_lists()
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d36cedf80739a6948b7921599400bf2ab17179e5
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp
@@ -0,0 +1,76 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <thread>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "DetectForceFlank.h"
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+DetectForceFlank::SubClassRegistry DetectForceFlank::Registry(DetectForceFlank::GetName(), &DetectForceFlank::CreateInstance);
+
+void DetectForceFlank::run()
+{
+ ARMARX_CHECK_EXPRESSION(in.getTriggerOnDecreasingForceVectorLength() || in.getTriggerOnIncreasingForceVectorLength());
+
+ const float forceThreshold = in.getForceVectorLengthThreshold();
+ DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+ const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
+ const float initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+
+ while (!isRunningTaskStopped()) // stop run function if returning true
+ {
+
+ const float force = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+ ARMARX_INFO << deactivateSpam(1) << VAROUT(force) << " " << VAROUT(initialForce);
+ if (
+ (
+ in.getTriggerOnDecreasingForceVectorLength() &&
+ force < initialForce &&
+ initialForce - force >= forceThreshold
+ ) ||
+ (
+ in.getTriggerOnIncreasingForceVectorLength() &&
+ force > initialForce &&
+ force - initialForce >= forceThreshold
+ )
+ )
+ {
+ emitSuccess();
+ return;
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds{10});
+ }
+ emitFailure();
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr DetectForceFlank::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new DetectForceFlank(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ac702f87040cc65d88883601292057168ede9bd
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.generated.h>
+
+namespace armarx
+{
+ namespace ForceTorqueUtility
+ {
+ class DetectForceFlank :
+ public DetectForceFlankGeneratedBase < DetectForceFlank >
+ {
+ 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
+ };
+ }
+}
+
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.xml b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.xml
new file mode 100644
index 0000000000000000000000000000000000000000..15db8ac43a4db785013ace55026607029ce3ac14
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="DetectForceFlank" uuid="177EFD66-2F0B-4544-A9AD-0B89A9C9E5E3" width="800" height="600" type="Normal State">
+ <InputParameters>
+ <Parameter name="FTDatafieldName" type="::armarx::StringVariantData" docType="string" optional="no"/>
+ <Parameter name="ForceVectorLengthThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":15}}' docValue="15"/>
+ </Parameter>
+ <Parameter name="ForceWeights" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":1.0,"z":1.0}}}' docValue="1\n1\n1"/>
+ </Parameter>
+ <Parameter name="TriggerOnDecreasingForceVectorLength" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="TriggerOnIncreasingForceVectorLength" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4061852d7a292f75866732b865fb1fd9d6a47567
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
@@ -0,0 +1,118 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <thread>
+
+#include "DetectForceSpike.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+DetectForceSpike::SubClassRegistry DetectForceSpike::Registry(DetectForceSpike::GetName(), &DetectForceSpike::CreateInstance);
+
+
+
+void DetectForceSpike::run()
+{
+ ARMARX_CHECK_EXPRESSION(in.getTriggerInAxisDirection() || in.getTriggerCounterAxisDirection());
+ ARMARX_CHECK_GREATER_EQUAL(in.getWindowSizeMs(), 10);
+ ARMARX_CHECK_GREATER_EQUAL(in.getWindowSizeMs(), 10);
+
+ const float forceThreshold = in.getForceThreshold();
+ ARMARX_CHECK_GREATER(forceThreshold, 0);
+
+ DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+ const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
+ const Eigen::Vector3f axis = in.getAxis()->toEigen().normalized();
+ auto getForceAlongAxis = [&]
+ {
+ return forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).transpose() * axis;
+ };
+
+ in.getTriggerInAxisDirection();
+ in.getTriggerCounterAxisDirection();
+ std::deque<float> spikes(in.getWindowSizeMs()/10, getForceAlongAxis());
+
+ while (!isRunningTaskStopped()) // stop run function if returning true
+ {
+ spikes.push_back(getForceAlongAxis());
+ spikes.pop_front();
+
+ float refValue = spikes.at(0);
+ bool low = true;
+ bool risingEdgeDetected = false;
+ bool fallingEdgeDetected = false;
+
+ bool f2rDetected = false;
+ bool r2fDetected = false;
+
+ for(const float spike : spikes)
+ {
+ if(low)
+ {
+ if(spike < refValue)
+ {
+ refValue = spike;
+ }
+ else if(spike > refValue + forceThreshold)
+ {
+ low = false;
+ risingEdgeDetected = true;
+ f2rDetected |= fallingEdgeDetected;
+ }
+ }
+
+ if(!low)
+ {
+ if(spike > refValue)
+ {
+ refValue = spike;
+ }
+ else if(spike < refValue - forceThreshold)
+ {
+ low = true;
+ fallingEdgeDetected = true;
+ r2fDetected |= risingEdgeDetected;
+ }
+ }
+ }
+ if ((in.getTriggerInAxisDirection() && r2fDetected) || (in.getTriggerCounterAxisDirection() && f2rDetected))
+ {
+ emitSuccess();
+ return;
+ }
+
+ std::this_thread::sleep_for(std::chrono::milliseconds{10});
+ }
+ emitFailure();
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr DetectForceSpike::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new DetectForceSpike(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
new file mode 100644
index 0000000000000000000000000000000000000000..03bd9170b4624887c5e22c97508899277a8e0064
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.generated.h>
+
+namespace armarx
+{
+ namespace ForceTorqueUtility
+ {
+ class DetectForceSpike :
+ public DetectForceSpikeGeneratedBase < DetectForceSpike >
+ {
+ public:
+ DetectForceSpike(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < DetectForceSpike > (stateData), DetectForceSpikeGeneratedBase < DetectForceSpike > (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
+ };
+ }
+}
+
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.xml b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5bb2523df598533989dfbb82c1f78741f2ed8e9f
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="DetectForceSpike" uuid="DCB1C3B2-FBBB-49ED-B0D0-70C35011E5E3" width="800" height="600" type="Normal State">
+ <InputParameters>
+ <Parameter name="Axis" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":0.0,"y":0.0,"z":-1.0}}}' docValue="0\n0\n-1"/>
+ </Parameter>
+ <Parameter name="FTDatafieldName" type="::armarx::StringVariantData" docType="string" optional="no"/>
+ <Parameter name="ForceThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
+ </Parameter>
+ <Parameter name="ForceWeights" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":1.0,"z":1.0}}}' docValue="1\n1\n1"/>
+ </Parameter>
+ <Parameter name="MinDeltaThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}}' docValue="1"/>
+ </Parameter>
+ <Parameter name="TriggerCounterAxisDirection" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="TriggerInAxisDirection" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="WindowSizeMs" type="::armarx::IntVariantData" docType="int" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":300}}' docValue="300"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtility.scgxml b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtility.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..b9bcb4fc6a1ebcd057d30f8e10875c2f24ae0a92
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtility.scgxml
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="ForceTorqueUtility" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPIInterfaces.forceTorqueObserver"/>
+ </Proxies>
+ <Configurations>
+ <Configuration profileName="Armar6Real">
+ArmarX.ForceTorqueUtilityRemoteStateOfferer.ForceTorqueUnitObserverName = Armar6ForceTorqueObserver
+</Configuration>
+ </Configurations>
+ <State filename="DetectForceFlank.xml" visibility="public"/>
+ <State filename="DetectForceSpike.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91dc4d2a822815cbbe916147702d9aab46b16c52
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility::ForceTorqueUtilityRemoteStateOfferer
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ForceTorqueUtilityRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+ForceTorqueUtilityRemoteStateOfferer::SubClassRegistry ForceTorqueUtilityRemoteStateOfferer::Registry(ForceTorqueUtilityRemoteStateOfferer::GetName(), &ForceTorqueUtilityRemoteStateOfferer::CreateInstance);
+
+
+
+ForceTorqueUtilityRemoteStateOfferer::ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > (reader)
+{
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string ForceTorqueUtilityRemoteStateOfferer::GetName()
+{
+ return "ForceTorqueUtilityRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr ForceTorqueUtilityRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+ return XMLStateOffererFactoryBasePtr(new ForceTorqueUtilityRemoteStateOfferer(reader));
+}
+
+
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1299f985f484abb4e31720ec2f1eddb1bd555ac
--- /dev/null
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ForceTorqueUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "ForceTorqueUtilityStatechartContext.generated.h"
+
+namespace armarx
+{
+ 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)
+ {
+ public:
+ ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // 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;
+
+
+ };
+ }
+}
+
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml
index 58dc416b8a4de1b86d3b582acc5d44b048279c9d..3092aeef62efb8951b00fd17bec68ceb79049889 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml
@@ -1,7 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<StatechartGroup name="OrientedTactileSensorGroup" package="RobotAPI" generateContext="true">
<Proxies/>
- <Configurations/>
<State filename="OrientedTactileSensorTest.xml" visibility="public"/>
</StatechartGroup>
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/CMakeLists.txt b/source/RobotAPI/statecharts/SpeechObserverTestGroup/CMakeLists.txt
index 32052c4eb43e2cc8fa23c099623edd6cecbc6757..633f5fb7f4e0475240e1e2b2ff71cedb38fc6404 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/CMakeLists.txt
@@ -25,8 +25,8 @@ armarx_component_set_name("SpeechObserverTestGroup")
#endif()
set(COMPONENT_LIBS
-# ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
- ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+ ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+ ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
# Sources
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
index 5c3cbe960bda92ae7bcd80a0ba9fa96f2631e19e..31abf88dfe93cad5399c7da4ece19e38c9dad1b9 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
@@ -4,7 +4,6 @@
<Proxy value="RobotAPIInterfaces.speechObserver"/>
<Proxy value="RobotAPIInterfaces.textToSpeech"/>
</Proxies>
- <Configurations/>
<State filename="TestTextToSpeech.xml" visibility="public"/>
</StatechartGroup>
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
index 5f7a0472c1ab973f2f32f66e8e35f973f4a84550..f9f78a7ca3843f868e16dfc20bf3da8661228229 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
@@ -6,3 +6,4 @@
</Proxies>
<State filename="WeissHapticSensorTest.xml" visibility="public"/>
</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
index 6e9d45bd33d9b1df4bac077169a9fdc5f01ca9a9..cb8448c7c9287cf7c959ee0243eaffecc49df3da 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
@@ -10,4 +10,3 @@
<Transitions/>
</State>
-