diff --git a/CMakeLists.txt b/CMakeLists.txt
index 345e2d5070556001ed0f2b7c895ac5f678bb269a..659a7beb69a6629d6949dcecda36091389554c66 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,6 +29,7 @@ armarx_find_package(PUBLIC MATHLIB QUIET)
armarx_find_package(PUBLIC Eigen3 QUIET)
armarx_find_package(PUBLIC SOEM)
# armarx_find_package(PUBLIC Simox ${ArmarX_Simox_VERSION} QUIET)
+armarx_find_package(PUBLIC SimoxControl QUIET)
add_subdirectory(source/armarx/control)
add_subdirectory(examples)
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..1c791ea242dccbdf0db4ec08ea4bf1d7a4f24526
--- /dev/null
+++ b/source/armarx/control/pointing/aron/PointingParams.xml
@@ -0,0 +1,48 @@
+<?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>
+
+
+ <!-- 'ShapeHand'-skill parameters -->
+ <Object name="::armarx::control::pointing::arondto::PointingParams">
+
+ <!-- Side of the arm to point with -->
+ <ObjectChild key="side">
+ <::armarx::control::pointing::arondto::Side />
+ </ObjectChild>
+
+ <!-- Name of the robot node relative to whose frame 'framedTarget' is given -->
+ <ObjectChild key="frame">
+ <string />
+ </ObjectChild>
+
+ <!-- Position to point at in frame 'frame' -->
+ <ObjectChild key="framedTarget">
+ <position />
+ </ObjectChild>
+
+ <!-- Name of the hand preshape to use. If none is provided, the hand will be
+ ignored. -->
+ <ObjectChild key="handShape">
+ <string optional="true" />
+ </ObjectChild>
+
+ <!-- Maximum velocity -->
+ <ObjectChild key="maxVelocity">
+ <float64/>
+ </ObjectChild>
+
+ <!-- Maximum acceleration -->
+ <ObjectChild key="maxAcceleration">
+ <float64/>
+ </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..5a21b793d874d6bcce780bc31be2cde1dd95e359
--- /dev/null
+++ b/source/armarx/control/pointing/core/CMakeLists.txt
@@ -0,0 +1,31 @@
+armarx_add_library(pointing_core
+ SOURCES
+ PointingIK.cpp
+ Pointing.cpp
+
+ HEADERS
+ Side.h
+ PointingIK.h
+ Pointing.h
+
+ DEPENDENCIES_PUBLIC
+ # ArmarXCore
+ ArmarXCore
+
+ #RobotAPI
+ RobotAPICore
+ RobotAPIInterfaces
+ armem
+ armem_robot_state
+ ArViz
+
+ # Simox
+ VirtualRobot
+
+ # SimoxControl
+ RobotAPICore
+ SimoxControl::example_method
+
+ # pointing
+ 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..f10ca43c321c3d6189d5119144088162f7873ebf
--- /dev/null
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -0,0 +1,168 @@
+#include "Pointing.h"
+
+#include <ArmarXCore/core/time.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) : remote_(remote)
+ {
+ }
+
+ void
+ Pointing::execute(const Pointing::Parameters params)
+ {
+ aborted = false;
+
+ ARMARX_INFO << "Pointing with " << ((params.side == Side::LEFT) ? "left" : "right")
+ << " arm at " << params.framedTarget << " in frame '" << params.frame << "'"
+ << (params.handShape ? " with hand shape '" + *params.handShape + "'" : "");
+
+ remote_.trajectoryPlayer->stopTrajectoryPlayer();
+
+ VirtualRobot::RobotPtr robot = getRobot();
+
+ auto target = PositionInGlobalFrame(robot, params.frame, params.framedTarget);
+ visualizeTarget(target);
+
+ armarx::TrajectoryPtr traj = computTrajectory(robot, params.side, target);
+
+ if (params.handShape)
+ {
+ auto shapeJointValues = GetShapeJointValues(robot, params.side, *params.handShape);
+ addHandTrajectory(robot, traj, shapeJointValues);
+ }
+
+ traj = traj->calculateTimeOptimalTrajectory(
+ params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
+
+ playTrajectory(traj);
+ }
+
+ void
+ Pointing::abort()
+ {
+ aborted = true;
+ }
+
+ VirtualRobot::RobotPtr
+ Pointing::getRobot()
+ {
+ 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 << "'";
+ throw PointingFailedException();
+ }
+ return robot;
+ }
+
+ void
+ Pointing::visualizeTarget(const Eigen::Vector3f& target)
+ {
+ auto layer = remote_.arviz.layer(constants::ARVIZ_LAYER_NAME);
+ layer.add(armarx::viz::Sphere("Pointing target")
+ .radius(50)
+ .color(armarx::viz::Color::orange())
+ .position(target));
+ remote_.arviz.commit(layer);
+ }
+
+ TrajectoryPtr
+ Pointing::computTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target)
+ {
+ ARMARX_VERBOSE << "Computing trajectory...";
+ auto ik = PointingIK(robot, side);
+ armarx::TrajectoryPtr traj = ik.computeTrajectory(target);
+ if (not traj)
+ {
+ ARMARX_WARNING << "IK didn't find a solution";
+ throw PointingFailedException();
+ }
+ ARMARX_VERBOSE << "Done computing trajectory";
+
+ if (traj->getLength() == 1) // already pointing at target
+ {
+ return armarx::TrajectoryPtr(new Trajectory);
+ }
+ return traj;
+ }
+
+ void
+ Pointing::addHandTrajectory(VirtualRobot::RobotPtr robot,
+ armarx::TrajectoryPtr& traj,
+ const std::map<std::string, float>& shapeJointValues)
+ {
+ for (const auto& [jointName, targetValue] : shapeJointValues)
+ {
+ traj->addDimension({robot->getRobotNode(jointName)->getJointValue(), targetValue},
+ Ice::DoubleSeq(),
+ jointName);
+ }
+ }
+
+ void
+ Pointing::playTrajectory(armarx::TrajectoryPtr traj)
+ {
+ ARMARX_INFO << "Playing trajectory (" << traj->getTimeLength() << "s)";
+ remote_.trajectoryPlayer->loadJointTraj(traj);
+ remote_.trajectoryPlayer->startTrajectoryPlayer();
+
+
+ using namespace armarx::core::time;
+ DateTime waitUnil = DateTime::Now() + Duration::Seconds(traj->getTimeLength());
+
+ Clock clock;
+ while (DateTime::Now() < waitUnil)
+ {
+ if (aborted)
+ {
+ ARMARX_INFO << "Pointing aborted. Stopping trajectory";
+ remote_.trajectoryPlayer->stopTrajectoryPlayer();
+ throw PointingAbortedException();
+ }
+ clock.waitFor(Duration::MilliSeconds(10));
+ }
+ ARMARX_INFO << "Pointing target reached";
+ }
+
+ std::map<std::string, float>
+ Pointing::GetShapeJointValues(VirtualRobot::RobotPtr robot,
+ Side side,
+ const std::string& shapeName)
+ {
+ auto eefName = std::string("Hand_") + (side == Side::LEFT ? "L" : "R") + "_EEF"; // TODO
+ auto eef = robot->getEndEffector(eefName);
+
+ auto shape = eef->getPreshape(shapeName);
+ if (not shape)
+ {
+ ARMARX_ERROR << "Hand '" << eefName << "' has no shape '" << shape << "'";
+ throw PointingFailedException();
+ }
+ return shape->getRobotNodeJointValueMap();
+ }
+
+ Eigen::Vector3f
+ Pointing::PositionInGlobalFrame(VirtualRobot::RobotPtr robot,
+ const std::string& frame,
+ const Eigen::Vector3f& framedPosition)
+ {
+ auto node = robot->getRobotNode(frame);
+ if (not node)
+ {
+ ARMARX_ERROR << "Robot '" << robot->getName() << "' has no node '" << frame << "'";
+ throw PointingFailedException();
+ }
+ return node->toGlobalCoordinateSystemVec(framedPosition);
+ }
+
+} // 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..e3ef764b9a08f97ca805475b22d68e5c771a8599
--- /dev/null
+++ b/source/armarx/control/pointing/core/Pointing.h
@@ -0,0 +1,117 @@
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+
+#include "Side.h"
+
+namespace armarx::control::pointing::core
+{
+
+ /**
+ * @brief Implementation of a 'Pointing'-gesture.
+ *
+ * Lets the robot point somewhere by aligning the forearm with the passed target.
+ *
+ * @ingroup Library-pointing
+ */
+ class Pointing
+ {
+ public:
+ struct Remote
+ {
+ std::string robotName;
+ armarx::viz::Client arviz;
+ armarx::armem::client::MemoryNameSystem memoryNameSystem;
+ armarx::TrajectoryPlayerInterfacePrx trajectoryPlayer;
+ };
+
+ struct Parameters
+ {
+ /** Side of the arm to point with. */
+ Side side;
+
+ /** Name of the robot node relative to whose frame @ref framedTarget is given. */
+ std::string frame;
+
+ /** Position to point at in frame @ref frame. */
+ Eigen::Vector3f framedTarget;
+
+ /** Name of the hand preshape to use. If none is provided, the hand will be ignored. */
+ std::optional<std::string> handShape;
+
+ /** Maximum velocity */
+ double maxVelocity;
+
+ /** Maximum acceleration */
+ double maxAcceleration;
+ };
+
+ class PointingException : public std::exception
+ {
+ };
+
+ class PointingFailedException : public PointingException
+ {
+ };
+
+ class PointingAbortedException : public PointingException
+ {
+ };
+
+
+ public:
+ Pointing(const Remote& remote);
+
+ /**
+ * @brief Executes the pointing gesture.
+ *
+ * Let's the robot point at the passed target. Blocks unitl the target configuration is
+ * reached or the execution is aborted with @ref abort.
+ *
+ * @param params Parameterization of the pointing gesture.
+ *
+ * @throws PointingFailedException Thrown if the pointing gesture could not be executed.
+ * @throws Thrown when the execution is aborted with @ref abort before reaching the target
+ * configuration.
+ */
+ void execute(const Parameters params);
+
+ /**
+ * @brief Aborts a running execution.
+ */
+ void abort();
+
+
+ private:
+ VirtualRobot::RobotPtr getRobot();
+
+ void visualizeTarget(const Eigen::Vector3f& target);
+
+ armarx::TrajectoryPtr
+ computTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target);
+
+ void addHandTrajectory(VirtualRobot::RobotPtr robot,
+ armarx::TrajectoryPtr& traj,
+ const std::map<std::string, float>& shapeJointValues);
+
+ void playTrajectory(armarx::TrajectoryPtr traj);
+
+ static Eigen::Vector3f PositionInGlobalFrame(VirtualRobot::RobotPtr robot,
+ const std::string& frame,
+ const Eigen::Vector3f& framedTarget);
+
+ static std::map<std::string, float>
+ GetShapeJointValues(VirtualRobot::RobotPtr robot, Side side, const std::string& shapeName);
+
+
+ private:
+ Remote remote_;
+ std::atomic_bool aborted;
+ };
+
+} // 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..043f0c3f541cc411b4422538adf807d2520bb591
--- /dev/null
+++ b/source/armarx/control/pointing/core/PointingIK.cpp
@@ -0,0 +1,71 @@
+#include "PointingIK.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+namespace armarx::control::pointing::core
+{
+
+ PointingIK::PointingIK(VirtualRobot::RobotPtr robot, Side side)
+ {
+ auto humanMapping = robot->getHumanMapping();
+ ARMARX_CHECK(humanMapping) << "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 palm = armDescription.segments.palm.nodeName;
+ auto tcp = armDescription.segments.tcp.nodeName;
+
+ // actuated joints = all arm joints until (excluding) wrist
+ auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
+ auto actuatedJoints = nodeSetArm->getNodeNames();
+ actuatedJoints.resize(nodeSetArm->getRobotNodeIndex(wrist));
+
+ // reduced robot must contain shoulder, elbow, wrist and palm for IK to work
+ reducedRobot_ = std::make_unique<simox::control::simox::robot::Robot>(
+ robot, actuatedJoints, std::vector({wrist, palm, tcp}), true);
+
+ // use elbow - tcp (instead of wrist) as forearm to line up with the target
+ ik_ = std::make_unique<simox::control::method::example::PointingIK>(
+ *reducedRobot_,
+ *reducedRobot_->getNode(tcp),
+ *reducedRobot_->getNode(elbow),
+ side == LEFT ? reducedRobot_->humanMapping()->leftArm
+ : reducedRobot_->humanMapping()->rightArm);
+ }
+
+ TrajectoryPtr
+ PointingIK::computeTrajectory(Eigen::Vector3f& target)
+ {
+ ik_->setPointingTarget(target.cast<double>() / 1000); // simox uses meters
+
+ // actuated joints x timesteps
+ std::vector<std::vector<double>> configs(ik_->getNodeNames().size());
+ auto appendConfig = [&configs](Eigen::VectorXd config)
+ {
+ for (size_t i = 0; i < configs.size(); i++)
+ {
+ configs[i].push_back(config[i]);
+ }
+ };
+
+ appendConfig(ik_->getJointValues());
+
+ Eigen::VectorXd prevJointValues = ik_->getJointValues();
+ while (not ik_->isTargetReached())
+ {
+ ik_->optimize();
+ if ((ik_->getJointValues() - prevJointValues).norm() < 1e-6)
+ {
+ return nullptr;
+ }
+
+ appendConfig(ik_->getJointValues());
+ prevJointValues = ik_->getJointValues();
+ }
+
+ return new Trajectory(configs, Ice::DoubleSeq(), ik_->getNodeNames());
+ }
+
+} // 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..028e5b4b97dcf70250b25b1b6ec59f27307a7647
--- /dev/null
+++ b/source/armarx/control/pointing/core/PointingIK.h
@@ -0,0 +1,59 @@
+#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
+{
+
+ /**
+ * @brief IK to compute joint configurations for a pointing gesture.
+ *
+ * Wrapper for simox-control's method::example::PointingIK. Computes pointing gesture joint
+ * configurations that line up the forearm with the target. Uses the robot's human mapping to
+ * obtain relevant joints: All arm-joints until (exluding) the wrist are beeing used.
+ *
+ * @ingroup Library-pointing
+ */
+ class PointingIK
+ {
+ public:
+ /**
+ * @brief Initialize a new PointingIK.
+ * @param robot Robot model with current robot configuration to be used by the IK. Must
+ * provide a human mapping.
+ * @param side Side of the arm to point with.
+ */
+ PointingIK(VirtualRobot::RobotPtr robot, Side side);
+
+
+ /**
+ * @brief Compute a joint trajectory ending in a configuration in which the robot points in
+ * the direction of the passed target position.
+ *
+ * The initial timestep of the trajectory contains the current joint configuration of the
+ * robot passed to the constructor. The following timesteps contain the step-by-step
+ * optimized configurations until the end configuration is reached. The trajectory is
+ * constructed without timestamps, i.e., has a duration of 1s with equidistant timesteps.
+ *
+ * Neither the generated trajectory nor the end configuration are guaranteed to be self-
+ * collision-free. Furthermore, generated trajectories aren't efficient in reaching the end
+ * configuration.
+ *
+ * This method may only be called once to ensure a well-defined initial robot configuration.
+ *
+ * @param target Target position to point at in the global coordinate system (mm).
+ * @return The computed trajectory. Null if no solution was found. If the robot has already
+ * pointed at the target, the trajectory only contains the initial timestep.
+ */
+ 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..e4d2d1a76570d26a74199be238d13218a214554d
--- /dev/null
+++ b/source/armarx/control/pointing/core/Side.h
@@ -0,0 +1,16 @@
+#pragma once
+
+namespace armarx::control::pointing::core
+{
+ /**
+ * @brief Describes the side of an arm.
+ *
+ * @ingroup Library-pointing
+ */
+ 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..a7cb04b733200f58beb3fb92cf12481169280504
--- /dev/null
+++ b/source/armarx/control/pointing/pointing.cpp
@@ -0,0 +1,5 @@
+#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..8d5a2173fd1e0e14cdc1eef47e05aaafb333302d
--- /dev/null
+++ b/source/armarx/control/pointing/pointing.h
@@ -0,0 +1,11 @@
+#pragma once
+
+namespace armarx::control::pointing
+{
+ /**
+ * @defgroup Library-pointing pointing
+ * @ingroup control
+ *
+ * Let the robot point somewhere.
+ */
+}
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..427b208d45eacaca1330cea7d5f180402c119848
--- /dev/null
+++ b/source/armarx/control/pointing/skill_ids/skill_ids.cpp
@@ -0,0 +1,9 @@
+#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};
+
+} // 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..c222c742f3af8e2044c0dcf13b22c9edfbe4a0e8
--- /dev/null
+++ b/source/armarx/control/pointing/skills/Pointing.cpp
@@ -0,0 +1,63 @@
+#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.side = core::Side::RIGHT;
+ defaultParams.frame = "root";
+ defaultParams.handShape = std::make_optional("Pointing");
+ defaultParams.maxVelocity = 0.4;
+ defaultParams.maxAcceleration = 0.3;
+
+ 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_);
+ try
+ {
+ impl_->execute(parameters);
+ }
+ catch (core::Pointing::PointingFailedException&)
+ {
+ return MakeFailedResult();
+ }
+ catch (core::Pointing::PointingAbortedException&)
+ {
+ return MakeAbortedResult();
+ }
+ return MakeSucceededResult();
+ }
+
+ void
+ Pointing::onStopRequested()
+ {
+ impl_->abort();
+ }
+
+ void
+ Pointing::onTimeoutReached()
+ {
+ impl_->abort();
+ }
+
+} // 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..f2374e357a54c97868d918ce0a187b49ae4190e1
--- /dev/null
+++ b/source/armarx/control/pointing/skills/Pointing.h
@@ -0,0 +1,30 @@
+#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
+{
+ 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&);
+
+ MainResult main(const SpecializedMainInput& in) override;
+
+ void onStopRequested() override;
+ void onTimeoutReached() 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..d9e47d78f76c3427479aca4c3151ad4b42072137
--- /dev/null
+++ b/source/armarx/control/pointing/skills/aron_conversions.cpp
@@ -0,0 +1,40 @@
+#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.frame = dto.frame;
+ bo.framedTarget = dto.framedTarget;
+ bo.handShape = dto.handShape;
+ bo.maxVelocity = dto.maxVelocity;
+ bo.maxAcceleration = dto.maxAcceleration;
+ }
+
+ 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..95b98e7ec33eb4f2b80366b51926f4400e0ec991
--- /dev/null
+++ b/source/armarx/control/pointing/skills/aron_conversions.h
@@ -0,0 +1,14 @@
+#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
+{
+ 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