Pose.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::Core
 * @author     ( stefan dot ulbrich at kit dot edu)
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
 
namespace armarx::VariantType
{
    // variant types
    const VariantTypeId Vector2 = Variant::addTypeName("::armarx::Vector2Base");
    const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
    const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
    const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
} // namespace armarx::VariantType
 
namespace armarx
{
    const Eigen::IOFormat
        ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
 
    /**
     * @brief The Vector2 class
     * @ingroup VariantsGrp
     */
    class Vector2 : virtual public Vector2Base
    {
    public:
        Vector2();
        Vector2(const Eigen::Vector2f&);
        Vector2(::Ice::Float x, ::Ice::Float y);
 
        void operator=(const Eigen::Vector2f& ves);
 
        virtual Eigen::Vector2f toEigen() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr
        ice_clone() const override
        {
            return this->clone();
        }
 
        VariantDataClassPtr
        clone(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return new Vector2(*this);
        }
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        VariantTypeId
        getType(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return VariantType::Vector2;
        }
 
        bool
        validate(const Ice::Current& = Ice::emptyCurrent) override
        {
            return true;
        }
 
        friend std::ostream&
        operator<<(std::ostream& stream, const Vector2& rhs)
        {
            stream << "Vector2: " << std::endl << rhs.output() << std::endl;
            return stream;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer,
                       const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
                         const ::Ice::Current& = Ice::emptyCurrent) override;
    };
 
    using Vector2Ptr = IceInternal::Handle<Vector2>;
 
    /**
     * @class Vector3
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The Vector3 class
     */
    class Vector3 : virtual public Vector3Base
    {
    public:
        Vector3();
        Vector3(const Eigen::Vector3f&);
        Vector3(const Eigen::Matrix4f&);
        Vector3(::Ice::Float x, ::Ice::Float y, ::Ice::Float z);
 
        void operator=(const Eigen::Vector3f& vec);
 
        virtual Eigen::Vector3f toEigen() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr
        ice_clone() const override
        {
            return this->clone();
        }
 
        VariantDataClassPtr
        clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new Vector3(*this);
        }
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        VariantTypeId
        getType(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return VariantType::Vector3;
        }
 
        bool
        validate(const Ice::Current& = Ice::emptyCurrent) override
        {
            return true;
        }
 
        friend std::ostream&
        operator<<(std::ostream& stream, const Vector3& rhs)
        {
            stream << "Vector3: " << std::endl << rhs.output() << std::endl;
            return stream;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer,
                       const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
                         const ::Ice::Current& = Ice::emptyCurrent) override;
    };
 
    using Vector3Ptr = IceInternal::Handle<Vector3>;
 
    /**
     * @class Quaternion
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The Quaternion class
     */
    class Quaternion : virtual public QuaternionBase
    {
    public:
        /// Construct an identity quaternion.
        Quaternion();
        Quaternion(const Eigen::Matrix4f&);
        Quaternion(const Eigen::Matrix3f&);
        Quaternion(const Eigen::Quaternionf&);
        Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz);
 
        Eigen::Matrix3f toEigen() const;
        Eigen::Quaternionf toEigenQuaternion() const;
        Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&);
 
        static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&, const Eigen::Matrix3f&);
 
        // inherited from VariantDataClass
        Ice::ObjectPtr
        ice_clone() const override
        {
            return this->clone();
        }
 
        VariantDataClassPtr
        clone(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return new Quaternion(*this);
        }
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        VariantTypeId
        getType(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return VariantType::Quaternion;
        }
 
        bool
        validate(const Ice::Current& = Ice::emptyCurrent) override
        {
            return true;
        }
 
        friend std::ostream&
        operator<<(std::ostream& stream, const Quaternion& rhs)
        {
            stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
            return stream;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer,
                       const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
                         const ::Ice::Current& = Ice::emptyCurrent) override;
 
    private:
        void init(const Eigen::Matrix3f&);
        void init(const Eigen::Quaternionf&);
    };
 
    using QuaternionPtr = IceInternal::Handle<Quaternion>;
 
    /**
     * @class Pose
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The Pose class
     */
    class Pose : virtual public PoseBase
    {
    public:
        Pose();
        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;
 
        void operator=(const Eigen::Matrix4f& matrix);
        virtual Eigen::Matrix4f toEigen() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr
        ice_clone() const override
        {
            return this->clone();
        }
 
        VariantDataClassPtr
        clone(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return new Pose(*this);
        }
 
        std::string output(const Ice::Current& = Ice::emptyCurrent) const override;
 
        VariantTypeId
        getType(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return VariantType::Pose;
        }
 
        bool
        validate(const Ice::Current& = Ice::emptyCurrent) override
        {
            return true;
        }
 
        friend std::ostream&
        operator<<(std::ostream& stream, const Pose& rhs)
        {
            stream << "Pose: " << std::endl << rhs.output() << std::endl;
            return stream;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer,
                       const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
                         const ::Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        void init();
        void ice_postUnmarshal() override;
 
    private:
        //! To void unnecessary upcasts
        QuaternionPtr c_orientation;
        Vector3Ptr c_position;
    };
 
    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);
    void fromIce(const QuaternionBasePtr& ice,
                 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
    void fromIce(const PoseBasePtr& ice, Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& 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::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
    void toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion);
    void toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose);
    void toIce(PoseBasePtr& ice, const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
 
    Vector3Ptr toIce(const Eigen::Vector3f& vector);
    QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
    QuaternionPtr toIce(const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
    QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
    PosePtr toIce(const Eigen::Matrix4f& pose);
    PosePtr toIce(const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
    PosePtr toIce(const Eigen::Isometry3f& pose);
 
} // namespace armarx
 
extern template class ::IceInternal::Handle<::armarx::Pose>;
extern template class ::IceInternal::Handle<::armarx::Vector2>;
extern template class ::IceInternal::Handle<::armarx::Vector3>;
extern template class ::IceInternal::Handle<::armarx::Quaternion>;