Skip to content
Snippets Groups Projects
Commit 3b3e9e67 authored by Patrick Dormanns's avatar Patrick Dormanns
Browse files

error handling for FramdPosition-conversion

parent ac16076f
No related branches found
No related tags found
1 merge request!53Implement 'PointAt'-skill
...@@ -30,7 +30,7 @@ namespace armarx::control::pointing::core ...@@ -30,7 +30,7 @@ namespace armarx::control::pointing::core
VirtualRobot::RobotPtr robot = getRobot(); VirtualRobot::RobotPtr robot = getRobot();
Eigen::Vector3f target = params.target.toGlobalEigen(robot); Eigen::Vector3f target = toGlobal(params.target, robot);
visualizeTarget(target); visualizeTarget(target);
armarx::TrajectoryPtr traj = computeArmTrajectory(robot, params.side, target); armarx::TrajectoryPtr traj = computeArmTrajectory(robot, params.side, target);
...@@ -70,6 +70,18 @@ namespace armarx::control::pointing::core ...@@ -70,6 +70,18 @@ namespace armarx::control::pointing::core
return robot; 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 void
Pointing::visualizeTarget(const Eigen::Vector3f& target) Pointing::visualizeTarget(const Eigen::Vector3f& target)
{ {
......
...@@ -92,6 +92,8 @@ namespace armarx::control::pointing::core ...@@ -92,6 +92,8 @@ namespace armarx::control::pointing::core
private: private:
VirtualRobot::RobotPtr getRobot(); VirtualRobot::RobotPtr getRobot();
Eigen::Vector3f toGlobal(const armarx::FramedPosition&, VirtualRobot::RobotPtr);
void visualizeTarget(const Eigen::Vector3f& target); void visualizeTarget(const Eigen::Vector3f& target);
armarx::TrajectoryPtr armarx::TrajectoryPtr
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment