FramedPose.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::RobotStateComponent::
 * @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 <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/interface/core/FramedPoseBase.h>
 
#include "Pose.h"
 
namespace armarx::VariantType
{
    // variant types
    const VariantTypeId FramedPose = Variant::addTypeName("::armarx::FramedPoseBase");
    const VariantTypeId FramedDirection = Variant::addTypeName("::armarx::FramedDirectionBase");
    const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
    const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
} // namespace armarx::VariantType
 
namespace VirtualRobot
{
    class LinkedCoordinate;
}
 
namespace IceProxy::armarx
{
    class SharedRobotInterface;
}
 
namespace Eigen
{
    using Matrix6f = Matrix<Ice::Float, 6, 6>;
}
 
namespace armarx
{
    using SharedRobotInterfacePrx =
        ::IceInternal::ProxyHandle<::IceProxy::armarx::SharedRobotInterface>;
    /**
     * @ingroup RobotAPI-FramedPose
     * Variable of the global coordinate system. use this if you are specifying a global pose.
     * */
    std::string const GlobalFrame = "Global";
    std::string const OdometryFrame = "Odom";
    std::string const MapFrame = "Map";
 
 
    /**
     * @class FramedDirection
     * @ingroup RobotAPI-FramedPose
     * @ingroup VariantsGrp
     * @brief FramedDirection is a 3 dimensional @b direction vector with a reference frame.
     * The reference frame can be used to change the coordinate system to which
     * the vector relates. The difference to a FramedPosition is, that on frame
     * changing only the orientation of the vector is changed. The length of the vector
     * remains unchanged. This class is usefull e.g. for forces and tcp
     * velocities.
     *
     * @see Vector3, FramedPosition
     */
    class FramedDirection;
    using FramedDirectionPtr = IceInternal::Handle<FramedDirection>;
 
    class FramedDirection : virtual public FramedDirectionBase, virtual public Vector3
    {
    public:
        FramedDirection();
        FramedDirection(const FramedDirection& source);
        FramedDirection(const Eigen::Vector3f& vec,
                        const std::string& frame,
                        const std::string& agent);
        FramedDirection(Ice::Float x,
                        ::Ice::Float y,
                        ::Ice::Float z,
                        const std::string& frame,
                        const std::string& agent);
        FramedDirection& operator=(const FramedDirection&) = default;
 
        std::string getFrame() const;
        static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotConstPtr& robot,
                                              const FramedDirection& framedVec,
                                              const std::string& newFrame);
        static FramedDirectionPtr ChangeFrame(const VirtualRobot::Robot& robot,
                                              const FramedDirection& framedVec,
                                              const std::string& newFrame);
        void changeFrame(const VirtualRobot::RobotConstPtr& robot, const std::string& newFrame);
        void changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame);
 
        void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
        void changeToGlobal(const VirtualRobot::RobotConstPtr& referenceRobot);
        void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
        FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedDirectionPtr toGlobal(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedDirectionPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
        FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedDirectionPtr toRootFrame(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedDirectionPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override;
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
        VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
 
        friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs);
 
    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:
        static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame,
                                                          const std::string& newFrame,
                                                          const VirtualRobot::Robot& robotState);
    };
 
    class FramedPosition;
    using FramedPositionPtr = IceInternal::Handle<FramedPosition>;
 
    /**
     * @class FramedPosition
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The FramedPosition class
     */
    class FramedPosition : virtual public FramedPositionBase, virtual public Vector3
    {
    public:
        FramedPosition();
        FramedPosition(const Eigen::Vector3f&, const std::string& frame, const std::string& agent);
        FramedPosition(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
        //FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
        FramedPosition(const FramedPosition& other);
 
        FramedPosition& operator=(const armarx::FramedPosition& other);
 
        std::string getFrame() const;
 
        void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
                         const std::string& newFrame);
        void changeFrame(const VirtualRobot::RobotConstPtr& referenceRobot, const std::string& newFrame);
        void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
        void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
        void changeToGlobal(const VirtualRobot::RobotConstPtr& referenceRobot);
        void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
        FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedPositionPtr toGlobal(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedPositionPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Vector3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
        FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedPositionPtr toRootFrame(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedPositionPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Vector3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override;
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
        VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
 
        friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs);
 
    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;
    };
 
    class FramedOrientation;
    using FramedOrientationPtr = IceInternal::Handle<FramedOrientation>;
 
    /**
     * @class FramedOrientation
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The FramedOrientation class
     */
    class FramedOrientation : virtual public FramedOrientationBase, virtual public Quaternion
    {
    public:
        FramedOrientation();
        FramedOrientation(const Eigen::Matrix4f&,
                          const std::string& frame,
                          const std::string& agent);
        FramedOrientation(const Eigen::Matrix3f&,
                          const std::string& frame,
                          const std::string& agent);
        FramedOrientation(const Eigen::Quaternionf& q,
                          const std::string& frame,
                          const std::string& agent);
 
        FramedOrientation& operator=(const FramedOrientation&) = default;
        // this doesnt work for an unknown reason
        //FramedOrientation(const QuaternionBasePtr ori, const std::string &frame );
        std::string getFrame() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override;
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
        VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
                         const std::string& newFrame);
        void changeFrame(const VirtualRobot::RobotConstPtr& referenceRobot, const std::string& newFrame);
        void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
        void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
        void changeToGlobal(const VirtualRobot::RobotConstPtr& referenceRobot);
        void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
        FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedOrientationPtr toGlobal(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedOrientationPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Matrix3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Matrix3f toGlobalEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Matrix3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
        FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedOrientationPtr toRootFrame(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedOrientationPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Matrix3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
 
        friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs);
 
    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;
    };
 
 
    class FramedPose;
    using FramedPosePtr = IceInternal::Handle<FramedPose>;
 
    /**
     * @class FramedPose
     * @ingroup VariantsGrp
     * @ingroup RobotAPI-FramedPose
     * @brief The FramedPose class
     */
    class FramedPose : virtual public FramedPoseBase, virtual public Pose
    {
    public:
        FramedPose();
        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;
 
        std::string getFrame() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override;
 
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
 
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
                         const std::string& newFrame);
        void changeFrame(const VirtualRobot::RobotConstPtr& referenceRobot, const std::string& newFrame);
        void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
        void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
        void changeToGlobal(const VirtualRobot::RobotConstPtr& referenceRobot);
        void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
        FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedPosePtr toGlobal(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedPosePtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Matrix4f toGlobalEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Matrix4f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
        FramedPosePtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedPosePtr toRootFrame(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        FramedPosePtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
        Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotConstPtr& referenceRobot) const;
        Eigen::Matrix4f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
        FramedPosePtr toFrame(const SharedRobotInterfacePrx& referenceRobot,
                              const std::string& newFrame) const;
        FramedPosePtr toFrame(const VirtualRobot::RobotConstPtr& referenceRobot,
                              const std::string& newFrame) const;
        FramedPosePtr toFrame(const VirtualRobot::Robot& referenceRobot,
                              const std::string& newFrame) const;
        Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot,
                                     const std::string& newFrame) const;
        Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotConstPtr& referenceRobot,
                                     const std::string& newFrame) const;
        Eigen::Matrix4f toFrameEigen(const VirtualRobot::Robot& referenceRobot,
                                     const std::string& newFrame) const;
 
        friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs);
        FramedPositionPtr getPosition() const;
        FramedOrientationPtr getOrientation() const;
 
        static VirtualRobot::LinkedCoordinate
        createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot,
                               const FramedPositionPtr& position,
                               const FramedOrientationPtr& orientation);
        friend bool operator==(const FramedPose& pose1, const FramedPose& pose2);
        friend bool operator!=(const FramedPose& pose1, const FramedPose& pose2);
 
    public:
        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 FramedPosePtr = IceInternal::Handle<FramedPose>;
 
} // namespace armarx
 
extern template class ::IceInternal::Handle<::armarx::FramedPose>;
extern template class ::IceInternal::Handle<::armarx::FramedPosition>;
extern template class ::IceInternal::Handle<::armarx::FramedDirection>;
extern template class ::IceInternal::Handle<::armarx::FramedOrientation>;