fixed building; added njointccdmpcontroller

source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice → source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice

@@ -86,6 +86,7 @@ module armarx
     {
 
         // dmp configuration
+        float DMPKd = 20;
         int kernelSize = 100;
         double tau = 1;
         int baseMode = 1;
@@ -100,7 +101,8 @@ module armarx
         double phaseKpOri = 0.1;
 
         // misc
-        double timeDuration = 10;
+        Ice::DoubleSeq timeDurations;
+        Ice::StringSeq dmpTypes;
         double posToOriRatio = 100;
 
         // velocity controller configuration
@@ -116,7 +118,7 @@ module armarx
         bool isFinished();
         void runDMP();
 
-        void setTemporalFactor(double tau);
+        void setTemporalFactor(int dmpId, double tau);
         void setViaPoints(int dmpId, double canVal, Ice::DoubleSeq point);
         void setGoals(int dmpId, Ice::DoubleSeq goals);
 

source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt

@@ -40,12 +40,17 @@ if (DMP_FOUND)
            DMPController/NJointJointSpaceDMPController.h
            DMPController/NJointTaskSpaceDMPController.h
            DMPController/NJointJSDMPController.h
+           DMPController/NJointTSDMPController.h
+           DMPController/NJointCCDMPController.h
 
            )
     list(APPEND LIB_FILES
            DMPController/NJointJointSpaceDMPController.cpp
            DMPController/NJointTaskSpaceDMPController.cpp
            DMPController/NJointJSDMPController.cpp
+           DMPController/NJointTSDMPController.cpp
+           DMPController/NJointCCDMPController.cpp
+
            )
 
     list(APPEND LIBS ${DMP_LIBRARIES})

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h

+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <dmp/representation/dmp/umidmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+    TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
+    TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
+
+    typedef std::pair<double, DVec > ViaPoint;
+    typedef std::vector<ViaPoint > ViaPointsSet;
+    class NJointCCDMPControllerControlData
+    {
+    public:
+        std::vector<float> targetTSVel;
+        // cartesian velocity control data
+        std::vector<float> nullspaceJointVelocities;
+        float avoidJointLimitsKp = 0;
+        std::vector<float> torqueKp;
+        std::vector<float> torqueKd;
+        VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+    };
+
+    class pidController
+    {
+    public:
+        float Kp = 0, Kd = 0;
+        float lastError = 0;
+        float update(float dt, float error)
+        {
+            float derivative = (error - lastError) / dt;
+            float retVal = Kp * error + Kd * derivative;
+            lastError = error;
+            return retVal;
+        }
+    };
+
+    class NJointCCDMPController :
+        public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
+        public NJointCCDMPControllerInterface
+    {
+    public:
+        using ConfigPtrT = NJointCCDMPControllerConfigPtr;
+        NJointCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+        // NJointControllerInterface interface
+        std::string getClassName(const Ice::Current&) const;
+
+        // NJointController interface
+
+        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+        // NJointCCDMPControllerInterface interface
+        void learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&);
+        bool isFinished(const Ice::Current&)
+        {
+            return finished;
+        }
+
+        void runDMP(const Ice::Current&);
+        void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current&);
+        void setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+        void setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current&);
+
+        void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&);
+        void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
+        void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
+    protected:
+        void rtPreActivateController() override;
+        void rtPostDeactivateController() override;
+        VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+        void onInitComponent();
+        void onDisconnectComponent();
+        void controllerRun();
+
+    private:
+        struct DebugBufferData
+        {
+            StringFloatDictionary latestTargetVelocities;
+            StringFloatDictionary dmpTargets;
+            double mpcFactor;
+            double error;
+            double phaseStop;
+            double posError;
+            double oriError;
+            double deltaT;
+            double canVal0;
+        };
+
+        TripleBuffer<DebugBufferData> debugOutputData;
+
+        struct NJointCCDMPControllerSensorData
+        {
+            double currentTime;
+            double deltaT;
+            Eigen::Matrix4f currentPose;
+        };
+        TripleBuffer<NJointCCDMPControllerSensorData> controllerSensorData;
+
+        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+
+        // velocity ik controller parameters
+        std::vector<pidController> torquePIDs;
+        CartesianVelocityControllerPtr tcpController;
+        std::string nodeSetName;
+
+        // dmp parameters
+        std::vector<UMIDMPPtr > dmpPtrList;
+        std::vector<double> canVals;
+        std::vector<double> timeDurations;
+        std::vector<std::string > dmpTypes;
+
+        std::vector<Vec<DMPState > > currentStates;
+        std::vector<DVec > targetSubStates;
+
+        bool finished;
+        double tau;
+        ViaPointsSet viaPoints;
+        bool isDisturbance;
+
+
+        // phaseStop parameters
+        double phaseL;
+        double phaseK;
+        double phaseDist0;
+        double phaseDist1;
+        double phaseKpPos;
+        double phaseKpOri;
+
+        double posToOriRatio;
+
+        std::vector<float> targetVels;
+
+        NJointCCDMPControllerConfigPtr cfg;
+        VirtualRobot::RobotNodePtr tcp;
+        Eigen::Vector3f tcpPosition;
+        Eigen::Vector3f tcpOrientation;
+
+        DMP::DVec targetState;
+        mutable MutexType controllerMutex;
+        PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
+    };
+
+} // namespace armarx
+

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h

@@ -9,7 +9,7 @@
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <dmp/representation/dmp/umidmp.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
 
 namespace armarx
 {

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h

@@ -8,7 +8,7 @@
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
 
 namespace armarx
 {

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp

@@ -115,12 +115,19 @@ namespace armarx
             currentState = controllerSensorData.getReadBuffer().currentState;
 
 
+            std::vector<double> currentposi;
+            currentposi.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
+                currentposi[i] = currentState[i].pos;
                 posError += pow(currentState[i].pos - targetState[i], 2);
             }
+
+            std::vector<double> currentori;
+            currentori.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
+                currentori[i] = currentState[i + 3].pos;
                 oriError += pow(currentState[i + 3].pos - targetState[i + 3], 2);
             }
 
@@ -140,7 +147,7 @@ namespace armarx
                 phaseDist = phaseDist0;
             }
 
-            phaseStop = 0;//phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
             mpcFactor = 1 - (phaseStop / phaseL);
 
             if (mpcFactor < 0.1)
@@ -164,13 +171,13 @@ namespace armarx
             for (size_t i = 0; i < 3; i++)
             {
                 vel0 = currentState[i].vel / timeDuration;
-                vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
+                vel1 = phaseKpPos * (targetState[i] - currentposi[i]);
                 targetVels[i] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
             }
             for (size_t i = 0; i < 3; i++)
             {
                 vel0 = currentState[i + 3].vel / timeDuration;
-                vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
+                vel1 = phaseKpOri * (targetState[i + 3] - currentori[i]);
                 targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
             }
         }

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h

@@ -8,7 +8,7 @@
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
 

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h

@@ -8,7 +8,7 @@
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>