diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index b4aced7b9e0fb437985e14d8bf8a2fa1adfb9440..71c9f1caad2438d2c0869dc64e70f01440907de6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -92,6 +92,11 @@ namespace armarx::RobotUnitModule
}
void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
+ bool isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return false;
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index c13ab4dcd519f7a19aec3f2af919b8a0a793604f..ffc177c8b38ff3f9e0569afba0ec2000070f7753 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -87,6 +87,9 @@ set(SLICE_FILES
components/TrajectoryPlayerInterface.ice
components/ViewSelectionInterface.ice
+ components/CartesianPositionControlInterface.ice
+ components/NaturalIKInterface.ice
+
visualization/DebugDrawerInterface.ice
visualization/DebugDrawerToArViz.ice
diff --git a/source/RobotAPI/interface/aron.ice b/source/RobotAPI/interface/aron.ice
index 5ad2c13812ee9ddf3e29a39cfa2f82a51669540f..3dbe06245692e4392a05e9d2b9997dfe1dac09bf 100644
--- a/source/RobotAPI/interface/aron.ice
+++ b/source/RobotAPI/interface/aron.ice
@@ -16,10 +16,12 @@ module aron
sequence<AronKeyValuePair> AronKeyValueList;
sequence<AronData> AronDataList;
sequence<byte> AronBlobValue;
+ sequence<int> AronIntSeq;
- class AronList extends AronData { AronDataList elements; };
- class AronObject extends AronData { AronKeyValueList elements; };
+ class AronList extends AronData { AronDataList elements; };
+ class AronDict extends AronData { AronKeyValueList elements; };
class AronNull extends AronData { };
+ class AronNDArray extends AronData { AronIntSeq dimensions; int itemSize; string type; AronBlobValue data; };
#define HANDLE_TYPE(cppType, upperType) \
class Aron##upperType extends AronData { cppType value; };
@@ -30,36 +32,10 @@ module aron
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
+
#undef HANDLE_TYPE
- //class AronString extends AronData { string value; };
- //class AronInt extends AronData { int value; };
- //class AronLong extends AronData { long value; };
- //class AronFloat extends AronData { float value; };
- //class AronDouble extends AronData { double value; };
- //class AronBool extends AronData { bool value; };
- //class AronBlob extends AronData { AronBlobValue value; };
- //class AronNull extends AronData { };
- //
- //class AronVector2f extends AronData { Eigen::Vector2f value; };
- //class AronVector3f extends AronData { Eigen::Vector3f value; };
- //class AronVector6f extends AronData { Eigen::Vector6f value; };
- //class AronVectorXf extends AronData { Eigen::VectorXf value; };
- //
- //class AronMatrix3f extends AronData { Eigen::Matrix3f value; };
- //class AronMatrix4f extends AronData { Eigen::Matrix4f value; };
- //class AronMatrixXf extends AronData { Eigen::MatrixXf value; };
- //class AronQuaternionf extends AronData { Eigen::Quaternionf value; };
module types
{
@@ -73,7 +49,7 @@ module aron
class ListType extends AbstractType {
AbstractType childtype;
};
- class ObjectType extends AbstractType {
+ class DictType extends AbstractType {
AbstractType childtype;
};
diff --git a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..13741869c90df9117d6f326036ae92b6e6a24ce0
--- /dev/null
+++ b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
@@ -0,0 +1,42 @@
+/*
+ * 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.h>
+
+module armarx
+{
+ interface CartesianPositionControlInterface
+ {
+ void setEnabled(string side, bool enabled);
+ void setTarget(string side, Eigen::Matrix4f target, bool setOrientation);
+ void setFTLimit(string side, float maxForce, float maxTorque);
+ void setCurrentFTAsOffset(string side);
+ void resetFTOffset(string side);
+ int getFTTresholdExceededCounter(string side);
+ void emulateFTTresholdExceeded(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/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 8749be775e87d64a4538fcfae4e2e1079e478612..ac4da1da07d1ac2c75526a2f98c32c547aa3d88d 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -62,6 +62,7 @@ module armarx
ApertureType preshape = AnyAperture;
ApproachType approach = AnyApproach;
string graspTrajectoryName;
+ //string graspTrajectoryPackage;
};
class GraspCandidateReachabilityInfo
{
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 8f3237b5312f56a01c71acc49d3bd7bbad401337..361cf690a3498a2a9ffcb95527ef27005c54fe86 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -187,6 +187,7 @@ module armarx
{
//state
["cpp:const"] idempotent bool isRunning();
+ ["cpp:const"] idempotent bool isSimulation();
};
interface RobotUnitLoggingInterface
{
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 1292e78fa38178ff7f555aae561465bbb4e27ac6..6567872ebd97ab3016b3af816f2048d473040464 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -13,11 +13,27 @@ set(LIB_FILES
RobotStateComponentPlugin.cpp
DebugDrawerHelperComponentPlugin.cpp
ArVizComponentPlugin.cpp
+ GraspCandidateObserverComponentPlugin.cpp
+ PlatformUnitComponentPlugin.cpp
+ RobotUnitComponentPlugin.cpp
+ RobotUnitObserverComponentPlugin.cpp
+ KinematicUnitComponentPlugin.cpp
+ TrajectoryPlayerComponentPlugin.cpp
+ CartesianPositionControlComponentPlugin.cpp
+ NaturalIKComponentPlugin.cpp
)
set(LIB_HEADERS
RobotStateComponentPlugin.h
DebugDrawerHelperComponentPlugin.h
ArVizComponentPlugin.h
+ GraspCandidateObserverComponentPlugin.h
+ PlatformUnitComponentPlugin.h
+ RobotUnitComponentPlugin.h
+ RobotUnitObserverComponentPlugin.h
+ 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/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b403e45ccfea923d1ead7b8e73fa5e7dbcf0de2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
@@ -0,0 +1,53 @@
+#include "CartesianPositionControlComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void CartesianPositionControlComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void CartesianPositionControlComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_cartesianPositionControl, PROPERTY_NAME);
+ }
+
+ void CartesianPositionControlComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the CartesianPositionControl");
+ }
+ }
+
+ CartesianPositionControlInterfacePrx CartesianPositionControlComponentPlugin::getCartesianPositionControl()
+ {
+ return _cartesianPositionControl;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ CartesianPositionControlComponentPluginUser::CartesianPositionControlComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ CartesianPositionControlInterfacePrx CartesianPositionControlComponentPluginUser::getCartesianPositionControl()
+ {
+ return plugin->getCartesianPositionControl();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d53885cc44d3d0d42f4357c6c7b7cf0cfa7db5f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class CartesianPositionControlComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ CartesianPositionControlInterfacePrx getCartesianPositionControl();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "CartesianPositionControlName";
+ CartesianPositionControlInterfacePrx _cartesianPositionControl;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use CartesianPositionControl.
+ */
+ class CartesianPositionControlComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ CartesianPositionControlComponentPluginUser();
+ CartesianPositionControlInterfacePrx getCartesianPositionControl();
+
+ private:
+ armarx::plugins::CartesianPositionControlComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using CartesianPositionControlComponentPluginUser = armarx::CartesianPositionControlComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e99ccfb4eb2994373380b03415f0e96b39b0e14
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "GraspCandidateObserverComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void GraspCandidateObserverComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void GraspCandidateObserverComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_graspCandidateObserver, PROPERTY_NAME);
+ }
+
+ void GraspCandidateObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "GraspCandidateObserver",
+ "Name of the GraspCandidateObserver");
+ }
+ }
+
+ grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPlugin::getGraspCandidateObserver()
+ {
+ return _graspCandidateObserver;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ GraspCandidateObserverComponentPluginUser::GraspCandidateObserverComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver()
+ {
+ return plugin->getGraspCandidateObserver();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..e15688a68568b1591ff5aeedfbb6d20337b002a2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class GraspCandidateObserverComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "GraspCandidateObserverName";
+ grasping::GraspCandidateObserverInterfacePrx _graspCandidateObserver;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use GraspCandidateObserver.
+ */
+ class GraspCandidateObserverComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ GraspCandidateObserverComponentPluginUser();
+ grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver();
+
+ private:
+ armarx::plugins::GraspCandidateObserverComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using GraspCandidateObserverComponentPluginUser = armarx::GraspCandidateObserverComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba8ba16d24d40bbb2312690ef87b4dd9f5619cd5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
@@ -0,0 +1,56 @@
+#include "KinematicUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void KinematicUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void KinematicUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_kinematicUnit, PROPERTY_NAME);
+ }
+
+ void KinematicUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the KinematicUnit");
+ }
+ }
+
+ KinematicUnitInterfacePrx KinematicUnitComponentPlugin::getKinematicUnit()
+ {
+ return _kinematicUnit;
+ }
+
+
+ }
+
+}
+
+namespace armarx
+{
+
+ KinematicUnitComponentPluginUser::KinematicUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ KinematicUnitInterfacePrx KinematicUnitComponentPluginUser::getKinematicUnit()
+ {
+ return plugin->getKinematicUnit();
+ }
+
+
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..e3b4062b59500f26314a6ccdb6e5480dd7d34a2b
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class KinematicUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ KinematicUnitInterfacePrx getKinematicUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "KinematicUnitName";
+ KinematicUnitInterfacePrx _kinematicUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use KinematicUnit.
+ */
+ class KinematicUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ KinematicUnitComponentPluginUser();
+ KinematicUnitInterfacePrx getKinematicUnit();
+
+ private:
+ armarx::plugins::KinematicUnitComponentPlugin* plugin = nullptr;
+ };
+
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using KinematicUnitComponentPluginUser = armarx::KinematicUnitComponentPluginUser;
+ }
+}
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/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f40b6cd34a5f5a8f028b714cc7a1517d590f2b3c
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
@@ -0,0 +1,53 @@
+#include "PlatformUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void PlatformUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void PlatformUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_platformUnit, PROPERTY_NAME);
+ }
+
+ void PlatformUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the PlatformUnit");
+ }
+ }
+
+ PlatformUnitInterfacePrx PlatformUnitComponentPlugin::getPlatformUnit()
+ {
+ return _platformUnit;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ PlatformUnitComponentPluginUser::PlatformUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ PlatformUnitInterfacePrx PlatformUnitComponentPluginUser::getPlatformUnit()
+ {
+ return plugin->getPlatformUnit();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..d2cec0f45b698f813d12a10920a122efbb63d00b
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class PlatformUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ PlatformUnitInterfacePrx getPlatformUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "PlatformUnitName";
+ PlatformUnitInterfacePrx _platformUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use PlatformUnit.
+ */
+ class PlatformUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ PlatformUnitComponentPluginUser();
+ PlatformUnitInterfacePrx getPlatformUnit();
+
+ private:
+ armarx::plugins::PlatformUnitComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using PlatformUnitComponentPluginUser = armarx::PlatformUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index e41d1f2aca5bfec043059be72033e9f35d5706a6..c5445458598bfcfe4ea427a4f519f6d5baacddaf 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -130,6 +130,11 @@ namespace armarx::plugins
}
}
+ RobotStateComponentInterfacePrx RobotStateComponentPlugin::getRobotStateComponent()
+ {
+ return _robotStateComponent;
+ }
+
bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return RemoteRobot::synchronizeLocalClone(robot, _robotStateComponent);
@@ -300,6 +305,11 @@ namespace armarx
getRobotStateComponentPlugin().setRobotRNSAndNode(id, rnsName, nodeName);
}
+ RobotStateComponentInterfacePrx RobotStateComponentPluginUser::getRobotStateComponent()
+ {
+ return getRobotStateComponentPlugin().getRobotStateComponent();
+ }
+
bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(robot);
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index 5dff431bcabf60f407a63f4f9899232b81eddab9..a338957d196d166494ad031ed3280abeef47b899 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -83,6 +83,8 @@ namespace armarx::plugins
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+
+ RobotStateComponentInterfacePrx getRobotStateComponent();
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
@@ -187,6 +189,9 @@ namespace armarx
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+
+ RobotStateComponentInterfacePrx getRobotStateComponent();
+
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..383de616a6001cd7358a2fd5604c44f7e669ecc6
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -0,0 +1,53 @@
+#include "RobotUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void RobotUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void RobotUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_robotUnit, PROPERTY_NAME);
+ }
+
+ void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the RobotUnit");
+ }
+ }
+
+ RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit()
+ {
+ return _robotUnit;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ RobotUnitComponentPluginUser::RobotUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit()
+ {
+ return plugin->getRobotUnit();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..4dc53dcb9fff70332663f1dd4a0211f3fdfd916d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class RobotUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ RobotUnitInterfacePrx getRobotUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "RobotUnitName";
+ RobotUnitInterfacePrx _robotUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use RobotUnit.
+ */
+ class RobotUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ RobotUnitComponentPluginUser();
+ RobotUnitInterfacePrx getRobotUnit();
+
+ private:
+ armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using RobotUnitComponentPluginUser = armarx::RobotUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..284533cf16e62834d85ff33e73d36b158943acef
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "RobotUnitObserverComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void RobotUnitObserverComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void RobotUnitObserverComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_robotUnitObserver, PROPERTY_NAME);
+ }
+
+ void RobotUnitObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "RobotUnitObserver",
+ "Name of the RobotUnitObserver");
+ }
+ }
+
+ ObserverInterfacePrx RobotUnitObserverComponentPlugin::getRobotUnitObserver()
+ {
+ return _robotUnitObserver;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ RobotUnitObserverComponentPluginUser::RobotUnitObserverComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ ObserverInterfacePrx RobotUnitObserverComponentPluginUser::getRobotUnitObserver()
+ {
+ return plugin->getRobotUnitObserver();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ebeff910700102871aa6229259e05f2efec866d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class RobotUnitObserverComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ ObserverInterfacePrx getRobotUnitObserver();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "RobotUnitObserverName";
+ ObserverInterfacePrx _robotUnitObserver;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use RobotUnitObserver.
+ */
+ class RobotUnitObserverComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ RobotUnitObserverComponentPluginUser();
+ ObserverInterfacePrx getRobotUnitObserver();
+
+ private:
+ armarx::plugins::RobotUnitObserverComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using RobotUnitObserverComponentPluginUser = armarx::RobotUnitObserverComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1de30ae07c9f8218666121c28350f32c1ca1f5dc
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "TrajectoryPlayerComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void TrajectoryPlayerComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void TrajectoryPlayerComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_trajectoryPlayer, PROPERTY_NAME);
+ }
+
+ void TrajectoryPlayerComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "TrajectoryPlayer",
+ "Name of the TrajectoryPlayer");
+ }
+ }
+
+ TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPlugin::getTrajectoryPlayer()
+ {
+ return _trajectoryPlayer;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ TrajectoryPlayerComponentPluginUser::TrajectoryPlayerComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPluginUser::getTrajectoryPlayer()
+ {
+ return plugin->getTrajectoryPlayer();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fc52df857f7a7225eacb25103dbd7fa16de4af5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class TrajectoryPlayerComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ TrajectoryPlayerInterfacePrx getTrajectoryPlayer();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "TrajectoryPlayerName";
+ TrajectoryPlayerInterfacePrx _trajectoryPlayer;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use TrajectoryPlayer.
+ */
+ class TrajectoryPlayerComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ TrajectoryPlayerComponentPluginUser();
+ TrajectoryPlayerInterfacePrx getTrajectoryPlayer();
+
+ private:
+ armarx::plugins::TrajectoryPlayerComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using TrajectoryPlayerComponentPluginUser = armarx::TrajectoryPlayerComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp
index b24b7f8dcf085c74f3767df01ab6ee42abebe99f..9c5f9118e3893d412b98fcef044ea37abb9d795b 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/AronNavigator.cpp
@@ -23,7 +23,7 @@
#include "AronNavigator.h"
#include <ArmarXCore/core/exceptions/Exception.h>
-
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
@@ -31,140 +31,66 @@ namespace armarx
{
template <typename Tin, typename Tout>
- Tout cast_and_check(const AronDataPtr& data, const std::string& path, const std::string& type)
+ Tout cast_and_check(const AronDataPtr& data, const AronPath& path, const std::string& type)
{
Tin tdata = Tin::dynamicCast(data);
if (!tdata)
{
- throw LocalException("value cannot be cast to ") << type << ". path: " << path;
+ throw LocalException("value cannot be cast to ") << type << ". path: " << path.toString();
}
return tdata->value;
}
AronNavigator::AronNavigator(const AronDataPtr& data)
- : data(data), path(""), key(""), index(-1)
+ : data(data)
{ }
- AronNavigator::AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index)
- : data(data), path(path), key(key), index(index)
+ AronNavigator::AronNavigator(const AronDataPtr& data, const AronPath& path)
+ : data(data), path(path)
{ }
- AronNavigator AronNavigator::atIndex(size_t index)
+ ListNavigator AronNavigator::asList()
{
AronListPtr list = AronListPtr::dynamicCast(data);
if (!list)
{
- throw LocalException("value is not a list. path: ") << path;
+ throw LocalException("value is not a list. path: ") << path.toString();
}
- return AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index);
+ return ListNavigator(list, path);
}
- AronNavigator AronNavigator::atKey(const std::string& key)
+ DictNavigator AronNavigator::asDict()
{
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (!obj)
- {
- throw LocalException("value is not an object. path: ") << path;
- }
- for (const AronKeyValuePair& pair : obj->elements)
+ AronDictPtr dict = AronDictPtr::dynamicCast(data);
+ if (!dict)
{
- if (pair.key == key)
- {
- return AronNavigator(pair.value, path + "/" + key, key, 0);
- }
+ throw LocalException("value is not a dict. path: ") << path.toString();
}
- throw LocalException("key '") << key << "'not found. path: " << path;
+ return DictNavigator(dict, path);
}
- std::vector<AronNavigator> AronNavigator::children()
- {
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (list)
- {
- std::vector<AronNavigator> result;
- for (size_t i = 0; i < list->elements.size(); i++)
- {
- result.emplace_back(AronNavigator(list->elements.at(i), path + "/" + std::to_string(i), "", i));
- }
- return result;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (obj)
- {
- std::vector<AronNavigator> result;
- for (const AronKeyValuePair& pair : obj->elements)
- {
- result.emplace_back(AronNavigator(pair.value, path + "/" + pair.key, pair.key, -1));
- }
- return result;
- }
- throw LocalException("value is not a list nor an object. path: ") << path;
- }
-
- void AronNavigator::append(const AronDataPtr& value)
+ Eigen::Vector3f AronNavigator::asVector3f()
{
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (!list)
- {
- throw LocalException("value is not a list. path: ") << path;
- }
- list->elements.push_back(value);
+ return Eigen::Vector3f(checkReinterpretNDArray<float>(3));
}
- bool AronNavigator::append(const std::string& key, const AronDataPtr& value)
+ Eigen::Matrix3f AronNavigator::asMatrix3f()
{
- int index = keyIndex(key);
- if (index >= 0)
- {
- return false;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.push_back({key, value});
- return true;
+ return Eigen::Matrix3f(checkReinterpretNDArray<float>(3, 3));
}
- void AronNavigator::set(const std::string& key, const AronDataPtr& value)
+ Eigen::Matrix4f AronNavigator::asMatrix4f()
{
- int index = keyIndex(key);
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.at(index).value = value;
+ return Eigen::Matrix4f(checkReinterpretNDArray<float>(4, 4));
}
- bool AronNavigator::hasKey(const std::string& key)
- {
- return keyIndex(key) >= 0;
- }
- int AronNavigator::keyIndex(const std::string& key)
- {
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (!obj)
- {
- throw LocalException("value is not an object. path: ") << path;
- }
- for (size_t i = 0; i < obj->elements.size(); i++)
- {
- if (obj->elements.at(i).key == key)
- {
- return i;
- }
- }
- return -1;
- }
- bool AronNavigator::deleteKey(const std::string& key)
- {
- int index = keyIndex(key);
- if (index < 0)
- {
- return false;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.erase(obj->elements.begin() + index);
- return true;
- }
+
+
+
#define HANDLE_TYPE(cppType, upperType) \
cppType AronNavigator::as##upperType() \
@@ -174,7 +100,7 @@ namespace armarx
bool AronNavigator::is##upperType() \
{ \
return Aron##upperType##Ptr::dynamicCast(data); \
- } \
+ } /*\
void AronNavigator::append##upperType(const cppType& value) \
{ \
append(new Aron##upperType(value)); \
@@ -182,7 +108,7 @@ namespace armarx
bool AronNavigator::append##upperType(const std::string& key, const cppType& value) \
{ \
return append(key, new Aron##upperType(value)); \
- }
+}*/
HANDLE_TYPE(std::string, String)
HANDLE_TYPE(int, Int)
@@ -190,15 +116,6 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
@@ -209,31 +126,7 @@ namespace armarx
return AronNullPtr::dynamicCast(data);
}
- bool AronNavigator::appendVec(const std::string& key, const std::vector<float>& vec)
- {
- AronListPtr list = new AronList();
- for (float f : vec)
- {
- list->elements.push_back(new AronFloat(f));
- }
- return append(key, list);
- }
- std::vector<float> armarx::aron::AronNavigator::AronNavigator::asVecFloat()
- {
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (!list)
- {
- throw LocalException("value is not a list. path: ") << path;
- }
- std::vector<float> result;
- for (size_t index = 0; index < list->elements.size(); index++)
- {
- float f = AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index).asFloat();
- result.push_back(f);
- }
- return result;
- }
void AronNavigator::writeXml(RapidXmlWriterNode parent)
{
@@ -285,14 +178,211 @@ namespace armarx
}*/
}
+
+
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size)
+ {
+ AronNDArrayPtr arr = AronNDArrayPtr::dynamicCast(data);
+ if (!arr)
+ {
+ throw LocalException("value is not an ndarray. path: ") << path.toString();
+ }
+ if (arr->itemSize != (int)size)
+ {
+ throw LocalException("item size mismatch for ndarray. itemSize: ") << arr->itemSize << ", requested: " << size << ", path: " << path.toString();
+ }
+ return arr;
+ }
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(size);
+ ARMARX_CHECK_W_HINT(arr->dimensions.size() == 1, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString());
+ return arr;
+ }
+
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1, size_t dim2)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(size);
+ ARMARX_CHECK_W_HINT(arr->dimensions.size() == 2, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(1) == (int)dim2, path.toString());
+ return arr;
+ }
+
bool AronNavigator::isList()
{
return AronListPtr::dynamicCast(data);
}
- bool AronNavigator::isObject()
+ bool AronNavigator::isDict()
+ {
+ return AronDictPtr::dynamicCast(data);
+ }
+
+ ListNavigator::ListNavigator(const AronListPtr& list, const AronPath& path)
+ : list(list), path(path)
+ {
+ }
+
+ AronNavigator ListNavigator::at(size_t index)
{
- return AronObjectPtr::dynamicCast(data);
+ return AronNavigator(list->elements.at(index), path.appendIndex(index));
}
+ std::vector<AronNavigator> ListNavigator::children()
+ {
+ std::vector<AronNavigator> result;
+ for (size_t i = 0; i < list->elements.size(); i++)
+ {
+ result.emplace_back(AronNavigator(list->elements.at(i), path.appendIndex(i)));
+ }
+ return result;
+ }
+
+ void ListNavigator::append(const AronDataPtr& value)
+ {
+ list->elements.push_back(value);
+ }
+
+ std::vector<AronDataPtr>& ListNavigator::get()
+ {
+ return list->elements;
+ }
+
+
+
+ std::vector<float> ListNavigator::asVecFloat()
+ {
+ std::vector<float> result;
+ for (size_t index = 0; index < list->elements.size(); index++)
+ {
+ float f = AronNavigator(list->elements.at(index), path.appendIndex(index)).asFloat();
+ result.push_back(f);
+ }
+ return result;
+ }
+
+ AronPath AronPath::appendIndex(size_t index) const
+ {
+ AronPath p;
+ p.path = path;
+ p.path.emplace_back(AronPathItem(index));
+ return p;
+ }
+
+ AronPath AronPath::appendKey(const std::string& key) const
+ {
+ AronPath p;
+ p.path = path;
+ p.path.emplace_back(AronPathItem(key));
+ return p;
+ }
+
+ std::string AronPath::toString() const
+ {
+ std::stringstream ss;
+ bool first = true;
+ for (const AronPathItem& i : path)
+ {
+ if (!first)
+ {
+ ss << "/";
+ }
+ first = false;
+ if (i.index >= 0)
+ {
+ ss << i.index;
+ }
+ else
+ {
+ ss << i.key;
+ }
+ }
+ return ss.str();
+ }
+
+ DictNavigator::DictNavigator(const AronDictPtr& dict, const AronPath& path)
+ : dict(dict), path(path)
+ {
+ }
+
+ AronNavigator DictNavigator::at(const std::string& key)
+ {
+ for (const AronKeyValuePair& pair : dict->elements)
+ {
+ if (pair.key == key)
+ {
+ return AronNavigator(pair.value, path.appendKey(key));
+ }
+ }
+ throw LocalException("key '") << key << "'not found. path: " << path.toString();
+ }
+
+ std::vector<AronNavigator> DictNavigator::children()
+ {
+ std::vector<AronNavigator> result;
+ for (const AronKeyValuePair& pair : dict->elements)
+ {
+ result.emplace_back(AronNavigator(pair.value, path.appendKey(pair.key)));
+ }
+ return result;
+ }
+
+ bool DictNavigator::append(const std::string& key, const AronDataPtr& value)
+ {
+ int index = indexOf(key);
+ if (index >= 0)
+ {
+ return false;
+ }
+ dict->elements.push_back({key, value});
+ return true;
+ }
+
+ bool DictNavigator::appendVec(const std::string& key, const std::vector<float>& vec)
+ {
+ AronListPtr list = new AronList();
+ for (float f : vec)
+ {
+ list->elements.push_back(new AronFloat(f));
+ }
+ return append(key, list);
+ }
+
+ void DictNavigator::set(const std::string& key, const AronDataPtr& value)
+ {
+ int index = indexOf(key);
+ dict->elements.at(index).value = value;
+ }
+
+ bool DictNavigator::has(const std::string& key)
+ {
+ return indexOf(key) >= 0;
+ }
+
+ int DictNavigator::indexOf(const std::string& key)
+ {
+ for (size_t i = 0; i < dict->elements.size(); i++)
+ {
+ if (dict->elements.at(i).key == key)
+ {
+ return i;
+ }
+ }
+ return -1;
+ }
+
+ bool DictNavigator::remove(const std::string& key)
+ {
+ int index = indexOf(key);
+ if (index < 0)
+ {
+ return false;
+ }
+ dict->elements.erase(dict->elements.begin() + index);
+ return true;
+ }
+
+
}
}
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h
index ae39728626f92f277c908c7b5bf8d176c7e7eb83..9c568530d13e22dc3f663beaa21e98fd5184481b 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.h
+++ b/source/RobotAPI/libraries/aron/AronNavigator.h
@@ -37,30 +37,54 @@ namespace armarx
enum { value = false };
};
+ struct AronPathItem
+ {
+ AronPathItem(int index): index(index), key("") {}
+ AronPathItem(const std::string& key): index(-1), key(key) {}
+ int index = -1;
+ std::string key = "";
+ };
+ struct AronPath
+ {
+ std::vector<AronPathItem> path;
+ AronPath appendIndex(size_t index) const;
+ AronPath appendKey(const std::string& key) const;
+ std::string toString() const;
+ };
+ class ListNavigator;
+ class DictNavigator;
+ template <typename T>
+ class NDArrayNavigator;
+
class AronNavigator
{
public:
AronNavigator(const AronDataPtr& data);
- AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index);
+ AronNavigator(const AronDataPtr& data, const AronPath& path);
- AronNavigator atIndex(size_t index);
- AronNavigator atKey(const std::string& key);
- std::vector<AronNavigator> children();
- void append(const AronDataPtr& value);
- bool append(const std::string& key, const AronDataPtr& value);
- void set(const std::string& key, const AronDataPtr& value);
- bool hasKey(const std::string& key);
- int keyIndex(const std::string& key);
- bool deleteKey(const std::string& key);
+ ListNavigator asList();
+ DictNavigator asDict();
+ template <typename T>
+ NDArrayNavigator<T> asNDArray()
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T));
+ return NDArrayNavigator<T>(arr, path);
+ }
+
+ Eigen::Vector3f asVector3f();
+ Eigen::Matrix3f asMatrix3f();
+ Eigen::Matrix4f asMatrix4f();
bool isList();
- bool isObject();
+ bool isDict();
bool isNull();
+
+
#define HANDLE_TYPE(cppType, upperType) \
cppType as##upperType(); \
bool is##upperType(); \
@@ -73,15 +97,6 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
@@ -91,21 +106,33 @@ namespace armarx
template <typename T>
std::vector<T> asStdVector();
- //TODO
- bool appendVec(const std::string& key, const std::vector<float>& vec);
- std::vector<float> asVecFloat();
+
template <typename T>
std::map<std::string, T> asStdMap();
void writeXml(RapidXmlWriterNode parent);
+ private:
+ AronNDArrayPtr checkCastToNDArray(size_t size);
+ AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1);
+ AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1, size_t dim2);
+ template <typename T>
+ T* checkReinterpretNDArray(size_t dim1)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1);
+ return reinterpret_cast<T*>(arr->data.data());
+ }
+ template <typename T>
+ T* checkReinterpretNDArray(size_t dim1, size_t dim2)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1, dim2);
+ return reinterpret_cast<T*>(arr->data.data());
+ }
private:
AronDataPtr data;
- std::string path;
- std::string key;
- int index;
+ AronPath path;
};
template<typename T>
@@ -126,18 +153,84 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
+ class ListNavigator
+ {
+ public:
+ ListNavigator(const AronListPtr& list, const AronPath& path);
+ AronNavigator at(size_t index);
+ std::vector<AronNavigator> children();
+ void append(const AronDataPtr& value);
+ std::vector<AronDataPtr>& get();
+
+ //TODO
+ std::vector<float> asVecFloat();
+
+ private:
+ AronListPtr list;
+ AronPath path;
+ };
+
+ class DictNavigator
+ {
+ public:
+ DictNavigator(const AronDictPtr& dict, const AronPath& path);
+ AronNavigator at(const std::string& key);
+ std::vector<AronNavigator> children();
+ bool append(const std::string& key, const AronDataPtr& value);
+ bool appendVec(const std::string& key, const std::vector<float>& vec);
+ void set(const std::string& key, const AronDataPtr& value);
+ bool has(const std::string& key);
+ int indexOf(const std::string& key);
+ bool remove(const std::string& key);
+ private:
+ AronDictPtr dict;
+ AronPath path;
+ };
+
+ template <typename T>
+ class NDArrayNavigator
+ {
+ public:
+ NDArrayNavigator(const AronNDArrayPtr& arr, const AronPath& path)
+ : arr(arr), path(path) {}
+ size_t dimensions()
+ {
+ return arr->dimensions.size();
+ }
+ int size(size_t dim)
+ {
+ return arr->dimensions.at(dim);
+ }
+
+ T& at(size_t i)
+ {
+ ARMARX_CHECK(dimensions() == 1);
+ return reinterpret_cast<T&>(_at(i));
+ }
+ T& at(size_t i, size_t j)
+ {
+ ARMARX_CHECK(dimensions() == 2);
+ return reinterpret_cast<T&>(_at(i * arr->dimensions.at(1) + j));
+ }
+ T* data()
+ {
+ return reinterpret_cast<T*>(arr->data.data());
+ }
+ private:
+ Ice::Byte& _at(size_t i)
+ {
+
+ return arr->data.at(arr->itemSize * i);
+ }
+
+ private:
+ AronNDArrayPtr arr;
+ AronPath path;
+ };
+
}
}
diff --git a/source/RobotAPI/libraries/aron/types/TypesMap.cpp b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
index 91b29d5b8d8a7723856ec1017160aeae57b493f6..2b3ca35316b4be12d4242d5a8168f931522dac1f 100644
--- a/source/RobotAPI/libraries/aron/types/TypesMap.cpp
+++ b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
@@ -37,7 +37,7 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type)
{
SingleTypePtr single = SingleTypePtr::dynamicCast(type);
ListTypePtr list = ListTypePtr::dynamicCast(type);
- ObjectTypePtr obj = ObjectTypePtr::dynamicCast(type);
+ DictTypePtr dict = DictTypePtr::dynamicCast(type);
if (single)
{
return getCppType(single->namespaces, single->typeName);
@@ -46,9 +46,9 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type)
{
return "std::vector<" + getCppType(list->childtype) + ">";
}
- if (obj)
+ if (dict)
{
- return "std::map<std::string, " + getCppType(obj->childtype) + ">";
+ return "std::map<std::string, " + getCppType(dict->childtype) + ">";
}
throw LocalException("Unsupported type");
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index cea227464abe29898488605e6dcb38f5e50fb4a5..90c68c71c1fd0ca963f8f4eb1bc05c1d926be1f7 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -1971,6 +1971,36 @@ namespace armarx
return getDeriv(dim, 0);
}
+ Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const
+ {
+ if (!trajectory)
+ {
+ throw LocalException("Ptr to trajectory is NULL");
+ }
+ size_t numDim = trajectory->dim();
+ Eigen::VectorXf result(numDim);
+ for (std::size_t i = 0; i < numDim; ++i)
+ {
+ result(i) = getPosition(i);
+ }
+ return result;
+ }
+
+ Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const
+ {
+ if (!trajectory)
+ {
+ throw LocalException("Ptr to trajectory is NULL");
+ }
+ size_t numDim = trajectory->dim();
+ Eigen::VectorXd result(numDim);
+ for (std::size_t i = 0; i < numDim; ++i)
+ {
+ result(i) = getPosition(i);
+ }
+ return result;
+ }
+
double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
{
if (!trajectory)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 2feaea611590e10f71b227bcb845c46dd37f59ce..990de647c0bb74092d96a7774830cd1fb6f96f98 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -128,6 +128,9 @@ namespace armarx
{
return getPositionsAs<double>();
}
+ Eigen::VectorXf getPositionsAsVectorXf() const;
+ Eigen::VectorXd getPositionsAsVectorXd() const;
+
template<class T>
void writePositionsToNameValueMap(std::map<std::string, T>& map) const
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index 9699d191373f2210ec6677e05a8af1b6315ca71d..05a777679813c9f2411dbe6cce56141ab75dae77 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -6,15 +6,22 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCore
RobotAPICore
+ StructuralJson
+ SimpleJsonLogger
)
set(LIB_FILES
NaturalDiffIK.cpp
SimpleDiffIK.cpp
+ RobotPlacement.cpp
+ GraspTrajectory.cpp
)
set(LIB_HEADERS
NaturalDiffIK.h
SimpleDiffIK.h
+ DiffIKProvider.h
+ RobotPlacement.h
+ GraspTrajectory.h
)
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
new file mode 100644
index 0000000000000000000000000000000000000000..a75434af401538fc92697010ee6fc3d6b43e9071
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
+
+ struct DiffIKResult
+ {
+ bool reachable;
+ float posError;
+ float oriError;
+ Eigen::VectorXf jointValues;
+
+ };
+ class DiffIKProvider
+ {
+ public:
+ virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
+ virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d76464e94326b8e864ace29ec294d10aff65402
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -0,0 +1,461 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "GraspTrajectory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+using namespace armarx;
+
+
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
+ feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0),
+ feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows()))
+{ }
+
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget,
+ float dt, const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+{ }
+
+Eigen::Vector3f GraspTrajectory::Keypoint::getTargetPosition() const
+{
+ return ::math::Helpers::GetPosition(tcpTarget);
+}
+
+Eigen::Matrix3f GraspTrajectory::Keypoint::getTargetOrientation() const
+{
+ return ::math::Helpers::GetOrientation(tcpTarget);
+}
+
+Eigen::Matrix4f GraspTrajectory::Keypoint::getTargetPose() const
+{
+ return tcpTarget;
+}
+
+void GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev, float dt)
+{
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
+ Eigen::VectorXf hnd0 = prev->handJointsTarget;
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
+ Eigen::VectorXf hnd1 = handJointsTarget;
+
+ Eigen::Vector3f dpos = pos1 - pos0;
+ Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
+ Eigen::VectorXf dhnd = hnd1 - hnd0;
+
+ this->dt = dt;
+ feedForwardPosVelocity = dpos / dt;
+ feedForwardOriVelocity = dori / dt;
+ feedForwardHandJointsVelocity = dhnd / dt;
+}
+
+GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart)
+{
+ KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
+ keypointMap[0] = keypoints.size();
+ keypoints.push_back(keypoint);
+}
+
+void GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ KeypointPtr prev = lastKeypoint();
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ float t = getDuration() + dt;
+ keypointMap[t] = keypoints.size();
+ keypoints.push_back(keypoint);
+}
+
+size_t GraspTrajectory::getKeypointCount() const
+{
+ return keypoints.size();
+}
+
+void GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ if (index <= 0 || index > keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ if (index < keypoints.size())
+ {
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(keypoint, next->dt);
+ }
+ keypoints.insert(keypoints.begin() + index, keypoint);
+ updateKeypointMap();
+}
+
+void GraspTrajectory::removeKeypoint(size_t index)
+{
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ keypoints.erase(keypoints.begin() + index);
+ if (index < keypoints.size())
+ {
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(prev, next->dt);
+ }
+ updateKeypointMap();
+}
+
+void GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ keypoints.at(index) = keypoint;
+ updateKeypointMap();
+}
+
+void GraspTrajectory::setKeypointDt(size_t index, float dt)
+{
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr& keypoint = keypoints.at(index);
+ keypoint->updateVelocities(prev, dt);
+ updateKeypointMap();
+}
+
+GraspTrajectory::KeypointPtr& GraspTrajectory::lastKeypoint()
+{
+ return keypoints.at(keypoints.size() - 1);
+}
+
+GraspTrajectory::KeypointPtr& GraspTrajectory::getKeypoint(int i)
+{
+ return keypoints.at(i);
+}
+
+Eigen::Matrix4f GraspTrajectory::getStartPose()
+{
+ return getKeypoint(0)->getTargetPose();
+}
+
+void GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+{
+ if (t <= 0)
+ {
+ i1 = 0;
+ i2 = 0;
+ f = 0;
+ return;
+ }
+ std::map<float, size_t>::const_iterator it, prev;
+ it = keypointMap.upper_bound(t);
+ if (it == keypointMap.end())
+ {
+ i1 = keypoints.size() - 1;
+ i2 = keypoints.size() - 1;
+ f = 0;
+ return;
+ }
+ prev = std::prev(it);
+ i1 = prev->second;
+ i2 = it->second;
+ f = ::math::Helpers::ILerp(prev->first, it->first, t);
+}
+
+Eigen::Vector3f GraspTrajectory::GetPosition(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+}
+
+Eigen::Matrix3f GraspTrajectory::GetOrientation(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ Eigen::Vector3f oriVel = GetOrientationDerivative(t);
+ if (oriVel.squaredNorm() == 0)
+ {
+ return getKeypoint(i1)->getTargetOrientation();
+ }
+ Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized());
+ aa.angle() = aa.angle() * f * getKeypoint(i2)->dt;
+ return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation();
+}
+
+Eigen::Matrix4f GraspTrajectory::GetPose(float t)
+{
+ return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
+}
+
+std::vector<Eigen::Matrix4f> GraspTrajectory::getAllKeypointPoses()
+{
+ std::vector<Eigen::Matrix4f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPose());
+ }
+ return res;
+}
+
+std::vector<Eigen::Vector3f> GraspTrajectory::getAllKeypointPositions()
+{
+ std::vector<Eigen::Vector3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPosition());
+ }
+ return res;
+}
+
+std::vector<Eigen::Matrix3f> GraspTrajectory::getAllKeypointOrientations()
+{
+ std::vector<Eigen::Matrix3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetOrientation());
+ }
+ return res;
+}
+
+Eigen::VectorXf GraspTrajectory::GetHandValues(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+
+ return getKeypoint(i1)->handJointsTarget * (1 - f) + getKeypoint(i2)->handJointsTarget * f;
+}
+
+Eigen::Vector3f GraspTrajectory::GetPositionDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardPosVelocity;
+}
+
+Eigen::Vector3f GraspTrajectory::GetOrientationDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardOriVelocity;
+}
+
+Eigen::Vector6f GraspTrajectory::GetTcpDerivative(float t)
+{
+ Eigen::Vector6f ffVel;
+ ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
+ ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
+ return ffVel;
+}
+
+Eigen::VectorXf GraspTrajectory::GetHandJointsDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardHandJointsVelocity;
+}
+
+float GraspTrajectory::getDuration() const
+{
+ return keypointMap.rbegin()->first;
+}
+
+GraspTrajectory::Length GraspTrajectory::calculateLength() const
+{
+ Length l;
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr k0 = keypoints.at(i - 1);
+ KeypointPtr k1 = keypoints.at(i);
+
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(k0->tcpTarget);
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(k1->tcpTarget);
+
+ l.pos += (pos1 - pos0).norm();
+ l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).angle());
+ }
+ return l;
+}
+
+int GraspTrajectory::GetHandJointCount() const
+{
+ return keypoints.at(0)->handJointsTarget.rows();
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation)
+{
+ GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+{
+ GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::getClone()
+{
+ return getTransformed(Eigen::Matrix4f::Identity());
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward)
+{
+ Eigen::Matrix4f startPose = getStartPose();
+ Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
+ Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ Eigen::Vector3f up(0, 0, 1);
+
+ float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
+ Eigen::AngleAxisf aa(angle, up);
+
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ return getTransformed(transform);
+}
+
+SimpleDiffIK::Reachability GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+{
+ return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+}
+
+void GraspTrajectory::writeToFile(const std::string& filename)
+{
+ RapidXmlWriter writer;
+ RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ SimpleJsonLoggerEntry e;
+ e.Add("dt", keypoint->dt);
+ e.AddAsArr("Pose", keypoint->tcpTarget);
+ e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
+ }
+ writer.saveToFile(filename, true);
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
+{
+ std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage;
+ armarx::CMakePackageFinder finder(packageName);
+ std::string dataDir = finder.getDataDir() + "/" + packageName;
+ return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
+{
+ RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
+ GraspTrajectoryPtr traj;
+ for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
+ {
+ StructuralJsonParser p(kpNode.value());
+ p.parse();
+ JPathNavigator nav(p.parsedJson);
+ float dt = nav.selectSingleNode("dt").asFloat();
+
+ Eigen::Matrix4f pose;
+ std::vector<JPathNavigator> rows = nav.select("Pose/*");
+ for (int i = 0; i < 4; i++)
+ {
+ std::vector<JPathNavigator> cells = rows.at(i).select("*");
+ for (int j = 0; j < 4; j++)
+ {
+ pose(i, j) = cells.at(j).asFloat();
+ }
+ }
+
+ Eigen::Vector3f handValues;
+ std::vector<JPathNavigator> cells = nav.select("HandValues/*");
+ for (int j = 0; j < 3; j++)
+ {
+ handValues(j) = cells.at(j).asFloat();
+ }
+
+ if (!traj)
+ {
+ traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ }
+ else
+ {
+ traj->addKeypoint(pose, handValues, dt);
+ }
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const std::string& filename)
+{
+ return ReadFromReader(RapidXmlReader::FromFile(filename));
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromString(const std::string& xml)
+{
+ return ReadFromReader(RapidXmlReader::FromXmlString(xml));
+}
+
+void GraspTrajectory::updateKeypointMap()
+{
+ keypointMap.clear();
+ float t = 0;
+ for (size_t i = 0; i < keypoints.size(); i++)
+ {
+ t += getKeypoint(i)->dt;
+ keypointMap[t] = i;
+ }
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e5633d59e9a2f2c8ad533735370a1f008694089
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
@@ -0,0 +1,151 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <map>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/interface/serialization/Eigen.h>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr;
+
+ class GraspTrajectory
+ {
+ public:
+ class Keypoint;
+ typedef boost::shared_ptr<Keypoint> KeypointPtr;
+
+ class Keypoint
+ {
+ public:
+ Eigen::Matrix4f tcpTarget;
+ Eigen::VectorXf handJointsTarget;
+ float dt;
+ Eigen::Vector3f feedForwardPosVelocity;
+ Eigen::Vector3f feedForwardOriVelocity;
+ Eigen::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity);
+ Eigen::Vector3f getTargetPosition() const;
+ Eigen::Matrix3f getTargetOrientation() const;
+ Eigen::Matrix4f getTargetPose() const;
+ void updateVelocities(const KeypointPtr& prev, float dt);
+ };
+
+ struct Length
+ {
+ float pos = 0;
+ float ori = 0;
+ };
+
+
+ public:
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart);
+
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ size_t getKeypointCount() const;
+
+ void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ void removeKeypoint(size_t index);
+
+ void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ void setKeypointDt(size_t index, float dt);
+
+ KeypointPtr& lastKeypoint();
+ KeypointPtr& getKeypoint(int i);
+ Eigen::Matrix4f getStartPose();
+
+ void getIndex(float t, int& i1, int& i2, float& f);
+
+ Eigen::Vector3f GetPosition(float t);
+
+ Eigen::Matrix3f GetOrientation(float t);
+
+ Eigen::Matrix4f GetPose(float t);
+
+ std::vector<Eigen::Matrix4f> getAllKeypointPoses();
+ std::vector<Eigen::Vector3f> getAllKeypointPositions();
+ std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
+
+ Eigen::VectorXf GetHandValues(float t);
+
+ Eigen::Vector3f GetPositionDerivative(float t);
+
+ Eigen::Vector3f GetOrientationDerivative(float t);
+
+ Eigen::Vector6f GetTcpDerivative(float t);
+
+ Eigen::VectorXf GetHandJointsDerivative(float t);
+
+ float getDuration() const;
+
+ Length calculateLength() const;
+ int GetHandJointCount() const;
+
+
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
+
+ GraspTrajectoryPtr getClone();
+
+ GraspTrajectoryPtr getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+
+ SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+
+ void writeToFile(const std::string& filename);
+
+ static GraspTrajectoryPtr ReadFromFile(const grasping::GraspCandidatePtr& cnd);
+
+ static GraspTrajectoryPtr ReadFromReader(const RapidXmlReaderPtr& reader);
+ static GraspTrajectoryPtr ReadFromFile(const std::string& filename);
+ static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+
+ private:
+
+ void updateKeypointMap();
+
+ private:
+ std::vector<KeypointPtr> keypoints;
+ std::map<float, size_t> keypointMap;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.cpp b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4075c9aae309b66ce51d7125ede732b4869ba88d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
@@ -0,0 +1,68 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotPlacement.h"
+
+#include <VirtualRobot/math/Helpers.h>
+
+using namespace armarx;
+
+RobotPlacement::RobotPlacement(const DiffIKProviderPtr& ikProvider)
+{
+}
+
+std::vector<Eigen::Matrix4f> RobotPlacement::CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa)
+{
+ std::vector<Eigen::Matrix4f> r;
+ for (int x = minx; x <= maxx; x++)
+ for (int y = miny; y <= maxy; y++)
+ for (int a = mina; a <= maxa; a++)
+ {
+ r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
+ }
+ return r;
+}
+
+std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params)
+{
+ std::vector<RobotPlacement::Result> r;
+ std::vector<Eigen::Matrix4f> tcpTargets;
+ for (const Eigen::Matrix4f& pp : params.prePoses)
+ {
+ tcpTargets.emplace_back(pp);
+ }
+ std::vector<Eigen::Matrix4f> grasPoses = params.graspTrajectory->getAllKeypointPoses();
+
+ for (const Eigen::Matrix4f& placement : robotPlacements)
+ {
+ Eigen::Matrix4f invPlacement = placement.inverse();
+ std::vector<Eigen::Matrix4f> localPoses;
+ for (const Eigen::Matrix4f& tcpPose : tcpTargets)
+ {
+ localPoses.emplace_back(invPlacement * tcpPose);
+ }
+ DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0));
+ throw LocalException("not implemented");
+ }
+ return {};
+}
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h
new file mode 100644
index 0000000000000000000000000000000000000000..a677a7a1181b1a88dc731da6f4859c188453f5bf
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DiffIKProvider.h"
+#include "GraspTrajectory.h"
+
+#include <boost/shared_ptr.hpp>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class RobotPlacement> RobotPlacementPtr;
+
+ class RobotPlacement
+ {
+ public:
+ struct Result
+ {
+ DiffIKResult ikResult;
+ };
+ struct PlacementParams
+ {
+ std::vector<Eigen::Matrix4f> prePoses;
+ GraspTrajectoryPtr graspTrajectory;
+ };
+ public:
+ RobotPlacement(const DiffIKProviderPtr& ikProvider);
+
+ static std::vector<Eigen::Matrix4f> CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa);
+
+ std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const PlacementParams& params);
+
+
+ private:
+ DiffIKProviderPtr ikProvider;
+ bool returnOnlyReachable = true;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index c1a91dacdec922aac74d8160fff0f8c03256353f..c3f1a32dbaa1001d06762522080ef5367c072b70 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -140,4 +140,35 @@ namespace armarx
return r;
}
+ SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+ : rns(rns), tcp(tcp), params(params)
+ {
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+ {
+ params.resetRnsValues = true;
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+ {
+ params.resetRnsValues = false;
+ rns->setJointValues(startJointValues);
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+ }
+
}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index ffd371a6fe8d8ea8cd080e97fc1658c632b46868..2adcf162d666013f1d61016be15394761ad30e02 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -23,6 +23,8 @@
#pragma once
+#include "DiffIKProvider.h"
+
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index e6987e894a504547847e1afd546dbb3af8daa254..557e31e5eb860f0e170a684aa6e7afa0b00487d4 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -33,6 +33,7 @@
using namespace armarx;
+
CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
: _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
{
@@ -87,7 +88,10 @@ void CartesianNaturalPositionControllerProxy::init()
_controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
_controllerCreated = true;
}
- _controller->activateController();
+ if (_activateControllerOnInit)
+ {
+ _controller->activateController();
+ }
}
bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight)
@@ -349,6 +353,11 @@ std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::
return result;
}
+void CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
+{
+ _activateControllerOnInit = value;
+}
+
void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
{
VelocityBaseSettings& v = _velocityBaseSettings;
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index eb75ba24394fb6deb1ae836b195e788d82957624..c2ccc06f5ce8b1a43106443ff3c169d0e12aa4e3 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -114,7 +114,6 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr rns;
};
-
CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
@@ -175,6 +174,8 @@ namespace armarx
DynamicKp getDynamicKp();
static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
+ void setActivateControllerOnInit(bool value);
+
private:
void updateDynamicKp();
void updateNullspaceTargets();
@@ -208,5 +209,6 @@ namespace armarx
std::map<std::string, WaypointConfig> _defaultWaypointConfigs;
bool _waypointChanged = false;
FTSensorValue _ftOffset;
+ bool _activateControllerOnInit = false;
};
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 88c57b35768281fee04794b07cf789318203f140..f0ad1b7309e7f33c84764363ca5cd150a370b18b 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -213,3 +213,33 @@ void NaturalIK::setLowerArmLength(float value)
{
lowerArmLength = value;
}
+
+NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params)
+ : natik(natik), arm(arm), setOri(setOri), params(params)
+{
+}
+
+DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+{
+ params.diffIKparams.resetRnsValues = true;
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
+
+DiffIKResult NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+{
+ params.diffIKparams.resetRnsValues = false;
+ arm.rns->setJointValues(startJointValues);
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 6db3aa965d5860f4bb5bc1e2bca48901b08e8b04..d6315096a58084776797e6ce5551b65651ff4026 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -27,6 +27,7 @@
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/libraries/diffik/DiffIKProvider.h>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <optional>
@@ -134,4 +135,19 @@ namespace armarx
float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH;
float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
};
+
+ class NaturalIKProvider
+ : public DiffIKProvider
+ {
+ public:
+ NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params = NaturalIK::Parameters());
+ DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+
+ private:
+ NaturalIK natik;
+ NaturalIK::ArmJoints arm;
+ NaturalDiffIK::Mode setOri;
+ NaturalIK::Parameters params;
+ };
}