add NJointHolonomicPlatformGlobalPositionController

source/RobotAPI/components/units/RobotUnit/CMakeLists.txt

@@ -43,6 +43,7 @@ set(LIB_FILES
     NJointControllers/NJointKinematicUnitPassThroughController.cpp
     NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
     NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+    NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
     NJointControllers/NJointTCPController.cpp
     NJointControllers/NJointCartesianVelocityController.cpp
     NJointControllers/NJointCartesianTorqueController.cpp
@@ -111,6 +112,7 @@ set(LIB_HEADERS
     NJointControllers/NJointKinematicUnitPassThroughController.h
     NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
     NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+    NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
     NJointControllers/NJointTCPController.h
     NJointControllers/NJointCartesianVelocityController.h
     NJointControllers/NJointCartesianTorqueController.h

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp

+/*
+ * 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     Markus Grotz (markus.grotz at kit dot edu)
+ * @date       2019
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#include "NJointHolonomicPlatformGlobalPositionController.h"
+
+namespace armarx
+{
+    NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
+    registrationNJointHolonomicPlatformGlobalPositionController("NJointHolonomicPlatformGlobalPositionController");
+
+
+    NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
+        const RobotUnitPtr&,
+        const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+        const VirtualRobot::RobotPtr&) :
+        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+
+    {
+        const SensorValueBase* sv = useSensorValue(cfg->platformName);
+        this->sv = sv->asA<SensorValueHolonomicPlatform>();
+        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+        ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity);
+
+        pid.threadSafe = false;
+
+        oriCtrl.maxV = cfg->maxRotationVelocity;
+        oriCtrl.acceleration = cfg->maxRotationAcceleration;
+        oriCtrl.deceleration = cfg->maxRotationAcceleration;
+        oriCtrl.maxDt = 0.1;
+        oriCtrl.pid->Kp = cfg->p;
+        oriCtrl.positionPeriodLo = -M_PI;
+        oriCtrl.positionPeriodHi = M_PI;
+        pid.preallocate(2);
+
+    }
+
+    void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
+    {
+        currentPosition << sv->relativePositionX, sv->relativePositionY;
+        currentOrientation = sv->relativePositionRotation;
+
+
+        if (rtGetControlStruct().newTargetSet)
+        {
+            pid.reset();
+            *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+        }
+
+        if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+        {
+
+            // ARMARX_RT_LOGF_WARNING  << deactivateSpam(0.5) << "Waiting for global pos";
+
+            target->velocityX = 0;
+            target->velocityY = 0;
+
+            target->velocityRotation = 0;
+
+            return;
+        }
+
+        float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
+        Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
+
+        Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
+        float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
+        updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));
+
+        pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+
+        oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
+        oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
+        oriCtrl.currentPosition = updatedOrientation;
+        oriCtrl.targetPosition =  getWriterControlStruct().targetOrientation;
+        oriCtrl.currentV = sv->velocityRotation;
+
+        Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+        target->velocityX = localTargetVelocity(0);
+        target->velocityY = localTargetVelocity(1);
+        target->velocityRotation = oriCtrl.run();
+
+        Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
+        if (posError.norm() < rtGetControlStruct().translationAccuracy)
+        {
+            // target->velocityX = 0;
+            // target->velocityY = 0;
+        }
+
+        float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
+        orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
+        if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
+        {
+            //target->velocityRotation = 0;
+        }
+    }
+
+    void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
+    {
+    }
+
+    void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+    {
+        // todo do we really need a recursive mutex?
+
+        std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+
+        getWriterControlStruct().target << x, y;
+        getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+        getWriterControlStruct().translationAccuracy = translationAccuracy;
+        getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+        getWriterControlStruct().newTargetSet = true;
+        writeControlStruct();
+    }
+
+
+    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(float x, float y, float yaw, const IceUtil::Time& time = ::IceUtil::Time::now())
+    {
+        // ..todo:: lock :)
+
+        getWriterControlStruct().globalPosition << x, y;
+        getWriterControlStruct().globalOrientation = yaw;
+
+        getWriterControlStruct().startPosition = currentPosition;
+        getWriterControlStruct().startOrientation = currentOrientation;
+
+        getWriterControlStruct().lastUpdate = time;
+        writeControlStruct();
+    }
+
+} // namespace armarx
+

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h

+/*
+ * 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     Markus Grotz (markus.grotz at kit dot edu)
+ * @date       2019
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+
+#include <atomic>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "NJointController.h"
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+namespace armarx
+{
+
+    TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionControllerConfig);
+    class NJointHolonomicPlatformGlobalPositionControllerConfig : virtual public NJointControllerConfig
+    {
+    public:
+        std::string platformName;
+        float p = 1.0f;
+        float i = 0.0f;
+        float d = 0.0f;
+        float maxVelocity = 300;
+        float maxAcceleration = 500;
+        float maxRotationVelocity = 0.5;
+        float maxRotationAcceleration = 0.5;
+    };
+
+    struct NJointHolonomicPlatformGlobalPositionControllerTarget
+    {
+        Eigen::Vector2f target; // x,y
+        float targetOrientation;
+        float translationAccuracy = 0.0f;
+        float rotationAccuracy = 0.0f;
+        bool newTargetSet = false;
+
+
+        Eigen::Vector2f startPosition;
+        float startOrientation;
+
+        Eigen::Vector2f globalPosition;
+        float globalOrientation;
+        IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
+    };
+
+    TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionController);
+
+    /**
+     * @brief The NJointHolonomicPlatformGlobalPositionController class
+     * @ingroup Library-RobotUnit-NJointControllers
+     */
+    class NJointHolonomicPlatformGlobalPositionController:
+        virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformGlobalPositionControllerTarget>
+    {
+    public:
+        using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
+
+        inline NJointHolonomicPlatformGlobalPositionController(
+            const RobotUnitPtr& robotUnit,
+            const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+            const VirtualRobot::RobotPtr&);
+
+        inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+        inline virtual void rtPreActivateController() override;
+
+        //ice interface
+        inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+        {
+            return "NJointHolonomicPlatformGlobalPositionController";
+        }
+
+        void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+
+        void setGlobalPos(float x, float y, float yaw, const IceUtil::Time&);
+
+
+    protected:
+        const SensorValueHolonomicPlatform* sv;
+        MultiDimPIDController pid;
+
+        PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
+        ControlTargetHolonomicPlatformVelocity* target;
+
+
+        Eigen::Vector2f currentPosition;
+        float currentOrientation;
+
+    };
+} // namespace armarx
+
+

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp

@@ -354,6 +354,24 @@ namespace armarx
             unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
             //add
             addUnit(std::move(unit));
+
+
+            NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+            configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
+            auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
+                                          _module<ControllerManagement>().createNJointController(
+                                              "NJointHolonomicPlatformGlobalPositionController",
+                                              getName() + "_" + configName + "_GlobalPositionContoller",
+                                              configGlobalPositionCtrlCfg, false, true
+                                          )
+                                      );
+            ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
+            unit->pt = ctrl;
+            unit->globalPosCtrl = ctrlGlobalPosition;
+
+            unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+            //add
+            addUnit(std::move(unit));
         }
 
         void Units::initializeForceTorqueUnit()

source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h

@@ -66,6 +66,7 @@ namespace armarx
 
         NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
         NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
+        NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
         std::size_t platformSensorIndex;
     protected:
         Eigen::Matrix4f getRelativePose() const;