Merge branch 'master' of git.h2t.iar.kit.edu:sw/armarx/skills/view-selection

source/armarx/view_selection/components/gaze_scheduler/CMakeLists.txt

@@ -11,7 +11,7 @@ armarx_add_component(gaze_scheduler
         Component.cpp
     HEADERS
         Component.h
-    DEPENDENCIES_PUBLIC
+    DEPENDENCIES
         ArmarXCore
         # RobotAPI
         RobotAPICore

source/armarx/view_selection/components/view_selection_skill_provider/CMakeLists.txt

@@ -13,6 +13,7 @@ armarx_add_component(view_selection_skill_provider
         ArmarXCore
         # RobotAPI
         RobotAPIComponentPlugins
+        armem_objects
         # view_selection
         armarx_view_selection::skills
 )

source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp

@@ -25,6 +25,7 @@
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 #include <armarx/view_selection/skills/LookAt.h>
+#include <armarx/view_selection/skills/LookAtArticulatedObjectFrame.h>
 #include <armarx/view_selection/skills/LookAtHumanFace.h>
 #include <armarx/view_selection/skills/LookAtHumanHand.h>
 #include <armarx/view_selection/skills/LookDirection.h>
@@ -40,6 +41,7 @@ namespace armarx::view_selection::components::view_selection_skill_provider
     Component::Component() : viewSelection(memoryNameSystem(), getName())
     {
         addPlugin(virtualRobotReaderPlugin);
+        addPlugin(articulatedObjectReaderPlugin);
     }
 
     const std::string Component::defaultName =
@@ -193,7 +195,19 @@ namespace armarx::view_selection::components::view_selection_skill_provider
             addSkillFactory<skills::LookAt>(
                 skills::LookAt::Context{.viewSelectionClient = viewSelection});
         }
+
+        ARMARX_INFO << "add skill factories, line " << __LINE__;
+
+        // LookAtArticulatedObjectFrame
+        {
+            addSkillFactory<skills::LookAtArticulatedObjectFrame>(
+                skills::LookAtArticulatedObjectFrame::Services{
+                    .articulatedObjectReader = std::experimental::make_observer<
+                        ::armarx::armem::articulated_object::Reader>(
+                        &articulatedObjectReaderPlugin->get())});
+        }
         ARMARX_INFO << "add skill factories, line " << __LINE__;
+
         {
             addSkillFactory<skills::LookAtHumanFace>(
                 skills::LookAtHumanFace::Services{.memoryNameSystem = memoryNameSystem(),

source/armarx/view_selection/components/view_selection_skill_provider/Component.h

@@ -36,6 +36,7 @@
 #include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
 #include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 #include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
 
 #include <armarx/view_selection/client/ViewSelection.h>
 #include <armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.h>
@@ -157,6 +158,9 @@ namespace armarx::view_selection::components::view_selection_skill_provider
         ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
             nullptr;
         VirtualRobot::RobotPtr robot_;
+        
+        ReaderWriterPlugin<::armarx::armem::articulated_object::Reader>* articulatedObjectReaderPlugin =
+            nullptr;
 
         client::ViewSelection viewSelection;
     };

source/armarx/view_selection/skills/CMakeLists.txt

@@ -9,6 +9,7 @@ armarx_add_aron_library(skills_aron
         aron/LookAtHumanHand.xml
         aron/LookDirection.xml
         aron/SetCustomGazeTarget.xml
+        aron/LookAtArticulatedObjectFrame.xml
 )
 
 armarx_add_library(skills
@@ -24,6 +25,7 @@ armarx_add_library(skills
         LookAtHumanHand.cpp
         LookDirection.cpp
         SetCustomGazeTarget.cpp
+        LookAtArticulatedObjectFrame.cpp
     HEADERS
         constants.h
         skill_ids.h
@@ -36,6 +38,7 @@ armarx_add_library(skills
         LookAtHumanHand.h
         LookDirection.h
         SetCustomGazeTarget.h
+        LookAtArticulatedObjectFrame.h
     DEPENDENCIES_PUBLIC
         # ArmarXCore
         ArmarXCoreInterfaces
@@ -43,6 +46,7 @@ armarx_add_library(skills
         # RobotAPI
         RobotAPISkills
         RobotAPIArmarXObjects
+        armem_objects
 
         # view_selection
         armarx_view_selection::skills_aron

source/armarx/view_selection/skills/LookAtArticulatedObjectFrame.cpp

+#include "LookAtArticulatedObjectFrame.h"
+
+#include <Eigen/Core>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include "RobotAPI/libraries/skills/core/SkillID.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/skills/provider/SpecializedSkillProxy.h>
+
+#include "armarx/view_selection/skills/constants.h"
+#include <armarx/view_selection/skills/aron/LookAt.aron.generated.h>
+#include <armarx/view_selection/skills/skill_ids.h>
+
+#include "ScanLocation.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);
+        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

source/armarx/view_selection/skills/LookAtArticulatedObjectFrame.h

+/*
+ * 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     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2023
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+#include <armarx/view_selection/client/ViewSelection.h>
+#include <armarx/view_selection/skills/aron/LookAtArticulatedObjectFrame.aron.generated.h>
+// #include <armarx/view_selection/skills/aron/LookAtArticulatedObjectFrameResult.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
+
+namespace armarx::view_selection::skills
+{
+    class LookAtArticulatedObjectFrame :
+        virtual public armarx::skills::SimpleSpecializedSkill<arondto::LookAtArticulatedObjectFrameParams>
+    {
+    public:
+        using Params = arondto::LookAtArticulatedObjectFrameParams;
+        using Result = arondto::LookAtArticulatedObjectFrameParams;
+        using Base = ::armarx::skills::SimpleSpecializedSkill<Params>;
+
+        struct Services
+        {
+            std::experimental::observer_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+        };
+
+        LookAtArticulatedObjectFrame(const Services& srv);
+
+    private:
+        ::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
+        ::armarx::skills::Skill::MainResult main(const Base::SpecializedMainInput& in) override;
+
+        void onStopRequested() override;
+
+
+    public:
+        static armarx::skills::SkillDescription DefaultSkillDescription();
+
+    private:
+        Services srv_;
+    };
+
+} // namespace armarx::view_selection::skills

source/armarx/view_selection/skills/aron/LookAtArticulatedObjectFrame.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <AronIncludes>
+        <Include include="RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml" />
+    </AronIncludes>
+
+    <GenerateTypes>
+        <Object name='armarx::view_selection::skills::arondto::LookAtArticulatedObjectFrameParams'>
+
+            <ObjectChild key='object'>
+                <armarx::arondto::ObjectID />
+            </ObjectChild>
+
+            <ObjectChild key='node'>
+                <string />
+            </ObjectChild>
+
+            <ObjectChild key='objectPoseProvider'>
+                <string optional="true" />
+            </ObjectChild>
+
+            <ObjectChild key='durationSeconds'>
+                <float64 />
+            </ObjectChild>
+
+            <ObjectChild key='priority'>
+                <float32 />
+            </ObjectChild>
+
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>

source/armarx/view_selection/skills/constants.cpp

@@ -34,6 +34,7 @@ namespace armarx::view_selection::skills::constants
         "LookForObjectsWithKinematicUnit";
     const std::string skill_names::LookForObjects = "LookForObjects";
     const std::string skill_names::LookAt = "LookAt";
+    const std::string skill_names::LookAtArticulatedObjectFrame = "LookAtArticulatedObjectFrame";
     const std::string skill_names::LookAtHumanFace = "LookAtHumanFace";
     const std::string skill_names::LookAtHumanHand = "LookAtHumanHand";
     const std::string skill_names::ScanLocation = "ScanLocation";

source/armarx/view_selection/skills/constants.h

@@ -32,6 +32,7 @@ namespace armarx::view_selection::skills::constants::skill_names
     extern const std::string LookForObjectsWithKinematicUnit;
     extern const std::string LookForObjects;
     extern const std::string LookAt;
+    extern const std::string LookAtArticulatedObjectFrame;
     extern const std::string LookAtHumanFace;
     extern const std::string LookAtHumanHand;
     extern const std::string ScanLocation;

source/armarx/view_selection/skills/skill_ids.cpp

@@ -40,6 +40,10 @@ namespace armarx::view_selection::skills
     const armarx::skills::SkillID skill_ids::LookAt{
         .providerId = ViewSelectionSkillProvider,
         .skillName = constants::skill_names::LookAt,
+    }; 
+    const armarx::skills::SkillID skill_ids::LookAtArticulatedObjectFrame{
+        .providerId = ViewSelectionSkillProvider,
+        .skillName = constants::skill_names::LookAtArticulatedObjectFrame,
     };
     const armarx::skills::SkillID skill_ids::ScanLocation{
         .providerId = ViewSelectionSkillProvider,

source/armarx/view_selection/skills/skill_ids.h

@@ -33,6 +33,7 @@ namespace armarx::view_selection::skills::skill_ids
     extern const armarx::skills::SkillID LookForObjects;
     extern const armarx::skills::SkillID LookAt;
     extern const armarx::skills::SkillID ScanLocation;
+    extern const armarx::skills::SkillID LookAtArticulatedObjectFrame;
 
     extern const armarx::skills::SkillID LookUp;
     extern const armarx::skills::SkillID LookDown;