diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp index e9824dacfeaddcbc4612a0039808666e29490cdd..2da46f9542d1356e885546407e6f1352dd880fba 100644 --- a/source/armarx/control/pointing/core/Pointing.cpp +++ b/source/armarx/control/pointing/core/Pointing.cpp @@ -30,7 +30,7 @@ namespace armarx::control::pointing::core VirtualRobot::RobotPtr robot = getRobot(); - Eigen::Vector3f target = params.target.toGlobalEigen(robot); + Eigen::Vector3f target = toGlobal(params.target, robot); visualizeTarget(target); armarx::TrajectoryPtr traj = computeArmTrajectory(robot, params.side, target); @@ -70,6 +70,18 @@ namespace armarx::control::pointing::core return robot; } + Eigen::Vector3f + Pointing::toGlobal(const FramedPosition& framedPosition, VirtualRobot::RobotPtr robot) + { + if (not(framedPosition.frame == armarx::GlobalFrame || + robot->hasRobotNode(framedPosition.frame))) + { + ARMARX_ERROR << "'" << framedPosition.frame << "' is not a valid frame name"; + throw armarx::exceptions::local::PointingFailedException(); + } + return framedPosition.toGlobalEigen(robot); + } + void Pointing::visualizeTarget(const Eigen::Vector3f& target) { diff --git a/source/armarx/control/pointing/core/Pointing.h b/source/armarx/control/pointing/core/Pointing.h index c36d9b88d8ac80d123d3cde5e5c62cbc12a30588..3e035804b4b4f14193ef108fa72122500dcc381c 100644 --- a/source/armarx/control/pointing/core/Pointing.h +++ b/source/armarx/control/pointing/core/Pointing.h @@ -92,6 +92,8 @@ namespace armarx::control::pointing::core private: VirtualRobot::RobotPtr getRobot(); + Eigen::Vector3f toGlobal(const armarx::FramedPosition&, VirtualRobot::RobotPtr); + void visualizeTarget(const Eigen::Vector3f& target); armarx::TrajectoryPtr