Added TCPControlUnit

interface/slice/units/TCPControlUnit.ice

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU 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

source/RobotAPI/applications/CMakeLists.txt

 
 add_subdirectory(ForceTorqueObserver)
 add_subdirectory(MotionControlTest)
+add_subdirectory(TCPControlUnit)
 

source/RobotAPI/applications/MotionControl/CMakeLists.txt

+
+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()

source/RobotAPI/applications/MotionControl/MotionControlApp.h

+/**
+ * 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) );
+        }
+    };
+}
+
+
+

source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt

+
+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()

source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h

+/**
+ * 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) );
+        }
+    };
+}
+
+
+

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
     )
 
 

source/RobotAPI/units/TCPControlUnit.cpp

+/**
+* 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()));
+    }
+
+
+}
+

source/RobotAPI/units/TCPControlUnit.h

+/**
+* 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