More ZeroForceControl updates

source/RobotAPI/core/RobotStatechartContext.h

@@ -31,6 +31,8 @@
 #include <Core/robotstate/remote/RemoteRobot.h>
 #include <Core/interface/units/KinematicUnitInterface.h>
 
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+
 #include <Core/units/KinematicUnitObserver.h>
 //#include <VirtualRobot/VirtualRobot.h>
 #include <IceUtil/Time.h>
@@ -82,7 +84,7 @@ namespace armarx
 		RobotStateComponentInterfacePrx robotStateComponent;
 		KinematicUnitInterfacePrx kinematicUnitPrx;
         KinematicUnitObserverInterfacePrx kinematicUnitObserverPrx;
-
+        TCPControlUnitInterfacePrx tcpControlPrx;
         //SystemObserverInterfacePrx	systemObserver; // already defined in StatechartContext
         VirtualRobot::RobotPtr remoteRobot;
     private:

source/RobotAPI/motioncontrol/CMakeLists.txt

@@ -15,7 +15,7 @@ if (ARMARX_BUILD)
     set(LIB_VERSION    0.1.0)
     set(LIB_SOVERSION  0)
 
-    set(LIBS RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+    set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
 
     set(LIB_FILES MotionControl.cpp
          ZeroForceControl.cpp)

source/RobotAPI/motioncontrol/ZeroForceControl.cpp

@@ -24,6 +24,9 @@
 #include "ZeroForceControl.h"
 
 #include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <Core/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
 
 namespace armarx {
 
@@ -49,6 +52,7 @@ namespace armarx {
 
     void ZeroForceControl::defineParameters()
     {
+        addToInput("robotNodeSetName",VariantType::String, false);
         addToInput("tcpName",VariantType::String, false);
         addToInput("sensitivity",VariantType::Float, false);
         addToInput("maxAcc",VariantType::Float, false);
@@ -61,14 +65,20 @@ namespace armarx {
 
     void ZeroForceControl::onEnter()
     {
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+        c->tcpControlPrx->request();
+        Eigen::Vector3f null(3);
+        null.setZero();
         setLocal("currentSensitivity", 0.0f);
-        setLocal("currentVelocity", 0.0f);
-        setLocal("currentAcc", 0.0f);
+        setLocal("currentVelocity", new FramedVector3(null, getInput<std::string>("tcpName")));
+        setLocal("currentAcc", new FramedVector3(null, getInput<std::string>("tcpName")));
         setLocal("timestamp", (float)IceUtil::Time::now().toMilliSecondsDouble());
     }
 
     void ZeroForceControl::onExit()
     {
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+        c->tcpControlPrx->release();
     }
 
 
@@ -83,7 +93,7 @@ namespace armarx {
 
     void ZeroForceControlForceToAcc::defineParameters()
     {
-        addToInput("tcpName",VariantType::String, false);
+        addToInput("robotNodeSetName",VariantType::String, false);
         addToInput("sensitivity",VariantType::Float, false);
         addToInput("maxAcc",VariantType::Float, false);
 
@@ -107,27 +117,33 @@ namespace armarx {
         installCondition<EvSensorUpdate>(update);
         IceUtil::Time duration = IceUtil::Time::now() - IceUtil::Time::milliSecondsDouble(getInput<float>("timestamp"));
 
-        std::string tcpName = getInput<std::string>("tcpName");
+        std::string robotNodeSetName = getInput<std::string>("robotNodeSetName");
 //        FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
         FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
         FramedVector3Ptr curAcc = getInput<FramedVector3>("currentAcc");
         FramedVector3Ptr curForce = getInput<FramedVector3>("currentForce");
-
+        ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
+        ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
+        float forceThreshold = 2.0f;
+        float maxSensitivity = 2.0f;
 
         Eigen::Vector3f newVel(3);
         Eigen::Vector3f newAcc(3);
-        if(curForce->toEigen().norm() > 3){
-            newAcc = 20*curForce->toEigen().normalized();
+        if(curForce->toEigen().norm() > forceThreshold){
+            newAcc = 2 *  curForce->toEigen() * 5;
         }
         else
         {
-            newAcc = -10*curForce->toEigen().normalized();
+            newAcc = -10*vel->toEigen().normalized();
         }
 
         newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble()*0.001;
 
 
         setOutput("currentAcc", Variant(new FramedVector3(newAcc, curAcc->frame)));
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+
+        c->tcpControlPrx->setTCPVelocity(robotNodeSetName, "", new FramedVector3(newVel, vel->frame), NULL);
     }
 
     void ZeroForceControlForceToAcc::onExit()

source/RobotAPI/units/ForceTorqueObserver.cpp

@@ -23,9 +23,17 @@ ForceTorqueObserver::ForceTorqueObserver()
 {
 }
 
+void ForceTorqueObserver::setTopicName(std::string topicName)
+{
+    this->topicName = topicName;
+}
+
 void ForceTorqueObserver::onInitObserver()
 {
-    usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+    if(topicName.empty())
+        usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+    else
+        usingTopic(topicName);
 
     // register all checks
     offerConditionCheck("updated", new ConditionCheckUpdated());

source/RobotAPI/units/ForceTorqueObserver.h

@@ -25,6 +25,8 @@ namespace armarx
     public:
         ForceTorqueObserver();
 
+        void setTopicName(std::string topicName);
+
         // framework hooks
         virtual std::string getDefaultName() const { return "ForceTorqueUnitObserver"; }
         void onInitObserver();
@@ -41,6 +43,7 @@ namespace armarx
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
     private:
         armarx::Mutex dataMutex;
+        std::string topicName;
 
     };
 }