From b0166504b1e5be52da78128c9789485fd8e9cc2b Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Mon, 4 Dec 2023 12:57:44 +0100
Subject: [PATCH] add FramedPositionAndOrientation aron type and add
constructor in pose and framed pose related to this type
---
.../libraries/aron/common/aron/FramedPose.xml | 5 +++++
.../RobotAPI/libraries/aron/common/aron/framed.xml | 13 +++++++++++++
source/RobotAPI/libraries/core/FramedPose.cpp | 8 ++++++++
source/RobotAPI/libraries/core/FramedPose.h | 1 +
source/RobotAPI/libraries/core/Pose.cpp | 7 +++++++
source/RobotAPI/libraries/core/Pose.h | 1 +
6 files changed, 35 insertions(+)
diff --git a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
index ec5aa0c96..9ec219210 100644
--- a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
@@ -1,6 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+ <!--
+ The types defined here are deprecated.
+ Use <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" /> instead.
+ -->
+
<GenerateTypes>
<Object name="armarx::arondto::FrameHeader">
diff --git a/source/RobotAPI/libraries/aron/common/aron/framed.xml b/source/RobotAPI/libraries/aron/common/aron/framed.xml
index 1fb5025df..0cb7d5e04 100644
--- a/source/RobotAPI/libraries/aron/common/aron/framed.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/framed.xml
@@ -43,6 +43,19 @@
</ObjectChild>
</Object>
+ <Object name="armarx::arondto::FramedPositionAndOrientation">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameID />
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ </Object>
+
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index e309cc113..0f18dcad5 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -382,6 +382,14 @@ namespace armarx
this->agent = agent;
}
+ FramedPose::FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent) :
+ Pose(pos, ori)
+ {
+ this->frame = frame;
+ this->agent = agent;
+ }
+
+
std::string FramedPose::getFrame() const
{
return frame;
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 0abe9ab2a..8538d6ba2 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -264,6 +264,7 @@ namespace armarx
FramedPose(const FramedPose& pose);
FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const std::string& agent);
FramedPose(const Eigen::Matrix4f& m, const std::string& frame, const std::string& agent);
+ FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent);
FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent);
FramedPose& operator=(const armarx::FramedPose&) = default;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 324ac1afa..496affc85 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -315,6 +315,13 @@ namespace armarx
init();
}
+ Pose::Pose(const Eigen::Vector3f& position, const Eigen::Quaternionf& quaternion)
+ {
+ this->position = new Vector3(position);
+ this->orientation = new Quaternion(quaternion);
+ init();
+ }
+
void
Pose::operator=(const Eigen::Matrix4f& matrix)
{
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 18332395d..42309c657 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -246,6 +246,7 @@ namespace armarx
Pose(const Pose& source);
Pose(const Eigen::Matrix4f&);
Pose(const Eigen::Matrix3f&, const Eigen::Vector3f&);
+ Pose(const Eigen::Vector3f&, const Eigen::Quaternionf&);
Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori);
Pose& operator=(const Pose&) = default;
--
GitLab