unittest_trajectory_functions.cpp File Reference

#include <gtest/gtest.h>
#include <map>
#include <math.h>
#include <string>
#include <vector>
#include <Eigen/Geometry>
#include <kdl/frames.hpp>
#include <kdl/path_roundedcomposite.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory.h>
#include <pilz_industrial_motion_planner/cartesian_trajectory_point.h>
#include <pilz_industrial_motion_planner/limits_container.h>
#include <pilz_industrial_motion_planner/trajectory_functions.h>
#include "test_utils.h"

Include dependency graph for unittest_trajectory_functions.cpp:

Go to the source code of this file.

Classes
class	TrajectoryFunctionsTestBase
	test fixtures base class More...
class	TrajectoryFunctionsTestFlangeAndGripper
	Parametrized class for tests with and without gripper. More...

Macros
#define	_USE_MATH_DEFINES

Functions
const std::string	PARAM_PLANNING_GROUP_NAME ("planning_group")
const std::string	GROUP_TIP_LINK_NAME ("group_tip_link")
const std::string	ROBOT_TCP_LINK_NAME ("tcp_link")
const std::string	IK_FAST_LINK_NAME ("ik_fast_link")
const std::string	RANDOM_TEST_NUMBER ("random_test_number")
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
	Test the forward kinematics function with simple robot poses for robot tip link using robot model without gripper.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
	Test the inverse kinematics directly through ikfast solver.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
	Test the inverse kinematics using RobotState class (setFromIK) using robot model.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityCollisionObject)
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedCollisionObject)
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentitySubframe)
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedSubframe)
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
	Test the wrapper function to compute inverse kinematics using robot model.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
	Test computePoseIK for invalid group_name.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
	Test computePoseIK for invalid link_name.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
	Test computePoseIK for invalid frame_id.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
	Test if self collision is considered by using a pose that always has self collision.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
	Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
	Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
	Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
	Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
	Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
	Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero.
int	main (int argc, char **argv)

Macro Definition Documentation

_USE_MATH_DEFINES

#define _USE_MATH_DEFINES

Definition at line 62 of file unittest_trajectory_functions.cpp.

Function Documentation

GROUP_TIP_LINK_NAME()

const std::string GROUP_TIP_LINK_NAME

(

"group_tip_link"

)

IK_FAST_LINK_NAME()

const std::string IK_FAST_LINK_NAME

(

"ik_fast_link"

)

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 1165 of file unittest_trajectory_functions.cpp.

PARAM_PLANNING_GROUP_NAME()

const std::string PARAM_PLANNING_GROUP_NAME

(

"planning_group"

)

RANDOM_TEST_NUMBER()

const std::string RANDOM_TEST_NUMBER

(

"random_test_number"

)

ROBOT_TCP_LINK_NAME()

const std::string ROBOT_TCP_LINK_NAME

(

"tcp_link"

)

TEST_F() [1/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIK
	)

Test the wrapper function to compute inverse kinematics using robot model.

Definition at line 525 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [2/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidFrameId
	)

Test computePoseIK for invalid frame_id.

Currently only robot_model_->getModelFrame() == frame_id

Definition at line 607 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [3/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidGroupName
	)

Test computePoseIK for invalid group_name.

Definition at line 573 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [4/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidLinkName
	)

Test computePoseIK for invalid link_name.

Definition at line 589 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [5/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKSelfCollisionForInvalidPose
	)

Test if self collision is considered by using a pose that always has self collision.

Definition at line 696 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [6/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeCorrectSamplingTime
	)

Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct.

Test Sequence:

1. Call function with trajectories which do NOT violate sampling time.

Expected Results:

1. Function returns 'true'.

Definition at line 960 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [7/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeInvalidVectorSize
	)

Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size.

Test Sequence:

1. Call function with vectors of incorrect size.

Expected Results:

1. Function returns 'false'.

Definition at line 932 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [8/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeViolateSamplingTime
	)

Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated.

Test Sequence:

1. Call function with trajectories which violate sampling time.

Expected Results:

1. Function returns 'false'.

Definition at line 994 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [9/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testGenerateJointTrajectoryWithInvalidCartesianTrajectory
	)

Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory.

Please note: Both function variants are tested in this test.

Test Sequence:

1. Call function with a cartesian trajectory which cannot be transformed into a joint trajectory by using an invalid group_name.

Expected Results:

1. Function returns 'false'.

Definition at line 883 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [10/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotState
	)

Test the inverse kinematics using RobotState class (setFromIK) using robot model.

Definition at line 349 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [11/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithIdentityCollisionObject
	)

Definition at line 407 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [12/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithIdentitySubframe
	)

Definition at line 461 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [13/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithTransformedCollisionObject
	)

Definition at line 433 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [14/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithTransformedSubframe
	)

Definition at line 488 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [15/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKSolver
	)

Test the inverse kinematics directly through ikfast solver.

Definition at line 286 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [16/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualAccelerationUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal.

Test Sequence:

1. Call function with robot states with different acceleration.

Expected Results:

1. Function returns 'false'.

Definition at line 1090 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [17/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualPositionUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal.

Test Sequence:

1. Call function with robot states with different positions.

Expected Results:

1. Function returns 'false'.

Definition at line 1033 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [18/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualVelocityUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal.

Test Sequence:

1. Call function with robot states with different velocities.

Expected Results:

1. Function returns 'false'.

Definition at line 1059 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [19/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateStationaryAccelerationUnequal
	)

Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero.

Test Sequence:

1. Call function with robot state with joint acceleration != 0.

Expected Results:

1. Function returns 'false'.

Definition at line 1149 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [20/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateStationaryVelocityUnequal
	)

Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero.

Test Sequence:

1. Call function with robot state with joint velocities != 0.

Expected Results:

1. Function returns 'false'.

Definition at line 1126 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [21/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsAccelerationViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation.

Test Sequence:

1. Call function with a acceleration violation.

Expected Results:

1. Function returns 'false'.

Definition at line 793 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [22/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsDecelerationViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation.

Test Sequence:

1. Call function with a deceleration violation.

Expected Results:

1. Function returns 'false'.

Definition at line 836 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [23/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsVelocityViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation.

Test Sequence:

1. Call function with a velocity violation.

Expected Results:

1. Function returns 'false'.

Definition at line 760 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [24/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsWithSmallDuration
	)

Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration.

Test Sequence:

1. Call function with very small sample duration.

Expected Results:

1. Function returns 'false'.

Definition at line 738 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [25/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		TipLinkFK
	)

Test the forward kinematics function with simple robot poses for robot tip link using robot model without gripper.

Definition at line 256 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function: