From 7b7c7ba562cafacbf753e2306698c87504df47a7 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 5 Dec 2013 15:53:32 +0100
Subject: [PATCH] Intermediate version of forcecontrol
---
source/RobotAPI/motioncontrol/CMakeLists.txt | 6 +-
.../RobotAPI/motioncontrol/MotionControl.cpp | 3 +
.../motioncontrol/ZeroForceControl.cpp | 133 ++++++++++++++++++
.../RobotAPI/motioncontrol/ZeroForceControl.h | 60 ++++++++
4 files changed, 200 insertions(+), 2 deletions(-)
create mode 100644 source/RobotAPI/motioncontrol/ZeroForceControl.cpp
create mode 100644 source/RobotAPI/motioncontrol/ZeroForceControl.h
diff --git a/source/RobotAPI/motioncontrol/CMakeLists.txt b/source/RobotAPI/motioncontrol/CMakeLists.txt
index 34e5cc6b7..a5d3bff31 100755
--- a/source/RobotAPI/motioncontrol/CMakeLists.txt
+++ b/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})
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index c52c05f1b..30feed820 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/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");
}
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
new file mode 100644
index 000000000..c320e9ede
--- /dev/null
+++ b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
@@ -0,0 +1,133 @@
+/**
+* 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()
+ {
+
+ }
+
+}
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.h b/source/RobotAPI/motioncontrol/ZeroForceControl.h
new file mode 100644
index 000000000..c135c4d8b
--- /dev/null
+++ b/source/RobotAPI/motioncontrol/ZeroForceControl.h
@@ -0,0 +1,60 @@
+/**
+* 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
--
GitLab