diff --git a/source/armarx/control/pointing/aron/PointAtParams.xml b/source/armarx/control/pointing/aron/PointAtParams.xml index 8d883275fde01a673a3a6f4d87f15649c6ad30f0..c700b4e2ec5ae34b85eeb5bfb926fa28661a9895 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 7352a446d0e65af31d8f26ffcb1a34e39609ef24..d39ff8b934cd453830ed075d8624e88efaf21016 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 bf34712a33735c3b0b0daf05a786b0f393f32c86..bd03e63a3b94d34f02c36d4e46e8c2892149a2be 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 bc5ab4727bd630175001688b49e27e98269bea51..fbe895cfe32e67c0e7d7a2c0f3d8b112a26c069c 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 491de2ba0f28b70cd36152a9458820c984d9c7c1..4aad0a3a55d94ca153c0b75b55131098fbb6652a 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);