diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt index e11b5d0e510ea5b8c9b9ea3a7179ddfe202569ea..a1864842ccae2e6d09be09a5d99940b77570e44b 100644 --- a/source/RobotAPI/interface/CMakeLists.txt +++ b/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 diff --git a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice index fbb776a6902e0b1455c54cc4d60d71a2eceacb33..419ed60db9f8a6b5e0f27857feeefb0d55636e36 100644 --- a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice +++ b/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); }; }; diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice new file mode 100644 index 0000000000000000000000000000000000000000..a5627f61ef822deaa31e59eb065a2032a877ddcd --- /dev/null +++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice @@ -0,0 +1,41 @@ +/* + * 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); + }; +}; diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt index 0be4a3ac9118c131fd48407307c7e371edb2d8e2..6567872ebd97ab3016b3af816f2048d473040464 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt +++ b/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}") diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..008d497652fbd0888ef82eb1e5b99650bda11a25 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp @@ -0,0 +1,54 @@ +#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(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..0532416d50ae164baf5645d0cbc30ff50f3e9f48 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h @@ -0,0 +1,58 @@ +#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; + } +} diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp index dabfb8bffe522ccf78da40726bf48d51db3bb8cc..9c5f9118e3893d412b98fcef044ea37abb9d795b 100644 --- a/source/RobotAPI/libraries/aron/AronNavigator.cpp +++ b/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)