diff --git a/.gitignore b/.gitignore
index efe7e6e621bb44ff3b61c296ce841445f5353451..6bbc3ffa11b5ac3f4e9acca1b3eeeb70d886e60d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,17 +1,23 @@
-.keep_in_git
+/build/*
+!/build/.gitkeep
+!.gitkeep
+!.gitignore
-/build*
-
-source/RobotAPI/Test.h
+source/*/Test.h
*.bak
*#
.#*
*~
+*.swp
+.*.kate-swp
+.*.swo
+*.pyc
.DS_Store
CMakeFiles
CMakeCache.txt
+CMakeLists.txt.user
*.o
*.os
@@ -22,3 +28,18 @@ CMakeCache.txt
*.dylib
moc_*
+# eclipse stuff
+.project
+.pydevproject
+.settings
+.metadata
+.cproject
+.project
+
+# MemoryX
+.cache
+mongod.log*
+data/db/
+data/dbdump/
+
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 19d6498c21e86f3eb2beeaff748c61a582a631fc..f178c8c26ec65b626e0b407d4327c125fb54a153 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,7 @@ include(${ArmarXCore_CMAKE_DIR}/ArmarXProject.cmake)
armarx_project("RobotAPI")
add_subdirectory(source)
+add_subdirectory(interface)
install_project()
diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..59c35598d7a14e225c75952ce619863348a8b74a
--- /dev/null
+++ b/interface/CMakeLists.txt
@@ -0,0 +1,7 @@
+###
+### CMakeLists.txt file for ArmarX Interfaces
+###
+
+set(ROBOTAPI_INTERFACE_DEPEND ArmarXCore)
+# generate the interface library
+armarx_interfaces_generate_library(RobotAPI 0.1.0 0 ${ROBOTAPI_INTERFACE_DEPEND})
diff --git a/interface/slice/units/ForceTorqueUnit.ice b/interface/slice/units/ForceTorqueUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..93fb1be4a0630b35ea49cab956775d4040974aa7
--- /dev/null
+++ b/interface/slice/units/ForceTorqueUnit.ice
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI
+ * @author Mirko Waechter <waechter at kit dot edu>
+ * @copyright 2013 Mirko Waechter
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
+
+#include <units/UnitInterface.ice>
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+#include <observers/VariantBase.ice>
+#include <observers/ObserverInterface.ice>
+#include <robotstate/RobotState.ice>
+
+module armarx
+{
+
+
+ sequence<string> StringMap;
+ interface ForceTorqueUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ void setOffset(FramedVector3Map forceTorqueOffsets);
+ void setToNull(StringMap sensorNames);
+ };
+
+
+
+ interface ForceTorqueUnitListener
+ {
+ void reportSensorValues(string type, FramedVector3Map forces);
+ };
+
+ interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
+ {
+
+ };
+
+};
+
+#endif
diff --git a/interface/slice/units/TCPControlUnit.ice b/interface/slice/units/TCPControlUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..6b7517f5f42fbe41cb08266ce4b053d859a38438
--- /dev/null
+++ b/interface/slice/units/TCPControlUnit.ice
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI
+ * @author Mirko Waechter <waechter at kit dot edu>
+ * @copyright 2013 Mirko Waechter
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
+
+#include <units/UnitInterface.ice>
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+#include <observers/VariantBase.ice>
+#include <observers/ObserverInterface.ice>
+#include <robotstate/RobotState.ice>
+#include <robotstate/PoseBase.ice>
+
+module armarx
+{
+
+ interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ void setCycleTime(int milliseconds);
+// void setTCPVelocity(string robotNodeSetName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+ void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+
+ };
+
+
+ dictionary<string, FramedPoseBase> FramedPoseBaseMap;
+ interface TCPControlUnitListener
+ {
+ void reportTCPPose(FramedPoseBaseMap tcpPoses);
+ void reportTCPVelocities(FramedVector3Map tcpTranslationVelocities, FramedVector3Map tcpOrienationVelocities);
+
+ };
+
+ interface TCPControlUnitObserverInterface extends ObserverInterface, TCPControlUnitListener
+ {
+
+ };
+
+
+};
+
+#endif
diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index 1000114d8d592e8078c0f1ad2b18b0e79a175ab9..475cc9398af64bbc32318c7edb7a311f92ae2e9c 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -1,4 +1,5 @@
add_subdirectory(core)
add_subdirectory(motioncontrol)
add_subdirectory(applications)
+add_subdirectory(units)
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 6135bbd62b00d9c085039eb23e53326e051c3334..578f1ace321c1843e987fc1151db6e1362cf3c7e 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -1,4 +1,5 @@
-
+add_subdirectory(ForceTorqueObserver)
add_subdirectory(MotionControlTest)
+add_subdirectory(TCPControlUnit)
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..3a02531313f90edf32ded55e11ec325969453925
--- /dev/null
+++ b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
@@ -0,0 +1,28 @@
+
+armarx_component_set_name(ForceTorqueObserver)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+ set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot)
+
+ set(SOURCES "")
+ set(HEADERS ForceTorqueObserverApp.h)
+
+ armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
new file mode 100755
index 0000000000000000000000000000000000000000..cab40777f7e5607e02dd0df0e5974c4f44d8af5f
--- /dev/null
+++ b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
@@ -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 Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/ForceTorqueObserver.h>
+
+namespace armarx
+{
+ class ForceTorqueObserverApp :
+ virtual public armarx::Application
+ {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<ForceTorqueObserver>(properties) );
+ }
+ };
+}
+
+
+
diff --git a/source/RobotAPI/applications/MotionControl/CMakeLists.txt b/source/RobotAPI/applications/MotionControl/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..ea23a4dc5095874712bbc14592dadc8104cb239f
--- /dev/null
+++ b/source/RobotAPI/applications/MotionControl/CMakeLists.txt
@@ -0,0 +1,28 @@
+
+armarx_component_set_name(MotionControl)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+ set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
+
+ set(SOURCES "")
+ set(HEADERS MotionControlApp.h)
+
+ armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()
diff --git a/source/RobotAPI/applications/MotionControl/MotionControlApp.h b/source/RobotAPI/applications/MotionControl/MotionControlApp.h
new file mode 100755
index 0000000000000000000000000000000000000000..62045985b2efc3df6a18e8216bfa0e2a2139d9eb
--- /dev/null
+++ b/source/RobotAPI/applications/MotionControl/MotionControlApp.h
@@ -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 Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/motioncontrol/MotionControl.h>
+
+namespace armarx
+{
+ class MotionControlApp :
+ virtual public armarx::Application
+ {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<MotionControl::MotionControlOfferer>(properties) );
+ }
+ };
+}
+
+
+
diff --git a/source/RobotAPI/applications/MotionControlTest/MotionControlTestApp.h b/source/RobotAPI/applications/MotionControlTest/MotionControlTestApp.h
index abac52768b1f277e1da164066332e9f3119d5cbc..b6f7a128c0e1f5b6e423905c2cbd55dad7b46756 100644
--- a/source/RobotAPI/applications/MotionControlTest/MotionControlTestApp.h
+++ b/source/RobotAPI/applications/MotionControlTest/MotionControlTestApp.h
@@ -32,7 +32,7 @@ namespace armarx
{
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<MotionControl::MotionControlHandler>(properties) );
+ registry->addObject( Component::create<MotionControl::MotionControlOfferer>(properties) );
}
};
}
diff --git a/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt b/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..c3b5fe509d9bce689deaef57f3a5d15400687d31
--- /dev/null
+++ b/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
@@ -0,0 +1,28 @@
+
+armarx_component_set_name(TCPControlUnit)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+ set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot)
+
+ set(SOURCES "")
+ set(HEADERS TCPControlUnitApp.h)
+
+ armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()
diff --git a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
new file mode 100755
index 0000000000000000000000000000000000000000..4ab7d2dee5cd1843c729a4ea0f3ab2571b6cf801
--- /dev/null
+++ b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
@@ -0,0 +1,43 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/TCPControlUnit.h>
+#include <RobotAPI/units/TCPControlUnitObserver.h>
+
+namespace armarx
+{
+ class TCPControlUnitApp :
+ virtual public armarx::Application
+ {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<TCPControlUnit>(properties) );
+ registry->addObject( Component::create<TCPControlUnitObserver>(properties) );
+ }
+ };
+}
+
+
+
diff --git a/source/RobotAPI/core/CMakeLists.txt b/source/RobotAPI/core/CMakeLists.txt
index 1761fc4df670e2dd3536f51daaa96a23e187925f..73413bd6fe23dab50c7542ca7763c872ae2b53f4 100644
--- a/source/RobotAPI/core/CMakeLists.txt
+++ b/source/RobotAPI/core/CMakeLists.txt
@@ -15,10 +15,13 @@ if (ARMARX_BUILD)
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)
- set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreRobotStateComponent)
+ set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreRobotStateComponent ${VirtualRobot_LIBRARIES})
+
+ set(LIB_FILES RobotStatechartContext.cpp
+ )
+ set(LIB_HEADERS RobotStatechartContext.h
+ )
- set(LIB_FILES RobotStatechartContext.cpp)
- set(LIB_HEADERS RobotStatechartContext.h)
add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index 98a186ff330cafb814ea1effd732d654262182e4..5b85b23d6dcd2f4c5e6c0e0fc1ae59c408d28f18 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -26,7 +26,6 @@
#include <Core/statechart/Statechart.h>
#include <Core/robotstate/remote/RemoteRobot.h>
#include <Core/robotstate/remote/ArmarPose.h>
-#include <Core/robotstate/RobotStateObjectFactories.h>
//#include <VirtualRobot/VirtualRobot.h>
@@ -43,10 +42,9 @@ namespace armarx
// StatechartContext::onInitStatechart();
ARMARX_LOG << eINFO << "Init RobotStatechartContext" << flush;
- RobotStateObjectFactories::addFactories(getIceManager()->getCommunicator());
kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
-
+ usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("RobotStateComponent");
usingProxy(kinematicUnitObserverName);
}
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 7a3e92b49e139fd80e97045377557bfe2cd39d9b..1992440daa299f9766d9d6aec9cab0efd2ab8452 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -62,7 +62,7 @@ namespace armarx
{
public:
// inherited from Component
- virtual std::string getDefaultName() { return "RobotStatechartContext"; };
+ virtual std::string getDefaultName() { return "RobotStatechartContext"; }
virtual void onInitStatechartContext();
virtual void onConnectStatechartContext();
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index ef91d7dd4a6bbe89301bd67dfc5f92d5357e3e8f..c52c05f1bd9019e307414614577acaf4d6b0abf2 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -15,13 +15,13 @@
using namespace armarx;
using namespace armarx::MotionControl;
-MotionControlHandler::MotionControlHandler()
+MotionControlOfferer::MotionControlOfferer()
{
}
-void MotionControlHandler::onInitRemoteStateOfferer()
+void MotionControlOfferer::onInitRemoteStateOfferer()
{
//addState<MoveJoints>("MoveJoints");
//addState<MotionControlTestState>("MotionControlTestState");
@@ -30,7 +30,7 @@ void MotionControlHandler::onInitRemoteStateOfferer()
-void MotionControlHandler::onConnectRemoteStateOfferer()
+void MotionControlOfferer::onConnectRemoteStateOfferer()
{
}
@@ -40,16 +40,16 @@ void MoveJoints::defineParameters()
{
context = getContext<RobotStatechartContext>();
- setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
- addToInput("targetJointValues", VariantType::Float, false);
+ addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
// TODO: add when we have decided how to do it
addToInput("jointMaxSpeeds", VariantType::List(VariantType::Float), true);
addToInput("jointMaxSpeed", VariantType::List(VariantType::Float), true);
- addToInput("targetTolerance", VariantType::Float, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
@@ -69,7 +69,9 @@ void MoveJoints::onEnter()
throw LocalException("Sizes of joint name list and joint value list do not match!");
}
- for(int i=0; i<jointNames->getSize(); i++ )
+ ARMARX_LOG << "number of joints that will be set: " << jointNames->getSize() << flush;
+
+ for (int i=0; i<jointNames->getSize(); i++)
{
targetJointAngles[jointNames->getVariant(i)->getString()] = targetJointValues->getVariant(i)->getFloat();
ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetJointValues->getVariant(i)->getFloat() << std::endl;
@@ -80,14 +82,15 @@ void MoveJoints::onEnter()
//ExpressionPtr cond = Expression::create();
Term cond;
- float tolerance = getInput<float>("targetTolerance");
- for(int i=0; i<jointNames->getSize(); i++ )
+ float tolerance = getInput<float>("jointTargetTolerance");
+ for (int i=0; i<jointNames->getSize(); i++)
{
+ //ARMARX_VERBOSE << "creating condition (" << i << " of " << jointNames->getSize() << ")" << flush;
ParameterList inrangeCheck;
inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()-tolerance));
inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()+tolerance));
- Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
+ Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(), "jointangles", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
cond = cond && inrangeLiteral;
}
@@ -135,7 +138,7 @@ void MoveJointsVelocityControl::defineParameters()
{
context = getContext<RobotStatechartContext>();
- setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
addToInput("targetJointVelocities", VariantType::List(VariantType::Float), false);
@@ -170,7 +173,7 @@ void MoveJointsVelocityControl::onEnter()
for(int i=0; i<jointNames->getSize(); i++)
{
targetJointVelocities[jointNames->getVariant(i)->getString()] = targetVelocitiesValues->getVariant(i)->getFloat();
- ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetVelocitiesValues->getVariant(i)->getFloat() << std::endl;
+ ARMARX_VERBOSE << "setting joint velocity for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetVelocitiesValues->getVariant(i)->getFloat() << std::endl;
}
// TODO: Set Max Velocities
@@ -225,7 +228,7 @@ void MoveTCPPoseIK::defineParameters()
{
context = getContext<RobotStatechartContext>();
- setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPosition", VariantType::FramedPosition, false);
@@ -237,6 +240,7 @@ void MoveTCPPoseIK::defineParameters()
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
@@ -260,12 +264,13 @@ void MoveTCPPoseIK::onExit()
void MoveTCPPoseIK::defineSubstates()
{
- StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
StatePtr calculateJointAngleConfiguration = addState<CalculateJointAngleConfiguration>("CalculateJointAngleConfiguration");
+ StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
StatePtr validateJointAngleConfiguration = addState<ValidateJointAngleConfiguration>("ValidateJointAngleConfiguration");
StatePtr movingDone = addState<SuccessState>("movingDone") ;
StatePtr movingFailed = addState<FailureState>("movingFailed") ;
- setInitState(calculateJointAngleConfiguration, PM::createMapping()->mapFromParent("*","*"));
+
+ setInitState(calculateJointAngleConfiguration, createMapping()->mapFromParent("*","*")); // PM::createMapping()->mapFromParent("*","*") ??
addTransition<EvCalculationDone>(calculateJointAngleConfiguration, moveJoints, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvJointTargetReached>(moveJoints, validateJointAngleConfiguration, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvSuccess>(validateJointAngleConfiguration, movingDone, createMapping()->mapFromOutput("*","*"));
@@ -279,7 +284,7 @@ void MoveTCPTrajectory::defineParameters()
{
context = getContext<RobotStatechartContext>();
- setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
@@ -320,7 +325,7 @@ void MoveTCPTrajectory::onEnter()
void MoveTCPTrajectory::onExit()
{
removeTimeoutEvent(timeoutEvent);
- ChannelRefPtr r = getLocal<ChannelRefPtr>("trajectoryPointCounterID");
+ ChannelRefPtr r = getLocal<ChannelRef>("trajectoryPointCounterID");
getContext()->systemObserverPrx->removeCounter(r);
//removeCounterEvent(trajectoryPointCounterID);
}
@@ -461,12 +466,13 @@ void MoveTCPTrajectoryNextPoint::defineSubstates()
void CalculateJointAngleConfiguration::defineParameters()
{
- addToInput("KinematicChainName", VariantType::String, false);
+ addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPosition", VariantType::FramedPosition, false);
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
+ addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToOutput("jointNames", VariantType::String, true);
- addToOutput("targetJointValues", VariantType::Float, true);
+ addToOutput("jointNames", VariantType::List(VariantType::String), true);
+ addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
}
@@ -475,15 +481,17 @@ void CalculateJointAngleConfiguration::run()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
VirtualRobot::RobotPtr robotPtr(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
+
+ // TODO: with the following line, the IK doesn't find a solution, so this terrible hack must be used. Fix it!!!
+ //VirtualRobot::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
std::string robotModelFile;
- ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
+ //ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
+ ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
- //VirtualRobot::RobotConfigPtr configPtr(new VirtualRobot::RobotConfig(robotPtr, "blub"));
- //robotPtr->getRobotNodeSet("Root")->getJointValues(configPtr);
- //robot->setJointValues(robotPtr->getRobotNodeSet("Root"), robotPtr->getRobotNodeSet("Root")->getJointValues());
- VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"))));
+ VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))));
ikSolver->setVerbose(true);
+ ikSolver->setMaximumError(getInput<float>("targetPositionDistanceTolerance"));
VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
@@ -507,14 +515,18 @@ void CalculateJointAngleConfiguration::run()
SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
- VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("KinematicChainName") );
+ VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName") );
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
+ ARMARX_LOG << " joint: " << kinematicChain->getNode(i)->getName() << " value: " << kinematicChain->getNode(i)->getJointValue() << flush;
}
- setOutput("targetJointValues",targetJointValues);
- setOutput("jointNames",jointNames);
+ ARMARX_LOG << "number of joints to be set: " << jointNames.getSize() << flush;
+ ARMARX_LOG << "setting output: jointNames" << flush;
+ setOutput("jointNames", jointNames);
+ ARMARX_LOG << "setting output: targetJointValues" << flush;
+ setOutput("targetJointValues", targetJointValues);
sendEvent<EvCalculationDone>();
}
}
@@ -542,7 +554,7 @@ void ValidateJointAngleConfiguration::onEnter()
RobotStatechartContext* context = getContext<RobotStatechartContext>();
VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("ValidateTCPPoseTime")));
VirtualRobot::LinkedCoordinate actualPose(robot);
- actualPose.set(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"))->getTCP()->getName(), Vector3f(0,0,0));
+ actualPose.set(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))->getTCP()->getName(), Vector3f(0,0,0));
actualPose.changeFrame(robot->getRootNode());
Vector3f actualPosition = actualPose.getPosition();
Quaternionf actualOrientation(actualPose.getPose().block<3,3>(0,0));
@@ -575,14 +587,14 @@ void ValidateJointAngleConfiguration::onEnter()
void MotionControlTestState::defineParameters()
{
- setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
addToInput("jointMaxSpeed", VariantType::Float, true);
- addToInput("targetTolerance", VariantType::Float, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
@@ -607,7 +619,7 @@ void MotionControlTestState::defineSubstates()
void MotionControlTestStateIK::defineParameters()
{
- setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
+ //setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
//addToInput("targetTCPPosition", VariantType::FramedPosition, false);
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index 9df288bd925ad7a067336ce2d3034ef1531919d8..47c97d2e378b7312c84461b9826a71c6d014bb93 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -47,7 +47,7 @@ namespace MotionControl
- jointNames: the names of the joints to be moved
- targetJointVelocities: the desired joint velocities
- targetJointVelocityTolerance: tolerance defining how precisely the joint velocity has to be reached
- - timeoutInMs: a timout after which the attempt is aborted
+ - timeoutInMs: a timeout after which the attempt is aborted
Output parameters:
- jointVelocitiesDistancesToTargets: the difference between the desired and actual joint velocities
@@ -59,7 +59,7 @@ namespace MotionControl
- targetTCPOrientation: the target orientation
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
- - timeoutInMs: a timout after which the motion is aborted
+ - timeoutInMs: a timeout after which the motion is aborted
Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position
- TCPOrientationDistanceToTarget: the difference between desired and actual orientation of the TCP
@@ -72,7 +72,7 @@ namespace MotionControl
- targetTCPOrientations: the list of target orientations
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
- - timeoutInMs: a timout after which the motion is aborted
+ - timeoutInMs: a timeout after which the motion is aborted
Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position
- TCPOrientationDistanceToTarget: the difference between desired and actual orientation of the TCP
@@ -90,13 +90,13 @@ namespace MotionControl
* i.e. direct and inverse kinematics for arbitrary kinematic chains and predefined
* parts like the arms.
*/
- class MotionControlHandler : public RemoteStateOfferer<RobotStatechartContext>
+ class MotionControlOfferer : public RemoteStateOfferer<RobotStatechartContext>
{
public:
- MotionControlHandler();
+ MotionControlOfferer();
void onInitRemoteStateOfferer();
void onConnectRemoteStateOfferer();
- std::string getHandlerName() const { return "MotionControl"; }
+ std::string getStateOffererName() const { return "MotionControl"; }
};
@@ -129,7 +129,7 @@ namespace MotionControl
* \param jointNames the names of the joints to be moved
* \param targetJointValues the desired joint values
* \param targetTolerance tolerance defining how precisely the joint position has to be reached
- * \param timeoutInMs a timout after which the attempt to move is aborted
+ * \param timeoutInMs a timeout after which the attempt to move is aborted
*/
struct MoveJoints : StateTemplate<MoveJoints>
{
@@ -145,11 +145,11 @@ namespace MotionControl
/**
* \ingroup MotionControl
- * Move the joints of a kinematic chain to the desired values using velocity control.
+ * Set the velocities of the joints of a kinematic chain.
* \param jointNames the names of the joints to be moved
* \param targetJointVelocities the desired joint velocities
* \param targetJointVelocityTolerance tolerance defining how precisely the joint velocity has to be reached
- * \param timeoutInMs a timout after which the attempt is aborted
+ * \param timeoutInMs a timeout after which the attempt is aborted
*/
struct MoveJointsVelocityControl : StateTemplate<MoveJointsVelocityControl>
{
@@ -170,7 +170,7 @@ namespace MotionControl
* \param targetTCPOrientation the target orientation
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
* \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
- * \param timeoutInMs a timout after which the motion is aborted
+ * \param timeoutInMs a timeout after which the motion is aborted
*/
struct MoveTCPPoseIK : StateTemplate<MoveTCPPoseIK>
{
@@ -195,7 +195,7 @@ namespace MotionControl
* \param targetTCPOrientations the list of target orientations
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
* \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
- * \param timeoutInMs a timout after which the motion is aborted
+ * \param timeoutInMs a timeout after which the motion is aborted
*/
struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
{
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..50c02b62cac78eb3aeb48e831c13cda9125baf48
--- /dev/null
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -0,0 +1,39 @@
+armarx_set_target("RobotAPI Units Library: RobotAPIUnits")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (ARMARX_BUILD)
+ include_directories(${Eigen3_INCLUDE_DIR})
+ include_directories(${VirtualRobot_INCLUDE_DIRS})
+
+ set(LIB_NAME RobotAPIUnits)
+ set(LIB_VERSION 0.1.0)
+ set(LIB_SOVERSION 0)
+
+ set(LIBS RobotAPIInterfaces RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${VirtualRobot_LIBRARIES})
+
+ set(LIB_HEADERS
+ ForceTorqueObserver.h
+ TCPControlUnit.h
+ TCPControlUnitObserver.h
+
+ )
+ set(LIB_FILES
+ ForceTorqueObserver.cpp
+ TCPControlUnit.cpp
+ TCPControlUnitObserver.cpp
+ )
+
+
+ add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
+
+ library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
+ target_link_ice(${LIB_NAME})
+ target_link_libraries(${LIB_NAME} ${LIBS})
+endif()
+
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd43ebb29a7ad5e331d6aed852f6e672f196bbbc
--- /dev/null
+++ b/source/RobotAPI/units/ForceTorqueObserver.cpp
@@ -0,0 +1,132 @@
+#include "ForceTorqueObserver.h"
+
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+
+#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+
+#define RAWFORCE "_rawforcevectors"
+#define OFFSETFORCE "_forceswithoffsetvectors"
+#define FILTEREDFORCE "_filteredforcesvectors"
+#define RAWTORQUE "_rawtorquevectors"
+#define OFFSETTORQUE "_torqueswithoffsetvectors"
+#define FORCETORQUEVECTORS "_forceTorqueVectors"
+
+
+using namespace armarx;
+
+ForceTorqueObserver::ForceTorqueObserver()
+{
+}
+
+void ForceTorqueObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+// offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
+ offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
+// offerConditionCheck("magnitudesmaller", new ConditionCheckSmaller());
+
+ // register all channels
+// offerChannel(FORCETORQUEVECTORS,"Force and Torque vectors on specific parts of the robot.");
+
+// offerChannel(RAWFORCE,"Forces on specific parts of the robot.");
+// offerChannel(OFFSETFORCE,"Force Torques of specific parts of the robot with an offset.");
+// offerChannel(FILTEREDFORCE,"Gaussian filtered force torques of specific parts of the robot.");
+
+// offerChannel(RAWTORQUE,"Torques on specific parts of the robot.");
+
+}
+
+void ForceTorqueObserver::onConnectObserver()
+{
+
+
+}
+
+//void ForceTorqueObserver::reportForceRaw(const FramedVector3Map &newForces, const Ice::Current &)
+//{
+
+//}
+
+//void ForceTorqueObserver::reportForceWithOffset(const FramedVector3Map &newForces, const Ice::Current &)
+//{
+// FramedVector3Map::const_iterator it = newForces.begin();
+// for(; it != newForces.end(); it++)
+// {
+
+// FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+// if(!existsDataField(OFFSETFORCE, it->first))
+// offerDataFieldWithDefault(OFFSETFORCE, it->first, Variant(it->second), "3D vector for Force Torques of " + it->first);
+// else
+// setDataField(OFFSETFORCE, it->first, Variant(it->second));
+
+
+// if(!existsChannel(it->first))
+// {
+// offerChannel(it->first,"Force on " + it->first);
+// offerDataFieldWithDefault(it->first,"force_x", Variant(vec->x), "Force on X axis");
+// offerDataFieldWithDefault(it->first,"force_y", Variant(vec->y), "Force on Y axis");
+// offerDataFieldWithDefault(it->first,"force_z", Variant(vec->z), "Force on Z axis");
+// }
+// else
+// {
+// setDataField(it->first,"force_x", Variant(vec->x));
+// setDataField(it->first,"force_y", Variant(vec->y));
+// setDataField(it->first,"force_z", Variant(vec->z));
+// }
+
+// }
+//}
+
+
+
+void ForceTorqueObserver::reportSensorValues(const std::string &type, const FramedVector3Map &newValues, const Ice::Current &)
+{
+ ScopedLock lock(dataMutex);
+ FramedVector3Map::const_iterator it = newValues.begin();
+ if(!existsChannel(type))
+ offerChannel(type, "Force and Torque vectors on specific parts of the robot.");
+ for(; it != newValues.end(); it++)
+ {
+
+ FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+ std::string identifier = it->first + "_" + type + "_in_frame_" + vec->frame;
+ if(!existsDataField(type, identifier))
+ offerDataFieldWithDefault(type, identifier, Variant(it->second), "3D vector for Force Torques of " + it->first);
+ else
+ setDataField(type, identifier, Variant(it->second));
+
+ if(!existsChannel(identifier))
+ {
+ offerChannel(identifier,type + " on " + it->first);
+ offerDataFieldWithDefault(identifier,type + "_x", Variant(vec->x), type + " on X axis");
+ offerDataFieldWithDefault(identifier,type + "_y", Variant(vec->y), type + " on Y axis");
+ offerDataFieldWithDefault(identifier,type + "_z", Variant(vec->z), type + " on Z axis");
+ offerDataFieldWithDefault(identifier,type + "_frame", Variant(vec->frame), "Frame of " + type);
+ }
+ else
+ {
+ setDataField(identifier,type + "_x", Variant(vec->x));
+ setDataField(identifier,type + "_y", Variant(vec->y));
+ setDataField(identifier,type + "_z", Variant(vec->z));
+ setDataField(identifier,type + "_frame", Variant(vec->frame));
+ }
+
+ }
+}
+
+PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/ForceTorqueObserver.h b/source/RobotAPI/units/ForceTorqueObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b9c34dc2276e7f88c74577ed65fbbaebd62e450
--- /dev/null
+++ b/source/RobotAPI/units/ForceTorqueObserver.h
@@ -0,0 +1,48 @@
+#ifndef _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
+#define _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
+
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <Core/observers/Observer.h>
+
+namespace armarx
+{
+
+ class ForceTorqueObserverPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ ForceTorqueObserverPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("ForceTorqueTopicName","Name of the ForceTorqueUnit Topic");
+ }
+ };
+
+ class ForceTorqueObserver :
+ virtual public Observer,
+ virtual public ForceTorqueUnitObserverInterface
+ {
+ public:
+ ForceTorqueObserver();
+
+ // framework hooks
+ virtual std::string getDefaultName() const { return "ForceTorqueUnitObserver"; }
+ void onInitObserver();
+ void onConnectObserver();
+
+ void reportSensorValues(const std::string & type, const ::armarx::FramedVector3Map& newForces, const ::Ice::Current& = ::Ice::Current());
+// void reportForceWithOffset(const ::armarx::FramedVector3Map& newForces, const ::Ice::Current& = ::Ice::Current());
+// void reportTorqueRaw(const ::armarx::FramedVector3Map& newTorques, const ::Ice::Current& = ::Ice::Current());
+// void reportTorqueWithOffset(const ::armarx::FramedVector3Map& newTorques, const ::Ice::Current& = ::Ice::Current());
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+ private:
+ armarx::Mutex dataMutex;
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f0123486f1bf598a746cbcd66397d93828eee71
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -0,0 +1,485 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "TCPControlUnit.h"
+#include <Core/robotstate/remote/LinkedPose.h>
+
+#include <boost/unordered_map.hpp>
+
+#include <Core/core/exceptions/local/ExpressionException.h>
+
+#include <Core/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+using namespace VirtualRobot;
+
+
+
+namespace armarx
+{
+
+ TCPControlUnit::TCPControlUnit() :
+ maxJointVelocity(30.f/180*3.141),
+ cycleTime(20)
+ {
+
+ }
+
+ void TCPControlUnit::onInitComponent()
+ {
+ usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
+ usingProxy("RobotStateComponent");
+ offeringTopic("TCPControlUnitState");
+ }
+
+
+ void TCPControlUnit::onConnectComponent()
+ {
+ ScopedLock lock(dataMutex);
+ if(getProperty<float>("MaxJointVelocity").isSet())
+ maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+
+
+
+ // retrieve Jacobian pseudo inverse for TCP and chain
+ remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
+// localRobot.reset(new RemoteRobot(remoteRobotPrx));
+// ARMARX_INFO << "hi limit : " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
+// localRobot = localRobot->clone("localRobot");
+// ARMARX_INFO << "hi limit after: " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
+
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
+ localRobot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);//localRobot->clone("LocalRobot");
+ localRobot->setUpdateVisualization(false);
+ localRobot->setThreadsafe(false);
+
+ // dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
+
+ FramedVector3Ptr tcpVel = new FramedVector3();
+ tcpVel->x = -30;
+ tcpVel->frame = "Root";
+ FramedVector3Ptr tcpOriVel = new FramedVector3();
+ tcpOriVel->frame = "Root";
+
+ // setTCPVelocity("TorsoLeftArm", tcpVel, NULL);
+ // setTCPVelocity("RightArm", tcpVel, NULL);
+ // setTCPVelocity("RightLeg", tcpVel, NULL);
+ // setTCPVelocity("LeftLeg", tcpVel, NULL);
+ // request();
+
+ listener = getTopic<TCPControlUnitListenerPrx>("TCPControlUnitState");
+ topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
+ topicTask->start();
+ }
+
+ void TCPControlUnit::onDisconnectComponent()
+ {
+ if(topicTask)
+ topicTask->stop();
+ release();
+ }
+
+ void TCPControlUnit::onExitComponent()
+ {
+ }
+
+
+
+ void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current & c)
+ {
+ ScopedLock lock(taskMutex);
+ cycleTime = milliseconds;
+ if(execTask)
+ execTask->changeInterval(cycleTime);
+ }
+
+ void TCPControlUnit::setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
+ {
+ ScopedLock lock(dataMutex);
+ if(translationVelocity)
+ ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << "\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
+ if(orientationVelocityRPY)
+ ARMARX_VERBOSE << "Orientation Velo: " << FramedVector3Ptr::dynamicCast(orientationVelocityRPY)->toEigen();
+ TCPVelocityData data;
+ data.nodeSetName = nodeSetName;
+ data.translationVelocity = FramedVector3Ptr::dynamicCast(translationVelocity);
+ data.orientationVelocity = FramedVector3Ptr::dynamicCast(orientationVelocityRPY);
+ if(tcpName.empty())
+ data.tcpName = localRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
+ else
+ data.tcpName = tcpName;
+ tcpVelocitiesMap[data.tcpName] = data;
+// if(translationVelocity)
+// targetTranslationVelocities[nodeSetName] = translationVelocity;
+// if(orientationVelocityRPY)
+// targetOrientationVelocitiesRPY[nodeSetName] = orientationVelocityRPY;
+
+ }
+
+
+ void TCPControlUnit::init(const Ice::Current & c)
+ {
+ }
+
+ void TCPControlUnit::start(const Ice::Current & c)
+ {
+ }
+
+ void TCPControlUnit::stop(const Ice::Current & c)
+ {
+ }
+
+ UnitExecutionState TCPControlUnit::getExecutionState(const Ice::Current & c)
+ {
+ switch (getState())
+ {
+ case eManagedIceObjectStarted:
+ return eUnitStarted;
+ case eManagedIceObjectInitialized:
+ case eManagedIceObjectStarting:
+ return eUnitInitialized;
+ case eManagedIceObjectExiting:
+ case eManagedIceObjectExited:
+ return eUnitStopped;
+ default:
+ return eUnitConstructed;
+ }
+ }
+
+ void TCPControlUnit::request(const Ice::Current & c)
+ {
+ ScopedLock lock(taskMutex);
+ int cycleTime;
+ {
+ cycleTime = this->cycleTime;
+ }
+ ARMARX_IMPORTANT << "Requesting TCPControlUnit";
+ if(execTask)
+ execTask->stop();
+ execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
+ execTask->start();
+ ARMARX_IMPORTANT << "Requested TCPControlUnit";
+ }
+
+ void TCPControlUnit::release(const Ice::Current & c)
+ {
+ ScopedLock lock(taskMutex);
+ ARMARX_IMPORTANT << "Releasing TCPControlUnit";
+ if(execTask)
+ execTask->stop();
+ kinematicUnitPrx->stop();
+ ARMARX_IMPORTANT << "Released TCPControlUnit";
+
+ }
+
+ void TCPControlUnit::periodicExec()
+ {
+ ARMARX_INFO << deactivateSpam(4) << "periodicExec for " << tcpVelocitiesMap.size();
+// IceUtil::Time startTime = IceUtil::Time::now();
+
+ ScopedLock lock(dataMutex);
+
+ NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
+// NameValueMap::iterator it = robotConfigMap.begin();
+ std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
+
+ localRobot->setJointValues(jointValues);
+
+// dIKMap.clear();
+ TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
+ for(; it != tcpVelocitiesMap.end(); it++)
+ {
+ const TCPVelocityData& data = it->second;
+ RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
+ std::string refFrame = nodeSet->getTCP()->getName();
+ DIKMap::iterator itDIK = dIKMap.find(data.nodeSetName);
+ if(itDIK == dIKMap.end())
+ dIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode()/*, VirtualRobot::JacobiProvider::eTranspose*/));
+ boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName])->clearGoals();
+ }
+
+ using namespace Eigen;
+
+ it = tcpVelocitiesMap.begin();
+ for(; it != tcpVelocitiesMap.end(); it++)
+ {
+
+ TCPVelocityData& data = it->second;
+ RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
+ std::string refFrame = localRobot->getRootNode()->getName();
+
+ IKSolver::CartesianSelection mode;
+ if(data.translationVelocity && data.orientationVelocity)
+ {
+ mode = IKSolver::All;
+// ARMARX_INFO << deactivateSpam(4) << "FullMode";
+ }
+ else if(data.translationVelocity && !data.orientationVelocity)
+ mode = IKSolver::Position;
+ else if(!data.translationVelocity && data.orientationVelocity)
+ mode = IKSolver::Orientation;
+ else
+ {
+// ARMARX_VERBOSE << deactivateSpam(2) << "No mode feasible for " << data.nodeSetName << " - skipping";
+ continue;
+ }
+
+ RobotNodePtr tcpNode = localRobot->getRobotNode(data.tcpName);
+ Eigen::Matrix4f m;
+ m.setIdentity();
+
+ if(data.orientationVelocity)
+ {
+ data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
+// ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->toEigen();
+ Eigen::Matrix3f rot;
+ rot = Eigen::AngleAxisf(data.orientationVelocity->z, Eigen::Vector3f::UnitZ())
+ * Eigen::AngleAxisf(data.orientationVelocity->y, Eigen::Vector3f::UnitY())
+ * Eigen::AngleAxisf(data.orientationVelocity->x, Eigen::Vector3f::UnitX());
+ m.block(0,0,3,3) = rot * tcpNode->getGlobalPose().block(0,0,3,3);
+
+ }
+
+// ARMARX_VERBOSE << deactivateSpam(1) << "Delta Mat: \n" << m;
+
+
+// m = m * tcpNode->getGlobalPose();
+
+ if(data.translationVelocity)
+ {
+ data.translationVelocity = FramedVector3::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
+// ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
+ m.block(0,3,3,1) = tcpNode->getGlobalPose().block(0,3,3,1) + data.translationVelocity->toEigen();
+ }
+
+
+ DifferentialIKPtr dIK = dIKMap[data.nodeSetName];
+ if(!dIK)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "DiffIK is NULL for robot node set: " << data.nodeSetName;
+ continue;
+ }
+// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
+// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
+ dIK->setGoal(m, tcpNode,mode);
+
+// ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
+ }
+
+
+ NameValueMap targetVelocities;
+ NameControlModeMap controlModes;
+ DIKMap::iterator itDIK = dIKMap.begin();
+ for(; itDIK != dIKMap.end(); itDIK++)
+ {
+ RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
+ EDifferentialIKPtr dIK = boost::shared_dynamic_cast<EDifferentialIK>(itDIK->second);
+ Eigen::VectorXf jointDelta = dIK->computeStep(1.0);
+
+ MatrixXf fullJac = dIK->calcFullJacobian();
+ MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
+ Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet,fullJac, fullJacInv);
+ jointDelta += jointLimitCompensation;
+
+ jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
+
+ Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
+ // ARMARX_INFO << "Current TCP Position: \n" << currentTCPPosition;
+ ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
+ lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
+
+ // build name value map
+
+ const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
+ std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
+ int i = 0;
+ while (iter != nodes.end() && i < jointDelta.rows())
+ {
+ if(targetVelocities.find((*iter)->getName()) != targetVelocities.end())
+ ARMARX_WARNING << deactivateSpam(2) << (*iter)->getName() << " is set from two joint delta calculations - overwriting first value";
+ targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
+
+ controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
+ i++;
+ iter++;
+ };
+ }
+
+
+
+
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointVelocities(targetVelocities);
+
+
+
+ }
+
+ void TCPControlUnit::periodicReport()
+ {
+ ScopedLock lock(dataMutex);
+
+ NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
+// NameValueMap::iterator it = robotConfigMap.begin();
+ std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
+
+ localRobot->setJointValues(jointValues);
+ std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
+ FramedPoseBaseMap tcpPoses;
+ std::string rootFrame = localRobot->getRootNode()->getName();
+ for(unsigned int i=0; i < nodeSets.size(); i++)
+ {
+ RobotNodeSetPtr nodeSet = nodeSets.at(i);
+ tcpPoses[nodeSet->getTCP()->getName()]= new FramedPose(nodeSet->getTCP()->getGlobalPose(),rootFrame);
+ }
+ listener->reportTCPPose(tcpPoses);
+ }
+
+
+ Eigen::VectorXf TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues)
+ {
+
+ std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
+ Eigen::VectorXf actualJointValues(nodes.size());
+ if(desiredJointValues.rows() == 0)
+ {
+
+ desiredJointValues.resize(nodes.size());
+
+ for(unsigned int i=0; i < nodes.size(); i++)
+ {
+ VirtualRobot::RobotNodePtr node = nodes.at(i);
+ desiredJointValues(i) = (node->getJointLimitHigh() - node->getJointLimitLow()) * 0.5f + node->getJointLimitLow();
+ }
+
+
+ }
+// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "desiredJointValues: " << desiredJointValues;
+ Eigen::VectorXf jointCompensationDeltas(desiredJointValues.rows());
+ for(unsigned int i = 0; i < desiredJointValues.rows(); i++)
+ {
+ VirtualRobot::RobotNodePtr node = nodes.at(i);
+ actualJointValues(i) = node->getJointValue();
+ jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i) ) / (node->getJointLimitHigh() - node->getJointLimitLow());
+ jointCompensationDeltas(i) = -pow(jointCompensationDeltas(i), 3)*pow(nodes.size()-i,2);
+ }
+// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "actualJointValues: " << actualJointValues;
+
+ return CalcNullspaceJointDeltas(jointCompensationDeltas,jacobian, jacobianInverse);
+ }
+
+ Eigen::VectorXf TCPControlUnit::CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse)
+ {
+ Eigen::MatrixXf I(jacobianInverse.rows(),jacobian.cols());
+ I.setIdentity();
+
+ Eigen::MatrixXf nullspaceProjection = I - jacobianInverse * jacobian;
+
+ Eigen::VectorXf delta = nullspaceProjection * desiredJointDeltas;
+ return delta;
+ }
+
+ Eigen::VectorXf TCPControlUnit::applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity)
+ {
+ float currentMaxJointVel = std::numeric_limits<float>::min();
+ unsigned int rows = jointVelocity.rows();
+ for(unsigned int i=0; i < rows; i++)
+ {
+ currentMaxJointVel = std::max(jointVelocity(i), currentMaxJointVel);
+ }
+
+ if(currentMaxJointVel > maxJointVelocity)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
+ return jointVelocity * (maxJointVelocity/currentMaxJointVel);
+ }
+ else
+ return jointVelocity;
+
+ }
+
+
+ PropertyDefinitionsPtr TCPControlUnit::createPropertyDefinitions()
+ {
+
+ return PropertyDefinitionsPtr(new TCPControlUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+ EDifferentialIK::EDifferentialIK(RobotNodeSetPtr rns, RobotNodePtr coordSystem, JacobiProvider::InverseJacobiMethod invJacMethod) :
+ DifferentialIK(rns, coordSystem, invJacMethod)
+ {
+
+ }
+
+ Eigen::MatrixXf EDifferentialIK::calcFullJacobian()
+ {
+ if (nRows==0) this->setNRows();
+ size_t nDoF = nodes.size();
+
+ using namespace Eigen;
+ MatrixXf Jacobian(nRows,nDoF);
+
+ size_t index=0;
+ for (size_t i=0; i<tcp_set.size();i++)
+ {
+ RobotNodePtr tcp = tcp_set[i];
+ if (this->targets.find(tcp)!=this->targets.end())
+ {
+ IKSolver::CartesianSelection mode = this->modes[tcp];
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
+ Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
+ }
+ else
+ VR_ERROR << "Internal error?!" << endl; // Error
+
+
+ }
+ return Jacobian;
+
+ }
+
+ void EDifferentialIK::clearGoals()
+ {
+ targets.clear();
+ modes.clear();
+ tolerancePosition.clear();
+ toleranceRotation.clear();
+ parents.clear();
+ }
+
+ void EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
+ {
+ this->coordSystem = coordSystem;
+ }
+
+
+}
+
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..d66e04be6b015669238fc104ab18c48d27a1e5d3
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -0,0 +1,145 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_TCPCONTROLUNIT_H
+#define _ARMARX_TCPCONTROLUNIT_H
+
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+#include <Core/core/services/tasks/PeriodicTask.h>
+#include <Core/core/Component.h>
+
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <Core/robotstate/remote/RemoteRobot.h>
+
+namespace armarx {
+
+
+ class TCPControlUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ TCPControlUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
+ defineOptionalProperty<float>("MaxJointVelocity",30.f/180*3.141, "Maximal joint velocity in rad/sec");
+ defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
+
+ }
+ };
+
+ class TCPControlUnit :
+ virtual public Component,
+ virtual public TCPControlUnitInterface
+ {
+ public:
+ TCPControlUnit();
+
+
+ void onInitComponent();
+ void onConnectComponent();
+ void onDisconnectComponent();
+ void onExitComponent();
+ std::string getDefaultName() const{return "TCPControlUnit";}
+
+ // TCPControlUnitInterface interface
+
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current &c = Ice::Current());
+ void setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const::armarx::FramedVector3BasePtr &translationVelocity, const::armarx::FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
+
+ // UnitExecutionManagementInterface interface
+ void init(const Ice::Current &c = Ice::Current());
+ void start(const Ice::Current &c = Ice::Current());
+ void stop(const Ice::Current &c = Ice::Current());
+ UnitExecutionState getExecutionState(const Ice::Current &c = Ice::Current());
+
+ // UnitResourceManagementInterface interface
+ void request(const Ice::Current &c = Ice::Current());
+ void release(const Ice::Current &c = Ice::Current());
+
+ // PropertyUser interface
+ PropertyDefinitionsPtr createPropertyDefinitions();
+
+ static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf &desiredJointDeltas, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse);
+ static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
+ private:
+ void periodicExec();
+ void periodicReport();
+ Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf &jointVelocity, float maxJointVelocity);
+
+ float maxJointVelocity;
+ int cycleTime;
+ Eigen::Matrix4f lastTCPPose;
+
+ struct TCPVelocityData
+ {
+ FramedVector3Ptr translationVelocity;
+ FramedVector3Ptr orientationVelocity;
+ std::string nodeSetName;
+ std::string tcpName;
+ };
+
+ typedef std::map<std::string, TCPVelocityData> TCPVelocityDataMap;
+ TCPVelocityDataMap tcpVelocitiesMap;
+
+ typedef std::map<std::string, VirtualRobot::DifferentialIKPtr> DIKMap;
+ DIKMap dIKMap;
+// FramedVector3Map targetTranslationVelocities;
+// FramedVector3Map targetOrientationVelocitiesRPY;
+
+ PeriodicTask<TCPControlUnit>::pointer_type execTask;
+ PeriodicTask<TCPControlUnit>::pointer_type topicTask;
+
+ RobotStateComponentInterfacePrx robotStateComponentPrx;
+ KinematicUnitInterfacePrx kinematicUnitPrx;
+ SharedRobotInterfacePrx remoteRobotPrx;
+ VirtualRobot::RobotPtr localRobot;
+ TCPControlUnitListenerPrx listener;
+// VirtualRobot::RobotNodeSetPtr robotNodeSet;
+// VirtualRobot::DifferentialIKPtr dIK;
+
+ Mutex dataMutex;
+ Mutex taskMutex;
+
+ };
+
+ class EDifferentialIK : public VirtualRobot::DifferentialIK
+ {
+ public:
+ EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
+
+ VirtualRobot::RobotNodePtr getRefFrame(){return coordSystem;}
+ int getJacobianRows(){ return nRows;}
+
+ Eigen::MatrixXf calcFullJacobian();
+
+ void clearGoals();
+ void setRefFrame(VirtualRobot:: RobotNodePtr coordSystem);
+ };
+ typedef boost::shared_ptr<EDifferentialIK> EDifferentialIKPtr;
+
+}
+
+#endif
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.cpp b/source/RobotAPI/units/TCPControlUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..20ae2d59ecdfecab25d2a9681d052aa732ec8e75
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnitObserver.cpp
@@ -0,0 +1,117 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "TCPControlUnitObserver.h"
+
+//#include <Core/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h>
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+
+namespace armarx {
+
+ TCPControlUnitObserver::TCPControlUnitObserver()
+ {
+ }
+
+ void TCPControlUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("TCPControlUnitName").getValue() + "State");
+
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+ // offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
+ offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
+// offerConditionCheck("equalsPoseWithTol", new ConditionCheckEqualsPoseWithTolerance());
+
+
+ offerChannel("TCPPose", "TCP poses of the robot.");
+ offerChannel("TCPVelocities", "TCP velocities of the robot.");
+
+ }
+
+ void TCPControlUnitObserver::onConnectObserver()
+ {
+
+ }
+
+ PropertyDefinitionsPtr TCPControlUnitObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new TCPControlUnitObserverPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+ void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &)
+ {
+ ScopedLock lock(dataMutex);
+ FramedPoseBaseMap::const_iterator it = poseMap.begin();
+ for(; it != poseMap.end(); it++)
+ {
+
+ FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
+ const std::string &tcpName = it->first;
+ if(!existsDataField("TCPPose", tcpName))
+ offerDataFieldWithDefault("TCPPose", tcpName, Variant(it->second), "Pose of " + tcpName);
+ else
+ setDataField("TCPPose", tcpName, Variant(it->second));
+
+ if(!existsChannel(tcpName))
+ {
+ offerChannel(tcpName, "pose components of " + tcpName);
+ offerDataFieldWithDefault(tcpName,"x", Variant(vec->position->x), "X axis");
+ offerDataFieldWithDefault(tcpName,"y", Variant(vec->position->y), "Y axis");
+ offerDataFieldWithDefault(tcpName,"z", Variant(vec->position->z), "Z axis");
+ offerDataFieldWithDefault(tcpName,"qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(tcpName,"qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(tcpName,"qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(tcpName,"qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(tcpName,"frame", Variant(vec->frame), "Reference Frame");
+ }
+ else
+ {
+ setDataField(tcpName,"x", Variant(vec->position->x));
+ setDataField(tcpName,"y", Variant(vec->position->y));
+ setDataField(tcpName,"z", Variant(vec->position->z));
+ setDataField(tcpName,"qx", Variant(vec->orientation->qx));
+ setDataField(tcpName,"qy", Variant(vec->orientation->qy));
+ setDataField(tcpName,"qz", Variant(vec->orientation->qz));
+ setDataField(tcpName,"qw", Variant(vec->orientation->qw));
+ setDataField(tcpName,"frame", Variant(vec->frame));
+ }
+
+ }
+ }
+
+ void TCPControlUnitObserver::reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &)
+ {
+ }
+
+}
+
+
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.h b/source/RobotAPI/units/TCPControlUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..727a9a17ac9300ad0afc94865ee4b952a58aa24a
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnitObserver.h
@@ -0,0 +1,69 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+#ifndef _ARMARX_TCPCONTROLUNITOBSERVER_H
+#define _ARMARX_TCPCONTROLUNITOBSERVER_H
+
+#include <Core/observers/Observer.h>
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+
+namespace armarx {
+
+ class TCPControlUnitObserverPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ TCPControlUnitObserverPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("TCPControlUnitName","Name of the TCPControlUnit");
+ }
+ };
+
+ class TCPControlUnitObserver :
+ virtual public Observer,
+ virtual public TCPControlUnitObserverInterface
+ {
+ public:
+ TCPControlUnitObserver();
+
+ // framework hooks
+ virtual std::string getDefaultName() const { return "TCPControlUnitObserver"; }
+ void onInitObserver();
+ void onConnectObserver();
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ // TCPControlUnitListener interface
+ public:
+ void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &);
+ void reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &);
+
+ Mutex dataMutex;
+ };
+
+}
+
+#endif