ObjectPose.cpp

Go to the documentation of this file.

#include "ObjectPose.h"
 
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include "ice_conversions.h"
 
namespace armarx::objpose
{
    ObjectPose::ObjectPose()
    {
    }
 
    ObjectPose::ObjectPose(const data::ObjectPose& ice)
    {
        fromIce(ice);
    }
 
    void
    ObjectPose::fromIce(const data::ObjectPose& ice)
    {
        armarx::fromIce(ice.objectID, objectID);
 
        providerName = ice.providerName;
        objectType = ice.objectType;
        isStatic = ice.isStatic;
 
        armarx::fromIce(ice.objectPoseRobot, objectPoseRobot);
        objpose::fromIce(ice.objectPoseRobotGaussian, objectPoseRobotGaussian);
 
        armarx::fromIce(ice.objectPoseGlobal, objectPoseGlobal);
        objpose::fromIce(ice.objectPoseGlobalGaussian, objectPoseGlobalGaussian);
 
        armarx::fromIce(ice.objectPoseOriginal, objectPoseOriginal);
        objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
        objpose::fromIce(ice.objectPoseOriginalGaussian, objectPoseOriginalGaussian);
 
        objectJointValues = ice.objectJointValues;
 
        robotConfig = ice.robotConfig;
        armarx::fromIce(ice.robotPose, robotPose);
        robotName = ice.robotName;
 
        objpose::fromIce(ice.attachment, this->attachment);
 
        confidence = ice.confidence;
        armarx::core::time::fromIce(ice.timestamp, timestamp);
 
        objpose::fromIce(ice.localOOBB, localOOBB);
    }
 
    data::ObjectPose
    ObjectPose::toIce() const
    {
        data::ObjectPose ice;
        toIce(ice);
        return ice;
    }
 
    void
    ObjectPose::toIce(data::ObjectPose& ice) const
    {
        armarx::toIce(ice.objectID, objectID);
 
        ice.providerName = providerName;
        ice.objectType = objectType;
        ice.isStatic = isStatic;
 
        armarx::toIce(ice.objectPoseRobot, objectPoseRobot);
        objpose::toIce(ice.objectPoseRobotGaussian, objectPoseRobotGaussian);
 
        armarx::toIce(ice.objectPoseGlobal, objectPoseGlobal);
        objpose::toIce(ice.objectPoseGlobalGaussian, objectPoseGlobalGaussian);
 
        armarx::toIce(ice.objectPoseOriginal, objectPoseOriginal);
        ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
        objpose::toIce(ice.objectPoseOriginalGaussian, objectPoseOriginalGaussian);
 
        ice.objectJointValues = objectJointValues;
 
        ice.robotConfig = robotConfig;
        ice.robotPose = new Pose(robotPose);
        ice.robotName = robotName;
 
        objpose::toIce(ice.attachment, this->attachment);
 
        ice.confidence = confidence;
        armarx::core::time::toIce(ice.timestamp, timestamp);
 
        objpose::toIce(ice.localOOBB, localOOBB);
    }
 
    void
    ObjectPose::fromProvidedPose(const data::ProvidedObjectPose& provided,
                                 VirtualRobot::RobotPtr robot)
    {
        armarx::fromIce(provided.objectID, objectID);
 
        providerName = provided.providerName;
        objectType = provided.objectType;
        isStatic = provided.isStatic;
 
        armarx::fromIce(provided.objectPose, objectPoseOriginal);
        objpose::fromIce(provided.objectPoseGaussian, objectPoseOriginalGaussian);
        objectPoseOriginalFrame = provided.objectPoseFrame;
 
        objectJointValues = provided.objectJointValues;
 
        if (robot)
        {
            armarx::FramedPose framed(
                objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
            framed.changeFrame(robot, robot->getRootNode()->getName());
            objectPoseRobot = framed.toEigen();
            objectPoseRobotGaussian = std::nullopt;
 
            objectPoseRobotGaussian = objectPoseOriginalGaussian;
            if (objectPoseOriginalGaussian.has_value() and
                objectPoseOriginalFrame != robot->getRootNode()->getName())
            {
                objectPoseRobotGaussian =
                    objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseRobot);
            }
 
            framed.changeToGlobal(robot);
            objectPoseGlobal = framed.toEigen();
 
            objectPoseGlobalGaussian = objectPoseOriginalGaussian;
            if (objectPoseOriginalGaussian.has_value() and
                objectPoseOriginalFrame != armarx::GlobalFrame)
            {
                objectPoseGlobalGaussian = objectPoseOriginalGaussian->getTransformed(
                    objectPoseOriginal, objectPoseGlobal);
            }
 
            robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
            robotPose = robot->getGlobalPose();
            robotName = robot->getName();
        }
        else
        {
            // Try to get what we can.
            if (objectPoseOriginalFrame == armarx::GlobalFrame)
            {
                objectPoseGlobal = objectPoseOriginal;
                objectPoseGlobalGaussian = objectPoseOriginalGaussian;
            }
            else
            {
                // Nothing we can do about it ... Let's not lie about it at least.
                objectPoseGlobal.setIdentity();
                objectPoseGlobalGaussian = {};
            }
 
            robotConfig.clear();
            robotPose.setIdentity();
            robotName = provided.robotName;
 
            // Keep original and robot consistent.
            objectPoseRobot = objectPoseGlobal;
            objectPoseRobotGaussian = objectPoseGlobalGaussian;
        }
 
        attachment = std::nullopt;
 
        confidence = provided.confidence;
        armarx::core::time::fromIce(provided.timestamp, timestamp);
 
        objpose::fromIce(provided.localOOBB, localOOBB);
    }
 
    // ToDo: We can provide ...Robot() and ...Original() versions as well.
    objpose::ProvidedObjectPose
    ObjectPose::toProvidedObjectPoseGlobal() const
    {
        objpose::ProvidedObjectPose providedObjectPose;
 
        providedObjectPose.objectID = this->objectID;
        providedObjectPose.providerName = this->providerName;
        providedObjectPose.objectType = this->objectType;
 
        providedObjectPose.isStatic = this->isStatic;
 
        providedObjectPose.objectPoseFrame = GlobalFrame;
        providedObjectPose.objectPose = this->objectPoseGlobal;
        providedObjectPose.objectPoseGaussian = this->objectPoseGlobalGaussian;
 
        providedObjectPose.objectJointValues = this->objectJointValues;
 
        providedObjectPose.confidence = this->confidence;
        providedObjectPose.timestamp = this->timestamp;
 
        providedObjectPose.localOOBB = this->localOOBB;
 
        return providedObjectPose;
    }
 
    void
    ObjectPose::setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot,
                                   bool updateObjectPoseGlobal)
    {
        this->objectPoseRobot = objectPoseRobot;
        if (updateObjectPoseGlobal)
        {
            this->objectPoseGlobal = robotPose * objectPoseRobot;
        }
    }
 
    void
    ObjectPose::setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal,
                                    bool updateObjectPoseRobot)
    {
        this->objectPoseGlobal = objectPoseGlobal;
        if (updateObjectPoseRobot)
        {
            this->objectPoseRobot = simox::math::inverted_pose(robotPose) * objectPoseGlobal;
        }
    }
 
    std::optional<simox::OrientedBoxf>
    ObjectPose::oobbRobot() const
    {
        if (localOOBB)
        {
            return localOOBB->transformed(objectPoseRobot);
        }
        return std::nullopt;
    }
 
    std::optional<simox::OrientedBoxf>
    ObjectPose::oobbGlobal() const
    {
        if (localOOBB)
        {
            return localOOBB->transformed(objectPoseGlobal);
        }
        return std::nullopt;
    }
 
    objpose::ObjectPose
    ObjectPose::getAttached(VirtualRobot::RobotPtr agent) const
    {
        ARMARX_CHECK(attachment);
 
        objpose::ObjectPose updated = *this;
        updated.updateAttached(agent);
        return updated;
    }
 
    void
    ObjectPose::updateAttached(VirtualRobot::RobotPtr agent)
    {
        ARMARX_CHECK(attachment);
        ARMARX_CHECK_NOT_NULL(agent);
 
        ARMARX_CHECK_EQUAL(attachment->agentName, agent->getName());
 
        VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment->frameName);
        ARMARX_CHECK_NOT_NULL(robotNode);
 
        // Overwrite object pose
        objectPoseRobot = robotNode->getPoseInRootFrame() * attachment->poseInFrame;
        objectPoseGlobal = agent->getGlobalPose() * objectPoseRobot;
 
        // Overwrite robot config.
        robotPose = agent->getGlobalPose();
        robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
    }
 
} // namespace armarx::objpose
 
namespace armarx
{
 
    void
    objpose::fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment)
    {
        attachment.agentName = ice.agentName;
        attachment.frameName = ice.frameName;
        attachment.poseInFrame = ::armarx::fromIce(ice.poseInFrame);
    }
 
    void
    objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice,
                     std::optional<ObjectAttachmentInfo>& attachment)
    {
        if (ice)
        {
            attachment = ObjectAttachmentInfo();
            fromIce(*ice, *attachment);
        }
        else
        {
            attachment = std::nullopt;
        }
    }
 
    std::optional<objpose::ObjectAttachmentInfo>
    objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice)
    {
        if (!ice)
        {
            return std::nullopt;
        }
        objpose::ObjectAttachmentInfo info;
        fromIce(*ice, info);
        return info;
    }
 
    void
    objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
    {
        ice.agentName = attachment.agentName;
        ice.frameName = attachment.frameName;
        ice.poseInFrame = new Pose(attachment.poseInFrame);
    }
 
    void
    objpose::toIce(data::ObjectAttachmentInfoPtr& ice,
                   const std::optional<ObjectAttachmentInfo>& attachment)
    {
        if (attachment)
        {
            ice = new objpose::data::ObjectAttachmentInfo();
            toIce(*ice, *attachment);
        }
        else
        {
            ice = nullptr;
        }
    }
 
    objpose::data::ObjectAttachmentInfoPtr
    objpose::toIce(const std::optional<ObjectAttachmentInfo>& attachment)
    {
        if (!attachment)
        {
            return nullptr;
        }
        objpose::data::ObjectAttachmentInfoPtr ice = new objpose::data::ObjectAttachmentInfo();
        toIce(*ice, *attachment);
        return ice;
    }
 
    void
    objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
    {
        pose.fromIce(ice);
    }
 
    objpose::ObjectPose
    objpose::fromIce(const data::ObjectPose& ice)
    {
        return ObjectPose(ice);
    }
 
    void
    objpose::fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
    {
        poses.clear();
        poses.reserve(ice.size());
        for (const auto& i : ice)
        {
            poses.emplace_back(i);
        }
    }
 
    objpose::ObjectPoseSeq
    objpose::fromIce(const data::ObjectPoseSeq& ice)
    {
        ObjectPoseSeq poses;
        fromIce(ice, poses);
        return poses;
    }
 
    void
    objpose::toIce(data::ObjectPose& ice, const ObjectPose& pose)
    {
        pose.toIce(ice);
    }
 
    objpose::data::ObjectPose
    objpose::toIce(const ObjectPose& pose)
    {
        return pose.toIce();
    }
 
    void
    objpose::toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses)
    {
        ice.clear();
        ice.reserve(poses.size());
        for (const auto& p : poses)
        {
            ice.emplace_back(p.toIce());
        }
    }
 
    objpose::data::ObjectPoseSeq
    objpose::toIce(const ObjectPoseSeq& poses)
    {
        data::ObjectPoseSeq ice;
        toIce(ice, poses);
        return ice;
    }
 
    objpose::ObjectPose*
    objpose::findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id)
    {
        for (objpose::ObjectPose& pose : objectPoses)
        {
            if (pose.objectID == id)
            {
                return &pose;
            }
        }
        return nullptr;
    }
 
    const objpose::ObjectPose*
    objpose::findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id)
    {
        for (const objpose::ObjectPose& pose : objectPoses)
        {
            if (pose.objectID == id)
            {
                return &pose;
            }
        }
        return nullptr;
    }
 
    objpose::data::ObjectPose*
    objpose::findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
    {
        for (objpose::data::ObjectPose& pose : objectPoses)
        {
            if (pose.objectID == id)
            {
                return &pose;
            }
        }
        return nullptr;
    }
 
    const objpose::data::ObjectPose*
    objpose::findObjectPoseByID(const data::ObjectPoseSeq& objectPoses,
                                const armarx::data::ObjectID& id)
    {
        for (const objpose::data::ObjectPose& pose : objectPoses)
        {
            if (pose.objectID == id)
            {
                return &pose;
            }
        }
        return nullptr;
    }
 
} // namespace armarx