added realtime dmp controller

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

@@ -3,6 +3,8 @@ set(LIB_NAME       RobotUnit)
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 
+find_package(DMP QUIET)
+
 find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
 armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
 if (Simox_FOUND)
@@ -118,6 +120,22 @@ set(LIB_HEADERS
     Devices/RTThreadTimingsSensorDevice.h
 )
 
+if (DMP_FOUND)
+    message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
+
+    include_directories(${DMP_INCLUDE_DIRS})
+    link_directories(${DMP_LIB_DIRS})
+
+    list(APPEND LIB_HEADERS
+           NJointControllers/NJointJointSpaceDMPController.h
+           )
+    list(APPEND LIB_FILES
+           NJointControllers/NJointJointSpaceDMPController.cpp)
+
+    list(APPEND LIBS ${DMP_LIBRARIES})
+endif ()
+
+
 add_subdirectory(test)
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

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

+#include "NJointJointSpaceDMPController.h"
+
+
+
+namespace armarx
+{
+
+    NJointControllerRegistration<NJointJointSpaceDMPController> registrationControllerNJointJointSpaceDMPController("NJointJointSpaceDMPController");
+
+    std::string NJointJointSpaceDMPController::getClassName(const Ice::Current&) const
+    {
+        return "NJointJointSpaceDMPController";
+    }
+
+    NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+    {
+        NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
+        ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
+
+        for (std::string jointName : cfg->jointNames)
+        {
+            ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+            const SensorValueBase* sv = prov->getSensorValue(jointName);
+            targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
+            positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
+            torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
+            gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
+        }
+        if (cfg->jointNames.size() == 0)
+        {
+            ARMARX_ERROR << "cfg->jointNames.size() == 0";
+        }
+
+        DMPAsForwardControl = cfg->DMPAsForwardControl;
+        dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->Kd, cfg->baseMode, cfg->tau));
+        timeDuration = cfg->timeDuration;
+        canVal = timeDuration;
+        finished = false;
+
+        NJointJointSpaceDMPControllerControlData initData;
+        initData.tau = 1.0;
+        initData.isStart = false;
+        reinitTripleBuffer(initData);
+    }
+
+    void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+    {
+        if (rtGetControlStruct().isStart)
+        {
+            currentState.clear();
+            for (size_t i = 0; i < dimNames.size(); i++)
+            {
+                const auto& jointName = dimNames.at(i);
+                DMP::DMPState currentPos;
+                currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
+                currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
+                currentState.push_back(currentPos);
+            }
+
+            double tau = rtGetControlStruct().tau;
+            double deltaT = timeSinceLastIteration.toSecondsDouble();
+            canVal -= 1 / tau * deltaT * 1 ;
+            double dmpDeltaT = deltaT / timeDuration;
+            dmpPtr->setTemporalFactor(tau);
+
+            DMP::DVec targetState;
+            currentState = dmpPtr->calculateNextPointWithEulerMethod(currentState, canVal / timeDuration, dmpDeltaT, targetState, 1.0);
+            if (canVal < 1e-8)
+            {
+                finished = true;
+            }
+
+            for (size_t i = 0; i < dimNames.size(); ++i)
+            {
+                const auto& jointName = dimNames.at(i);
+                if (targets.count(jointName) == 1)
+                {
+                    targets[jointName]->velocity = finished ? 0.0f : currentState[i].vel;
+                }
+            }
+        }
+    }
+
+    void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+    {
+        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+        DMP::DVec ratios;
+        for (size_t i = 0; i < fileNames.size(); ++i)
+        {
+            DMP::SampledTrajectoryV2 traj;
+            traj.readFromCSVFile(fileNames.at(i));
+            dimNames = traj.getDimensionNames();
+            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+            trajs.push_back(traj);
+
+            if (i == 0)
+            {
+                ratios.push_back(1.0);
+            }
+            else
+            {
+                ratios.push_back(0.0);
+            }
+        }
+        dmpPtr->learnFromTrajectories(trajs);
+        dmpPtr->setOneStepMPC(true);
+
+
+        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
+
+        ARMARX_INFO << "Learned DMP ... ";
+    }
+
+    void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
+    {
+        currentState.clear();
+        for (size_t i = 0; i < dimNames.size(); i++)
+        {
+            const auto& jointName = dimNames.at(i);
+            DMP::DMPState currentPos;
+            currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
+            currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
+            currentState.push_back(currentPos);
+        }
+        dmpPtr->prepareExecution(goals, currentState, 1,  tau);
+        finished = false;
+
+        this->goals = goals;
+        this->tau = tau;
+
+        LockGuardType guard {controlDataMutex};
+        getWriterControlStruct().tau = tau;
+        getWriterControlStruct().isStart = true;
+        writeControlStruct();
+
+    }
+
+    void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
+    {
+        this->tau = tau;
+        LockGuardType guard {controlDataMutex};
+        getWriterControlStruct().tau = tau;
+        getWriterControlStruct().isStart = true;
+        writeControlStruct();
+    }
+
+    void NJointJointSpaceDMPController::rtPreActivateController()
+    {
+    }
+
+    void NJointJointSpaceDMPController::rtPostDeactivateController()
+    {
+
+    }
+
+
+}

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

+
+#pragma once
+
+
+#include "NJointController.h"
+#include <VirtualRobot/Robot.h>
+#include "../RobotUnit.h"
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include <dmp/representation/dmp/umidmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
+
+namespace armarx
+{
+
+    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPController);
+
+    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPControllerControlData);
+    class NJointJointSpaceDMPControllerControlData
+    {
+    public:
+        double tau;
+        bool isStart;
+    };
+
+
+    //    class SimplePID
+    //    {
+    //    public:
+    //        float Kp = 0, Kd = 0;
+    //        float lastError = 0;
+    //        float update(float dt, float error);
+    //    };
+
+    class NJointJointSpaceDMPController :
+        public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>,
+        public NJointJointSpaceDMPControllerInterface
+    {
+    public:
+        using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
+        NJointJointSpaceDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+        // NJointControllerInterface interface
+        std::string getClassName(const Ice::Current&) const override;
+
+        // NJointController interface
+        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+        //        static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+        //        NJointCartesianVelocityController(
+        //            NJointControllerDescriptionProviderInterfacePtr prov,
+        //            NJointCartesianVelocityControllerConfigPtr config,
+        //            const boost::shared_ptr<VirtualRobot::Robot>& r);
+
+
+        //
+        bool isFinished(const Ice::Current&)
+        {
+            return finished;
+        }
+
+        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+        void setTemporalFactor(double tau, const Ice::Current&);
+
+        void runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&);
+
+    protected:
+        void rtPreActivateController() override;
+        void rtPostDeactivateController() override;
+    private:
+        std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
+        std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+        std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
+        std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
+        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
+
+
+        std::vector<double> goals;
+        DMP::UMIDMPPtr dmpPtr;
+        bool DMPAsForwardControl;
+        double timeDuration;
+
+        double canVal;
+
+        double tau;
+        double finished;
+        std::vector<std::string> dimNames;
+        DMP::Vec<DMP::DMPState> currentState;
+    };
+
+} // namespace armarx
+

source/RobotAPI/interface/CMakeLists.txt

@@ -47,6 +47,8 @@ set(SLICE_FILES
     units/RobotUnit/NJointCartesianActiveImpedanceController.ice
     units/RobotUnit/RobotUnitInterface.ice
 
+    units/RobotUnit/NJointJointSpaceDMPController.ice
+
     components/ViewSelectionInterface.ice
     components/TrajectoryPlayerInterface.ice
     components/RobotNameServiceInterface.ice

source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.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::NJointControllerInterface
+ * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+    class NJointJointSpaceDMPControllerConfig extends NJointControllerConfig
+    {
+        Ice::StringSeq jointNames;
+        float Kd = 20;
+        int kernelSize = 100;
+        double tau = 1;
+        int baseMode = 1;
+
+        double timeDuration = 10;
+        bool DMPAsForwardControl = true;
+    };
+
+
+    interface NJointJointSpaceDMPControllerInterface extends NJointControllerInterface
+    {
+        void learnDMPFromFiles(Ice::StringSeq trajfiles);
+        bool isFinished();
+        void runDMP(Ice::DoubleSeq goals, double tau);
+        void setTemporalFactor(double tau);
+    };
+};
+#endif

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -2,6 +2,7 @@ armarx_set_target("RobotAPI Core Library: RobotAPICore")
 
 find_package(Eigen3 QUIET)
 find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+#find_package(DMP QUIET)
 
 armarx_build_if(Eigen3_FOUND "Eigen3 not available")
 armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
@@ -78,6 +79,24 @@ set(LIB_HEADERS
     CartesianPositionController.h
 )
 
+
+#if (DMP_FOUND)
+#    message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
+
+#    include_directories(${DMP_INCLUDE_DIRS})
+#    link_directories(${DMP_LIB_DIRS})
+
+#    list(APPEND LIB_HEADERS
+#           DMPController.h
+#           )
+#    list(APPEND LIB_FILES
+#           DMPController.cpp
+#           )
+
+#    list(APPEND LIBS ${DMP_LIBRARIES})
+#endif ()
+
+
 add_subdirectory(test)
 
 armarx_add_library("${LIB_NAME}"  "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")