Merge branch 'feature/motion-planning' into 'main'

Feature/motion planning

See merge request !15

source/armarx/manipulation/components/articulated_object_augmented_visualization/CMakeLists.txt

@@ -2,8 +2,10 @@
 armarx_add_component(articulated_object_augmented_visualization
     SOURCES
         Component.cpp
+        util.cpp
     HEADERS
         Component.h
+        util.h
     DEPENDENCIES
         # ArmarXCore
         ArmarXCore

source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp

@@ -33,6 +33,7 @@
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/XML/RobotIO.h>
 
+#include "ArmarXCore/core/PackagePath.h"
 #include "ArmarXCore/core/exceptions/LocalException.h"
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include "ArmarXCore/core/logging/Logging.h"
@@ -51,6 +52,7 @@
 #include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
 #include "RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h"
 
+#include "armarx/manipulation/components/articulated_object_augmented_visualization/util.h"
 #include "armarx/manipulation/core/types.h"
 #include "armarx/manipulation/geometric_planning/ArVizDrawer.h"
 
@@ -97,7 +99,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
             ARMARX_VERBOSE << "Found the following robot descriptions: ";
             for (const auto& description : descriptions)
             {
-                ARMARX_VERBOSE << "Loading robot `" << description.name << "` from "
+                ARMARX_IMPORTANT << "Loading robot `" << description.name << "` from "
                                << description.xml;
                 const auto robot = VirtualRobot::RobotIO::loadRobot(
                     description.xml.toSystemPath(),
@@ -338,7 +340,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
 
         if (robots.count(grasp->getRobotType()) == 0)
         {
-            ARMARX_WARNING << "Cannot draw grasp. Unknown robot `" << grasp->getRobotType() << "`";
+            ARMARX_VERBOSE << "Cannot draw grasp. Unknown robot `" << grasp->getRobotType() << "`";
             return;
         }
 
@@ -356,6 +358,8 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         {
             Eigen::Isometry3f tcp_T_hand_root;
             std::map<std::string, float> jointValues;
+
+            PackagePath modelPackagePath;
         };
 
         const Info info = [&]()
@@ -369,39 +373,39 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
             const std::string handRootNodeName = "Hand " + side.substr(0, 1) + " Root";
 
             const auto eefName = arm.getEndEffector();
-
             const auto eef = robot->getEndEffector(eefName);
 
             ARMARX_VERBOSE << "preshape: " << grasp->getPreshapeName();
 
             const auto preshape = eef->getPreshape(grasp->getPreshapeName());
 
-            Info foo;
-
+            std::map<std::string, float> jointValues;
             if (preshape != nullptr)
             {
-                foo.jointValues = preshape->getRobotNodeJointValueMap();
-                ARMARX_VERBOSE << "preshape with joint values " << foo.jointValues;
+                jointValues = preshape->getRobotNodeJointValueMap();
+                ARMARX_VERBOSE << "preshape with joint values " << info.jointValues;
             }
 
             ARMARX_TRACE;
             const Eigen::Isometry3f tcp_T_hand_root(
                 robotArm.getTCP()->getPoseInRootFrame().inverse() *
                 robot->getRobotNode(handRootNodeName)->getPoseInRootFrame());
-            foo.tcp_T_hand_root = tcp_T_hand_root;
 
-            return foo;
+            return Info
+            {
+                .tcp_T_hand_root = tcp_T_hand_root,
+                .jointValues = jointValues,
+                .modelPackagePath = PackagePath(arm.getHandModelPackage(), arm.getHandModelPath())
+            };
         }();
 
         ARMARX_TRACE;
 
-        std::string modelFile = "armar6_rt/robotmodel/Armar6-SH/Armar6-" + side + "Hand-v3.xml";
-
 
         ARMARX_TRACE;
         const auto vizElement =
             viz::Robot(side + "/" + grasp->getName())
-                .file("armar6_rt", modelFile)
+                .file(info.modelPackagePath.serialize().package, info.modelPackagePath.serialize().path)
                 .pose(Eigen::Matrix4f{(global_T_tcp * info.tcp_T_hand_root).matrix()})
                 .joints(info.jointValues)
                 .overrideColor(simox::Color::azure());

source/armarx/manipulation/components/articulated_object_augmented_visualization/util.cpp

+#include "util.h"
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+namespace armarx::manipulation::util
+{
+    armarx::PackagePath
+    robotPackagePath(const std::string& robotAbsFilename)
+    {
+        const std::vector<std::string> packages =
+            armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+        const std::string package = armarx::ArmarXDataPath::getProject(packages, robotAbsFilename);
+
+        ARMARX_CHECK_NOT_EMPTY(package) << robotAbsFilename;
+
+        // make sure that the relative path is without the 'package/' prefix
+        const std::string robotFileRelPath = [&robotAbsFilename, &package]() -> std::string
+        {
+            if (simox::alg::starts_with(robotAbsFilename, package))
+            {
+                // remove "package" + "/"
+                const std::string fixedRobotFilename =
+                    robotAbsFilename.substr(package.size() + 1, -1);
+                return fixedRobotFilename;
+            }
+
+            return robotAbsFilename;
+        }();
+
+        return {package, robotFileRelPath};
+    }
+} // namespace armarx::robot_placement::util

source/armarx/manipulation/components/articulated_object_augmented_visualization/util.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 "ArmarXCore/core/PackagePath.h"
+ 
+ namespace armarx::manipulation::util
+ {
+     PackagePath robotPackagePath(const std::string& robotAbsFilename);
+
+ } // namespace armarx::robot_placement::nlp_wb_target

source/armarx/manipulation/core/Robot.h

@@ -59,7 +59,7 @@ namespace armarx::manipulation::core
             void
             registerPropertyDefinitions(const PropertyDefinitionsPtr& def)
             {
-                def->component(robotUnitPrx);
+                def->component(robotUnitPrx, "", "RobotUnitName");
 
                 def->component(handUnitLeft, "LeftHandUnit", "LeftHandUnit");
                 def->component(handUnitRight, "RightHandUnit", "RightHandUnit");

source/armarx/manipulation/skills/ReachGrasp.cpp

@@ -147,6 +147,33 @@ namespace armarx::manipulation::skills
         object->setName(objectID.str()); // this is optional, name not used directly
         object->applyJointValues();
 
+        for (const auto& environment : in.params.environment)
+        {
+            ObjectID environmentID;
+            fromAron(environment, environmentID);
+
+            const std::string objectName = environmentID.dataset() + "/" + environmentID.className();
+            ARMARX_INFO << "Searching for object with name `" << objectName << "`";
+
+            const auto isMatchingDescription = [&environmentID](const auto& description) -> bool
+            { return environmentID.equalClass(ObjectID(description.name)); };
+
+            const auto descriptionIt =
+                std::find_if(descriptions.begin(), descriptions.end(), isMatchingDescription);
+            ARMARX_CHECK(descriptionIt != descriptions.end())
+                << "Description " << environmentID << " not available";
+
+            ARMARX_INFO << "getting object";
+            std::optional<armarx::armem::articulated_object::ArticulatedObject> objectOpt =
+                ctx.articulatedObjectReader.get(*descriptionIt, Clock::Now(), environmentID.instanceName());
+            ARMARX_CHECK(objectOpt);
+
+            armarx::armem::articulated_object::ArticulatedObject articulatedObject = *objectOpt;
+            articulatedObject.instance = environmentID.instanceName();
+
+            this->environment[environmentID.str()] = articulatedObject;
+        }
+
         const std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
             graspInfo = simox::geometric_planning::findGraspByName(*object, in.params.graspSetName, in.params.graspName);
         ARMARX_CHECK(graspInfo.has_value()) << in.params.graspName << " not found";

source/armarx/manipulation/skills/ReachGrasp.h

@@ -119,6 +119,9 @@ namespace armarx::manipulation::skills
         Context ctx;
         Parameters parameters;
 
+        std::map<std::string, armarx::armem::articulated_object::ArticulatedObject> environment;
+
+
     private:
         VirtualRobot::RobotPtr object;
 

source/armarx/manipulation/skills/aron/ReachGraspParams.xml

@@ -28,6 +28,12 @@
                 <string />
             </ObjectChild>
 
+            <ObjectChild key='environment'>
+                <List>
+                    <armarx::arondto::ObjectID />
+                </List>
+            </ObjectChild>
+
         </Object>
     </GenerateTypes>
 </AronTypeDefinition>

source/armarx/manipulation/skills/constants.h

@@ -24,7 +24,7 @@
 namespace armarx::manipulation::skills::constants
 {
 
-    inline const std::string ActiculatedObjectsSkillProvider = "ActiculatedObjectsSkillProvider";
+    inline const std::string ActiculatedObjectsSkillProvider = "ArticulatedObjectsSkillProvider";
 
     inline const std::string Open = "Open";
     inline const std::string OpenDishwasherDoor = "OpenDishwasherDoor";