add FramedPositionAndOrientation aron type and add constructor in pose and...

add FramedPositionAndOrientation aron type and add constructor in pose and framed pose related to this type

source/RobotAPI/libraries/aron/common/aron/FramedPose.xml

 <?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">

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>

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;

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;

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)
     {

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;