Add fromIce() / toIce() methods for Vector3Ptr, QuaternionPtr, PosePtr in Pose.h

source/RobotAPI/libraries/core/Pose.cpp

@@ -340,4 +340,88 @@ namespace armarx
     {
         init();
     }
+
+}
+
+
+void armarx::fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector)
+{
+    vector = fromIce(ice );
+}
+
+void armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion)
+{
+    quaternion = fromIce(ice);
+}
+
+void armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation)
+{
+    rotation = fromIce(ice).toRotationMatrix();
+}
+
+void armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose)
+{
+    pose = fromIce(ice);
+}
+
+
+Eigen::Vector3f armarx::fromIce(const Vector3BasePtr& pose)
+{
+    auto cast = Vector3Ptr::dynamicCast(pose);
+    ARMARX_CHECK_NOT_NULL(cast) << "Vector3BasePtr must be a Vector3Ptr.";
+    return cast->toEigen();
+}
+Eigen::Quaternionf armarx::fromIce(const QuaternionBasePtr& pose)
+{
+    auto cast = QuaternionPtr::dynamicCast(pose);
+    ARMARX_CHECK_NOT_NULL(cast) << "QuaternionBasePtr must be a QuaternionPtr.";
+    return cast->toEigenQuaternion();
+}
+Eigen::Matrix4f armarx::fromIce(const PoseBasePtr& pose)
+{
+    auto cast = PosePtr::dynamicCast(pose);
+    ARMARX_CHECK_NOT_NULL(cast) << "PoseBasePtr must be a PosePtr.";
+    return cast->toEigen();
+}
+
+
+void armarx::toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector)
+{
+    ice = new Vector3(vector);
+}
+
+void armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation)
+{
+    ice = new Quaternion(rotation);
+}
+
+void armarx::toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion)
+{
+    ice = new Quaternion(quaternion);
+}
+
+void armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose)
+{
+    ice = new Pose(pose);
+}
+
+
+armarx::Vector3Ptr armarx::toIce(const Eigen::Vector3f& vector)
+{
+    return new Vector3(vector);
+}
+
+armarx::QuaternionPtr armarx::toIce(const Eigen::Matrix3f& rotation)
+{
+    return new Quaternion(rotation);
+}
+
+armarx::QuaternionPtr armarx::toIce(const Eigen::Quaternionf& quaternion)
+{
+    return new Quaternion(quaternion);
+}
+
+armarx::PosePtr armarx::toIce(const Eigen::Matrix4f& pose)
+{
+    return new Pose(pose);
 }

source/RobotAPI/libraries/core/Pose.h

@@ -270,6 +270,28 @@ namespace armarx
 
     using PosePtr = IceInternal::Handle<Pose>;
 
+
+    // Ice conversion functions
+
+    void fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector);
+    void fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion);
+    void fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation);
+    void fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose);
+
+    Eigen::Vector3f fromIce(const Vector3BasePtr& position);
+    Eigen::Quaternionf fromIce(const QuaternionBasePtr& rotation);
+    Eigen::Matrix4f fromIce(const PoseBasePtr& pose);
+
+    void toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector);
+    void toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation);
+    void toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion);
+    void toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose);
+
+    Vector3Ptr toIce(const Eigen::Vector3f& vector);
+    QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
+    QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
+    PosePtr toIce(const Eigen::Matrix4f& pose);
+
 }
 
 extern template class ::IceInternal::Handle< ::armarx::Pose>;