From 71c075630ae8e948ba2c25426c55dd7940d4f960 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <waechter@kit.edu>
Date: Fri, 6 Dec 2013 22:04:08 +0100
Subject: [PATCH] More ZeroForceControl updates
---
source/RobotAPI/core/RobotStatechartContext.h | 4 ++-
source/RobotAPI/motioncontrol/CMakeLists.txt | 2 +-
.../motioncontrol/ZeroForceControl.cpp | 32 ++++++++++++++-----
source/RobotAPI/units/ForceTorqueObserver.cpp | 10 +++++-
source/RobotAPI/units/ForceTorqueObserver.h | 3 ++
5 files changed, 40 insertions(+), 11 deletions(-)
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 1992440da..87497c103 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/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:
diff --git a/source/RobotAPI/motioncontrol/CMakeLists.txt b/source/RobotAPI/motioncontrol/CMakeLists.txt
index a5d3bff31..88550f6ae 100755
--- a/source/RobotAPI/motioncontrol/CMakeLists.txt
+++ b/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)
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
index 9188a476d..16f32e51b 100644
--- a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
+++ b/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()
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
index dd43ebb29..23a978f70 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.cpp
+++ b/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());
diff --git a/source/RobotAPI/units/ForceTorqueObserver.h b/source/RobotAPI/units/ForceTorqueObserver.h
index 8b9c34dc2..50c5ea1d7 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.h
+++ b/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;
};
}
--
GitLab