added NJointBimanualForceMPController

source/RobotAPI/interface/CMakeLists.txt

@@ -52,6 +52,7 @@ set(SLICE_FILES
 
     units/RobotUnit/NJointJointSpaceDMPController.ice
     units/RobotUnit/NJointTaskSpaceDMPController.ice
+    units/RobotUnit/NJointBimanualForceMPController.ice
 
     units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
     components/ViewSelectionInterface.ice

source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.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
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+    class NJointBimanualForceMPControllerConfig extends NJointControllerConfig
+    {
+
+        // dmp configuration
+        int kernelSize = 100;
+        string dmpMode = "MinimumJerk";
+        string dmpStyle = "Discrete";
+        double dmpAmplitude = 1;
+
+        double phaseL = 10;
+        double phaseK = 10;
+        double phaseDist0 = 50;
+        double phaseDist1 = 10;
+        double phaseKpPos = 1;
+        double phaseKpOri = 0.1;
+        double timeDuration = 10;
+        double posToOriRatio = 100;
+
+        double maxLinearVel;
+        double maxAngularVel;
+
+        // velocity control
+        float Kp_LinearVel;
+        float Kd_LinearVel;
+        float Kp_AngularVel;
+        float Kd_AngularVel;
+
+        // force control
+        float forceP;
+        float forceI;
+        float forceD;
+
+        float filterCoeff;
+        float KpJointLimitAvoidanceScale;
+
+        float targetSupportForce;
+        Ice::FloatSeq leftForceOffset;
+        Ice::FloatSeq rightForceOffset;
+
+        string debugName;
+
+    };
+
+
+    interface NJointBimanualForceMPControllerInterface extends NJointControllerInterface
+    {
+        void learnDMPFromFiles(string whichMP, Ice::StringSeq trajfiles);
+        bool isFinished();
+        void runDMP(Ice::DoubleSeq leftGoals, Ice::DoubleSeq rightGoals);
+        double getCanVal();
+    };
+};
+

source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt

@@ -46,6 +46,7 @@ if (DMP_FOUND )
            DMPController/NJointCCDMPController.h
            DMPController/NJointBimanualCCDMPController.h
            DMPController/NJointTaskSpaceImpedanceDMPController.h
+           DMPController/NJointBimanualForceMPController.h
 
            )
     list(APPEND LIB_FILES
@@ -55,6 +56,8 @@ if (DMP_FOUND )
            DMPController/NJointCCDMPController.cpp
            DMPController/NJointBimanualCCDMPController.cpp
            DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+           DMPController/NJointBimanualForceMPController.cpp
+
            )
 
     list(APPEND LIBS ${DMP_LIBRARIES} DMPController)

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.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/umitsmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+using namespace DMP;
+namespace armarx
+{
+    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
+    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
+
+    typedef std::pair<double, DVec > ViaPoint;
+    typedef std::vector<ViaPoint > ViaPointsSet;
+    class NJointBimanualForceMPControllerControlData
+    {
+    public:
+        Eigen::Matrix4f leftTargetPose;
+        Eigen::Matrix4f rightTargetPose;
+    };
+
+    /**
+     * @brief The NJointBimanualForceMPController class
+     * @ingroup Library-RobotUnit-NJointControllers
+     */
+    class NJointBimanualForceMPController :
+        public NJointControllerWithTripleBuffer<NJointBimanualForceMPControllerControlData>,
+        public NJointBimanualForceMPControllerInterface
+    {
+    public:
+        using ConfigPtrT = NJointBimanualForceMPControllerConfigPtr;
+        NJointBimanualForceMPController(const RobotUnitPtr&, 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);
+
+        // NJointBimanualForceMPControllerInterface interface
+        void learnDMPFromFiles(const string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&);
+        bool isFinished(const Ice::Current&)
+        {
+            return finished;
+        }
+
+        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
+
+        double getCanVal(const Ice::Current&)
+        {
+            return canVal;
+        }
+
+    protected:
+        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+        void onInitComponent();
+        void onDisconnectComponent();
+
+        void controllerRun();
+    private:
+        struct DebugBufferData
+        {
+            StringFloatDictionary dmpTargets;
+            StringFloatDictionary currentPose;
+            double canVal;
+            float forceOnHands;
+        };
+        TripleBuffer<DebugBufferData> debugOutputData;
+
+        struct NJointBimanualForceMPControllerSensorData
+        {
+            Eigen::Matrix4f leftPoseInRootFrame;
+            Eigen::Vector6f leftTwistInRootFrame;
+
+            Eigen::Matrix4f rightPoseInRootFrame;
+            Eigen::Vector6f rightTwistInRootFrame;
+
+            Eigen::Vector3f leftForceInRootFrame;
+            Eigen::Vector3f rightForceInRootFrame;
+            double deltaT;
+        };
+        TripleBuffer<NJointBimanualForceMPControllerSensorData> controllerSensorData;
+
+        struct NJointBimanualForceMPControllerInterfaceData
+        {
+            Eigen::Matrix4f leftTcpPoseInRootFrame;
+            Eigen::Matrix4f rightTcpPoseInRootFrame;
+        };
+        TripleBuffer<NJointBimanualForceMPControllerInterfaceData> interfaceData;
+
+        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
+        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
+        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+        const SensorValueForceTorque* rightForceTorque;
+        const SensorValueForceTorque* leftForceTorque;
+
+        VirtualRobot::DifferentialIKPtr leftIK;
+        VirtualRobot::DifferentialIKPtr rightIK;
+
+
+        TaskSpaceDMPControllerPtr leftDMPController;
+        TaskSpaceDMPControllerPtr rightDMPController;
+
+        // velocity ik controller parameters
+        CartesianVelocityControllerPtr leftTCPController;
+        CartesianVelocityControllerPtr rightTCPController;
+
+        // dmp parameters
+        bool finished;
+        VirtualRobot::RobotNodePtr leftTCP;
+        VirtualRobot::RobotNodePtr rightTCP;
+
+
+        Eigen::VectorXf targetVels;
+        Eigen::Matrix4f targetPose;
+
+        NJointBimanualForceMPControllerConfigPtr cfg;
+        mutable MutexType controllerMutex;
+        PeriodicTask<NJointBimanualForceMPController>::pointer_type controllerTask;
+
+        // velocity control
+        float Kp_LinearVel;
+        float Kd_LinearVel;
+        float Kp_AngularVel;
+        float Kd_AngularVel;
+
+        // force control
+        float Kp_f;
+        float Kd_f;
+        float Ki_f;
+
+        double canVal;
+        double xvel;
+
+        float forceIterm;
+        bool leftLearned;
+        bool rightLearned;
+
+        Eigen::Vector3f leftFilteredValue;
+        Eigen::Vector3f rightFilteredValue;
+
+        Eigen::Vector3f leftForceOffset;
+        Eigen::Vector3f rightForceOffset;
+        float targetSupportForce;
+
+        double I_decay;
+
+        float forceSign;
+
+    };
+
+} // namespace armarx
+