working

CMakeLists.txt

@@ -18,6 +18,7 @@ armarx_find_package(PUBLIC Qt5 COMPONENTS Xml Script Designer QUIET)
 # Specify each ArmarX Package dependency with the following macro
 armarx_find_package(PUBLIC ArmarXGui)
 armarx_find_package(PUBLIC RobotAPI REQUIRED)
+armarx_find_package(PUBLIC SimoxControl REQUIRED)  # TODO QUIET wie in OML? Dep auf SIMOX?
 
 # Optional dependencies
 armarx_find_package(PUBLIC DMP QUIET)

source/armarx/control/CMakeLists.txt

@@ -19,6 +19,7 @@ add_subdirectory(client)
 
 # Skills
 add_subdirectory(skills)
+add_subdirectory(pointing)
 
 # UI.
 add_subdirectory(ui)
@@ -26,4 +27,3 @@ add_subdirectory(ui)
 # Components
 add_subdirectory(retrieve_hand)
 add_subdirectory(components)
-

source/armarx/control/components/control_skill_provider/CMakeLists.txt

@@ -25,4 +25,5 @@ armarx_add_component(control_skill_provider
         # armarx_control
         armarx_control::skills_constants
         armarx_control::skills
+        armarx_control::pointing_skills
 )

source/armarx/control/components/control_skill_provider/Component.cpp

@@ -2,6 +2,7 @@
 
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
+#include <armarx/control/pointing/skills/Pointing.h>
 #include <armarx/control/skills/skills/MoveJointsToPosition.h>
 #include <armarx/control/skills/skills/MoveJointsWithVelocity.h>
 #include <armarx/control/skills/skills/RelaxHand.h>
@@ -77,6 +78,11 @@ namespace armarx::control::components::control_skill_provider
                     .robotUnitPlugin_ = remote.robotUnitPlugin};
                 skillBlueprints.zeroTorque->connectTo(context);
             }
+            {
+                armarx::control::pointing::skills::Pointing::Remote remote{
+                    properties.robotName, arviz, memoryNameSystem(), getTrajectoryPlayer()};
+                addSkillFactory<armarx::control::pointing::skills::Pointing>(remote);
+            }
         }
     }
 

source/armarx/control/components/control_skill_provider/Component.h

@@ -4,7 +4,9 @@
 
 #include <RobotAPI/interface/units/HandUnitInterface.h>
 #include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h>
 #include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
 #include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
 #include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
@@ -20,7 +22,9 @@ namespace armarx::control::components::control_skill_provider
         virtual public ::armarx::Component,
         virtual public ::armarx::control::components::control_skill_provider::ComponentInterface,
         virtual public ::armarx::SkillProviderComponentPluginUser,
-        virtual public ::armarx::armem::client::PluginUser
+        virtual public ::armarx::armem::client::PluginUser,
+        virtual public ::armarx::ArVizComponentPluginUser,
+        virtual public ::armarx::TrajectoryPlayerComponentPluginUser
     {
 
     public:

source/armarx/control/pointing/CMakeLists.txt

+add_subdirectory(constants)
+add_subdirectory(aron)
+add_subdirectory(core)
+add_subdirectory(skill_ids)
+add_subdirectory(skills)

source/armarx/control/pointing/aron/CMakeLists.txt

+armarx_add_aron_library(pointing_aron
+    ARON_FILES
+        PointingParams.xml
+)

source/armarx/control/pointing/aron/PointingParams.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <GenerateTypes>
+
+        <IntEnum name="::armarx::control::pointing::arondto::Side">
+            <EnumValue key="Left" value="0"/>
+            <EnumValue key="Right" value="1"/>
+        </IntEnum>
+
+
+        <!-- TODO documentation -->
+        <Object name="::armarx::control::pointing::arondto::PointingParams">
+
+            <ObjectChild key="side">
+                <::armarx::control::pointing::arondto::Side />
+            </ObjectChild>
+
+            <ObjectChild key="frameName">
+                <string />
+            </ObjectChild>
+
+            <ObjectChild key="pointAt">
+                <position />
+            </ObjectChild>
+
+            <ObjectChild key="handShape">
+                <string optional="true" />
+            </ObjectChild>
+
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition>

source/armarx/control/pointing/constants/CMakeLists.txt

+armarx_add_library(pointing_constants
+    SOURCES
+        constants.cpp
+    HEADERS
+        constants.h
+)

source/armarx/control/pointing/constants/constants.cpp

+#include "constants.h"
+
+namespace armarx::control::pointing
+{
+    const std::string constants::ARVIZ_LAYER_NAME = "Pointing";
+    const std::string constants::POINTING_SKILL_NAME = "Pointing";
+
+} // namespace armarx::control::pointing

source/armarx/control/pointing/constants/constants.h

+#pragma once
+
+#include <string>
+
+namespace armarx::control::pointing::constants
+{
+    extern const std::string ARVIZ_LAYER_NAME;
+    extern const std::string POINTING_SKILL_NAME;
+
+} // namespace armarx::control::pointing::constants

source/armarx/control/pointing/core/CMakeLists.txt

+armarx_add_library(pointing_core
+    SOURCES
+        PointingIK.cpp
+        Pointing.cpp
+
+    HEADERS
+        Side.h
+        PointingIK.h
+        Pointing.h
+
+    DEPENDENCIES_PUBLIC
+        ArmarXCore
+        RobotAPIInterfaces
+        ArViz
+
+        # TODO Simox VirtualRobot?
+        # ArmarXCoreInterfaces?
+
+        armem_robot_state
+
+        SimoxControl::example_method # namespace??? Armar6-specific right-arm stuff in there?
+
+        pointing_constants
+)

source/armarx/control/pointing/core/Pointing.cpp

+#include "Pointing.h"
+
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#include <armarx/control/pointing/constants/constants.h>
+
+#include "PointingIK.h"
+
+namespace armarx::control::pointing::core
+{
+    Pointing::Pointing(const Remote& remote, const Parameters& properties) :
+        remote_(remote), parameters_(properties)
+    {
+    }
+
+    bool
+    Pointing::execute()
+    {
+        remote_.trajectoryPlayer->stopTrajectoryPlayer();
+
+        // load robot
+        armarx::armem::robot_state::VirtualRobotReader robotReader;
+        robotReader.connect(remote_.memoryNameSystem);
+
+        auto robot = robotReader.getSynchronizedRobot(remote_.robotName);
+        if (not robot)
+        {
+            ARMARX_ERROR << "Could not get synchronized robot '" << remote_.robotName << "'";
+            return false;
+        }
+
+        // get target
+        Eigen::Vector3f target;
+
+        auto node = robot->getRobotNode(parameters_.frameName); // TODO var name
+        if (not node)
+        {
+            ARMARX_ERROR << "Could not get robot node '" << parameters_.frameName
+                         << "'"; // TODO "robot node for frame"
+        }
+        target = node->toGlobalCoordinateSystemVec(parameters_.pointAt);
+
+        ARMARX_INFO << "Pointing at " << parameters_.pointAt << " in frame '"
+                    << parameters_.frameName << "'";
+
+
+        visualizeTarget(target);
+
+        auto ik = PointingIK(robot, parameters_.side);
+
+        auto trajectory =
+            ik.computeTrajectory(target); // TODO komische traj, weil wir die aus der ik nutzen
+
+        if (not trajectory)
+        {
+            ARMARX_WARNING << " Target cannot be reached";
+            return false;
+        }
+
+        if (trajectory->size() == 1) // TODO handshape
+        {
+            ARMARX_INFO << "Already pointing a target";
+            return true;
+        }
+
+        trajectory = trajectory->calculateTimeOptimalTrajectory( // TODO failed mit size == 1
+            0.4,
+            0.3,
+            0.0,
+            IceUtil::Time::milliSeconds(10)); // TODO memory leak   // TODO parameter
+
+        if (!remote_.trajectoryPlayer)
+        {
+            ARMARX_ERROR << "TrajectoryPlayerProxy is invalid";
+        }
+        remote_.trajectoryPlayer->loadJointTraj(trajectory);
+
+        ARMARX_INFO << "Starting trajectory (" << trajectory->getTimeLength() << "s)";
+        remote_.trajectoryPlayer->startTrajectoryPlayer();
+
+
+        // TODO wrist position (nicht knicken)
+
+        // TODO wrist orientation (wie gedreht, abh. von shape?)
+
+        // TODO hands
+
+        // TODO blocking
+
+        return true;
+    }
+
+    void
+    Pointing::visualizeTarget(Eigen::Vector3f target)
+    {
+        auto layer = remote_.arviz.layer(constants::ARVIZ_LAYER_NAME);
+        layer.add(armarx::viz::Sphere("PointAt")
+                      .radius(50)
+                      .color(armarx::viz::Color::orange())
+                      .position(target));
+        remote_.arviz.commit(layer);
+    }
+
+} // namespace armarx::control::pointing::core

source/armarx/control/pointing/core/Pointing.h

+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include "Side.h"
+
+namespace VirtualRobot
+{
+    class Robot;
+    using RobotPtr = std::shared_ptr<Robot>;
+} // namespace VirtualRobot
+
+namespace simox::control::method::example
+{
+    class PointingIK;
+}
+
+namespace armarx::control::pointing::core
+{
+
+    /**
+     * @class Pointing
+     * @brief Implementation of the 'Pointing'-skill.
+     * @ingroup TODO
+     */
+    class Pointing
+    {
+    public:
+        struct Remote
+        {
+            std::string robotName;
+            armarx::viz::Client arviz;
+            armarx::armem::client::MemoryNameSystem memoryNameSystem;
+            armarx::TrajectoryPlayerInterfacePrx trajectoryPlayer;
+        };
+
+        struct Parameters
+        {
+            Side side;
+            std::string frameName;
+            Eigen::Vector3f pointAt;
+            std::optional<std::string> handShape;
+        };
+
+        Pointing(const Remote&, const Parameters&);
+
+        bool execute();
+
+    private:
+        void visualizeTarget(Eigen::Vector3f target);
+
+    private:
+        Remote remote_;
+        Parameters parameters_;
+    };
+
+} // namespace armarx::control::pointing::core

source/armarx/control/pointing/core/PointingIK.cpp

+#include "PointingIK.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx::control::pointing::core
+{
+
+    PointingIK::PointingIK(VirtualRobot::RobotPtr robot, Side side)
+    {
+        auto humanMapping = robot->getHumanMapping();
+        ARMARX_CHECK(humanMapping.has_value())
+            << "Robot '" << robot->getName() << "' has no human mapping";
+
+        auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
+
+        auto elbow = armDescription.segments.elbow.nodeName;
+        auto wrist = armDescription.segments.wrist.nodeName;
+
+        auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
+        auto joints = nodeSetArm->getNodeNames();
+        joints.resize(nodeSetArm->getRobotNodeIndex(wrist));
+
+
+        reducedRobot_ = std::make_unique<simox::control::simox::robot::Robot>(
+            robot,
+            joints,
+            std::vector(
+                {wrist})); // TODO true flag for reduced -> fueht zu self coll? (auch ohne ...)
+
+        ik_ = std::make_unique<simox::control::method::example::PointingIK>(
+            *reducedRobot_,
+            *reducedRobot_->getNode(
+                wrist), // TODO TCP instead of wrist? IK parameter is named "wrist"...
+            *reducedRobot_->getNode(elbow),
+            side == LEFT ? reducedRobot_->humanMapping()->leftArm
+                         : reducedRobot_->humanMapping()->rightArm);
+    }
+
+    TrajectoryPtr
+    PointingIK::computeTrajectory(Eigen::Vector3f& target) // self coll
+    {
+        ik_->setPointingTarget(target.cast<double>() / 1000); // simox uses meters
+
+        // #joints x #timestamps
+        std::vector<std::vector<double>> data(ik_->getNodeNames().size());
+        auto appendPositions = [&data](Eigen::VectorXd pos)
+        {
+            for (size_t i = 0; i < data.size(); i++)
+            {
+                data[i].push_back(pos[i]);
+            }
+        };
+        appendPositions(ik_->getJointValues());
+
+        Eigen::VectorXd prevJointValues = ik_->getJointValues();
+        while (not ik_->isTargetReached())
+        {
+            ik_->optimize();
+
+            if ((ik_->getJointValues() - prevJointValues).norm() < 1e-6)
+            {
+                return nullptr;
+            }
+            prevJointValues = ik_->getJointValues();
+
+            appendPositions(ik_->getJointValues());
+        }
+        return new Trajectory(data, Ice::DoubleSeq(), ik_->getNodeNames()); // TODO memory leak?
+    }
+
+} // namespace armarx::control::pointing::core

source/armarx/control/pointing/core/PointingIK.h

+#pragma once
+
+#include <RobotAPI/libraries/core/Trajectory.h>
+
+#include "Side.h"
+#include <simox/control/impl/simox/robot/Robot.h>
+#include <simox/example/method/PointingIK.h>
+
+namespace armarx::control::pointing::core
+{
+
+    class PointingIK
+    {
+    public:
+        PointingIK(VirtualRobot::RobotPtr robot, Side side);
+        TrajectoryPtr computeTrajectory(Eigen::Vector3f& target);
+
+    private:
+        std::unique_ptr<simox::control::simox::robot::Robot> reducedRobot_;
+        std::unique_ptr<simox::control::method::example::PointingIK> ik_;
+    };
+
+} // namespace armarx::control::pointing::core

source/armarx/control/pointing/core/Side.h

+#pragma once
+
+namespace armarx::control::pointing::core
+{
+    enum Side
+    {
+        LEFT,
+        RIGHT
+    };
+
+} // namespace armarx::control::pointing::core

source/armarx/control/pointing/pointing.cpp

+/*
+ * 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/>.
+ *
+ * @package    control::ArmarXObjects::pointing
+ * @author     Patrick Dormanns ( patrick dot dormanns at student dot kit dot edu )
+ * @date       2024
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "pointing.h"
+
+namespace armarx::control::pointing
+{
+
+}

source/armarx/control/pointing/pointing.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/>.
+ *
+ * @package    control::ArmarXObjects::pointing
+ * @author     Patrick Dormanns ( patrick dot dormanns at student dot kit dot edu )
+ * @date       2024
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx::control::pointing
+{
+    /**
+    * @defgroup Library-pointing pointing
+    * @ingroup control
+    * A description of the library pointing.
+    *
+    * @class pointing
+    * @ingroup Library-pointing
+    * @brief Brief description of class pointing.
+    *
+    * Detailed description of class pointing.
+    */
+    class pointing
+    {
+    public:
+
+    };
+
+}

source/armarx/control/pointing/skill_ids/CMakeLists.txt

+armarx_add_library(pointing_skill_ids
+    SOURCES
+        skill_ids.cpp
+
+    HEADERS
+        skill_ids.h
+
+    DEPENDENCIES_PUBLIC
+        # RobotAPI
+        RobotAPISkills
+
+        #pointing
+        armarx_control::pointing_constants
+)