3 #include <VirtualRobot/Robot.h>
17 goal_(goal), scene(scene), config(config), reachedCount(0)
28 const core::Pose world_T_robot(scene.robot->getGlobalPose());
31 const core::Pose robot_T_goal = world_T_robot.inverse() * world_T_goal;
35 const bool posReached = robot_T_goal.translation().head<2>().
norm() <= config.posTh;
36 const bool oriReached = Eigen::AngleAxisf(robot_T_goal.linear()).angle() <= config.oriTh;
40 ARMARX_DEBUG <<
"Position goal `" << robot_T_goal.translation().head<2>()
49 const bool linearVelocityLow = scene.platformVelocity.linear.norm() < config.linearVelTh;
50 const bool angularVelocityLow = scene.platformVelocity.angular.norm() < config.angularVelTh;
52 const bool velocityLow = linearVelocityLow and angularVelocityLow;
59 bool reached = posReached and oriReached and velocityLow;
64 ARMARX_DEBUG <<
"Goal reached! reachedCount: " << reachedCount;
65 return !filter || (reachedCount >= config.filterCount);
81 goal_{other.goal_}, scene{other.scene}, config(other.config)