Merge remote-tracking branch 'origin/master' into Ice3.7

etc/cmake/FindNLOPT.cmake

+# Copyright (c) 2015, Georgia Tech Graphics Lab and Humanoid Robotics Lab
+# This file is provided under the "BSD-style" License
+
+# Find NLOPT
+#
+# This sets the following variables:
+# NLOPT_FOUND
+# NLOPT_INCLUDE_DIRS
+# NLOPT_LIBRARIES
+# NLOPT_DEFINITIONS
+# NLOPT_VERSION
+
+find_package(PkgConfig QUIET)
+
+# Check to see if pkgconfig is installed.
+pkg_check_modules(PC_NLOPT nlopt QUIET)
+
+# Definitions
+set(NLOPT_DEFINITIONS ${PC_NLOPT_CFLAGS_OTHER})
+
+# Include directories
+find_path(NLOPT_INCLUDE_DIRS
+    NAMES nlopt.h
+    HINTS ${PC_NLOPT_INCLUDEDIR}
+    PATHS "${CMAKE_INSTALL_PREFIX}/include")
+
+# Libraries
+find_library(NLOPT_LIBRARIES
+    NAMES nlopt nlopt_cxx
+    HINTS ${PC_NLOPT_LIBDIR})
+
+# Version
+set(NLOPT_VERSION ${PC_NLOPT_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(NLOPT
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS NLOPT_INCLUDE_DIRS NLOPT_LIBRARIES
+    VERSION_VAR   NLOPT_VERSION)
+ 

source/RobotAPI/interface/CMakeLists.txt

@@ -53,6 +53,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();
+
+        void setViaPoints(string whichDMP, double canVal, Ice::DoubleSeq viaPoint);
+    };
+};
+

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;
+        }
+
+        void setViaPoints(const string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&);
+
+    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
+

source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp

@@ -71,6 +71,11 @@ void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>
     obj->add(key, ToArr(value));
 }
 
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
+{
+    obj->add(key, ToObj(value));
+}
+
 
 JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
 {
@@ -101,6 +106,16 @@ JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
     return obj;
 }
 
+JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
+{
+    JsonObjectPtr obj(new JsonObject);
+    for (const std::pair<std::string, float>& pair : value)
+    {
+        obj->add(pair.first, JsonValue::Create(pair.second));
+    }
+    return obj;
+}
+
 JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
 {
     JsonArrayPtr arr(new JsonArray);

source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h

@@ -27,6 +27,7 @@
 
 #include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
 #include <Eigen/Dense>
+#include <map>
 
 namespace armarx
 {
@@ -46,10 +47,12 @@ namespace armarx
         void Add(const std::string& key, const std::string& value);
         void Add(const std::string& key, float value);
         void Add(const std::string& key, const std::vector<float>& value);
+        void Add(const std::string& key, const std::map<std::string, float>& value);
 
         static JsonArrayPtr ToArr(const Eigen::VectorXf& vec);
         static JsonArrayPtr ToArr(const std::vector<float>& vec);
         static JsonObjectPtr ToObj(Eigen::Vector3f vec);
+        static JsonObjectPtr ToObj(const std::map<std::string, float>& value);
         static JsonArrayPtr ToArr(Eigen::Matrix4f mat);
         static JsonArrayPtr MatrixToArr(Eigen::MatrixXf mat);