moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
#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"
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. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) | |
Test the inverse kinematics directly through ikfast solver. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) | |
Test the inverse kinematics using RobotState class (setFromIK) using robot model. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) | |
Test the wrapper function to compute inverse kinematics using robot model. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName) | |
Test computePoseIK for invalid group_name. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName) | |
Test computePoseIK for invalid link_name. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) | |
Test computePoseIK for invalid frame_id. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose) | |
Test if self collision is considered by using a pose that always has self collision. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration) | |
Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation) | |
Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation) | |
Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation) | |
Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory) | |
Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize) | |
Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime) | |
Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime) | |
Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal) | |
Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal) | |
Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal) | |
Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal) | |
Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero. More... | |
TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal) | |
Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero. More... | |
int | main (int argc, char **argv) |
#define _USE_MATH_DEFINES |
Definition at line 62 of file unittest_trajectory_functions.cpp.
const std::string GROUP_TIP_LINK_NAME | ( | "group_tip_link" | ) |
const std::string IK_FAST_LINK_NAME | ( | "ik_fast_link" | ) |
int main | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 974 of file unittest_trajectory_functions.cpp.
const std::string PARAM_PLANNING_GROUP_NAME | ( | "planning_group" | ) |
const std::string RANDOM_TEST_NUMBER | ( | "random_test_number" | ) |
const std::string ROBOT_TCP_LINK_NAME | ( | "tcp_link" | ) |
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testComputePoseIK | |||
) |
Test the wrapper function to compute inverse kinematics using robot model.
Definition at line 334 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testComputePoseIKInvalidFrameId | |||
) |
Test computePoseIK for invalid frame_id.
Currently only robot_model_->getModelFrame() == frame_id
Definition at line 416 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testComputePoseIKInvalidGroupName | |||
) |
Test computePoseIK for invalid group_name.
Definition at line 382 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testComputePoseIKInvalidLinkName | |||
) |
Test computePoseIK for invalid link_name.
Definition at line 398 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testComputePoseIKSelfCollisionForInvalidPose | |||
) |
Test if self collision is considered by using a pose that always has self collision.
Definition at line 505 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testDetermineAndCheckSamplingTimeCorrectSamplingTime | |||
) |
Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct.
Test Sequence:
Expected Results:
Definition at line 769 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testDetermineAndCheckSamplingTimeInvalidVectorSize | |||
) |
Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size.
Test Sequence:
Expected Results:
Definition at line 741 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testDetermineAndCheckSamplingTimeViolateSamplingTime | |||
) |
Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated.
Test Sequence:
Expected Results:
Definition at line 803 of file unittest_trajectory_functions.cpp.
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:
Expected Results:
Definition at line 692 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIKRobotState | |||
) |
Test the inverse kinematics using RobotState class (setFromIK) using robot model.
Definition at line 276 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIKSolver | |||
) |
Test the inverse kinematics directly through ikfast solver.
Definition at line 217 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIsRobotStateEqualAccelerationUnequal | |||
) |
Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal.
Test Sequence:
Expected Results:
Definition at line 899 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIsRobotStateEqualPositionUnequal | |||
) |
Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal.
Test Sequence:
Expected Results:
Definition at line 842 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIsRobotStateEqualVelocityUnequal | |||
) |
Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal.
Test Sequence:
Expected Results:
Definition at line 868 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIsRobotStateStationaryAccelerationUnequal | |||
) |
Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero.
Test Sequence:
Expected Results:
Definition at line 958 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testIsRobotStateStationaryVelocityUnequal | |||
) |
Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero.
Test Sequence:
Expected Results:
Definition at line 935 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testVerifySampleJointLimitsAccelerationViolation | |||
) |
Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation.
Test Sequence:
Expected Results:
Definition at line 602 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testVerifySampleJointLimitsDecelerationViolation | |||
) |
Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation.
Test Sequence:
Expected Results:
Definition at line 645 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testVerifySampleJointLimitsVelocityViolation | |||
) |
Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation.
Test Sequence:
Expected Results:
Definition at line 569 of file unittest_trajectory_functions.cpp.
TEST_F | ( | TrajectoryFunctionsTestFlangeAndGripper | , |
testVerifySampleJointLimitsWithSmallDuration | |||
) |
Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration.
Test Sequence:
Expected Results:
Definition at line 547 of file unittest_trajectory_functions.cpp.
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 187 of file unittest_trajectory_functions.cpp.