From bc0c073246468dd14251436faa5ed15782adf94c Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 6 Nov 2015 13:26:42 +0100
Subject: [PATCH] Pose convenience functions
---
source/RobotAPI/libraries/core/FramedPose.cpp | 78 +++++++++++++++----
source/RobotAPI/libraries/core/FramedPose.h | 8 ++
2 files changed, 71 insertions(+), 15 deletions(-)
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index a3a92fc86..fcb1ddd3e 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -132,9 +132,21 @@ namespace armarx
}
FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
- FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
- newPos->changeToGlobal(referenceRobot);
- return newPos;
+ FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
+ newDirection->changeToGlobal(referenceRobot);
+ return newDirection;
+ }
+
+ Eigen::Vector3f FramedDirection::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toGlobalEigen(sharedRobot);
+ }
+ Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedDirection newDirection(toEigen(), frame, agent);
+ newDirection.changeToGlobal(referenceRobot);
+ return newDirection.toEigen();
}
FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
@@ -144,9 +156,9 @@ namespace armarx
}
FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
- FramedDirectionPtr newDir = FramedDirectionPtr::dynamicCast(this->clone());
- newDir->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- return newDir;
+ FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
+ newDirection->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newDirection;
}
Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
@@ -156,9 +168,9 @@ namespace armarx
}
Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
- FramedDirection newDir(toEigen(), frame, agent);
- newDir.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- return newDir.toEigen();
+ FramedDirection newDirection(toEigen(), frame, agent);
+ newDirection.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newDirection.toEigen();
}
string FramedDirection::output(const Ice::Current& c) const
@@ -351,6 +363,18 @@ namespace armarx
return newPose;
}
+ Eigen::Matrix4f FramedPose::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toGlobalEigen(sharedRobot);
+ }
+ Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPose newPose(toEigen(), frame, agent);
+ newPose.changeToGlobal(referenceRobot);
+ return newPose.toEigen();
+ }
+
FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
@@ -531,9 +555,21 @@ namespace armarx
}
FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
- FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
- newPos->changeToGlobal(referenceRobot);
- return newPos;
+ FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
+ newPosition->changeToGlobal(referenceRobot);
+ return newPosition;
+ }
+
+ Eigen::Vector3f FramedPosition::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toGlobalEigen(sharedRobot);
+ }
+ Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPosition newPosition(toEigen(), frame, agent);
+ newPosition.changeToGlobal(referenceRobot);
+ return newPosition.toEigen();
}
FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
@@ -716,9 +752,21 @@ namespace armarx
}
FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
- FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
- newPos->changeToGlobal(referenceRobot);
- return newPos;
+ FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
+ newOrientation->changeToGlobal(referenceRobot);
+ return newOrientation;
+ }
+
+ Eigen::Matrix3f FramedOrientation::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toGlobalEigen(sharedRobot);
+ }
+ Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedOrientation newOrientation(toEigen(), frame, agent);
+ newOrientation.changeToGlobal(referenceRobot);
+ return newOrientation.toEigen();
}
FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index a1800b495..ec61e8f28 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -94,6 +94,8 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -158,6 +160,8 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -239,6 +243,8 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+ Eigen::Matrix3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
@@ -308,6 +314,8 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+ Eigen::Matrix4f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedPosePtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
--
GitLab