Merge branch 'master' into SensorPackage

Conflicts:
	source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
	source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
	source/RobotAPI/libraries/CMakeLists.txt

source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp

@@ -106,7 +106,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
     QComboBox* deviceBox = widget.deviceComboBox;
 
     boost::mutex::scoped_lock lock(scanMutex);
-    for (auto & pair : scans)
+    for (auto& pair : scans)
     {
         QString deviceName(QString::fromStdString(pair.first.c_str()));
         if (deviceBox->findText(deviceName) < 0)
@@ -117,7 +117,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
 
     std::string deviceName(deviceBox->currentText().toUtf8().data());
     float stepSize = 0.25f;
-    for (LaserScannerInfo const & scanner : laserScanners)
+    for (LaserScannerInfo const& scanner : laserScanners)
     {
         if (scanner.device == deviceName)
         {
@@ -147,7 +147,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
 
     LaserScan& scan = scans[deviceName];
     float maxDistance = 1000.0f;
-    for (LaserScanStep & step : scan)
+    for (LaserScanStep& step : scan)
     {
         if (step.distance > maxDistance)
         {
@@ -165,7 +165,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
     int maxNumberOfRings = *std::max_element(history.begin(), history.end());
     float outerRadius = maxNumberOfRings * ringDistance;
 
-    for (LaserScanStep & step : scan)
+    for (LaserScanStep& step : scan)
     {
         line(step.angle, step.distance / outerRadius);
     }
@@ -186,7 +186,7 @@ void LaserScannerPluginWidgetController::onDeviceSelected(int index)
         return;
     }
     std::string deviceName = widget.deviceComboBox->itemText(index).toStdString();
-    for (LaserScannerInfo const & scanner : laserScanners)
+    for (LaserScannerInfo const& scanner : laserScanners)
     {
         if (scanner.device == deviceName)
         {

source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h

@@ -50,8 +50,8 @@ namespace armarx
      */
     class ARMARXCOMPONENT_IMPORT_EXPORT
         LaserScannerPluginWidgetController:
-    public armarx::ArmarXComponentWidgetController,
-    public armarx::LaserScannerUnitListener
+        public armarx::ArmarXComponentWidgetController,
+        public armarx::LaserScannerUnitListener
     {
         Q_OBJECT
 

source/RobotAPI/interface/CMakeLists.txt

@@ -37,14 +37,19 @@ set(SLICE_FILES
     units/UnitInterface.ice
     units/ATINetFTUnit.ice
     units/OrientedTactileSensorUnit.ice
+    units/GamepadUnit.ice
+    units/MetaWearIMUInterface.ice
+    units/MetaWearIMU.ice
 
     components/ViewSelectionInterface.ice
 
     visualization/DebugDrawerInterface.ice
 
-    libraries/RTControllers/LVL1Controller.ice
-    libraries/RTControllers/PassThroughController.ice
-    libraries/RTControllers/RobotUnitInterface.ice
+    libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice
+    libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
+    libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
+    libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice
+    libraries/RTRobotUnit/RobotUnitInterface.ice
 )
     #core/RobotIK.ice
 

source/RobotAPI/interface/CMakeLists.txt.orig

-###
-### CMakeLists.txt file for ArmarX Interfaces
-###
-
-set(ROBOTAPI_INTERFACE_DEPEND ArmarXCore)
-
-set(SLICE_FILES
-    observers/KinematicUnitObserverInterface.ice
-    observers/PlatformUnitObserverInterface.ice
-    observers/ObserverFilters.ice
-
-    core/PoseBase.ice
-    core/OrientedPoint.ice
-    core/LinkedPoseBase.ice
-    core/FramedPoseBase.ice
-    core/RobotState.ice
-    core/RobotStateObserverInterface.ice
-    core/Trajectory.ice
-
-    selflocalisation/SelfLocalisationProcess.ice
-
-    speech/SpeechInterface.ice
-
-    units/ForceTorqueUnit.ice
-    units/InertialMeasurementUnit.ice
-    units/OptoForceUnit.ice
-    units/HandUnitInterface.ice
-    units/HapticUnit.ice
-    units/WeissHapticUnit.ice
-    units/HeadIKUnit.ice
-    units/KinematicUnitInterface.ice
-    units/PlatformUnitInterface.ice
-    units/RobotPoseUnitInterface.ice
-    units/TCPControlUnit.ice
-    units/TCPMoverUnitInterface.ice
-    units/UnitInterface.ice
-    units/ATINetFTUnit.ice
-<<<<<<< HEAD
-    units/OrientedTactileSensorUnit.ice
-=======
->>>>>>> master
-
-    components/ViewSelectionInterface.ice
-
-    visualization/DebugDrawerInterface.ice
-
-    libraries/RTControllers/LVL1Controller.ice
-    libraries/RTControllers/PassThroughController.ice
-    libraries/RTControllers/RobotUnitInterface.ice
-)
-    #core/RobotIK.ice
-
-# generate the interface library
-armarx_interfaces_generate_library(RobotAPI "${ROBOTAPI_INTERFACE_DEPEND}")

source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice

+/*
+ * 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::LVL1KinematicUnitControllerConfigs
+ * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
+
+module armarx
+{
+    class LVL1HolonomicPlatformVelocityPassThroughControllerConfig extends LVL1ControllerConfig
+    {
+        string platformName;
+        float initialVelocityX;
+        float initialVelocityY;
+        float initialVelocityRotation;
+    };
+
+    interface LVL1HolonomicPlatformVelocityPassThroughControllerInterface extends LVL1ControllerInterface
+    {
+        idempotent void setVelocites(float velocityX, float velocityY, float velocityRotation);
+    };
+};
+#endif

source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice

+/*
+ * 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::LVL1KinematicUnitControllerConfigs
+ * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
+
+module armarx
+{
+    class LVL1KinematicUnitVelocityControllerConfig extends LVL1ControllerConfig
+    {
+        string jointName;
+
+        float maxVelocity;
+        float acceleration;
+        float deceleration;
+        float maxDt;
+        float directSetLimit;
+
+        float limitPositionLo;
+        float limitPositionHi;
+
+        float initialVelocityTargetValue;
+    };
+
+    class LVL1KinematicUnitTorqueControllerConfig extends LVL1ControllerConfig
+    {
+        string jointName;
+        float maxTorque;
+        float initialTorqueTargetValue;
+    };
+
+    class LVL1KinematicUnitPositionControllerConfig extends LVL1ControllerConfig
+    {
+        string jointName;
+
+        float maxVelocity;
+        float acceleration;
+        float deceleration;
+        float maxDt;
+        float directPControlLimit;
+        float p;
+
+        float limitPositionLo;
+        float limitPositionHi;
+
+        float initialPositionTargetValue;
+    };
+};
+#endif

source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice → source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice

@@ -13,27 +13,27 @@
  * 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::PassThroughController
+ * @package    RobotAPI::LVL1PassThroughController
  * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
  * @date       2017
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
 
-#ifndef _ARMARX_ROBOTAPI_PassThroughController_SLICE_
-#define _ARMARX_ROBOTAPI_PassThroughController_SLICE_
+#ifndef _ARMARX_ROBOTAPI_LVL1PassThroughController_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1PassThroughController_SLICE_
 
 #include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
 
 module armarx
 {
-    class PassThroughControllerConfig extends LVL1ControllerConfig
+    class LVL1PassThroughControllerConfig extends LVL1ControllerConfig
     {
         Ice::StringSeq jointNames;
     };
 
-    interface PassThroughControllerInterface extends LVL1ControllerInterface
+    interface LVL1PassThroughControllerInterface extends LVL1ControllerInterface
     {
         idempotent void setJoint(string joint, float value) throws InvalidArgumentException;
         idempotent void setJoints(StringFloatDictionary values) throws InvalidArgumentException;

source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice → source/RobotAPI/interface/libraries/RTRobotUnit/RobotUnitInterface.ice

@@ -25,7 +25,7 @@
 
 #include <ArmarXCore/interface/core/UserException.ice>
 
-#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
 
 #include <RobotAPI/interface/units/KinematicUnitInterface.ice>
 #include <RobotAPI/interface/units/ForceTorqueUnit.ice>

source/RobotAPI/interface/units/GamepadUnit.ice

+/*
+ * 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/>.
+ *
+ * @package    RobotAPI
+ * @author     Markus Grotz <markus dot grotz at kit dot edu>
+ * @date       2015
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_GAMEPAD_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_GAMEPAD_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Matrix.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+    struct GamepadData {
+        float leftStickX;
+        float leftStickY;
+        float rightStickX;
+        float rightStickY;
+        float dPadX;
+        float dPadY;
+        float leftTrigger;
+        float rightTrigger;
+
+        bool leftButton;
+        bool rightButton;
+        bool backButton;
+        bool startButton;
+        bool xButton;
+        bool yButton;
+        bool aButton;
+        bool bButton;
+        bool theMiddleButton;
+        bool leftStickButton;
+        bool rightStickButton;
+
+    };
+    interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
+    {
+    };
+
+    interface GamepadUnitListener
+    {	
+        void reportGamepadState(string device, string name, GamepadData values, TimestampBase timestamp);
+    };
+	/**
+	* Implements an interface to an GamepadUnitObserver.
+	**/
+    interface GamepadUnitObserverInterface extends ObserverInterface, GamepadUnitListener
+    {
+    };
+
+};
+
+#endif

source/RobotAPI/interface/units/MetaWearIMU.ice

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-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    RobotAPI
+ * @author     Lukas Kaul ( lukas dot kaul at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_MetaWearIMU_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_MetaWearIMU_SLICE_
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
+module armarx
+{
+    struct MetaWearIMUData {
+        Ice::FloatSeq orientationQuaternion;
+        Ice::FloatSeq magnetic;
+        Ice::FloatSeq gyro;
+        Ice::FloatSeq acceleration;
+    };
+
+    interface MetaWearIMUListener
+    {
+        void reportIMUValues(string name, MetaWearIMUData data, TimestampBase timestamp);
+    };
+    interface MetaWearIMUObserverInterface extends ObserverInterface, MetaWearIMUListener
+    {
+    };
+};
+
+#endif

source/RobotAPI/interface/units/MetaWearIMUInterface.ice

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-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    RobotAPI
+ * @author     Lukas Kaul ( lukas dot kaul at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_MetaWearIMUInterface_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_MetaWearIMUInterface_SLICE_
+
+
+module armarx
+{
+    sequence<float> FloatSeq;
+
+    struct MetaWearIMUDataExt {
+        FloatSeq orientationQuaternion;
+        FloatSeq magnetic;
+        FloatSeq gyro;
+        FloatSeq acceleration;
+    };
+
+    interface MetaWearIMUInterface
+    {
+        void reportIMUValues(string name, MetaWearIMUDataExt data);
+    };
+};
+
+#endif

source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt

-armarx_component_set_name("BasicRTControllers")
-armarx_set_target("Library: BasicRTControllers")
-
-set(LIB_NAME       BasicRTControllers)
-
-set(LIBS
-	ArmarXCoreInterfaces 
-	ArmarXCore
-    RobotAPIInterfaces
-    RobotRTControllers
-)
-
-set(LIB_FILES
-    PassThroughController.cpp
-)
-set(LIB_HEADERS
-    PassThroughController.h
-)
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

source/RobotAPI/libraries/CMakeLists.txt

 add_subdirectory(core)
 add_subdirectory(widgets)
-add_subdirectory(RobotRTControllers)
-add_subdirectory(BasicRTControllers)
+add_subdirectory(RTRobotUnit)
 add_subdirectory(SimpleJsonLogger)
-

source/RobotAPI/libraries/CMakeLists.txt.orig

+add_subdirectory(core)
+add_subdirectory(widgets)
+<<<<<<< HEAD
+add_subdirectory(RobotRTControllers)
+add_subdirectory(BasicRTControllers)
+add_subdirectory(SimpleJsonLogger)
+=======
+add_subdirectory(RTRobotUnit)
+>>>>>>> master
+

source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp

+/*
+ * 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
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "BasicControllers.h"
+
+#include <boost/algorithm/clamp.hpp>
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+///REMOVE
+#include <iostream>
+
+namespace armarx
+{
+    float velocityControlWithAccelerationBounds(
+        float dt, float maxDt,
+        const float currentV, float targetV, float maxV,
+        float acceleration, float deceleration,
+        float directSetVLimit
+    )
+    {
+        dt = std::min(std::abs(dt), std::abs(maxDt));
+        maxV = std::abs(maxV);
+        acceleration = std::abs(acceleration);
+        deceleration = std::abs(deceleration);
+        targetV = boost::algorithm::clamp(targetV, -maxV, maxV);
+
+        //we can have 3 cases:
+        // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+        // 2. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
+        // 3. we need to decellerate               (other cases)
+
+        //handle case 1
+        const float curverror = targetV - currentV;
+        if (std::abs(curverror) < directSetVLimit)
+        {
+            return targetV;
+        }
+
+        //handle case 2 + 3
+        const bool accelerate = sign(targetV) == sign(currentV) &&     // v in same direction
+                                std::abs(targetV) > std::abs(currentV); // currently to slow
+
+        const float usedacc = accelerate ? acceleration : -deceleration;
+        const float maxDeltaV = std::abs(usedacc * dt);
+        if (maxDeltaV >= std::abs(curverror))
+        {
+            //we change our v too much with the used acceleration
+            //but we could reach our target with a lower acceleration ->set the target
+
+            return targetV;
+        }
+        const float deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * dt, -maxDeltaV, maxDeltaV);
+        const float nextV = currentV + deltaVel;
+        return nextV;
+    }
+
+    ////////////////////////////
+    //wip?
+    float velocityControlWithAccelerationAndPositionBounds(
+        float dt, float maxDt,
+        float currentV, float targetV, float maxV,
+        float acceleration, float deceleration,
+        float directSetVLimit,
+        float currentPosition,
+        float positionLimitLoSoft, float positionLimitHiSoft,
+        float positionSoftLimitViolationVelocity,
+        float positionLimitLoHard, float positionLimitHiHard)
+    {
+        if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
+        {
+            return std::nanf("1");
+        }
+
+        float softLimitViolation = 0;
+        if (currentPosition <= positionLimitLoSoft)
+        {
+            softLimitViolation = -1;
+        }
+        if (currentPosition >= positionLimitHiSoft)
+        {
+            softLimitViolation = 1;
+        }
+
+        const float upperDt = std::max(std::abs(dt), std::abs(maxDt));
+        dt = std::min(std::abs(dt), std::abs(maxDt));
+        maxV = std::abs(maxV);
+
+        //we can have 4 cases:
+        // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them, e.g. if we violated them initially)
+        // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+        // 3. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
+        // 4. we need to decellerate               (other cases)
+        float nextv;
+        //handle case 1
+        const float vsquared = currentV * currentV;
+        const float brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+
+
+        const float posIfBrakingNow = currentPosition + brakingDist;
+        if (posIfBrakingNow <= positionLimitLoSoft || posIfBrakingNow >= positionLimitHiSoft)
+        {
+            //case 1. -> brake now! (we try to have v=0 at the limit)
+            const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+            const float wayToGo = limit - currentPosition;
+
+            //decellerate!
+            // s = v²/(2a)  <=>  a = v²/(2s)
+            const float dec = std::abs(vsquared / 2.f / wayToGo);
+            const float vel = currentV - sign(currentV) * dec * upperDt;
+            nextv = boost::algorithm::clamp(vel, -maxV, maxV);
+            if (sign(currentV) != sign(nextv))
+            {
+                //stop now
+                nextv = 0;
+            }
+        }
+        else
+        {
+            //handle 2-3
+            nextv = velocityControlWithAccelerationBounds(dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
+        }
+        if (softLimitViolation == sign(nextv))
+        {
+            //the area between soft and hard limits is sticky
+            //the controller can only move out of it (not further in)
+            return 0;
+        }
+
+        //the next velocity will not violate the pos bounds harder than they are already
+        return nextv;
+    }
+
+
+    float positionThroughVelocityControlWithAccelerationBounds(
+        float dt, float maxDt,
+        float currentV, float maxV,
+        float acceleration, float deceleration,
+        float currentPosition, float targetPosition,
+        float pControlPosErrorLimit, float pControlVelLimit, float p
+    )
+    {
+        dt = std::min(std::abs(dt), std::abs(maxDt));
+        maxV = std::abs(maxV);
+        acceleration = std::abs(acceleration);
+        deceleration = std::abs(deceleration);
+        pControlPosErrorLimit = std::abs(pControlPosErrorLimit);
+        pControlVelLimit = std::abs(pControlVelLimit);
+        const float signV = sign(currentV);
+        //we can have 3 cases:
+        // 1. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentV| < pControlVelLimit)
+        // 2. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
+        //                                                 the brakingDistance have the same sign and brakingDistance < e
+        //                                                 and currentVel <= maxV)
+        // 3. we need to decellerate                   (other cases)
+
+        //handle case 1
+        const float positionError = targetPosition - currentPosition;
+        if (std::abs(positionError) < pControlPosErrorLimit && std::abs(currentV) < pControlVelLimit)
+        {
+            return positionError * p;
+        }
+
+        //handle case 2-3
+        const float brakingDistance = signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+        const float posIfBrakingNow = currentPosition + brakingDistance;
+        const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
+
+        const bool decelerate =
+            std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
+            std::abs(posIfBrakingNow - targetPosition) <= pControlPosErrorLimit || // we want to hit the target
+            sign(posErrorIfBrakingNow) != signV;  // we are moving away from the target
+        const float usedacc = decelerate ?  -deceleration : acceleration;
+        const float maxDeltaV = std::abs(usedacc * dt);
+        const float deltaVel = boost::algorithm::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
+        return boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+    }
+
+    float positionThroughVelocityControlWithAccelerationAndPositionBounds(
+        float dt, float maxDt,
+        float currentV, float maxV,
+        float acceleration, float deceleration,
+        float currentPosition, float targetPosition,
+        float pControlPosErrorLimit, float pControlVelLimit, float p,
+        float positionLimitLo, float positionLimitHi
+
+    )
+    {
+        dt = std::min(std::abs(dt), std::abs(maxDt));
+        maxV = std::abs(maxV);
+        acceleration = std::abs(acceleration);
+        deceleration = std::abs(deceleration);
+        pControlPosErrorLimit = std::abs(pControlPosErrorLimit);
+        const float signV = sign(currentV);
+        //we can have 4 cases:
+        // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them)
+        // 2. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentPosition - targetPosition|*p < maxV)
+        // 4. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
+        //                                                 and the brakingDistance have the same sign and brakingDistance < e
+        //                                                 and currentVel <= maxV)
+        // 5. we need to decellerate                   (other cases)
+
+        //handle case 1
+        const float vsquared = currentV * currentV;
+        const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
+        const float posIfBrakingNow = currentPosition + brakingDistance;
+        if (posIfBrakingNow <= positionLimitLo || posIfBrakingNow >= positionLimitHi)
+        {
+            //case 1. -> brake now! (we try to have v=0 at the limit)
+            const auto limit = brakingDistance > 0 ? positionLimitHi : positionLimitLo;
+            const float wayToGo = std::abs(limit - currentPosition);
+            // s = v²/(2a)  <=>  a = v²/(2s)
+            const float dec = std::abs(vsquared / 2.f / wayToGo);
+            const float vel = currentV - signV * dec * dt;
+            return vel;
+        }
+
+        //handle case 2-3
+        return positionThroughVelocityControlWithAccelerationBounds(
+                   dt, maxDt,
+                   currentV, maxV,
+                   acceleration, deceleration,
+                   currentPosition, boost::algorithm::clamp(targetPosition, positionLimitLo, positionLimitHi),
+                   pControlPosErrorLimit, pControlVelLimit, p
+               );
+    }
+
+    ////////////////////////////
+    //wip
+
+
+    float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+        float dt, float maxDt,
+        float currentV, float maxV,
+        float acceleration, float deceleration,
+        float currentPosition, float targetPosition,
+        float pControlPosErrorLimit, float pControlVelLimit, float p,
+        float& direction,
+        float positionPeriodLo, float positionPeriodHi)
+    {
+        currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
+        targetPosition  = periodicClamp(targetPosition , positionPeriodLo, positionPeriodHi);
+
+        const float brakingDist = brakingDistance(currentV, deceleration);
+        const float posIfBrakingNow = currentPosition + brakingDist;
+        const float posIfBrakingNowError = targetPosition - posIfBrakingNow;
+        const float posError = targetPosition - currentPosition;
+        if (
+            std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
+            std::abs(posError) <= pControlPosErrorLimit
+        )
+        {
+            //this allows slight overshooting (in the limits of the p controller)
+            return positionThroughVelocityControlWithAccelerationBounds(
+                       dt, maxDt,
+                       currentV, maxV,
+                       acceleration, deceleration,
+                       currentPosition, targetPosition,
+                       pControlPosErrorLimit, pControlVelLimit, p
+                   );
+        }
+
+        //we transform the problem with periodic bounds into
+        //a problem with position bounds
+        const float positionPeriodLength  = std::abs(positionPeriodHi - positionPeriodLo);
+
+        //we shift the positions such that 0 == target
+        currentPosition = periodicClamp(currentPosition - targetPosition,  0.f, positionPeriodLength);
+        //how many times will we go ovet the target if we simply bake now?
+        const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
+
+        if (true || direction == 0)
+        {
+            //determine the direction to go (1 == pos vel, -1 == neg vel)
+            direction = (periodicClamp(currentPosition + brakingDist, 0.f , positionPeriodLength) >= positionPeriodLength / 2) ? 1 : -1;
+        }
+        //shift the target away from 0
+        targetPosition = (overshoot - std::min(0.f, -direction)) * positionPeriodLength; // - direction * pControlPosErrorLimit;
+
+        //move
+        return positionThroughVelocityControlWithAccelerationBounds(
+                   dt, maxDt,
+                   currentV, maxV,
+                   acceleration, deceleration,
+                   currentPosition, targetPosition,
+                   pControlPosErrorLimit, pControlVelLimit, p
+               );
+    }
+
+
+}

source/RobotAPI/libraries/RTRobotUnit/BasicControllers.h

+/*
+ * 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
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_BasicControllers_H
+#define _ARMARX_LIB_RobotAPI_BasicControllers_H
+
+#include <cmath>
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+    /**
+     * @param v0 The initial velocity.
+     * @param deceleration The deceleration.
+     * @return The braking distance given the parameters.
+     */
+    inline float brakingDistance(float v0, float deceleration)
+    {
+        return sign(v0) * std::pow(v0, 2.f) / 2.f / std::abs(deceleration);
+    }
+
+    template<class T>
+    T periodicClamp(T value, T periodLo, T periodHi)
+    {
+        float dist = periodHi - periodLo;
+        return std::fmod(value - periodLo + dist, dist) + periodLo;
+    }
+
+    float velocityControlWithAccelerationBounds(
+        float dt, float maxDt,
+        const float currentV, float targetV, float maxV,
+        float acceleration, float deceleration,
+        float directSetVLimit);
+    /**
+     * @brief Performs velocity control while staying in positional bounds, obeying acceleration / deceleration and staying below a velocity bound.
+     *
+     * we can have 4 cases:
+     * 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them. e.f. if we already violate them or the robot can't brake fast enough)
+     * 2. we directly set v and ignore acc/dec (if |currentV - targetV| <= directSetVLimit)
+     * 3. we need to accelerate                (if currentV and targetV have same sign and |currentV| < |currentV|)
+     * 4. we need to decellerate               (other cases)
+     * @param dt The time in seconds until the next call is made. (use the time since the last call as an approximate)
+     * @param maxDt Limits dt in case the given approximation has a sudden high value.
+     * @param currentV
+     * @param targetV
+     * @param maxV
+     * @param acceleration
+     * @param deceleration
+     * @param directSetVLimit In case |currentV - targetV| <= directSetVLimit this function will return targetV (if the position bounds are not violated)
+     * @param currentPosition
+     * @param positionLimitLo
+     * @param positionLimitHi
+     * @return The next velocity.
+     */
+    float velocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
+            float currentV, float targetV, float maxV,
+            float acceleration, float deceleration,
+            float directSetVLimit,
+            float currentPosition,
+            float positionLimitLoSoft, float positionLimitHiSoft,
+            float positionSoftLimitViolationVelocity,
+            float positionLimitLoHard, float positionLimitHiHard);
+
+    float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
+            float currentV, float maxV,
+            float acceleration, float deceleration,
+            float currentPosition, float targetPosition,
+            float pControlPosErrorLimit, float pControlVelLimit, float p
+                                                              );
+
+    float positionThroughVelocityControlWithAccelerationAndPositionBounds(
+        float dt, float maxDt,
+        float currentV, float maxV,
+        float acceleration, float deceleration,
+        float currentPosition, float targetPosition,
+        float pControlPosErrorLimit, float pControlVelLimit, float p,
+        float positionLimitLo, float positionLimitHi);
+
+    //    float positionThroughVelocityControlWithAccelerationBounds(
+    //            float dt, float maxDt,
+    //            float currentV, float maxV,
+    //            float acceleration, float deceleration,
+    //            float currentPosition, float targetPosition,
+    //            float pControlPosErrorLimit, float p);
+
+
+
+    float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(float dt, float maxDt,
+            float currentV, float maxV,
+            float acceleration, float deceleration,
+            float currentPosition, float targetPosition,
+            float pControlPosErrorLimit, float pControlVelLimit, float p,
+            float& direction,
+            float positionPeriodLo, float positionPeriodHi);
+}
+
+#endif

source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt

+set(LIB_NAME       RTRobotUnit)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+	ArmarXCoreInterfaces 
+        ArmarXCore
+        RobotAPIUnits
+        RobotAPIInterfaces
+)
+
+set(LIB_FILES
+    SyntaxCheck.cpp
+
+    LVL0Controllers/LVL0Controller.cpp
+
+    LVL1Controllers/LVL1Controller.cpp
+    LVL1Controllers/LVL1PassThroughController.cpp
+    LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
+    LVL1Controllers/LVL1KinematicUnitPositionController.cpp
+    LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
+    LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
+
+    RobotUnit.cpp
+    BasicControllers.cpp
+)
+set(LIB_HEADERS
+    Constants.h
+    ControlModes.h
+
+    Targets/TargetBase.h
+    Targets/ActuatorPositionTarget.h
+    Targets/ActuatorVelocityTarget.h
+    Targets/ActuatorTorqueTarget.h
+    Targets/PlatformWheelVelocityTarget.h
+    Targets/HolonomicPlatformVelocityTarget.h
+
+    DataUnits/ForceTorqueDataUnit.h
+    DataUnits/HapticDataUnit.h
+    DataUnits/IMUDataUnit.h
+    DataUnits/KinematicDataUnit.h
+    DataUnits/PlatformDataUnit.h
+
+    LVL0Controllers/LVL0Controller.h
+
+    LVL1Controllers/LVL1Controller.h
+    LVL1Controllers/LVL1PassThroughController.h
+    LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
+    LVL1Controllers/LVL1KinematicUnitPositionController.h
+    LVL1Controllers/LVL1KinematicUnitTorqueController.h
+    LVL1Controllers/LVL1KinematicUnitVelocityController.h
+
+    RobotUnit.h
+    BasicControllers.h
+)
+
+#add_subdirectory(test)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+

source/RobotAPI/libraries/RobotRTControllers/ControlModes.h → source/RobotAPI/libraries/RTRobotUnit/ControlModes.h

@@ -29,11 +29,16 @@ namespace armarx
 {
     namespace ControlModes
     {
-        static const std::string VelocityMode      = "VelocityMode";
-        static const std::string PositionMode      = "PositionMode";
-        static const std::string TorqueMode        = "TorqueMode";
+        //'normal' actor modes
+        static const std::string PositionMode = "PositionMode";
+        static const std::string TorqueMode   = "TorqueMode";
+        static const std::string VelocityMode = "VelocityMode";
+
+        //modes for holonomic platforms
+        static const std::string HolonomicPlatformVelocityMode = "HolonomicPlatformVelocityMode";
+
+        //special sentinel modes
         static const std::string EmergencyStopMode = "EmergencyStopMode";
     }
 }
-
 #endif