LookAtArticulatedObjectFrame.cpp

Go to the documentation of this file.

#include "LookAtArticulatedObjectFrame.h"
 
#include <Eigen/Core>
 
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/view_selection/skills/aron/LookAt.aron.generated.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
    LookAtArticulatedObjectFrame::LookAtArticulatedObjectFrame(const Services& srv) :
        Base(DefaultSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::InitResult
    LookAtArticulatedObjectFrame::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookAtArticulatedObjectFrame::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_INFO << "Querying descriptions";
        const auto descriptions = srv_.articulatedObjectReader->queryDescriptions(
            Clock::Now(), in.parameters.objectPoseProvider);
 
        ARMARX_INFO << "Available descriptions are:";
        for (const auto& description : descriptions)
        {
            ARMARX_INFO << description;
        }
 
        ObjectID objectID;
        fromAron(in.parameters.object, objectID);
 
        const std::string objectName = objectID.dataset() + "/" + objectID.className();
        ARMARX_INFO << "Searching for object name `" << objectName << "`";
 
        const auto isMatchingDescription = [&objectID](const auto& description) -> bool
        { return objectID.equalClass(ObjectID(description.name)); };
 
        const auto descriptionIt =
            std::find_if(descriptions.begin(), descriptions.end(), isMatchingDescription);
        ARMARX_CHECK(descriptionIt != descriptions.end())
            << "Description " << objectID << " not available";
 
        ARMARX_INFO << "getting object";
        std::optional<armarx::armem::articulated_object::ArticulatedObject> objectOpt =
            srv_.articulatedObjectReader->get(*descriptionIt,
                                              Clock::Now(),
                                              objectID.instanceName(),
                                              in.parameters.objectPoseProvider);
        ARMARX_CHECK(objectOpt);
 
        // load object
 
        armarx::armem::articulated_object::ArticulatedObject articulatedObject = *objectOpt;
 
        const std::string xmlFilename = articulatedObject.description.xml.toSystemPath();
        ARMARX_INFO << "Loading (virtual) robot '" << articulatedObject.description.name
                    << "' from XML file '" << xmlFilename << "'";
 
        auto object = VirtualRobot::RobotIO::loadRobot(
            xmlFilename, VirtualRobot::RobotIO::RobotDescription::eStructure);
        ARMARX_CHECK_NOT_NULL(object);
 
        object->setJointValues(articulatedObject.config.jointMap);
        object->setGlobalPose(articulatedObject.config.globalPose.matrix());
        object->setName(objectID.str()); // this is optional, name not used directly
 
        // obtain global pose of frame
 
        const auto node = object->getRobotNode(in.parameters.node);
        ARMARX_CHECK_NOT_NULL(node) << ("`" + in.parameters.node + "` node does not exist");
 
        const Eigen::Vector3f globalPose = node->getGlobalPosition();
 
        // generate view target
        const armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget = [&]()
        {
            armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget;
            gazeTarget.name =
                "LookAtArticulatedObjectFrame_" + objectID.str() + ":" + in.parameters.node;
 
            gazeTarget.position.header.agent = ""; // not needed
            gazeTarget.position.header.frame = armarx::GlobalFrame; // not needed
 
            gazeTarget.position.position = globalPose;
 
            gazeTarget.duration =
                armarx::core::time::Duration::SecondsDouble(in.parameters.durationSeconds);
 
            gazeTarget.priority.attentionType =
                gaze_targets::arondto::AttentionType::ImplEnum::TaskDriven;
            gazeTarget.priority.priority = in.parameters.priority;
 
            gazeTarget.creationTimestamp = Clock::Now();
            gazeTarget.keepInQueue = false;
 
            return gazeTarget;
        }();
 
        // execute subskill
        {
            using Parameters = armarx::view_selection::skills::arondto::LookAtParams;
 
            armarx::skills::SpecializedSkillProxy<Parameters> subSkill(
                manager,
                ::armarx::skills::SkillID{
                    .providerId =
                        ::armarx::skills::ProviderID{.providerName =
                                                         armarx::view_selection::skills::constants::
                                                             ViewSelectionSkillProviderName},
                    .skillName = armarx::view_selection::skills::constants::skill_names::LookAt});
            Parameters subSkillParams = subSkill.getRootProfileParameters();
 
            subSkillParams.target = gazeTarget;
 
            auto update = callSubskill(subSkill, subSkillParams.toAron());
 
            if (update.has_value() and not update->hasBeenSucceeded())
            {
                return MakeFailedResult();
            }
        }
 
        return MakeSucceededResult();
    }
 
    void
    LookAtArticulatedObjectFrame::onStopRequested()
    {
    }
 
    ::armarx::skills::SkillDescription
    LookAtArticulatedObjectFrame::DefaultSkillDescription()
    {
        Params defaults;
        defaults.priority = 100;
        defaults.objectPoseProvider = std::nullopt; // "any"
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::LookAtArticulatedObjectFrame,
            .description = "",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::Duration::Minutes(10),
            .parametersType = Params::ToAronType(),
            .resultType = Result::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills