FramedOrientedPoint.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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/>.
*
* @author     Martin Miller (martin dot miller at student dot kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "FramedOrientedPoint.h"
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    FramedOrientedPoint::FramedOrientedPoint() = default;
 
    FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        OrientedPointBase(source),
        FramedOrientedPointBase(source),
        OrientedPoint(source)
    {
    }
 
    FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position,
                                             const Eigen::Vector3f& normal,
                                             const std::string& frame,
                                             const std::string& agent) :
        OrientedPoint(position, normal)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal,
                                             const std::string& frame,
                                             const std::string& agent) :
        FramedOrientedPoint(pointWithNormal.positionToEigen(),
                            pointWithNormal.normalToEigen(),
                            frame,
                            agent)
    {
    }
 
    FramedOrientedPoint::FramedOrientedPoint(Ice::Float px,
                                             ::Ice::Float py,
                                             ::Ice::Float pz,
                                             Ice::Float nx,
                                             ::Ice::Float ny,
                                             ::Ice::Float nz,
                                             const std::string& frame,
                                             const std::string& agent) :
        OrientedPoint(px, py, pz, nx, ny, nz)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    std::string
    FramedOrientedPoint::getFrame() const
    {
        return frame;
    }
 
    void
    FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot,
                                     const std::string& newFrame)
    {
        FramedPosition framedPos(positionToEigen(), frame, agent);
        FramedDirection framedDir(normalToEigen(), frame, agent);
 
        framedPos.changeFrame(robot, newFrame);
        framedDir.changeFrame(robot, newFrame);
        this->px = framedPos.x;
        this->py = framedPos.y;
        this->pz = framedPos.z;
 
        this->nx = framedDir.x;
        this->ny = framedDir.y;
        this->nz = framedDir.z;
    }
 
    void
    FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, GlobalFrame);
    }
 
    void
    FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        changeFrame(referenceRobot, GlobalFrame);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedPosition().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedNormal().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedPosition().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedNormal().toGlobalEigen(referenceRobot);
    }
 
    FramedOrientedPointPtr
    FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return new FramedOrientedPoint(
            positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
    }
 
    FramedOrientedPointPtr
    FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return new FramedOrientedPoint(
            positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedPosition().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedNormal().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedPosition().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f
    FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedNormal().toRootEigen(referenceRobot);
    }
 
    FramedPosition
    FramedOrientedPoint::getFramedPosition() const
    {
        return FramedPosition(positionToEigen(), frame, agent);
    }
 
    FramedDirection
    FramedOrientedPoint::getFramedNormal() const
    {
        return FramedDirection(normalToEigen(), frame, agent);
    }
 
    std::string
    FramedOrientedPoint::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << OrientedPoint::output(c) << std::endl
          << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
    void
    FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer,
                                   const Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        OrientedPoint::serialize(serializer);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
    }
 
    void
    FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        OrientedPoint::deserialize(serializer);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
} // namespace armarx