moveit2
The MoveIt Motion Planning Framework for ROS 2.
Classes | Functions
testutils Namespace Reference

Classes

struct  BlendTestData
 Test data for blending, which contains three joint position vectors of three robot state. More...
 

Functions

const std::string JOINT_NAME_PREFIX ("prbt_joint_")
 
std::string getJointName (size_t joint_number, const std::string &joint_prefix)
 
pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits (const std::vector< std::string > &joint_names)
 Create limits for tests to avoid the need to get the limits from the node parameter. More...
 
std::string demangle (char const *name)
 
sensor_msgs::msg::JointState generateJointState (const std::vector< double > &pos, const std::vector< double > &vel, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
sensor_msgs::msg::JointState generateJointState (const std::vector< double > &pos, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
moveit_msgs::msg::Constraints generateJointConstraint (const std::vector< double > &pos_list, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
bool getExpectedGoalPose (const moveit::core::RobotModelConstPtr &robot_model, const planning_interface::MotionPlanRequest &req, std::string &link_name, Eigen::Isometry3d &goal_pose_expect)
 Determines the goal position from the given request. TRUE if successful, FALSE otherwise. More...
 
void createDummyRequest (const moveit::core::RobotModelConstPtr &robot_model, const std::string &planning_group, planning_interface::MotionPlanRequest &req)
 create a dummy motion plan request with zero start state No goal constraint is given. More...
 
void createPTPRequest (const std::string &planning_group, const moveit::core::RobotState &start_state, const moveit::core::RobotState &goal_state, planning_interface::MotionPlanRequest &req)
 
bool isGoalReached (const trajectory_msgs::msg::JointTrajectory &trajectory, const std::vector< moveit_msgs::msg::JointConstraint > &goal, const double joint_position_tolerance, const double joint_velocity_tolerance=1.0e-6)
 check if the goal given in joint space is reached Only the last point in the trajectory is veryfied. More...
 
bool isGoalReached (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double pos_tolerance, const double orientation_tolerance)
 check if the goal given in cartesian space is reached Only the last point in the trajectory is veryfied. More...
 
bool isGoalReached (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double tolerance)
 
bool checkCartesianLinearity (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
 Check that given trajectory is straight line. More...
 
bool checkSLERP (const Eigen::Isometry3d &start_pose, const Eigen::Isometry3d &goal_pose, const Eigen::Isometry3d &wp_pose, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
 check SLERP. The orientation should rotate around the same axis linearly. More...
 
int getWayPointIndex (const robot_trajectory::RobotTrajectoryPtr &trajectory, const double time_from_start)
 get the waypoint index from time from start More...
 
bool checkJointTrajectory (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 check joint trajectory of consistency, position, velocity and acceleration limits More...
 
::testing::AssertionResult hasStrictlyIncreasingTime (const robot_trajectory::RobotTrajectoryPtr &trajectory)
 Checks that every waypoint in the trajectory has a non-zero duration between itself and its predecessor. More...
 
bool isTrajectoryConsistent (const trajectory_msgs::msg::JointTrajectory &trajectory)
 check if the sizes of the joint position/veloicty/acceleration are correct More...
 
bool isPositionBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Position Bounded More...
 
bool isVelocityBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Velocity Bounded More...
 
bool isAccelerationBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Acceleration Bounded More...
 
bool toTCPPose (const moveit::core::RobotModelConstPtr &robot_model, const std::string &link_name, const std::vector< double > &joint_values, geometry_msgs::msg::Pose &pose, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 compute the tcp pose from joint values More...
 
bool computeLinkFK (const moveit::core::RobotModelConstPtr &robot_model, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
 computeLinkFK More...
 
bool checkOriginalTrajectoryAfterBlending (const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const double time_tolerance)
 checkOriginalTrajectoryAfterBlending More...
 
bool checkBlendingJointSpaceContinuity (const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, double joint_velocity_tolerance, double joint_accleration_tolerance)
 check the blending result, if the joint space continuity is fulfilled More...
 
bool checkBlendingCartSpaceContinuity (const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const pilz_industrial_motion_planner::LimitsContainer &planner_limits)
 
bool checkThatPointsInRadius (const std::string &link_name, const double &r, Eigen::Isometry3d &circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res)
 Checks if all points of the blending trajectory lie within the blending radius. More...
 
void computeCartVelocity (const Eigen::Isometry3d &pose_1, const Eigen::Isometry3d &pose_2, double duration, Eigen::Vector3d &v, Eigen::Vector3d &w)
 compute Cartesian velocity More...
 
void getLinLinPosesWithoutOriChange (const std::string &frame_id, sensor_msgs::msg::JointState &initialJointState, geometry_msgs::msg::PoseStamped &p1, geometry_msgs::msg::PoseStamped &p2)
 Returns an initial joint state and two poses which can be used to perform a Lin-Lin movement. More...
 
void getOriChange (Eigen::Matrix3d &ori1, Eigen::Matrix3d &ori2)
 
void createFakeCartTraj (const robot_trajectory::RobotTrajectoryPtr &traj, const std::string &link_name, moveit_msgs::msg::RobotTrajectory &fake_traj)
 
geometry_msgs::msg::Quaternion fromEuler (double a, double b, double c)
 
bool getBlendTestData (const rclcpp::Node::SharedPtr &node, const size_t &dataset_num, const std::string &name_prefix, std::vector< BlendTestData > &datasets)
 fetch test datasets from node parameters More...
 
bool checkBlendResult (const pilz_industrial_motion_planner::TrajectoryBlendRequest &blend_req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &blend_res, const pilz_industrial_motion_planner::LimitsContainer &limits, double joint_velocity_tolerance, double joint_acceleration_tolerance, double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance)
 check the blending result of lin-lin More...
 
bool generateTrajFromBlendTestData (const planning_scene::PlanningSceneConstPtr &scene, const std::shared_ptr< pilz_industrial_motion_planner::TrajectoryGenerator > &tg, const std::string &group_name, const std::string &link_name, const BlendTestData &data, const double &sampling_time_1, const double &sampling_time_2, planning_interface::MotionPlanResponse &res_lin_1, planning_interface::MotionPlanResponse &res_lin_2, double &dis_lin_1, double &dis_lin_2)
 generate two LIN trajectories from test data set More...
 
void generateRequestMsgFromBlendTestData (const moveit::core::RobotModelConstPtr &robot_model, const BlendTestData &data, const std::string &planner_id, const std::string &group_name, const std::string &link_name, moveit_msgs::msg::MotionSequenceRequest &req_list)
 
void checkRobotModel (const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
 
::testing::AssertionResult hasTrapezoidVelocity (std::vector< double > accelerations, const double acc_tol)
 Check that a given vector of accelerations represents a trapezoid velocity profile. More...
 
::testing::AssertionResult checkCartesianTranslationalPath (const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
 Check that the translational path of a given trajectory has a trapezoid velocity profile. More...
 
::testing::AssertionResult checkCartesianRotationalPath (const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double rot_axis_tol=DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
 Check that the rotational path of a given trajectory is a quaternion slerp. More...
 
bool isMonotonouslyDecreasing (const std::vector< double > &vec, const double &tol)
 

Function Documentation

◆ checkBlendingCartSpaceContinuity()

bool testutils::checkBlendingCartSpaceContinuity ( const pilz_industrial_motion_planner::TrajectoryBlendRequest req,
const pilz_industrial_motion_planner::TrajectoryBlendResponse res,
const pilz_industrial_motion_planner::LimitsContainer planner_limits 
)

Definition at line 775 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkBlendingJointSpaceContinuity()

bool testutils::checkBlendingJointSpaceContinuity ( const pilz_industrial_motion_planner::TrajectoryBlendResponse res,
double  joint_velocity_tolerance,
double  joint_accleration_tolerance 
)

check the blending result, if the joint space continuity is fulfilled

Parameters
restrajectory blending response, contains three trajectories. Between these three trajectories should be continuous.
Returns
true if joint position/velocity is continuous. joint acceleration can have jumps.

Definition at line 671 of file test_utils.cpp.

Here is the caller graph for this function:

◆ checkBlendResult()

bool testutils::checkBlendResult ( const pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req,
const pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res,
const pilz_industrial_motion_planner::LimitsContainer limits,
double  joint_velocity_tolerance,
double  joint_acceleration_tolerance,
double  cartesian_velocity_tolerance,
double  cartesian_angular_velocity_tolerance 
)

check the blending result of lin-lin

Parameters
lin_res_1
lin_res_2
blend_req
blend_res
checkAcceleration

Definition at line 1117 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkCartesianLinearity()

bool testutils::checkCartesianLinearity ( const moveit::core::RobotModelConstPtr &  robot_model,
const trajectory_msgs::msg::JointTrajectory &  trajectory,
const planning_interface::MotionPlanRequest req,
const double  translation_norm_tolerance,
const double  rot_axis_norm_tolerance,
const double  rot_angle_tolerance = 10e-5 
)

Check that given trajectory is straight line.

Definition at line 222 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkCartesianRotationalPath()

testing::AssertionResult testutils::checkCartesianRotationalPath ( const robot_trajectory::RobotTrajectoryConstPtr &  trajectory,
const std::string &  link_name,
const double  rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE,
const double  acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE 
)

Check that the rotational path of a given trajectory is a quaternion slerp.

Parameters
rot_axis_toltolerance for comparing rotation axes (in the L2 norm)
acc_toltolerance for comparing angular acceleration values

Definition at line 1390 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkCartesianTranslationalPath()

testing::AssertionResult testutils::checkCartesianTranslationalPath ( const robot_trajectory::RobotTrajectoryConstPtr &  trajectory,
const std::string &  link_name,
const double  acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE 
)

Check that the translational path of a given trajectory has a trapezoid velocity profile.

Parameters
acc_toltolerance for comparing acceleration values

Definition at line 1361 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkJointTrajectory()

bool testutils::checkJointTrajectory ( const trajectory_msgs::msg::JointTrajectory &  trajectory,
const pilz_industrial_motion_planner::JointLimitsContainer joint_limits 
)

check joint trajectory of consistency, position, velocity and acceleration limits

Parameters
trajectory
joint_limits
Returns

Definition at line 460 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkOriginalTrajectoryAfterBlending()

bool testutils::checkOriginalTrajectoryAfterBlending ( const pilz_industrial_motion_planner::TrajectoryBlendRequest req,
const pilz_industrial_motion_planner::TrajectoryBlendResponse res,
const double  time_tolerance 
)

checkOriginalTrajectoryAfterBlending

Parameters
blend_res
traj_1_res
traj_2_res
time_tolerance
Returns

Definition at line 561 of file test_utils.cpp.

Here is the caller graph for this function:

◆ checkRobotModel()

void testutils::checkRobotModel ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::string &  group_name,
const std::string &  link_name 
)

Definition at line 1258 of file test_utils.cpp.

Here is the caller graph for this function:

◆ checkSLERP()

bool testutils::checkSLERP ( const Eigen::Isometry3d &  start_pose,
const Eigen::Isometry3d &  goal_pose,
const Eigen::Isometry3d &  wp_pose,
const double  rot_axis_norm_tolerance,
const double  rot_angle_tolerance = 10e-5 
)

check SLERP. The orientation should rotate around the same axis linearly.

Parameters
start_pose
goal_pose
wp_pose
rot_axis_norm_tolerance
Returns

Definition at line 292 of file test_utils.cpp.

Here is the caller graph for this function:

◆ checkThatPointsInRadius()

bool testutils::checkThatPointsInRadius ( const std::string &  link_name,
const double &  r,
Eigen::Isometry3d &  circ_pose,
const pilz_industrial_motion_planner::TrajectoryBlendResponse res 
)

Checks if all points of the blending trajectory lie within the blending radius.

Definition at line 929 of file test_utils.cpp.

Here is the caller graph for this function:

◆ computeCartVelocity()

void testutils::computeCartVelocity ( const Eigen::Isometry3d &  pose_1,
const Eigen::Isometry3d &  pose_2,
double  duration,
Eigen::Vector3d &  v,
Eigen::Vector3d &  w 
)

compute Cartesian velocity

Parameters
pose_1
pose_2
duration
v
w

Definition at line 946 of file test_utils.cpp.

Here is the caller graph for this function:

◆ computeLinkFK()

bool testutils::computeLinkFK ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::string &  link_name,
const std::map< std::string, double > &  joint_state,
Eigen::Isometry3d &  pose 
)

computeLinkFK

Parameters
robot_model
link_name
joint_state
pose
Returns

Definition at line 321 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createDummyRequest()

void testutils::createDummyRequest ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::string &  planning_group,
planning_interface::MotionPlanRequest req 
)

create a dummy motion plan request with zero start state No goal constraint is given.

Parameters
robot_model
planning_group
req

Definition at line 504 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createFakeCartTraj()

void testutils::createFakeCartTraj ( const robot_trajectory::RobotTrajectoryPtr &  traj,
const std::string &  link_name,
moveit_msgs::msg::RobotTrajectory &  fake_traj 
)

Definition at line 990 of file test_utils.cpp.

◆ createFakeLimits()

pilz_industrial_motion_planner::JointLimitsContainer testutils::createFakeLimits ( const std::vector< std::string > &  joint_names)

Create limits for tests to avoid the need to get the limits from the node parameter.

Definition at line 48 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createPTPRequest()

void testutils::createPTPRequest ( const std::string &  planning_group,
const moveit::core::RobotState start_state,
const moveit::core::RobotState goal_state,
planning_interface::MotionPlanRequest req 
)

Definition at line 516 of file test_utils.cpp.

Here is the call graph for this function:

◆ demangle()

std::string testutils::demangle ( char const *  name)
inline

Definition at line 91 of file test_utils.h.

Here is the caller graph for this function:

◆ fromEuler()

geometry_msgs::msg::Quaternion testutils::fromEuler ( double  a,
double  b,
double  c 
)
inline

Definition at line 378 of file test_utils.h.

◆ generateJointConstraint()

moveit_msgs::msg::Constraints testutils::generateJointConstraint ( const std::vector< double > &  pos_list,
const std::string &  joint_prefix = testutils::JOINT_NAME_PREFIX 
)
inline

Definition at line 129 of file test_utils.h.

Here is the call graph for this function:

◆ generateJointState() [1/2]

sensor_msgs::msg::JointState testutils::generateJointState ( const std::vector< double > &  pos,
const std::string &  joint_prefix = testutils::JOINT_NAME_PREFIX 
)
inline

Definition at line 122 of file test_utils.h.

Here is the call graph for this function:

◆ generateJointState() [2/2]

sensor_msgs::msg::JointState testutils::generateJointState ( const std::vector< double > &  pos,
const std::vector< double > &  vel,
const std::string &  joint_prefix = testutils::JOINT_NAME_PREFIX 
)
inline

Definition at line 100 of file test_utils.h.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ generateRequestMsgFromBlendTestData()

void testutils::generateRequestMsgFromBlendTestData ( const moveit::core::RobotModelConstPtr &  robot_model,
const BlendTestData data,
const std::string &  planner_id,
const std::string &  group_name,
const std::string &  link_name,
moveit_msgs::msg::MotionSequenceRequest &  req_list 
)

Definition at line 1197 of file test_utils.cpp.

Here is the call graph for this function:

◆ generateTrajFromBlendTestData()

bool testutils::generateTrajFromBlendTestData ( const planning_scene::PlanningSceneConstPtr &  scene,
const std::shared_ptr< pilz_industrial_motion_planner::TrajectoryGenerator > &  tg,
const std::string &  group_name,
const std::string &  link_name,
const BlendTestData data,
const double &  sampling_time_1,
const double &  sampling_time_2,
planning_interface::MotionPlanResponse res_lin_1,
planning_interface::MotionPlanResponse res_lin_2,
double &  dis_lin_1,
double &  dis_lin_2 
)

generate two LIN trajectories from test data set

Parameters
datacontains joint poisitons of start/mid/end states
sampling_time_1sampling time for first LIN
sampling_time_2sampling time for second LIN
[out]res_lin_1result of the first LIN motion planning
[out]res_lin_2result of the second LIN motion planning
[out]dis_lin_1translational distance of the first LIN
[out]dis_lin_2translational distance of the second LIN
Returns
true if succeed

Definition at line 1052 of file test_utils.cpp.

Here is the call graph for this function:

◆ getBlendTestData()

bool testutils::getBlendTestData ( const rclcpp::Node::SharedPtr &  node,
const size_t &  dataset_num,
const std::string &  name_prefix,
std::vector< BlendTestData > &  datasets 
)

fetch test datasets from node parameters

Parameters
nh
Returns

◆ getExpectedGoalPose()

bool testutils::getExpectedGoalPose ( const moveit::core::RobotModelConstPtr &  robot_model,
const planning_interface::MotionPlanRequest req,
std::string &  link_name,
Eigen::Isometry3d &  goal_pose_expect 
)

Determines the goal position from the given request. TRUE if successful, FALSE otherwise.

Definition at line 71 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getJointName()

std::string testutils::getJointName ( size_t  joint_number,
const std::string &  joint_prefix 
)
inline

Definition at line 80 of file test_utils.h.

Here is the caller graph for this function:

◆ getLinLinPosesWithoutOriChange()

void testutils::getLinLinPosesWithoutOriChange ( const std::string &  frame_id,
sensor_msgs::msg::JointState &  initialJointState,
geometry_msgs::msg::PoseStamped &  p1,
geometry_msgs::msg::PoseStamped &  p2 
)

Returns an initial joint state and two poses which can be used to perform a Lin-Lin movement.

Parameters
frame_id
initialJointStateAs cartesian position: (0.3, 0, 0.65, 0, 0, 0)
p1(0.05, 0, 0.65, 0, 0, 0)
p2(0.05, 0.4, 0.65, 0, 0, 0)

Definition at line 964 of file test_utils.cpp.

Here is the call graph for this function:

◆ getOriChange()

void testutils::getOriChange ( Eigen::Matrix3d &  ori1,
Eigen::Matrix3d &  ori2 
)

Definition at line 984 of file test_utils.cpp.

◆ getWayPointIndex()

int testutils::getWayPointIndex ( const robot_trajectory::RobotTrajectoryPtr &  trajectory,
const double  time_from_start 
)
inline

get the waypoint index from time from start

Parameters
trajectory
time_from_start
Returns

Definition at line 229 of file test_utils.h.

Here is the caller graph for this function:

◆ hasStrictlyIncreasingTime()

testing::AssertionResult testutils::hasStrictlyIncreasingTime ( const robot_trajectory::RobotTrajectoryPtr &  trajectory)

Checks that every waypoint in the trajectory has a non-zero duration between itself and its predecessor.

Usage in tests:

EXPECT_TRUE(HasStrictlyIncreasingTime(trajectory));

Definition at line 487 of file test_utils.cpp.

Here is the caller graph for this function:

◆ hasTrapezoidVelocity()

testing::AssertionResult testutils::hasTrapezoidVelocity ( std::vector< double >  accelerations,
const double  acc_tol 
)

Check that a given vector of accelerations represents a trapezoid velocity profile.

Parameters
acc_toltolerance for comparing acceleration values

Definition at line 1269 of file test_utils.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ isAccelerationBounded()

bool testutils::isAccelerationBounded ( const trajectory_msgs::msg::JointTrajectory &  trajectory,
const pilz_industrial_motion_planner::JointLimitsContainer joint_limits 
)

is Acceleration Bounded

Parameters
trajectory
joint_limits
Returns

Definition at line 393 of file test_utils.cpp.

Here is the caller graph for this function:

◆ isGoalReached() [1/3]

bool testutils::isGoalReached ( const moveit::core::RobotModelConstPtr &  robot_model,
const trajectory_msgs::msg::JointTrajectory &  trajectory,
const planning_interface::MotionPlanRequest req,
const double  pos_tolerance,
const double  orientation_tolerance 
)

check if the goal given in cartesian space is reached Only the last point in the trajectory is veryfied.

Parameters
robot_model
trajectory
req
matrix_norm_toleranceused to compare the transformation matrix
joint_velocity_tolerance
Returns

Definition at line 153 of file test_utils.cpp.

Here is the call graph for this function:

◆ isGoalReached() [2/3]

bool testutils::isGoalReached ( const moveit::core::RobotModelConstPtr &  robot_model,
const trajectory_msgs::msg::JointTrajectory &  trajectory,
const planning_interface::MotionPlanRequest req,
const double  tolerance 
)

Definition at line 215 of file test_utils.cpp.

Here is the call graph for this function:

◆ isGoalReached() [3/3]

bool testutils::isGoalReached ( const trajectory_msgs::msg::JointTrajectory &  trajectory,
const std::vector< moveit_msgs::msg::JointConstraint > &  goal,
const double  joint_position_tolerance,
const double  joint_velocity_tolerance = 1.0e-6 
)

check if the goal given in joint space is reached Only the last point in the trajectory is veryfied.

Parameters
trajectorygenerated trajectory
goalgoal in joint space
joint_position_tolerance
joint_velocity_tolerance
Returns
true if satisfied

Definition at line 118 of file test_utils.cpp.

Here is the caller graph for this function:

◆ isMonotonouslyDecreasing()

bool testutils::isMonotonouslyDecreasing ( const std::vector< double > &  vec,
const double &  tol 
)
inline

Definition at line 477 of file test_utils.h.

Here is the caller graph for this function:

◆ isPositionBounded()

bool testutils::isPositionBounded ( const trajectory_msgs::msg::JointTrajectory &  trajectory,
const pilz_industrial_motion_planner::JointLimitsContainer joint_limits 
)

is Position Bounded

Parameters
trajectory
joint_limits
Returns

Definition at line 435 of file test_utils.cpp.

Here is the caller graph for this function:

◆ isTrajectoryConsistent()

bool testutils::isTrajectoryConsistent ( const trajectory_msgs::msg::JointTrajectory &  trajectory)

check if the sizes of the joint position/veloicty/acceleration are correct

Parameters
trajectory
Returns

Definition at line 376 of file test_utils.cpp.

Here is the caller graph for this function:

◆ isVelocityBounded()

bool testutils::isVelocityBounded ( const trajectory_msgs::msg::JointTrajectory &  trajectory,
const pilz_industrial_motion_planner::JointLimitsContainer joint_limits 
)

is Velocity Bounded

Parameters
trajectory
joint_limits
Returns

Definition at line 353 of file test_utils.cpp.

Here is the caller graph for this function:

◆ JOINT_NAME_PREFIX()

const std::string testutils::JOINT_NAME_PREFIX ( "prbt_joint_"  )

◆ toTCPPose()

bool testutils::toTCPPose ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::string &  link_name,
const std::vector< double > &  joint_values,
geometry_msgs::msg::Pose &  pose,
const std::string &  joint_prefix = testutils::JOINT_NAME_PREFIX 
)

compute the tcp pose from joint values

Parameters
robot_model
link_name
joint_values
pose
joint_prefixPrefix of the joint names
Returns
false if forward kinematics failed

Definition at line 531 of file test_utils.cpp.

Here is the call graph for this function: