From 490299f15f789ee978940b0310d03f184ee4b4b7 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <waechter@kit.edu>
Date: Wed, 3 Jun 2015 18:21:33 +0200
Subject: [PATCH] renamed FramedVector3 to FramedDirection
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 4 +-
.../components/units/ForceTorqueObserver.cpp | 6 +--
.../components/units/ForceTorqueObserver.h | 4 +-
.../units/ForceTorqueUnitSimulation.cpp | 10 ++--
.../units/ForceTorqueUnitSimulation.h | 6 +--
.../components/units/TCPControlUnit.cpp | 20 ++++----
.../components/units/TCPControlUnit.h | 6 +--
.../units/TCPControlUnitObserver.cpp | 12 ++---
.../components/units/TCPControlUnitObserver.h | 2 +-
.../ArmarXTCPMover/TCPMover.cpp | 4 +-
.../interface/core/FramedPoseBase.ice | 4 +-
.../interface/core/LinkedPoseBase.ice | 2 +-
.../interface/units/ForceTorqueUnit.ice | 4 +-
.../interface/units/TCPControlUnit.ice | 4 +-
source/RobotAPI/libraries/core/FramedPose.cpp | 42 ++++++++--------
source/RobotAPI/libraries/core/FramedPose.h | 38 +++++++--------
source/RobotAPI/libraries/core/LinkedPose.cpp | 40 ++++++++--------
source/RobotAPI/libraries/core/LinkedPose.h | 32 ++++++-------
.../libraries/core/RobotAPIObjectFactories.h | 16 +++----
.../checks/ConditionCheckMagnitudeChecks.cpp | 48 +++++++++----------
.../core/observerfilters/OffsetFilter.h | 10 ++--
.../core/observerfilters/PoseMedianFilter.h | 14 +++---
.../core/remoterobot/RobotStateObserver.cpp | 20 ++++----
.../core/remoterobot/RobotStateObserver.h | 2 +-
.../motioncontrol/ZeroForceControl.cpp | 24 +++++-----
25 files changed, 187 insertions(+), 187 deletions(-)
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 34de6dfff..f135ca81b 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -6,11 +6,11 @@
<Variant baseType="::armarx::QuaternionBase" dataType="::armarx::Quaternion" humanName="Quaternion" />
<Variant baseType="::armarx::PoseBase" dataType="::armarx::Pose" humanName="Pose" />
<Variant baseType="::armarx::FramedPoseBase" dataType="::armarx::FramedPose" humanName="FramedPose" />
- <Variant baseType="::armarx::FramedVector3Base" dataType="::armarx::FramedVector3" humanName="FramedVector3" />
+ <Variant baseType="::armarx::FramedDirectionBase" dataType="::armarx::FramedDirection" humanName="FramedDirection" />
<Variant baseType="::armarx::FramedPositionBase" dataType="::armarx::FramedPosition" humanName="FramedPosition" />
<Variant baseType="::armarx::FramedOrientationBase" dataType="::armarx::FramedOrientation" humanName="FramedOrientation" />
<Variant baseType="::armarx::LinkedPoseBase" dataType="::armarx::LinkedPose" humanName="LinkedPose" />
- <Variant baseType="::armarx::LinkedVector3Base" dataType="::armarx::LinkedVector3" humanName="LinkedVector3" />
+ <Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" />
<Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
humanName="Kinematic Unit"
typeName="KinematicUnitInterfacePrx"
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index b7608ee9d..dcf5ecabd 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -65,9 +65,9 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
}
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &type, FramedVector3BasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
{
- FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(value);
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
if(!existsChannel(id->channelName))
{
@@ -96,7 +96,7 @@ void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &ty
offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
}
-void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNodeName, const FramedVector3BasePtr &forces, const FramedVector3BasePtr &torques, const Ice::Current &)
+void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNodeName, const FramedDirectionBasePtr &forces, const FramedDirectionBasePtr &torques, const Ice::Current &)
{
ScopedLock lock(dataMutex);
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 6cf636ad4..0272f731e 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -39,7 +39,7 @@ namespace armarx
void onInitObserver();
void onConnectObserver();
- virtual void reportSensorValues(const std::string &sensorNodeName, const FramedVector3BasePtr &forces, const FramedVector3BasePtr &torques, const Ice::Current &);
+ virtual void reportSensorValues(const std::string &sensorNodeName, const FramedDirectionBasePtr &forces, const FramedDirectionBasePtr &torques, const Ice::Current &);
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -56,7 +56,7 @@ namespace armarx
std::string topicName;
- void offerValue(std::string nodeName, const std::string &type, FramedVector3BasePtr value, const DataFieldIdentifierPtr& id);
+ void offerValue(std::string nodeName, const std::string &type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
// ForceTorqueUnitObserverInterface interface
public:
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index 9683c8372..42fb1c8ba 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -38,11 +38,11 @@ void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
//for(std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
//{
- // forces[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
- // torques[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
+ // forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s);
+ // torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s);
// }
- forces = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
- torques = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
+ forces = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
+ torques = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation");
@@ -68,7 +68,7 @@ void ForceTorqueUnitSimulation::simulationFunction()
//listenerPrx->reportSensorValues(torques);
}
-void ForceTorqueUnitSimulation::setOffset(const FramedVector3BasePtr &forceOffsets, const FramedVector3BasePtr &torqueOffsets, const Ice::Current &c)
+void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current &c)
{
// Ignore
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
index 5ef02b36e..59c738208 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
@@ -76,7 +76,7 @@ namespace armarx
virtual void onExitForceTorqueUnit();
void simulationFunction();
- virtual void setOffset(const FramedVector3BasePtr &forceOffsets, const FramedVector3BasePtr &torqueOffsets, const Ice::Current& c = ::Ice::Current());
+ virtual void setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current& c = ::Ice::Current());
virtual void setToNull(const Ice::Current& c = ::Ice::Current());
/**
@@ -85,8 +85,8 @@ namespace armarx
virtual PropertyDefinitionsPtr createPropertyDefinitions();
protected:
- armarx::FramedVector3Ptr forces;
- armarx::FramedVector3Ptr torques;
+ armarx::FramedDirectionPtr forces;
+ armarx::FramedDirectionPtr torques;
PeriodicTask<ForceTorqueUnitSimulation>::pointer_type simulationTask;
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index c974eed9e..c0c5cf9a1 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -159,20 +159,20 @@ namespace armarx
execTask->changeInterval(cycleTime);
}
- void TCPControlUnit::setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
+ void TCPControlUnit::setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const FramedDirectionBasePtr &translationVelocity, const FramedDirectionBasePtr &orientationVelocityRPY, const Ice::Current &c)
{
ScopedLock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
if(translationVelocity)
- ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
+ ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
if(orientationVelocityRPY)
- ARMARX_VERBOSE << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedVector3Ptr::dynamicCast(orientationVelocityRPY)->toEigen();
+ ARMARX_VERBOSE << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
TCPVelocityData data;
data.nodeSetName = nodeSetName;
- data.translationVelocity = FramedVector3Ptr::dynamicCast(translationVelocity);
- data.orientationVelocity = FramedVector3Ptr::dynamicCast(orientationVelocityRPY);
+ data.translationVelocity = FramedDirectionPtr::dynamicCast(translationVelocity);
+ data.orientationVelocity = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
if(tcpName.empty())
{
data.tcpName = jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
@@ -368,7 +368,7 @@ namespace armarx
if(data.translationVelocity)
{
- data.translationVelocity = FramedVector3::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
+ data.translationVelocity = FramedDirection::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
m.block(0,3,3,1) = tcpNode->getGlobalPose().block(0,3,3,1) + data.translationVelocity->toEigen()*cycleTime*0.001;
}
@@ -1146,8 +1146,8 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
if(!localVelReportRobot)
localVelReportRobot = localReportRobot->clone(localReportRobot->getName());
// ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
- FramedVector3Map tcpTranslationVelocities;
- FramedVector3Map tcpOrientationVelocities;
+ FramedDirectionMap tcpTranslationVelocities;
+ FramedDirectionMap tcpOrientationVelocities;
std::string rootFrame = localReportRobot->getRootNode()->getName();
NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
@@ -1179,7 +1179,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, localReportRobot->getName());
+ tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, localReportRobot->getName());
const Eigen::Matrix4f mat = currentPose * lastPose->toEigen().inverse();
// const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
@@ -1187,7 +1187,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
// Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
- tcpOrientationVelocities[tcpName] = new FramedVector3(rpy/tDelta, rootFrame, localReportRobot->getName());
+ tcpOrientationVelocities[tcpName] = new FramedDirection(rpy/tDelta, rootFrame, localReportRobot->getName());
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index eaafbd502..13506f640 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -105,7 +105,7 @@ namespace armarx {
*
* @see request(), release()
*/
- void setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const::armarx::FramedVector3BasePtr &translationVelocity, const::armarx::FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
+ void setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const::armarx::FramedDirectionBasePtr &translationVelocity, const::armarx::FramedDirectionBasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
// UnitExecutionManagementInterface interface
/**
@@ -167,8 +167,8 @@ namespace armarx {
struct TCPVelocityData
{
- FramedVector3Ptr translationVelocity;
- FramedVector3Ptr orientationVelocity;
+ FramedDirectionPtr translationVelocity;
+ FramedDirectionPtr orientationVelocity;
std::string nodeSetName;
std::string tcpName;
};
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
index 28a8fe782..9a9465a9a 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
@@ -115,18 +115,18 @@ namespace armarx {
}
}
- void TCPControlUnitObserver::reportTCPVelocities(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities, const Ice::Current &)
+ void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &)
{
ScopedLock lock(dataMutex);
- FramedVector3Map::const_iterator it = tcpTranslationVelocities.begin();
+ FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
for(; it != tcpTranslationVelocities.end(); it++)
{
- FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
- FramedVector3Ptr vecOri;
- FramedVector3Map::const_iterator itOri = tcpOrientationVelocities.find(it->first);
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
+ FramedDirectionPtr vecOri;
+ FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
if(itOri != tcpOrientationVelocities.end())
- vecOri = FramedVector3Ptr::dynamicCast(itOri->second);
+ vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
const std::string &tcpName = it->first;
ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index 44dc8db80..8efaf63e6 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -59,7 +59,7 @@ namespace armarx {
// TCPControlUnitListener interface
public:
void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &c = Ice::Current());
- void reportTCPVelocities(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities, const Ice::Current &c = Ice::Current());
+ void reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &c = Ice::Current());
Mutex dataMutex;
};
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
index fcad49ce8..44dcd92fd 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
@@ -303,11 +303,11 @@ void TCPMover::execMove()
vec << tcpData[selectedTCP][0], tcpData[selectedTCP][1], tcpData[selectedTCP][2];
vec *= ui.sbFactor->value();
const auto agentName = robotPrx->getName();
- FramedVector3Ptr tcpVel = new FramedVector3(vec, refFrame, agentName);
+ FramedDirectionPtr tcpVel = new FramedDirection(vec, refFrame, agentName);
Eigen::Vector3f vecOri;
vecOri << tcpData[selectedTCP].at(3)/180.f*3.145f, tcpData[selectedTCP].at(4)/180.f*3.145f, tcpData[selectedTCP].at(5)/180.f*3.145f;
vecOri *= ui.sbFactor->value();
- FramedVector3Ptr tcpOri = new FramedVector3(vecOri, refFrame, agentName);
+ FramedDirectionPtr tcpOri = new FramedDirection(vecOri, refFrame, agentName);
if(!ui.cbTranslation->isChecked() )
tcpVel = NULL;
diff --git a/source/RobotAPI/interface/core/FramedPoseBase.ice b/source/RobotAPI/interface/core/FramedPoseBase.ice
index b2b75d179..e618d59af 100644
--- a/source/RobotAPI/interface/core/FramedPoseBase.ice
+++ b/source/RobotAPI/interface/core/FramedPoseBase.ice
@@ -32,7 +32,7 @@ module armarx
{
["cpp:virtual"]
- class FramedVector3Base extends Vector3Base
+ class FramedDirectionBase extends Vector3Base
{
string frame;
string agent;
@@ -60,7 +60,7 @@ module armarx
string agent;
};
- dictionary<string, FramedVector3Base> FramedVector3Map;
+ dictionary<string, FramedDirectionBase> FramedDirectionMap;
dictionary<string, FramedPositionBase> FramedPositionMap;
["cpp:virtual"]
diff --git a/source/RobotAPI/interface/core/LinkedPoseBase.ice b/source/RobotAPI/interface/core/LinkedPoseBase.ice
index 156fcb33d..14f78c6d1 100644
--- a/source/RobotAPI/interface/core/LinkedPoseBase.ice
+++ b/source/RobotAPI/interface/core/LinkedPoseBase.ice
@@ -37,7 +37,7 @@ module armarx
};
["cpp:virtual"]
- class LinkedVector3Base extends FramedVector3Base
+ class LinkedDirectionBase extends FramedDirectionBase
{
SharedRobotInterface* referenceRobot;
void changeFrame(string newFrame);
diff --git a/source/RobotAPI/interface/units/ForceTorqueUnit.ice b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
index 7c11e9e46..52ef14f2d 100644
--- a/source/RobotAPI/interface/units/ForceTorqueUnit.ice
+++ b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
@@ -37,7 +37,7 @@ module armarx
{
interface ForceTorqueUnitInterface extends armarx::SensorActorUnitInterface
{
- void setOffset(FramedVector3Base forceOffsets, FramedVector3Base torqueOffsets);
+ void setOffset(FramedDirectionBase forceOffsets, FramedDirectionBase torqueOffsets);
void setToNull();
};
@@ -48,7 +48,7 @@ module armarx
\param forces Set to zero if not available.
\param torques Set to zero if not available.
*/
- void reportSensorValues(string sensorNodeName, FramedVector3Base forces, FramedVector3Base torques);
+ void reportSensorValues(string sensorNodeName, FramedDirectionBase forces, FramedDirectionBase torques);
};
interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
diff --git a/source/RobotAPI/interface/units/TCPControlUnit.ice b/source/RobotAPI/interface/units/TCPControlUnit.ice
index e470c8e2b..fafcd2735 100644
--- a/source/RobotAPI/interface/units/TCPControlUnit.ice
+++ b/source/RobotAPI/interface/units/TCPControlUnit.ice
@@ -39,7 +39,7 @@ module armarx
interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface, armarx::KinematicUnitListener
{
void setCycleTime(int milliseconds);
- void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+ void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedDirectionBase translationVelocity, FramedDirectionBase orientationVelocityRPY);
};
@@ -48,7 +48,7 @@ module armarx
interface TCPControlUnitListener
{
void reportTCPPose(FramedPoseBaseMap tcpPoses);
- void reportTCPVelocities(FramedVector3Map tcpTranslationVelocities, FramedVector3Map tcpOrienationVelocities);
+ void reportTCPVelocities(FramedDirectionMap tcpTranslationVelocities, FramedDirectionMap tcpOrienationVelocities);
};
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 9ec280596..1f972824f 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -19,19 +19,19 @@ namespace armarx
{
- FramedVector3::FramedVector3()
+ FramedDirection::FramedDirection()
{
}
- FramedVector3::FramedVector3(const FramedVector3 &source) :
+ FramedDirection::FramedDirection(const FramedDirection &source) :
IceUtil::Shared(source),
Vector3Base(source),
- FramedVector3Base(source),
+ FramedDirectionBase(source),
Vector3(source)
{
}
- FramedVector3::FramedVector3(const Eigen::Vector3f &vec, const string &frame, const string &agent) :
+ FramedDirection::FramedDirection(const Eigen::Vector3f &vec, const string &frame, const string &agent) :
Vector3(vec)
{
this->frame = frame;
@@ -39,21 +39,21 @@ namespace armarx
}
- FramedVector3::FramedVector3(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame) :
+ FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame) :
Vector3(x, y, z)
{
this->frame = frame;
}
- string FramedVector3::getFrame() const
+ string FramedDirection::getFrame() const
{
return frame;
}
- FramedVector3Ptr FramedVector3::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedVector3 &framedVec, const string &newFrame)
+ FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection &framedVec, const string &newFrame)
{
if(framedVec.getFrame() == newFrame)
- return FramedVector3Ptr::dynamicCast(framedVec.clone());
+ return FramedDirectionPtr::dynamicCast(framedVec.clone());
if(!robot->hasRobotNode(newFrame))
throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot->getName();
@@ -63,7 +63,7 @@ namespace armarx
Eigen::Vector3f newVec = rotToNewFrame.block(0,0,3,3).inverse() * vecOldFrame;
- FramedVector3Ptr result = new FramedVector3();
+ FramedDirectionPtr result = new FramedDirection();
result->x = newVec(0);
result->y = newVec(1);
result->z = newVec(2);
@@ -71,7 +71,7 @@ namespace armarx
return result;
}
- void FramedVector3::changeFrame(const VirtualRobot::RobotPtr robot, const string &newFrame)
+ void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string &newFrame)
{
if(getFrame() == newFrame)
return;
@@ -95,13 +95,13 @@ namespace armarx
frame = newFrame;
}
- void FramedVector3::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
+ void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedVector3::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
+ void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr &referenceRobot)
{
if(frame == GlobalFrame)
return;
@@ -114,27 +114,27 @@ namespace armarx
agent = "";
}
- FramedVector3Ptr FramedVector3::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
+ FramedDirectionPtr FramedDirection::toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedVector3Ptr FramedVector3::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
+ FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr &referenceRobot) const
{
- FramedVector3Ptr newPos = FramedVector3Ptr::dynamicCast(this->clone());
+ FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
newPos->changeToGlobal(referenceRobot);
return newPos;
}
- string FramedVector3::output(const Ice::Current &c) const
+ string FramedDirection::output(const Ice::Current &c) const
{
std::stringstream s;
s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
return s.str();
}
- Eigen::Matrix4f FramedVector3::__GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState)
+ Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState)
{
Eigen::Matrix4f toNewFrame;
if (oldFrame.compare(GlobalFrame) == 0)
@@ -148,7 +148,7 @@ namespace armarx
return toNewFrame;
}
- int FramedVector3::readFromXML(const string &xmlData, const Ice::Current &c)
+ int FramedDirection::readFromXML(const string &xmlData, const Ice::Current &c)
{
int result = Vector3::readFromXML(xmlData);
@@ -159,7 +159,7 @@ namespace armarx
return result;
}
- string FramedVector3::writeAsXML(const Ice::Current &c)
+ string FramedDirection::writeAsXML(const Ice::Current &c)
{
using namespace boost::property_tree;
ptree pt = Variant::GetPropertyTree(Vector3::writeAsXML());
@@ -177,7 +177,7 @@ namespace armarx
return stream.str();
}
- void FramedVector3::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ void FramedDirection::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::serialize(serializer);
@@ -186,7 +186,7 @@ namespace armarx
}
- void FramedVector3::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ void FramedDirection::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::deserialize(serializer);
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 128f0eed5..423a50c32 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -50,7 +50,7 @@ namespace armarx
{
// variant types
const VariantTypeId FramedPose = Variant::addTypeName("::armarx::FramedPoseBase");
- const VariantTypeId FramedVector3 = Variant::addTypeName("::armarx::FramedVector3Base");
+ const VariantTypeId FramedDirection = Variant::addTypeName("::armarx::FramedDirectionBase");
const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
}
@@ -61,10 +61,10 @@ namespace armarx
/**
- * @class FramedVector3
+ * @class FramedDirection
* @ingroup RobotAPI-FramedPose
* @ingroup Variants
- * @brief FramedVector3 is a 3 dimensional @b direction vector with a reference frame.
+ * @brief FramedDirection is a 3 dimensional @b direction vector with a reference frame.
* The reference frame can be used to change the coordinate system to which
* the vector relates. The difference to a FramedPosition is, that on frame
* changing only the orientation of the vector is changed. The length of the vector
@@ -73,27 +73,27 @@ namespace armarx
*
* @see Vector3, FramedPosition
*/
- class FramedVector3;
- typedef IceInternal::Handle<FramedVector3> FramedVector3Ptr;
+ class FramedDirection;
+ typedef IceInternal::Handle<FramedDirection> FramedDirectionPtr;
- class FramedVector3 :
- virtual public FramedVector3Base,
+ class FramedDirection :
+ virtual public FramedDirectionBase,
virtual public Vector3
{
public:
- FramedVector3();
- FramedVector3(const FramedVector3& source);
- FramedVector3(const Eigen::Vector3f & vec, const std::string &frame, const std::string& agent );
- FramedVector3(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame);
+ FramedDirection();
+ FramedDirection(const FramedDirection& source);
+ FramedDirection(const Eigen::Vector3f & vec, const std::string &frame, const std::string& agent );
+ FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string &frame);
std::string getFrame() const;
- static FramedVector3Ptr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedVector3& framedVec, const std::string &newFrame);
+ static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string &newFrame);
void changeFrame(const VirtualRobot::RobotPtr robot, const std::string &newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
- FramedVector3Ptr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
- FramedVector3Ptr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
+ FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
@@ -102,12 +102,12 @@ namespace armarx
}
VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
{
- return new FramedVector3(*this);
+ return new FramedDirection(*this);
}
std::string output(const Ice::Current& c = ::Ice::Current()) const;
VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
{
- return VariantType::FramedVector3;
+ return VariantType::FramedDirection;
}
bool validate(const Ice::Current& c = ::Ice::Current())
{
@@ -115,7 +115,7 @@ namespace armarx
}
/**
- * @brief Implementation of virtual function to read a FramedVector3 from an XML-file.
+ * @brief Implementation of virtual function to read a FramedDirection from an XML-file.
*Example xml-layout:
* @code
* <frame>leftHand</frame>
@@ -131,9 +131,9 @@ namespace armarx
int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const FramedVector3& rhs)
+ friend std::ostream &operator<<(std::ostream &stream, const FramedDirection& rhs)
{
- stream << "FramedVector3: " << std::endl << rhs.output() << std::endl;
+ stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
}
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 09c8e05d4..d7089a569 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -214,17 +214,17 @@ namespace armarx {
}
- LinkedVector3::LinkedVector3()
+ LinkedDirection::LinkedDirection()
{
}
- LinkedVector3::LinkedVector3(const LinkedVector3 &source) :
+ LinkedDirection::LinkedDirection(const LinkedDirection &source) :
IceUtil::Shared(source),
Vector3Base(source),
- FramedVector3Base(source),
- LinkedVector3Base(source),
+ FramedDirectionBase(source),
+ LinkedDirectionBase(source),
Vector3(source),
- FramedVector3(source)
+ FramedDirection(source)
{
referenceRobot = source.referenceRobot;
if(referenceRobot)
@@ -234,14 +234,14 @@ namespace armarx {
}
}
- LinkedVector3::LinkedVector3(const Eigen::Vector3f &v, const std::string &frame, const SharedRobotInterfacePrx &referenceRobot) :
- FramedVector3(v, frame, referenceRobot->getName())
+ LinkedDirection::LinkedDirection(const Eigen::Vector3f &v, const std::string &frame, const SharedRobotInterfacePrx &referenceRobot) :
+ FramedDirection(v, frame, referenceRobot->getName())
{
referenceRobot->ref();
this->referenceRobot = referenceRobot;
}
- LinkedVector3::~LinkedVector3()
+ LinkedDirection::~LinkedDirection()
{
try{
if(referenceRobot)
@@ -255,50 +255,50 @@ namespace armarx {
}
}
- void LinkedVector3::changeFrame(const std::string &newFrame, const Ice::Current &c)
+ void LinkedDirection::changeFrame(const std::string &newFrame, const Ice::Current &c)
{
if(newFrame == frame)
return;
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
- FramedVector3Ptr frVec = ChangeFrame(sharedRobot, *this, newFrame);
+ FramedDirectionPtr frVec = ChangeFrame(sharedRobot, *this, newFrame);
x = frVec->x;
y = frVec->y;
z = frVec->z;
frame = frVec->frame;
}
- int LinkedVector3::readFromXML(const std::string &xmlData, const Ice::Current &c)
+ int LinkedDirection::readFromXML(const std::string &xmlData, const Ice::Current &c)
{
- throw LocalException("LinkedVector3 cannot be serialized! Serialize FramedVector3");
+ throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
}
- std::string LinkedVector3::writeAsXML(const Ice::Current &c)
+ std::string LinkedDirection::writeAsXML(const Ice::Current &c)
{
- throw LocalException("LinkedVector3 cannot be deserialized! Serialize FramedVector3");
+ throw LocalException("LinkedDirection cannot be deserialized! Serialize FramedDirection");
}
- void LinkedVector3::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ void LinkedDirection::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
{
- throw LocalException("LinkedVector3 cannot be serialized! Serialize FramedVector3");
+ throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
}
- void LinkedVector3::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ void LinkedDirection::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
{
- throw LocalException("LinkedVector3 cannot be deserialized! Deserialize FramedVector3");
+ throw LocalException("LinkedDirection cannot be deserialized! Deserialize FramedDirection");
}
- void LinkedVector3::ice_postUnmarshal()
+ void LinkedDirection::ice_postUnmarshal()
{
if(referenceRobot)
{
// ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
referenceRobot->ref();
}
- FramedVector3::ice_postUnmarshal();
+ FramedDirection::ice_postUnmarshal();
}
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index 8c8db0ccd..58a2ef675 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -46,7 +46,7 @@ namespace armarx
{
// variant types
const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
- const VariantTypeId LinkedVector3 = Variant::addTypeName("::armarx::LinkedVector3Base");
+ const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
}
@@ -123,22 +123,22 @@ namespace armarx
/**
- * @class LinkedVector3 is a direction vector (NOT a position vector) with an attached robotstate proxy
+ * @class LinkedDirection is a direction vector (NOT a position vector) with an attached robotstate proxy
* for frame changes.
* @ingroup Variants
* @ingroup RobotAPI-FramedPose
- * @brief The LinkedVector3 class
+ * @brief The LinkedDirection class
*/
- class LinkedVector3 :
- virtual public LinkedVector3Base,
- virtual public FramedVector3
+ class LinkedDirection :
+ virtual public LinkedDirectionBase,
+ virtual public FramedDirection
{
public:
- LinkedVector3();
- LinkedVector3(const LinkedVector3& source);
- LinkedVector3(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+ LinkedDirection();
+ LinkedDirection(const LinkedDirection& source);
+ LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
- virtual ~LinkedVector3();
+ virtual ~LinkedDirection();
void changeFrame(const std::string &newFrame, const Ice::Current &c = Ice::Current());
@@ -150,19 +150,19 @@ namespace armarx
VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
{
- return new LinkedVector3(*this);
+ return new LinkedDirection(*this);
}
std::string output(const Ice::Current& c = ::Ice::Current()) const
{
std::stringstream s;
- s << FramedVector3::toEigen() << std::endl << "reference robot: " << referenceRobot;
+ s << FramedDirection::toEigen() << std::endl << "reference robot: " << referenceRobot;
return s.str();
}
VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
{
- return VariantType::LinkedVector3;
+ return VariantType::LinkedDirection;
}
bool validate(const Ice::Current& c = ::Ice::Current())
@@ -174,9 +174,9 @@ namespace armarx
int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
- friend std::ostream &operator<<(std::ostream &stream, const LinkedVector3& rhs)
+ friend std::ostream &operator<<(std::ostream &stream, const LinkedDirection& rhs)
{
- stream << "LinkedVector3: " << std::endl << rhs.output() << std::endl;
+ stream << "LinkedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
};
@@ -189,7 +189,7 @@ namespace armarx
};
- typedef IceInternal::Handle<LinkedVector3> LinkedVector3Ptr;
+ typedef IceInternal::Handle<LinkedDirection> LinkedDirectionPtr;
}
#endif
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index fdb7d914c..f4ded6efc 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -60,25 +60,25 @@ namespace armarx
{}
};
- class FramedVector3ObjectFactory : public Ice::ObjectFactory
+ class FramedDirectionObjectFactory : public Ice::ObjectFactory
{
public:
virtual Ice::ObjectPtr create(const std::string& type)
{
- assert(type == armarx::FramedVector3Base::ice_staticId());
- return new FramedVector3();
+ assert(type == armarx::FramedDirectionBase::ice_staticId());
+ return new FramedDirection();
}
virtual void destroy()
{}
};
- class LinkedVector3ObjectFactory : public Ice::ObjectFactory
+ class LinkedDirectionObjectFactory : public Ice::ObjectFactory
{
public:
virtual Ice::ObjectPtr create(const std::string& type)
{
- assert(type == armarx::LinkedVector3Base::ice_staticId());
- return new LinkedVector3();
+ assert(type == armarx::LinkedDirectionBase::ice_staticId());
+ return new LinkedDirection();
}
virtual void destroy()
{}
@@ -160,8 +160,8 @@ namespace armarx
ObjectFactoryMap map;
map.insert(std::make_pair(armarx::Vector3Base::ice_staticId(), new Vector3ObjectFactory));
- map.insert(std::make_pair(armarx::FramedVector3Base::ice_staticId(), new FramedVector3ObjectFactory));
- map.insert(std::make_pair(armarx::LinkedVector3Base::ice_staticId(), new LinkedVector3ObjectFactory));
+ map.insert(std::make_pair(armarx::FramedDirectionBase::ice_staticId(), new FramedDirectionObjectFactory));
+ map.insert(std::make_pair(armarx::LinkedDirectionBase::ice_staticId(), new LinkedDirectionObjectFactory));
map.insert(std::make_pair(armarx::QuaternionBase::ice_staticId(), new QuaternionObjectFactory));
map.insert(std::make_pair(armarx::PoseBase::ice_staticId(), new PoseObjectFactory));
map.insert(std::make_pair(armarx::FramedPoseBase::ice_staticId(), new FramedPoseObjectFactory));
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
index 8ab3c0e87..dccad8f96 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
@@ -6,8 +6,8 @@ namespace armarx {
{
setNumberParameters(1);
addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::FramedVector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::LinkedVector3, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::FramedDirection, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection, createParameterTypeList(1, VariantType::Float));
}
@@ -34,17 +34,17 @@ namespace armarx {
return (vec).norm() > getParameter(0).getFloat();
}
- if(type == VariantType::FramedVector3)
+ if(type == VariantType::FramedDirection)
{
- FramedVector3Ptr fV1 = value.getClass<FramedVector3>();
- Eigen::Vector3f vec = value.getClass<FramedVector3>()->toEigen();
+ FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
+ Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return (vec).norm() > getParameter(0).getFloat();
}
- if(type == VariantType::LinkedVector3)
+ if(type == VariantType::LinkedDirection)
{
- LinkedVector3Ptr lV1 = value.getClass<LinkedVector3>();
+ LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
- Eigen::Vector3f vec = value.getClass<LinkedVector3>()->toEigen();
+ Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
return (vec).norm() > getParameter(0).getFloat();
}
@@ -56,8 +56,8 @@ namespace armarx {
{
setNumberParameters(1);
addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::FramedVector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::LinkedVector3, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::FramedDirection, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection, createParameterTypeList(1, VariantType::Float));
}
@@ -84,16 +84,16 @@ namespace armarx {
return (vec).norm() < getParameter(0).getFloat();
}
- if(type == VariantType::FramedVector3)
+ if(type == VariantType::FramedDirection)
{
-// FramedVector3Ptr fV1 = value.getClass<FramedVector3>();
- Eigen::Vector3f vec = value.getClass<FramedVector3>()->toEigen();
+// FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
+ Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return (vec).norm() < getParameter(0).getFloat();
}
- if(type == VariantType::LinkedVector3)
+ if(type == VariantType::LinkedDirection)
{
-// LinkedVector3Ptr lV1 = value.getClass<LinkedVector3>();
- Eigen::Vector3f vec = value.getClass<LinkedVector3>()->toEigen();
+// LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
+ Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
return (vec).norm() < getParameter(0).getFloat();
}
@@ -104,8 +104,8 @@ namespace armarx {
{
setNumberParameters(2);
addSupportedType(VariantType::Vector3, createParameterTypeList(2, VariantType::Float, VariantType::Float));
- addSupportedType(VariantType::FramedVector3, createParameterTypeList(2, VariantType::Float, VariantType::Float));
- addSupportedType(VariantType::LinkedVector3, createParameterTypeList(2, VariantType::Float, VariantType::Float));
+ addSupportedType(VariantType::FramedDirection, createParameterTypeList(2, VariantType::Float, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection, createParameterTypeList(2, VariantType::Float, VariantType::Float));
}
@@ -131,17 +131,17 @@ namespace armarx {
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
- if(type == VariantType::FramedVector3)
+ if(type == VariantType::FramedDirection)
{
-// FramedVector3Ptr fV1 = value.getClass<FramedVector3>();
- Eigen::Vector3f vec = value.getClass<FramedVector3>()->toEigen();
+// FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
+ Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
- if(type == VariantType::LinkedVector3)
+ if(type == VariantType::LinkedDirection)
{
-// LinkedVector3Ptr lV1 = value.getClass<LinkedVector3>();
- Eigen::Vector3f vec = value.getClass<LinkedVector3>()->toEigen();
+// LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
+ Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index e7b25a820..db05fc811 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -82,13 +82,13 @@ namespace armarx {
double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
newVariant = new Variant(newValue);
}
- else if(type == VariantType::FramedVector3)
+ else if(type == VariantType::FramedDirection)
{
- FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(currentValue->get<FramedVector3>());
- FramedVector3Ptr intialVec = FramedVector3Ptr::dynamicCast(initialValue->get<FramedVector3>());
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
+ FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
- newVariant = new Variant(new FramedVector3(newValue, vec->frame, vec->agent));
+ newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
}
else if(type == VariantType::FramedPosition)
{
@@ -114,7 +114,7 @@ namespace armarx {
result.push_back(VariantType::Int);
result.push_back(VariantType::Float);
result.push_back(VariantType::Double);
- result.push_back(VariantType::FramedVector3);
+ result.push_back(VariantType::FramedDirection);
result.push_back(VariantType::FramedPosition);
result.push_back(VariantType::MatrixFloat);
return result;
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 2744da772..7a6c80fa8 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -33,7 +33,7 @@ namespace armarx {
VariantTypeId type = dataHistory.begin()->second->getType();
- if ((type == VariantType::Vector3) || (type == VariantType::FramedVector3) || (type == VariantType::FramedPosition))
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
{
Eigen::Vector3f vec;
@@ -41,9 +41,9 @@ namespace armarx {
std::string frame = "";
std::string agent = "";
VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
- if (type == VariantType::FramedVector3)
+ if (type == VariantType::FramedDirection)
{
- FramedVector3Ptr p = var->get<FramedVector3>();
+ FramedDirectionPtr p = var->get<FramedDirection>();
frame = p->frame;
agent = p->agent;
}
@@ -70,10 +70,10 @@ namespace armarx {
Vector3Ptr vecVar = new Vector3(vec);
return new Variant(vecVar);
}
- else if (type == VariantType::FramedVector3)
+ else if (type == VariantType::FramedDirection)
{
- FramedVector3Ptr vecVar = new FramedVector3(vec, frame, agent);
+ FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
return new Variant(vecVar);
}
else if (type == VariantType::FramedPosition)
@@ -101,14 +101,14 @@ namespace armarx {
}
/**
- * @brief This filter supports: Vector3, FramedVector3, FramedPosition
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
* @return List of VariantTypes
*/
ParameterTypeList getSupportedTypes(const Ice::Current &c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
- result.push_back(VariantType::FramedVector3);
+ result.push_back(VariantType::FramedDirection);
result.push_back(VariantType::FramedPosition);
return result;
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index a5c40b50f..d0ffd0c33 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -164,8 +164,8 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap &jointVel)
velocityReportRobot = robot->clone(robot->getName());
// IceUtil::Time start = IceUtil::Time::now();
// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
- FramedVector3Map tcpTranslationVelocities;
- FramedVector3Map tcpOrientationVelocities;
+ FramedDirectionMap tcpTranslationVelocities;
+ FramedDirectionMap tcpOrientationVelocities;
std::string rootFrame = robot->getRootNode()->getName();
NameValueMap tempJointAngles = robot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
@@ -200,13 +200,13 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap &jointVel)
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, robot->getName());
+ tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, robot->getName());
mat = currentPose * lastPose->toEigen().inverse();
VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
- tcpOrientationVelocities[tcpName] = new FramedVector3(rpy/tDelta, rootFrame, robot->getName());
+ tcpOrientationVelocities[tcpName] = new FramedDirection(rpy/tDelta, rootFrame, robot->getName());
}
@@ -215,17 +215,17 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap &jointVel)
updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void RobotStateObserver::updateVelocityDatafields(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities)
+void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities)
{
- FramedVector3Map::const_iterator it = tcpTranslationVelocities.begin();
+ FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
for(; it != tcpTranslationVelocities.end(); it++)
{
- FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
- FramedVector3Ptr vecOri;
- FramedVector3Map::const_iterator itOri = tcpOrientationVelocities.find(it->first);
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
+ FramedDirectionPtr vecOri;
+ FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
if(itOri != tcpOrientationVelocities.end())
- vecOri = FramedVector3Ptr::dynamicCast(itOri->second);
+ vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
const std::string &tcpName = it->first;
ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index fbd074b6d..a4f230e58 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -91,7 +91,7 @@ namespace armarx
void updateNodeVelocities(const NameValueMap &jointVel);
protected:
- void updateVelocityDatafields(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities);
+ void updateVelocityDatafields(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities);
void udpatePoseDatafields(const FramedPoseBaseMap &poseMap);
private:
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp
index 4ada24890..0c32196cd 100644
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp
+++ b/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp
@@ -70,8 +70,8 @@ namespace armarx {
Eigen::Vector3f null(3);
null.setZero();
setLocal("currentSensitivity", 0.0f);
- setLocal("currentVelocity", new FramedVector3(null, getInput<std::string>("tcpName")));
- setLocal("currentAcc", new FramedVector3(null, getInput<std::string>("tcpName")));
+ setLocal("currentVelocity", new FramedDirection(null, getInput<std::string>("tcpName")));
+ setLocal("currentAcc", new FramedDirection(null, getInput<std::string>("tcpName")));
setLocal("timestamp", (float)IceUtil::Time::now().toMilliSecondsDouble());
}
@@ -99,10 +99,10 @@ namespace armarx {
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("currentVelocity",VariantType::FramedDirection, false);
+ addToInput("currentAcc",VariantType::FramedDirection, false);
+ addToInput("currentForce",VariantType::FramedDirection, false);
+ addToInput("currentTorque",VariantType::FramedDirection, false);
addToInput("timestamp",VariantType::Float, false);
addToOutput("currentSensitivity",VariantType::Float, false);
@@ -118,10 +118,10 @@ namespace armarx {
IceUtil::Time duration = IceUtil::Time::now() - IceUtil::Time::milliSecondsDouble(getInput<float>("timestamp"));
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");
+// FramedDirectionPtr vel = getInput<FramedDirection>("currentVelocity");
+ FramedDirectionPtr vel = getInput<FramedDirection>("currentVelocity");
+ FramedDirectionPtr curAcc = getInput<FramedDirection>("currentAcc");
+ FramedDirectionPtr curForce = getInput<FramedDirection>("currentForce");
ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
float forceThreshold = 2.0f;
@@ -140,10 +140,10 @@ namespace armarx {
newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble()*0.001;
- setOutput("currentAcc", Variant(new FramedVector3(newAcc, curAcc->frame)));
+ setOutput("currentAcc", Variant(new FramedDirection(newAcc, curAcc->frame)));
RobotStatechartContext* c = getContext<RobotStatechartContext>();
- c->tcpControlPrx->setTCPVelocity(robotNodeSetName, "", new FramedVector3(newVel, vel->frame), NULL);
+ c->tcpControlPrx->setTCPVelocity(robotNodeSetName, "", new FramedDirection(newVel, vel->frame), NULL);
}
void ZeroForceControlForceToAcc::onExit()
--
GitLab