aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/Pose.h>
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
                           armarx::grasping::BoundingBox& bo)
{
    ARMARX_TRACE;
    bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
                         const armarx::grasping::BoundingBox& bo)
{
    ARMARX_TRACE;
    dto.center = fromIce(bo.center);
    dto.ha1 = fromIce(bo.ha1);
    dto.ha2 = fromIce(bo.ha2);
    dto.ha3 = fromIce(bo.ha3);
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
                           armarx::grasping::GraspCandidateSourceInfo& bo)
{
    ARMARX_TRACE;
    bo.bbox = new BoundingBox();
    if (dto.bbox)
    {
        fromAron(dto.bbox.value(), *bo.bbox);
    }
    else
        bo.bbox = nullptr;
    bo.referenceObjectName = dto.referenceObjectName;
    if (dto.referenceObjectPose)
    {
        bo.referenceObjectPose = toIce(dto.referenceObjectPose.value());
    }
    else
        bo.referenceObjectPose = nullptr;
    bo.segmentationLabelID = dto.segmentationLabelID;
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
                         const armarx::grasping::GraspCandidateSourceInfo& bo)
{
    ARMARX_TRACE;
    if (bo.bbox)
    {
        dto.bbox = arondto::BoundingBox();
        toAron(dto.bbox.value(), *bo.bbox);
    }
    dto.referenceObjectName = bo.referenceObjectName;
    if (bo.referenceObjectPose)
    {
        dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
    }
    dto.segmentationLabelID = bo.segmentationLabelID;
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
                           armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
    ARMARX_TRACE;
    bo = GraspCandidateReachabilityInfo(dto.reachable,
                                        dto.minimumJointLimitMargin,
                                        dto.jointLimitMargins,
                                        dto.maxPosError,
                                        dto.maxOriError);
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
                         const armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
    ARMARX_TRACE;
    dto.jointLimitMargins = bo.jointLimitMargins;
    dto.maxOriError = bo.maxOriError;
    dto.maxPosError = bo.maxPosError;
    dto.minimumJointLimitMargin = bo.minimumJointLimitMargin;
    dto.reachable = bo.reachable;
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
                           armarx::grasping::GraspCandidate& bo)
{
    ARMARX_TRACE;
    bo = GraspCandidate();
    bo.graspPose = toIce(dto.graspPose);
    bo.robotPose = toIce(dto.robotPose);
    bo.tcpPoseInHandRoot = dto.tcpPoseInHandRoot ? toIce(dto.tcpPoseInHandRoot.value()) : nullptr;
    bo.approachVector = dto.approachVector ? toIce(dto.approachVector.value()) : nullptr;
    bo.sourceFrame = dto.sourceFrame;
    bo.targetFrame = dto.targetFrame;
    bo.side = dto.side;
    bo.graspSuccessProbability =
        dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
    fromAron(dto.objectType, bo.objectType);
    if (dto.executionHints)
    {
        bo.executionHints = new GraspCandidateExecutionHints();
        fromAron(dto.executionHints.value(), *bo.executionHints);
    }
    else
    {
        bo.executionHints = nullptr;
    }
    bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
    bo.providerName = dto.providerName;
    if (dto.reachabilityInfo)
    {
        bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
        fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
    }
    else
    {
        bo.reachabilityInfo = nullptr;
    }
    if (dto.sourceInfo)
    {
        bo.sourceInfo = new GraspCandidateSourceInfo();
        fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
    }
    else
    {
        bo.sourceInfo = nullptr;
    }
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
                         const armarx::grasping::GraspCandidate& bo)
{
    ARMARX_TRACE;
    if (bo.approachVector)
    {
        dto.approachVector = fromIce(bo.approachVector);
    }
    if (bo.executionHints)
    {
        dto.executionHints = arondto::GraspCandidateExecutionHints();
        toAron(dto.executionHints.value(), *bo.executionHints);
    }
    else
    {
        dto.executionHints = std::nullopt;
    }
    dto.graspPose = fromIce(bo.graspPose);
    if (bo.graspSuccessProbability < 0 || bo.graspSuccessProbability > 1.0)
    {
        dto.graspSuccessProbability = std::nullopt;
    }
    else
    {
        dto.graspSuccessProbability = bo.graspSuccessProbability;
    }
    if (bo.groupNr < 0)
    {
        dto.groupNr = std::nullopt;
    }
    else
    {
        dto.groupNr = bo.groupNr;
    }
    toAron(dto.objectType, bo.objectType);
    dto.providerName = bo.providerName;
    if (bo.reachabilityInfo)
    {
        dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
        toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
    }
    else
    {
        dto.reachabilityInfo = std::nullopt;
    }
    dto.robotPose = fromIce(bo.robotPose);
    if (bo.tcpPoseInHandRoot)
    {
        dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
    }
 
    dto.side = bo.side;
    dto.sourceFrame = bo.sourceFrame;
    if (bo.sourceInfo)
    {
        dto.sourceInfo = arondto::GraspCandidateSourceInfo();
        toAron(dto.sourceInfo.value(), *bo.sourceInfo);
    }
    else
    {
        dto.sourceInfo = std::nullopt;
    }
    dto.targetFrame = bo.targetFrame;
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
                           armarx::grasping::BimanualGraspCandidate& bo)
{
    ARMARX_TRACE;
    bo = BimanualGraspCandidate();
    bo.graspPoseRight = toIce(dto.graspPoseRight);
    bo.graspPoseLeft = toIce(dto.graspPoseLeft);
    bo.robotPose = toIce(dto.robotPose);
    bo.tcpPoseInHandRootRight =
        dto.tcpPoseInHandRootRight ? toIce(dto.tcpPoseInHandRootRight.value()) : nullptr;
    bo.tcpPoseInHandRootLeft =
        dto.tcpPoseInHandRootLeft ? toIce(dto.tcpPoseInHandRootLeft.value()) : nullptr;
    bo.approachVectorRight =
        dto.approachVectorRight ? toIce(dto.approachVectorRight.value()) : nullptr;
    bo.approachVectorLeft =
        dto.approachVectorLeft ? toIce(dto.approachVectorLeft.value()) : nullptr;
    bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
    bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
    bo.sourceFrame = dto.sourceFrame;
    bo.targetFrame = dto.targetFrame;
    fromAron(dto.objectType, bo.objectType);
    if (dto.executionHintsRight)
    {
        bo.executionHintsRight = new GraspCandidateExecutionHints();
        fromAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
    }
    else
    {
        bo.executionHintsRight = nullptr;
    }
    if (dto.executionHintsLeft)
    {
        bo.executionHintsLeft = new GraspCandidateExecutionHints();
        fromAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
    }
    else
    {
        bo.executionHintsLeft = nullptr;
    }
    bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
    bo.providerName = dto.providerName;
    if (dto.reachabilityInfoRight)
    {
        bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
        fromAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
    }
    else
    {
        bo.reachabilityInfoRight = nullptr;
    }
    if (dto.reachabilityInfoLeft)
    {
        bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
        fromAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
    }
    else
    {
        bo.reachabilityInfoLeft = nullptr;
    }
    if (dto.sourceInfo)
    {
        bo.sourceInfo = new GraspCandidateSourceInfo();
        fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
    }
    else
    {
        bo.sourceInfo = nullptr;
    }
    bo.graspName = dto.graspName;
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
                         const armarx::grasping::BimanualGraspCandidate& bo)
{
    if (bo.tcpPoseInHandRootRight)
    {
        dto.tcpPoseInHandRootRight = fromIce(bo.tcpPoseInHandRootRight);
    }
    if (bo.tcpPoseInHandRootLeft)
    {
        dto.tcpPoseInHandRootLeft = fromIce(bo.tcpPoseInHandRootLeft);
    }
    if (bo.approachVectorRight)
    {
        dto.approachVectorRight = fromIce(bo.approachVectorRight);
    }
    if (bo.approachVectorLeft)
    {
        dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
    }
    if (bo.executionHintsRight)
    {
        dto.executionHintsRight = arondto::GraspCandidateExecutionHints();
        toAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
    }
    else
    {
        dto.executionHintsRight = std::nullopt;
    }
    if (bo.executionHintsLeft)
    {
        dto.executionHintsLeft = arondto::GraspCandidateExecutionHints();
        toAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
    }
    else
    {
        dto.executionHintsLeft = std::nullopt;
    }
    dto.graspPoseRight = fromIce(bo.graspPoseRight);
    dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
    if (bo.groupNr < 0)
    {
        dto.groupNr = std::nullopt;
    }
    else
    {
        dto.groupNr = bo.groupNr;
    }
    toAron(dto.objectType, bo.objectType);
    dto.providerName = bo.providerName;
    if (bo.reachabilityInfoRight)
    {
        dto.reachabilityInfoRight = arondto::GraspCandidateReachabilityInfo();
        toAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
    }
    else
    {
        dto.reachabilityInfoRight = std::nullopt;
    }
    if (bo.reachabilityInfoLeft)
    {
        dto.reachabilityInfoLeft = arondto::GraspCandidateReachabilityInfo();
        toAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
    }
    else
    {
        dto.reachabilityInfoLeft = std::nullopt;
    }
    dto.robotPose = fromIce(bo.robotPose);
    dto.sourceFrame = bo.sourceFrame;
    if (bo.sourceInfo)
    {
        dto.sourceInfo = arondto::GraspCandidateSourceInfo();
        toAron(dto.sourceInfo.value(), *bo.sourceInfo);
    }
    else
    {
        dto.sourceInfo = std::nullopt;
    }
    dto.targetFrame = bo.targetFrame;
    dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
    dto.inwardsVectorLeft = fromIce(bo.inwardsVectorLeft);
    dto.graspName = bo.graspName;
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
                           armarx::grasping::GraspCandidateExecutionHints& bo)
{
    bo = GraspCandidateExecutionHints();
    fromAron(dto.approach, bo.approach);
    fromAron(dto.preshape, bo.preshape);
    bo.graspTrajectoryName = dto.graspTrajectoryName;
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
                         const armarx::grasping::GraspCandidateExecutionHints& bo)
{
    toAron(dto.approach, bo.approach);
    toAron(dto.preshape, bo.preshape);
    dto.graspTrajectoryName = bo.graspTrajectoryName;
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
                           armarx::grasping::ApproachType& bo)
{
    switch (dto.value)
    {
        case arondto::ApproachType::AnyApproach:
            bo = ApproachType::AnyApproach;
            return;
        case arondto::ApproachType::TopApproach:
            bo = ApproachType::TopApproach;
            return;
        case arondto::ApproachType::SideApproach:
            bo = ApproachType::SideApproach;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
                         const armarx::grasping::ApproachType& bo)
{
    switch (bo)
    {
        case ApproachType::AnyApproach:
            dto.value = arondto::ApproachType::AnyApproach;
            return;
        case ApproachType::TopApproach:
            dto.value = arondto::ApproachType::TopApproach;
            return;
        case ApproachType::SideApproach:
            dto.value = arondto::ApproachType::SideApproach;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
}
 
void
armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
                           armarx::grasping::ApertureType& bo)
{
    switch (dto.value)
    {
        case arondto::ApertureType::AnyAperture:
            bo = ApertureType::AnyAperture;
            return;
        case arondto::ApertureType::OpenAperture:
            bo = ApertureType::OpenAperture;
            return;
        case arondto::ApertureType::PreshapedAperture:
            bo = ApertureType::PreshapedAperture;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
 
void
armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
                         const armarx::grasping::ApertureType& bo)
{
    switch (bo)
    {
        case ApertureType::AnyAperture:
            dto.value = arondto::ApertureType::AnyAperture;
            return;
        case ApertureType::OpenAperture:
            dto.value = arondto::ApertureType::OpenAperture;
            return;
        case ApertureType::PreshapedAperture:
            dto.value = arondto::ApertureType::PreshapedAperture;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
}