diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 2bbf03b242fa4be240287ca624050274f0af19ca..89a1d7833a64a2bdab3d46d00f07a619f60f32fb 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -40,6 +40,7 @@ set(LIB_FILES
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+ NJointControllers/NJointTCPController.cpp
Devices/SensorDevice.cpp
Devices/ControlDevice.cpp
@@ -79,6 +80,7 @@ set(LIB_HEADERS
NJointControllers/NJointTrajectoryController.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+ NJointControllers/NJointTCPController.h
Devices/DeviceBase.h
Devices/SensorDevice.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f0ef41fa60c6d2881a1575312b3854a925ad2d9
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -0,0 +1,199 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointTCPController.h"
+#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
+
+#define DEFAULT_TCP_STRING "default TCP"
+
+namespace armarx
+{
+
+ NJointControllerRegistration<NJointTCPController> registrationControllerNJointTCPController("NJointTCPController");
+
+ std::string NJointTCPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTCPController";
+ }
+
+ void NJointTCPController::rtPreActivateController()
+ {
+ xVel = 0;
+ yVel = 0;
+ zVel = 0;
+ rollVel = 0;
+ pitchVel = 0;
+ yawVel = 0;
+ }
+
+ void NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::VectorXf x(6);
+ x << this->xVel, yVel, zVel, rollVel, pitchVel, yawVel;
+ Eigen::MatrixXf jacobiInv = ik->getPseudoInverseJacobianMatrix(tcp);
+ Eigen::VectorXf jointTargetVelocities = jacobiInv * x;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+ }
+ }
+
+
+ ::armarx::WidgetDescription::WidgetSeq createSliders()
+ {
+ using namespace armarx::WidgetDescription;
+ ::armarx::WidgetDescription::WidgetSeq widgets;
+ auto addSlider = [&](const std::string & label, float limit)
+ {
+ widgets.emplace_back(new Label(false, label));
+ {
+ FloatSliderPtr c_x = new FloatSlider;
+ c_x->name = label;
+ c_x->min = -limit;
+ c_x->defaultValue = 0.0f;
+ c_x->max = limit;
+ widgets.emplace_back(c_x);
+ }
+ };
+
+ addSlider("x", 100);
+ addSlider("y", 100);
+ addSlider("z", 100);
+ addSlider("r", 0.5);
+ addSlider("p", 0.5);
+ addSlider("y", 0.5);
+ return widgets;
+ }
+
+ WidgetDescription::WidgetPtr NJointTCPController::GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+
+ LabelPtr label = new Label;
+ label->text = "nodeset name";
+ layout->children.emplace_back(label);
+ StringComboBoxPtr box = new StringComboBox;
+ box->defaultIndex = 0;
+
+ box->options = robot->getRobotNodeSetNames();
+ box->name = "nodeSetName";
+ layout->children.emplace_back(box);
+
+ LabelPtr labelTCP = new Label;
+ labelTCP->text = "tcp name";
+ layout->children.emplace_back(labelTCP);
+ StringComboBoxPtr boxTCP = new StringComboBox;
+ boxTCP->defaultIndex = 0;
+
+ boxTCP->options = robot->getRobotNodeNames();
+ boxTCP->options.insert(boxTCP->options.begin(), DEFAULT_TCP_STRING);
+ boxTCP->name = "tcpName";
+ layout->children.emplace_back(boxTCP);
+
+ LabelPtr labelMode = new Label;
+ labelMode->text = "mode";
+ layout->children.emplace_back(labelMode);
+ StringComboBoxPtr boxMode = new StringComboBox;
+ boxMode->defaultIndex = 0;
+
+ boxMode->options = {"Position", "Orientation", "Both"};
+ boxMode->name = "mode";
+ layout->children.emplace_back(boxMode);
+
+
+ // auto sliders = createSliders();
+ // layout->children.insert(layout->children.end(),
+ // sliders.begin(),
+ // sliders.end());
+ layout->children.emplace_back(new HSpacer);
+ return layout;
+ }
+
+ NJointTCPControllerConfigPtr NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ {
+ return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
+ }
+
+ NJointTCPController::NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, NJointTCPControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
+ {
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
+ auto nodeset = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(nodeset, config->nodeSetName);
+ for (size_t i = 0; i < nodeset->getSize(); ++i)
+ {
+ auto jointName = nodeset->getNode(i)->getName();
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
+ };
+ ik.reset(new VirtualRobot::DifferentialIK(nodeset, robotUnit->getRtRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
+ }
+
+ void NJointTCPController::rtPostDeactivateController()
+ {
+
+ }
+
+ WidgetDescription::StringWidgetDictionary NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+ auto sliders = createSliders();
+ layout->children.insert(layout->children.end(),
+ sliders.begin(),
+ sliders.end());
+ return {{"ControllerConfig", layout}};
+ }
+
+ void NJointTCPController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ {
+ if (name == "ControllerConfig")
+ {
+ xVel = valueMap.at("x")->getFloat();
+ yVel = valueMap.at("y")->getFloat();
+ zVel = valueMap.at("z")->getFloat();
+ rollVel = valueMap.at("r")->getFloat();
+ pitchVel = valueMap.at("p")->getFloat();
+ yawVel = valueMap.at("y")->getFloat();
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown function name called: " << name;
+ }
+ }
+
+
+
+} // namespace armarx
+
+
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..356457a7ea461d0f0755ff4b71328b2d1109be3e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+
+#include "NJointController.h"
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointTCPController);
+ TYPEDEF_PTRS_HANDLE(NJointTCPControllerConfig);
+
+ class NJointTCPControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName):
+ nodeSetName(nodeSetName),
+ tcpName(tcpName)
+ {}
+
+ const std::string nodeSetName;
+ const std::string tcpName;
+ };
+
+
+ class NJointTCPController : public NJointController
+ {
+ public:
+ using ConfigPtrT = NJointTCPControllerConfigPtr;
+ NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const override;
+ WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr GenerateConfigDescription(
+ const boost::shared_ptr<VirtualRobot::Robot>&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointTCPController(
+ NJointControllerDescriptionProviderInterfacePtr prov,
+ NJointTCPControllerConfigPtr config,
+ const boost::shared_ptr<VirtualRobot::Robot>& r);
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+ private:
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+
+ std::atomic<float> xVel;
+ std::atomic<float> yVel;
+ std::atomic<float> zVel;
+ std::atomic<float> rollVel;
+ std::atomic<float> pitchVel;
+ std::atomic<float> yawVel;
+ VirtualRobot::RobotNodePtr tcp;
+ VirtualRobot::DifferentialIKPtr ik;
+
+
+
+
+
+ };
+
+} // namespace armarx
+