unittest_trajectory_functions.cpp File Reference

#include <cstdint>
#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.hpp>
#include <moveit/robot_model/joint_model_group.hpp>
#include <moveit/robot_model/robot_model.hpp>
#include <moveit/robot_model_loader/robot_model_loader.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory_point.hpp>
#include <pilz_industrial_motion_planner/limits_container.hpp>
#include <pilz_industrial_motion_planner/trajectory_functions.hpp>
#include "test_utils.hpp"

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	RANDOM_TEST_NUMBER ("random_test_number")
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
	Parametrized class for tests, that only run with a 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 activated self collision for a pose that would be in self collision without the check results in a valid ik solution.
	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 63 of file unittest_trajectory_functions.cpp.

Function Documentation

main()

int main	(	int	argc,
		char **	argv )

Definition at line 1162 of file unittest_trajectory_functions.cpp.

RANDOM_TEST_NUMBER()

const std::string RANDOM_TEST_NUMBER

(

"random_test_number"

)

TEST_F() [1/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIK	)

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

Definition at line 522 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 604 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 570 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 586 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [5/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKSelfCollisionForInvalidPose	)

Test if activated self collision for a pose that would be in self collision without the check results in a valid ik solution.

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

Definition at line 693 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 957 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 929 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 991 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 880 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 346 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 404 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 458 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 430 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 485 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 283 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 1087 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 1030 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 1056 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 1146 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 1123 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 790 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 833 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 757 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 735 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

TEST_F() [25/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		TipLinkFK	)

Parametrized class for tests, that only run with a gripper.

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

Definition at line 253 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function: