diff --git a/CMakeLists.txt b/CMakeLists.txt
index 345e2d5070556001ed0f2b7c895ac5f678bb269a..284cf7acd46deb754da966d3d8b84afa08500b4f 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/source/armarx/control/CMakeLists.txt b/source/armarx/control/CMakeLists.txt
index 6c4dfbdb4b339daa6a43254f05bff3e0fd6f2221..6a153dd249ec17728d32e4734f1d77a256d8973e 100644
--- a/source/armarx/control/CMakeLists.txt
+++ b/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)
-
diff --git a/source/armarx/control/components/control_skill_provider/CMakeLists.txt b/source/armarx/control/components/control_skill_provider/CMakeLists.txt
index 5996bbdb04c7c3867edbe4271af7ee01cafaa3b1..895d107463135be98812dddd17ba9a6ea0c1a365 100644
--- a/source/armarx/control/components/control_skill_provider/CMakeLists.txt
+++ b/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
)
diff --git a/source/armarx/control/components/control_skill_provider/Component.cpp b/source/armarx/control/components/control_skill_provider/Component.cpp
index 870adff151685f83a4229b6f2dee5e635e315fea..7c2f9ceae6e69dc627391313008c42eae058d6a8 100644
--- a/source/armarx/control/components/control_skill_provider/Component.cpp
+++ b/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);
+ }
}
}
diff --git a/source/armarx/control/components/control_skill_provider/Component.h b/source/armarx/control/components/control_skill_provider/Component.h
index cad36113a39024cb8246f54f9958cf1f31f11a29..d4610c2700f89ba93dc0c87924b75f775453e02b 100644
--- a/source/armarx/control/components/control_skill_provider/Component.h
+++ b/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:
diff --git a/source/armarx/control/pointing/CMakeLists.txt b/source/armarx/control/pointing/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..33983f74805a12eb9e23ef3469483724e8418354
--- /dev/null
+++ b/source/armarx/control/pointing/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_subdirectory(constants)
+add_subdirectory(aron)
+add_subdirectory(core)
+add_subdirectory(skill_ids)
+add_subdirectory(skills)
diff --git a/source/armarx/control/pointing/aron/CMakeLists.txt b/source/armarx/control/pointing/aron/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..93f1e1e4dc69d6e0172ede4a66b7b4dc58667fd2
--- /dev/null
+++ b/source/armarx/control/pointing/aron/CMakeLists.txt
@@ -0,0 +1,4 @@
+armarx_add_aron_library(pointing_aron
+ ARON_FILES
+ PointingParams.xml
+)
diff --git a/source/armarx/control/pointing/aron/PointingParams.xml b/source/armarx/control/pointing/aron/PointingParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8b786295ccf73a27c221049362683b1e21dbe5c5
--- /dev/null
+++ b/source/armarx/control/pointing/aron/PointingParams.xml
@@ -0,0 +1,33 @@
+<?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>
diff --git a/source/armarx/control/pointing/constants/CMakeLists.txt b/source/armarx/control/pointing/constants/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6a21d03876098b56f2154e5c86ce170d0e084e80
--- /dev/null
+++ b/source/armarx/control/pointing/constants/CMakeLists.txt
@@ -0,0 +1,6 @@
+armarx_add_library(pointing_constants
+ SOURCES
+ constants.cpp
+ HEADERS
+ constants.h
+)
diff --git a/source/armarx/control/pointing/constants/constants.cpp b/source/armarx/control/pointing/constants/constants.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..767d21bce99716f000a3e1bfb740af9710739724
--- /dev/null
+++ b/source/armarx/control/pointing/constants/constants.cpp
@@ -0,0 +1,8 @@
+#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
diff --git a/source/armarx/control/pointing/constants/constants.h b/source/armarx/control/pointing/constants/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..426db639e01e28c2ef7dcde1d24806dd510a2d76
--- /dev/null
+++ b/source/armarx/control/pointing/constants/constants.h
@@ -0,0 +1,10 @@
+#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
diff --git a/source/armarx/control/pointing/core/CMakeLists.txt b/source/armarx/control/pointing/core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..debb915446663d656971a9dac512dbcd0942ed99
--- /dev/null
+++ b/source/armarx/control/pointing/core/CMakeLists.txt
@@ -0,0 +1,24 @@
+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
+)
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fca6f30d355454d66c2fe3838c97f2cff471204
--- /dev/null
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -0,0 +1,104 @@
+#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
diff --git a/source/armarx/control/pointing/core/Pointing.h b/source/armarx/control/pointing/core/Pointing.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca5af29ed1a636cc82c37117442cf0f14f0cf88f
--- /dev/null
+++ b/source/armarx/control/pointing/core/Pointing.h
@@ -0,0 +1,59 @@
+#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
diff --git a/source/armarx/control/pointing/core/PointingIK.cpp b/source/armarx/control/pointing/core/PointingIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31362109ed77de2c5dd956bbfd12cd650966d292
--- /dev/null
+++ b/source/armarx/control/pointing/core/PointingIK.cpp
@@ -0,0 +1,74 @@
+#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
diff --git a/source/armarx/control/pointing/core/PointingIK.h b/source/armarx/control/pointing/core/PointingIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..d69fd2c88c8305da60c20a5a0abf5b743d3cc15c
--- /dev/null
+++ b/source/armarx/control/pointing/core/PointingIK.h
@@ -0,0 +1,23 @@
+#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
diff --git a/source/armarx/control/pointing/core/Side.h b/source/armarx/control/pointing/core/Side.h
new file mode 100644
index 0000000000000000000000000000000000000000..8fed926c1c56d08e1a74a5088e75c5172fcdb32f
--- /dev/null
+++ b/source/armarx/control/pointing/core/Side.h
@@ -0,0 +1,11 @@
+#pragma once
+
+namespace armarx::control::pointing::core
+{
+ enum Side
+ {
+ LEFT,
+ RIGHT
+ };
+
+} // namespace armarx::control::pointing::core
diff --git a/source/armarx/control/pointing/pointing.cpp b/source/armarx/control/pointing/pointing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d87cfeb79910786609d02e571057c27db21bf4b
--- /dev/null
+++ b/source/armarx/control/pointing/pointing.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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
+{
+
+}
diff --git a/source/armarx/control/pointing/pointing.h b/source/armarx/control/pointing/pointing.h
new file mode 100644
index 0000000000000000000000000000000000000000..27de7a0c3c367364790c1b03e3f11c377c2a9719
--- /dev/null
+++ b/source/armarx/control/pointing/pointing.h
@@ -0,0 +1,45 @@
+/*
+ * 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:
+
+ };
+
+}
diff --git a/source/armarx/control/pointing/skill_ids/CMakeLists.txt b/source/armarx/control/pointing/skill_ids/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2cab8c40a65b8fffa95b85b57df4d218c2e9362a
--- /dev/null
+++ b/source/armarx/control/pointing/skill_ids/CMakeLists.txt
@@ -0,0 +1,14 @@
+armarx_add_library(pointing_skill_ids
+ SOURCES
+ skill_ids.cpp
+
+ HEADERS
+ skill_ids.h
+
+ DEPENDENCIES_PUBLIC
+ # RobotAPI
+ RobotAPISkills
+
+ #pointing
+ armarx_control::pointing_constants
+)
diff --git a/source/armarx/control/pointing/skill_ids/skill_ids.cpp b/source/armarx/control/pointing/skill_ids/skill_ids.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b3ed91f304ba20612ef8c1f12c1f855d2c2d02b
--- /dev/null
+++ b/source/armarx/control/pointing/skill_ids/skill_ids.cpp
@@ -0,0 +1,11 @@
+#include "skill_ids.h"
+
+#include <armarx/control/pointing/constants/constants.h>
+
+namespace armarx::control::pointing::skill_ids
+{
+ const armarx::skills::SkillID Pointing{
+ .skillName = constants::POINTING_SKILL_NAME // TODO provider id?
+ };
+
+} // namespace armarx::control::pointing::skill_ids
diff --git a/source/armarx/control/pointing/skill_ids/skill_ids.h b/source/armarx/control/pointing/skill_ids/skill_ids.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0a60170f0b8d4c9acc3090a88502692bcf3b622
--- /dev/null
+++ b/source/armarx/control/pointing/skill_ids/skill_ids.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/SkillID.h>
+
+namespace armarx::control::pointing::skill_ids
+{
+ extern const armarx::skills::SkillID Pointing;
+
+} // namespace armarx::control::pointing::skill_ids
diff --git a/source/armarx/control/pointing/skills/CMakeLists.txt b/source/armarx/control/pointing/skills/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a8b04636300bf9d799036177480c4ac630528d06
--- /dev/null
+++ b/source/armarx/control/pointing/skills/CMakeLists.txt
@@ -0,0 +1,22 @@
+armarx_add_library(pointing_skills
+ SOURCES
+ Pointing.cpp
+ aron_conversions.cpp
+
+ HEADERS
+ Pointing.h
+ aron_conversions.h
+
+ DEPENDENCIES_PUBLIC
+ # ArmarXCore
+ ArmarXCore
+
+ # RobotAPI
+ RobotAPISkills
+
+ # pointing
+ armarx_control::pointing_constants
+ armarx_control::pointing_aron
+ armarx_control::pointing_core
+ armarx_control::pointing_skill_ids
+)
diff --git a/source/armarx/control/pointing/skills/Pointing.cpp b/source/armarx/control/pointing/skills/Pointing.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e58943698450de02426c945c1e691098b77b53b
--- /dev/null
+++ b/source/armarx/control/pointing/skills/Pointing.cpp
@@ -0,0 +1,40 @@
+#include "Pointing.h"
+
+#include <armarx/control/pointing/skill_ids/skill_ids.h>
+
+#include "aron_conversions.h"
+
+namespace armarx::control::pointing::skills
+{
+
+ armarx::skills::SkillDescription
+ Pointing::GetSkillDescription()
+ {
+ ParamType defaultParams;
+ defaultParams.frameName = "root";
+ defaultParams.handShape = std::make_optional("Pointing");
+
+ return armarx::skills::SkillDescription{.skillId = skill_ids::Pointing,
+ .description = "Lets the robot point at something.",
+ .rootProfileDefaults = defaultParams.toAron(),
+ .timeout = armarx::core::time::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ Pointing::Pointing(const Remote& remote) : Base(GetSkillDescription()), remote_(remote)
+ {
+ }
+
+ Pointing::MainResult
+ Pointing::main(const SpecializedMainInput& in)
+ {
+ core::Pointing::Parameters parameters = fromAron(in.parameters);
+ impl_.emplace(remote_, parameters);
+ if (impl_->execute())
+ {
+ return MakeSucceededResult();
+ }
+ return MakeFailedResult();
+ }
+
+} // namespace armarx::control::pointing::skills
diff --git a/source/armarx/control/pointing/skills/Pointing.h b/source/armarx/control/pointing/skills/Pointing.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd1a0c9424a1dade0bff520fec4cf51db77a390c
--- /dev/null
+++ b/source/armarx/control/pointing/skills/Pointing.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+#include <armarx/control/pointing/aron/PointingParams.aron.generated.h>
+#include <armarx/control/pointing/core/Pointing.h>
+
+namespace armarx::control::pointing::skills
+{
+
+ // TODO compiler warning
+ class Pointing : public armarx::skills::SimpleSpecializedSkill<arondto::PointingParams>
+ {
+ public:
+ using Base = armarx::skills::SimpleSpecializedSkill<arondto::PointingParams>;
+
+ static armarx::skills::SkillDescription GetSkillDescription();
+
+ using Remote = core::Pointing::Remote;
+ Pointing(const Remote&);
+
+ Base::MainResult main(const SpecializedMainInput& in) override;
+
+ private:
+ Remote remote_;
+ std::optional<core::Pointing> impl_;
+ };
+
+} // namespace armarx::control::pointing::skills
diff --git a/source/armarx/control/pointing/skills/aron_conversions.cpp b/source/armarx/control/pointing/skills/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..47eee6e0031f25fbcafe861b62a19854c391071a
--- /dev/null
+++ b/source/armarx/control/pointing/skills/aron_conversions.cpp
@@ -0,0 +1,38 @@
+#include "aron_conversions.h"
+
+namespace armarx::control::pointing
+{
+ void
+ skills::fromAron(const arondto::Side& dto, core::Side& bo)
+ {
+ switch (dto.value)
+ {
+ case arondto::Side::Left:
+ bo = core::Side::LEFT;
+ break;
+ case arondto::Side::Right:
+ bo = core::Side::RIGHT;
+ break;
+ default:
+ ARMARX_ERROR << "Invalid side";
+ }
+ }
+
+ void
+ skills::fromAron(const arondto::PointingParams& dto, core::Pointing::Parameters& bo)
+ {
+ fromAron(dto.side, bo.side);
+ bo.frameName = dto.frameName; // TODO fromAron ueberall
+ bo.pointAt = dto.pointAt;
+ bo.handShape = dto.handShape;
+ }
+
+ core::Pointing::Parameters
+ skills::fromAron(const arondto::PointingParams& dto)
+ {
+ core::Pointing::Parameters bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+} // namespace armarx::control::pointing
diff --git a/source/armarx/control/pointing/skills/aron_conversions.h b/source/armarx/control/pointing/skills/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2790ec2600aff2d17166e8b7efd14160a9f0ff6e
--- /dev/null
+++ b/source/armarx/control/pointing/skills/aron_conversions.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <armarx/control/pointing/aron/PointingParams.aron.generated.h>
+#include <armarx/control/pointing/core/Pointing.h>
+#include <armarx/control/pointing/core/Side.h>
+
+namespace armarx::control::pointing::skills
+{
+
+ // TODO Conversions wo? typen wo?
+
+ void fromAron(const arondto::Side& dto, core::Side& bo);
+
+ void fromAron(const arondto::PointingParams& dto, core::Pointing::Parameters& bo);
+ core::Pointing::Parameters fromAron(const arondto::PointingParams& dto);
+
+} // namespace armarx::control::pointing::skills