From 9fb96c1655e79742bc1f851a6c63a80cb429a214 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 6 Nov 2015 11:47:00 +0100
Subject: [PATCH] Pose convenience functions
---
source/RobotAPI/libraries/core/FramedPose.cpp | 107 ++++++++++++++++--
source/RobotAPI/libraries/core/FramedPose.h | 25 +++-
2 files changed, 120 insertions(+), 12 deletions(-)
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 4b22c5cff..a3a92fc86 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -34,10 +34,11 @@ namespace armarx
}
- FramedDirection::FramedDirection(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, const string& agent) :
Vector3(x, y, z)
{
this->frame = frame;
+ this->agent = agent;
}
string FramedDirection::getFrame() const
@@ -45,7 +46,7 @@ namespace armarx
return frame;
}
- FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const string& newFrame)
+ FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const string& newFrame)
{
if (framedVec.getFrame() == newFrame)
{
@@ -71,7 +72,7 @@ namespace armarx
return result;
}
- void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string& newFrame)
+ void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const string& newFrame)
{
if (getFrame() == newFrame)
{
@@ -129,7 +130,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
-
FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
@@ -137,6 +137,30 @@ namespace armarx
return newPos;
}
+ FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootFrame(sharedRobot);
+ }
+ FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedDirectionPtr newDir = FramedDirectionPtr::dynamicCast(this->clone());
+ newDir->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newDir;
+ }
+
+ Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootEigen(sharedRobot);
+ }
+ Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedDirection newDir(toEigen(), frame, agent);
+ newDir.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newDir.toEigen();
+ }
+
string FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
@@ -320,7 +344,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
-
FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
@@ -328,6 +351,30 @@ namespace armarx
return newPose;
}
+ FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootFrame(sharedRobot);
+ }
+ FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
+ newPose->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newPose;
+ }
+
+ Eigen::Matrix4f FramedPose::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootEigen(sharedRobot);
+ }
+ Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPose newPose(toEigen(), frame, agent);
+ newPose.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newPose.toEigen();
+ }
+
FramedPositionPtr FramedPose::getPosition() const
{
@@ -482,7 +529,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
-
FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
@@ -490,6 +536,30 @@ namespace armarx
return newPos;
}
+ FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootFrame(sharedRobot);
+ }
+ FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
+ newPosition->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newPosition;
+ }
+
+ Eigen::Vector3f FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootEigen(sharedRobot);
+ }
+ Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedPosition newPosition(toEigen(), frame, agent);
+ newPosition.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newPosition.toEigen();
+ }
+
string FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
@@ -644,7 +714,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
-
FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
@@ -652,6 +721,30 @@ namespace armarx
return newPos;
}
+ FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootFrame(sharedRobot);
+ }
+ FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
+ newOrientation->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newOrientation;
+ }
+
+ Eigen::Matrix3f FramedOrientation::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ return toRootEigen(sharedRobot);
+ }
+ Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ FramedOrientation newOrientation(toEigen(), frame, agent);
+ newOrientation.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ return newOrientation.toEigen();
+ }
+
void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index fb60bb5e3..a1800b495 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -84,16 +84,20 @@ namespace armarx
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);
+ FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent);
std::string getFrame() const;
- static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string& newFrame);
- void changeFrame(const VirtualRobot::RobotPtr robot, 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);
FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toGlobal(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;
+ Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
@@ -123,8 +127,8 @@ namespace armarx
public: // serialization
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
- private:
+ private:
static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
};
@@ -154,7 +158,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toGlobal(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;
+ Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
@@ -232,6 +239,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toGlobal(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;
+ Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
{
@@ -297,6 +308,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toGlobal(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;
+ Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
{
--
GitLab