From 04528ae64379aeb3e1332a2beac7cc7930a02375 Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Sun, 25 Feb 2024 21:59:51 +0100
Subject: [PATCH] use armarx::FramedPosition for target
---
.../control/pointing/aron/PointAtParams.xml | 12 ++++--------
.../armarx/control/pointing/core/Pointing.cpp | 18 ++----------------
source/armarx/control/pointing/core/Pointing.h | 13 +++----------
.../armarx/control/pointing/skills/PointAt.cpp | 2 +-
.../pointing/skills/aron_conversions.cpp | 3 +--
5 files changed, 11 insertions(+), 37 deletions(-)
diff --git a/source/armarx/control/pointing/aron/PointAtParams.xml b/source/armarx/control/pointing/aron/PointAtParams.xml
index 8d883275..c700b4e2 100644
--- a/source/armarx/control/pointing/aron/PointAtParams.xml
+++ b/source/armarx/control/pointing/aron/PointAtParams.xml
@@ -2,6 +2,7 @@
<AronTypeDefinition>
<AronIncludes>
+ <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" />
<Include include="armarx/control/pointing/aron/Side.xml" />
</AronIncludes>
@@ -15,14 +16,9 @@
<::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 />
+ <!-- Position to point at -->
+ <ObjectChild key="target">
+ <armarx::arondto::FramedPosition />
</ObjectChild>
<!-- Name of the hand preshape to use. If none is provided, the hand will be
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index 7352a446..d39ff8b9 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -22,14 +22,14 @@ namespace armarx::control::pointing::core
aborted = false;
ARMARX_INFO << "Pointing with " << ((params.side == Side::LEFT) ? "left" : "right")
- << " arm at " << params.framedTarget << " in frame '" << params.frame << "'"
+ << " arm at " << params.target
<< (params.handShape ? " with hand shape '" + *params.handShape + "'" : "");
remote_.trajectoryPlayer->stopTrajectoryPlayer();
VirtualRobot::RobotPtr robot = getRobot();
- auto target = PositionInGlobalFrame(robot, params.frame, params.framedTarget);
+ Eigen::Vector3f target = params.target.toGlobalEigen(robot);
visualizeTarget(target);
armarx::TrajectoryPtr traj = computTrajectory(robot, params.side, target);
@@ -184,18 +184,4 @@ namespace armarx::control::pointing::core
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
index bf34712a..bd03e63a 100644
--- a/source/armarx/control/pointing/core/Pointing.h
+++ b/source/armarx/control/pointing/core/Pointing.h
@@ -5,6 +5,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include "Side.h"
@@ -36,11 +37,8 @@ namespace armarx::control::pointing::core
/** 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;
+ /** Position to point at. */
+ armarx::FramedPosition target;
/** Name of the hand preshape to use. If none is provided, the hand will be ignored. */
std::optional<std::string> handShape;
@@ -105,14 +103,9 @@ namespace armarx::control::pointing::core
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;
diff --git a/source/armarx/control/pointing/skills/PointAt.cpp b/source/armarx/control/pointing/skills/PointAt.cpp
index bc5ab472..fbe895cf 100644
--- a/source/armarx/control/pointing/skills/PointAt.cpp
+++ b/source/armarx/control/pointing/skills/PointAt.cpp
@@ -12,7 +12,7 @@ namespace armarx::control::pointing::skills
{
ParamType defaultParams;
defaultParams.side = core::Side::RIGHT;
- defaultParams.frame = "root";
+ defaultParams.target.header.frame = armarx::GlobalFrame;
defaultParams.handShape = std::make_optional("Pointing");
defaultParams.maxVelocity = 0.4;
defaultParams.maxAcceleration = 0.3;
diff --git a/source/armarx/control/pointing/skills/aron_conversions.cpp b/source/armarx/control/pointing/skills/aron_conversions.cpp
index 491de2ba..4aad0a3a 100644
--- a/source/armarx/control/pointing/skills/aron_conversions.cpp
+++ b/source/armarx/control/pointing/skills/aron_conversions.cpp
@@ -26,8 +26,7 @@ namespace armarx::control::pointing
skills::fromAron(const arondto::PointAtParams& dto, core::Pointing::Parameters& bo)
{
fromAron(dto.side, bo.side);
- armarx::aron::fromAron(dto.frame, bo.frame);
- armarx::aron::fromAron(dto.framedTarget, bo.framedTarget);
+ armarx::aron::fromAron(dto.target, bo.target);
armarx::aron::fromAron(dto.handShape, bo.handShape);
armarx::aron::fromAron(dto.maxVelocity, bo.maxVelocity);
armarx::aron::fromAron(dto.maxAcceleration, bo.maxAcceleration);
--
GitLab