diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp index a3a92fc860965272aee0f2e1723868aab86848e3..fcb1ddd3ef4775627c8d91f9e6004570a4741bf6 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 a1800b4951588ee9d5fd7ce62ecaa6c042f3e91f..ec61e8f282d7a32eced6fe47a1a40213b5537d65 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;