From 51c9fecf9dfd4902d4f48a58676dfafa8199c280 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 25 Nov 2013 16:09:30 +0100
Subject: [PATCH] Added TCPControlUnit
---
interface/slice/units/TCPControlUnit.ice | 48 ++++
source/RobotAPI/applications/CMakeLists.txt | 1 +
.../applications/MotionControl/CMakeLists.txt | 28 +++
.../MotionControl/MotionControlApp.h | 41 ++++
.../TCPControlUnit/CMakeLists.txt | 28 +++
.../TCPControlUnit/TCPControlUnitApp.h | 41 ++++
source/RobotAPI/units/CMakeLists.txt | 2 +
source/RobotAPI/units/TCPControlUnit.cpp | 207 ++++++++++++++++++
source/RobotAPI/units/TCPControlUnit.h | 103 +++++++++
9 files changed, 499 insertions(+)
create mode 100644 interface/slice/units/TCPControlUnit.ice
create mode 100755 source/RobotAPI/applications/MotionControl/CMakeLists.txt
create mode 100755 source/RobotAPI/applications/MotionControl/MotionControlApp.h
create mode 100755 source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
create mode 100755 source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
create mode 100644 source/RobotAPI/units/TCPControlUnit.cpp
create mode 100644 source/RobotAPI/units/TCPControlUnit.h
diff --git a/interface/slice/units/TCPControlUnit.ice b/interface/slice/units/TCPControlUnit.ice
new file mode 100644
index 000000000..b35bb035f
--- /dev/null
+++ b/interface/slice/units/TCPControlUnit.ice
@@ -0,0 +1,48 @@
+/*
+ * 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
+{
+
+ interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ void setCycleTime(int milliseconds);
+ void setTCPVelocity(string tcpNodeName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+
+ };
+
+
+
+};
+
+#endif
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 1c8b369ef..578f1ace3 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/MotionControl/CMakeLists.txt b/source/RobotAPI/applications/MotionControl/CMakeLists.txt
new file mode 100755
index 000000000..ea23a4dc5
--- /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 000000000..62045985b
--- /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/TCPControlUnit/CMakeLists.txt b/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
new file mode 100755
index 000000000..c3b5fe509
--- /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 000000000..7aa35ef13
--- /dev/null
+++ b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.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/TCPControlUnit.h>
+
+namespace armarx
+{
+ class TCPControlUnitApp :
+ virtual public armarx::Application
+ {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<TCPControlUnit>(properties) );
+ }
+ };
+}
+
+
+
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
index e711efd93..c5a957fdd 100755
--- a/source/RobotAPI/units/CMakeLists.txt
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -19,9 +19,11 @@ if (ARMARX_BUILD)
set(LIB_FILES
ForceTorqueObserver.h
+ TCPControlUnit.h
)
set(LIB_HEADERS
ForceTorqueObserver.cpp
+ TCPControlUnit.cpp
)
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
new file mode 100644
index 000000000..88b63a2e2
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -0,0 +1,207 @@
+/**
+* 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>
+
+
+
+
+namespace armarx
+{
+
+ TCPControlUnit::TCPControlUnit()
+ {
+
+ }
+
+ void TCPControlUnit::onInitComponent()
+ {
+ usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
+ usingProxy("RobotStateComponent");
+ }
+
+
+ void TCPControlUnit::onConnectComponent()
+ {
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+
+
+ // retrieve Jacobian pseudo inverse for TCP and chain
+ remoteRobot.reset( new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
+
+
+ // dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
+ execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, 30, true);
+ execTask->start();
+
+ FramedVector3 tcpVel;
+ tcpVel.x = 10;
+ tcpVel.frame = "Root";
+ FramedVector3 tcpOriVel;
+ tcpOriVel.frame = "Root";
+// setTCPVelocity("LeftArm", tcpVel, tcpOriVel);
+ }
+
+ void TCPControlUnit::onDisconnectComponent()
+ {
+ }
+
+ void TCPControlUnit::onExitComponent()
+ {
+ }
+
+
+
+ void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current & c)
+ {
+ }
+
+ void TCPControlUnit::setTCPVelocity(const std::string &tcpNodeName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
+ {
+ targetTranslationVelocities[tcpNodeName] = translationVelocity;
+ targetOrientationVelocitiesRPY[tcpNodeName] = 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)
+ {
+ return eUnitStarted;
+ }
+
+ void TCPControlUnit::request(const Ice::Current & c)
+ {
+ }
+
+ void TCPControlUnit::release(const Ice::Current & c)
+ {
+ }
+
+ void TCPControlUnit::periodicExec()
+ {
+
+
+ FramedVector3Map::const_iterator itTrans = targetTranslationVelocities.begin();
+ for(; itTrans != targetTranslationVelocities.end(); itTrans++)
+ {
+
+
+
+ FramedVector3Ptr targetTranslationVelocity = FramedVector3Ptr::dynamicCast(itTrans->second);
+ FramedVector3Map::const_iterator itOri = targetOrientationVelocitiesRPY.find(itTrans->first);
+ if(itOri != targetOrientationVelocitiesRPY.end())
+ {
+ FramedVector3Ptr targetOrientationVelocitiesRPY = FramedVector3Ptr::dynamicCast(itOri->second);;
+ if(targetTranslationVelocity->getFrame() == ""
+ || targetOrientationVelocitiesRPY->getFrame() == "")
+ continue;
+
+
+ if(targetTranslationVelocity->toEigen().norm() == 0
+ && targetOrientationVelocitiesRPY->toEigen().norm() == 0)
+ continue;
+
+ VirtualRobot::RobotNodeSetPtr robotNodeSet = remoteRobot->getRobotNodeSet(itTrans->first);
+
+ std::string refFrame = targetTranslationVelocity->getFrame();
+
+ FramedVector3Ptr lVecTrans = FramedVector3::ChangeFrame(remoteRobot, *targetTranslationVelocity, refFrame);
+ FramedVector3Ptr lVecOri = FramedVector3::ChangeFrame(remoteRobot, *targetOrientationVelocitiesRPY, refFrame);
+
+ Eigen::VectorXf tcpDelta(6);
+ tcpDelta.head(3) = lVecTrans->toEigen();
+ tcpDelta.tail(3) = lVecOri->toEigen();
+ Eigen::VectorXf jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, refFrame);
+
+
+ // build name value map
+ NameValueMap targetVelocities;
+ NameControlModeMap controlModes;
+ 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.cols())
+ {
+ targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
+ controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
+ i++;
+ iter++;
+ };
+
+ // execute velocities
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointVelocities(targetVelocities);
+ }
+
+ }
+ }
+ Eigen::VectorXf TCPControlUnit::calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame)
+ {
+
+ // TODO: Turn into member variable!
+ VirtualRobot::DifferentialIKPtr dIK(new VirtualRobot::DifferentialIK(robotNodeSet, remoteRobot->getRobotNode(refFrame)));
+ VirtualRobot::RobotNodePtr tcpNode = robotNodeSet->getTCP();
+
+
+ Eigen::MatrixXf Ji = dIK->getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::All);
+
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcpDelta.cols() != 6, "TCPDelta Vector must have 6 items")
+
+ // calculat joint error
+ Eigen::VectorXf deltaJoint = Ji * tcpDelta;
+
+ return deltaJoint;
+ }
+
+
+
+
+ PropertyDefinitionsPtr TCPControlUnit::createPropertyDefinitions()
+ {
+
+ return PropertyDefinitionsPtr(new TCPControlUnitPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+
+}
+
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
new file mode 100644
index 000000000..a705af1a3
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -0,0 +1,103 @@
+/**
+* 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");
+ defineRequiredProperty<std::string>("RobotNodeSetName","Name of the KinematicUnit");
+
+
+ }
+ };
+
+ 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 &tcpNodeName, 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();
+
+ private:
+ void periodicExec();
+ Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame);
+
+ FramedVector3Map targetTranslationVelocities;
+ FramedVector3Map targetOrientationVelocitiesRPY;
+
+ PeriodicTask<TCPControlUnit>::pointer_type execTask;
+
+ RobotStateComponentInterfacePrx robotStateComponentPrx;
+ KinematicUnitInterfacePrx kinematicUnitPrx;
+ VirtualRobot::RobotPtr remoteRobot;
+// VirtualRobot::RobotNodeSetPtr robotNodeSet;
+// VirtualRobot::DifferentialIKPtr dIK;
+
+ };
+
+}
+
+#endif
--
GitLab