NaturalIKInterface & NaturalIKComponentPlugin

source/RobotAPI/interface/CMakeLists.txt

@@ -81,6 +81,7 @@ set(SLICE_FILES
     components/RobotHealthInterface.ice
     components/FrameTrackingInterface.ice
     components/CartesianPositionControlInterface.ice
+    components/NaturalIKInterface.ice
 
     visualization/DebugDrawerInterface.ice
     visualization/DebugDrawerToArViz.ice

source/RobotAPI/interface/components/CartesianPositionControlInterface.ice

@@ -35,6 +35,7 @@ module armarx
         void setFTLimit(string side, float maxForce, float maxTorque);
         void setCurrentFTAsOffset(string side);
         void resetFTOffset(string side);
+        int getFTTresholdExceededCounter(string side);
 
     };
 };

source/RobotAPI/interface/components/NaturalIKInterface.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/>.
+ *
+ * @author     Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date       2019
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#pragma once
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/aron.ice>
+
+module armarx
+{
+    struct NaturalIKResult
+    {
+        bool reached;
+        Ice::FloatSeq jointValues;
+    };
+
+    interface NaturalIKInterface
+    {
+        NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::AronDict args);
+    };
+};

source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt

@@ -20,6 +20,7 @@ set(LIB_FILES
     KinematicUnitComponentPlugin.cpp
     TrajectoryPlayerComponentPlugin.cpp
     CartesianPositionControlComponentPlugin.cpp
+    NaturalIKComponentPlugin.cpp
 )
 set(LIB_HEADERS
     RobotStateComponentPlugin.h
@@ -32,6 +33,7 @@ set(LIB_HEADERS
     KinematicUnitComponentPlugin.h
     TrajectoryPlayerComponentPlugin.h
     CartesianPositionControlComponentPlugin.h
+    NaturalIKComponentPlugin.h
 )
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp

+#include "NaturalIKComponentPlugin.h"
+
+namespace armarx
+{
+    namespace plugins
+    {
+
+        void NaturalIKComponentPlugin::preOnInitComponent()
+        {
+            parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+        }
+
+        void NaturalIKComponentPlugin::preOnConnectComponent()
+        {
+            parent<Component>().getProxyFromProperty(_naturalIK, PROPERTY_NAME);
+        }
+
+        void NaturalIKComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+        {
+            if (!properties->hasDefinition(PROPERTY_NAME))
+            {
+                properties->defineOptionalProperty<std::string>(
+                    PROPERTY_NAME,
+                    "NaturalIK",
+                    "Name of the NaturalIK");
+            }
+        }
+
+        NaturalIKInterfacePrx NaturalIKComponentPlugin::getNaturalIK()
+        {
+            return _naturalIK;
+        }
+
+
+    }
+}
+
+namespace armarx
+{
+
+    NaturalIKComponentPluginUser::NaturalIKComponentPluginUser()
+    {
+        addPlugin(plugin);
+    }
+
+    NaturalIKInterfacePrx NaturalIKComponentPluginUser::getNaturalIK()
+    {
+        return plugin->getNaturalIK();
+    }
+
+
+}
+
+

source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h

+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/NaturalIKInterface.h>
+
+
+namespace armarx
+{
+    namespace plugins
+    {
+
+        class NaturalIKComponentPlugin : public ComponentPlugin
+        {
+        public:
+            using ComponentPlugin::ComponentPlugin;
+
+            void preOnInitComponent() override;
+
+            void preOnConnectComponent() override;
+
+            void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+            NaturalIKInterfacePrx getNaturalIK();
+
+        private:
+            static constexpr const char* PROPERTY_NAME = "NaturalIKName";
+            NaturalIKInterfacePrx _naturalIK;
+        };
+
+    }
+}
+
+namespace armarx
+{
+    /**
+     * @brief Provides a ready-to-use NaturalIK.
+     */
+    class NaturalIKComponentPluginUser : virtual public ManagedIceObject
+    {
+    public:
+        NaturalIKComponentPluginUser();
+        NaturalIKInterfacePrx getNaturalIK();
+
+    private:
+        armarx::plugins::NaturalIKComponentPlugin* plugin = nullptr;
+    };
+
+}
+
+
+namespace armarx
+{
+    namespace plugins
+    {
+        // Legacy typedef.
+        using NaturalIKComponentPluginUser = armarx::NaturalIKComponentPluginUser;
+    }
+}

source/RobotAPI/libraries/aron/AronNavigator.cpp

@@ -43,6 +43,7 @@ namespace armarx
 
 
         AronNavigator::AronNavigator(const AronDataPtr& data)
+            : data(data)
         { }
 
         AronNavigator::AronNavigator(const AronDataPtr& data, const AronPath& path)