Intermediate version of forcecontrol

source/RobotAPI/motioncontrol/CMakeLists.txt

@@ -17,8 +17,10 @@ if (ARMARX_BUILD)
 
     set(LIBS RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
 
-    set(LIB_FILES MotionControl.cpp)
-    set(LIB_HEADERS MotionControl.h)
+    set(LIB_FILES MotionControl.cpp
+         ZeroForceControl.cpp)
+    set(LIB_HEADERS MotionControl.h
+            ZeroForceControl.h)
 
     add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
 

source/RobotAPI/motioncontrol/MotionControl.cpp

@@ -12,6 +12,8 @@
 
 #include <Eigen/src/Geometry/Quaternion.h>
 
+#include "ZeroForceControl.h"
+
 using namespace armarx;
 using namespace armarx::MotionControl;
 
@@ -26,6 +28,7 @@ void MotionControlOfferer::onInitRemoteStateOfferer()
     //addState<MoveJoints>("MoveJoints");
     //addState<MotionControlTestState>("MotionControlTestState");
     addState<MotionControlTestStateIK>("MotionControlTestStateIK");
+    addState<ZeroForceControl>("ZeroForceControl");
 }
 
 

source/RobotAPI/motioncontrol/ZeroForceControl.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 "ZeroForceControl.h"
+
+#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+
+namespace armarx {
+
+    ZeroForceControl::ZeroForceControl()
+    {
+    }
+
+
+
+    void ZeroForceControl::defineSubstates()
+    {
+        StatePtr calc = addState<ZeroForceControlForceToAcc>("ZeroForceControlForceToAcc");
+
+        PMPtr mapping = PM::createMapping()
+                ->mapFromParent("*")
+                ->mapFromDataField("..", "currentForce")
+                ->mapFromDataField("..", "currentTorque")
+                ;
+
+        setInitState(calc, mapping);
+        addTransition<EvSensorUpdate>(calc,calc, mapping->mapFromOutput("*") );
+    }
+
+    void ZeroForceControl::defineParameters()
+    {
+        addToInput("tcpName",VariantType::String, false);
+        addToInput("sensitivity",VariantType::Float, false);
+        addToInput("maxAcc",VariantType::Float, false);
+
+        addToLocal("currentSensitivity",VariantType::Float, false);
+        addToLocal("currentVelocity",VariantType::Float, false);
+        addToLocal("currentAcc",VariantType::Float, false);
+        addToLocal("timestamp",VariantType::Float, false);
+    }
+
+    void ZeroForceControl::onEnter()
+    {
+        setLocal("currentSensitivity", 0.0f);
+        setLocal("currentVelocity", 0.0f);
+        setLocal("currentAcc", 0.0f);
+        setLocal("timestamp", (float)IceUtil::Time::now().toMilliSecondsDouble());
+    }
+
+    void ZeroForceControl::onExit()
+    {
+    }
+
+
+    ////////////////////////////////////////////////////////////////
+    ////////////////////////////////////////////////////////////////
+    ////////////////////////////////////////////////////////////////
+
+    void ZeroForceControlForceToAcc::defineParameters()
+    {
+        addToInput("tcpName",VariantType::String, false);
+        addToInput("sensitivity",VariantType::Float, false);
+        addToInput("maxAcc",VariantType::Float, false);
+
+
+        addToInput("currentSensitivity",VariantType::Float, false);
+        addToInput("currentVelocity",VariantType::FramedVector3, false);
+        addToInput("currentAcc",VariantType::FramedVector3, false);
+        addToInput("currentForce",VariantType::FramedVector3, false);
+        addToInput("currentTorque",VariantType::FramedVector3, false);
+        addToInput("timestamp",VariantType::Float, false);
+
+        addToOutput("currentSensitivity",VariantType::Float, false);
+        addToOutput("currentVelocity",VariantType::Float, false);
+        addToOutput("currentAcc",VariantType::Float, false);
+
+    }
+
+    void ZeroForceControlForceToAcc::onEnter()
+    {
+        Literal update("ForceTorqueUnit","updated", Literal::createParameterList());
+        installCondition<EvSensorUpdate>(update);
+        IceUtil::Time duration = IceUtil::Time::now() - IceUtil::Time::milliSecondsDouble(getInput<float>("timestamp"));
+
+        std::string tcpName = getInput<std::string>("tcpName");
+//        FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
+        FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
+        FramedVector3Ptr curAcc = getInput<FramedVector3>("currentAcc");
+        FramedVector3Ptr curForce = getInput<FramedVector3>("currentForce");
+
+
+        Eigen::Vector3f newVel(3);
+        Eigen::Vector3f newAcc(3);
+        if(curForce->toEigen().norm() > 3){
+            newAcc = 20*curForce->toEigen().normalized();
+        }
+        else
+        {
+            newAcc = -10*curForce->toEigen().normalized();
+        }
+
+        newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble()*0.001;
+
+
+        setOutput("currentAcc", Variant(new FramedVector3(newAcc, curAcc->frame)));
+    }
+
+    void ZeroForceControlForceToAcc::onExit()
+    {
+
+    }
+
+}

source/RobotAPI/motioncontrol/ZeroForceControl.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_ZEROFORCECONTROL_H
+#define _ARMARX_ZEROFORCECONTROL_H
+
+#include <Core/statechart/State.h>
+
+namespace armarx {
+
+    DEFINEEVENT(EvSensorUpdate)
+    class ZeroForceControl : public StateTemplate<ZeroForceControl>
+    {
+    public:
+        ZeroForceControl();
+
+        // StateBase interface
+    protected:
+        void defineSubstates();
+        void defineParameters();
+        void onEnter();
+        void onExit();
+    };
+
+
+
+    class ZeroForceControlForceToAcc : public StateTemplate<ZeroForceControlForceToAcc>
+    {
+    public:
+        ZeroForceControlForceToAcc();
+
+        // StateBase interface
+    protected:
+        void defineParameters();
+        void onEnter();
+        void onExit();
+    };
+
+}
+
+#endif