moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_integration.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2023, PickNik Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of PickNik Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author : V Mohammed Ibrahim
36 Desc : Integration tests for the servo c++ API
37 Title : test_integration.cpp
38 Project : moveit_servo
39 Created : 07/07/2023
40*/
41
42#include "servo_cpp_fixture.hpp"
43
44namespace
45{
46
47TEST_F(ServoCppFixture, JointJogTest)
48{
49 moveit_servo::StatusCode status_curr, status_next, status_initial;
50 moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } };
51 moveit_servo::JointJogCommand zero_joint_jog;
52 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
53
54 // Compute next state.
55 servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG);
56 status_initial = servo_test_instance_->getStatus();
57 ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
58
59 moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_joint_jog);
60 status_curr = servo_test_instance_->getStatus();
61 ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
62
63 moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, joint_jog_z);
64 status_next = servo_test_instance_->getStatus();
65 ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
66
67 // Check against manually verified value
68 double delta = next_state.positions[6] - curr_state.positions[6];
69 constexpr double tol = 0.00001;
70 ASSERT_NEAR(delta, 0.02, tol);
71}
72
73TEST_F(ServoCppFixture, TwistTest)
74{
75 moveit_servo::StatusCode status_curr, status_next, status_initial;
76 moveit_servo::TwistCommand twist_non_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
77 moveit_servo::TwistCommand twist_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
78 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
79
80 servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
81 status_initial = servo_test_instance_->getStatus();
82 ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
83 moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, twist_zero);
84 status_curr = servo_test_instance_->getStatus();
85 ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
86
87 moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, twist_non_zero);
88 status_next = servo_test_instance_->getStatus();
89 ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
90
91 // Check against manually verified value
92 constexpr double expected_delta = -0.001693;
93 double delta = next_state.positions[6] - curr_state.positions[6];
94 constexpr double tol = 0.00001;
95 ASSERT_NEAR(delta, expected_delta, tol);
96}
97
98TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
99{
100 moveit_servo::StatusCode status_curr, status_next, status_initial;
101 moveit_servo::TwistCommand twist_non_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
102 moveit_servo::TwistCommand twist_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
103 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
104
105 servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
106 status_initial = servo_test_instance_->getStatus();
107 ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
108 moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, twist_zero);
109 status_curr = servo_test_instance_->getStatus();
110 ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
111
112 moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, twist_non_zero);
113 status_next = servo_test_instance_->getStatus();
114 ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
115
116 // Check against manually verified value
117 constexpr double expected_delta = 0.001693;
118 double delta = next_state.positions[6] - curr_state.positions[6];
119 constexpr double tol = 0.00001;
120 ASSERT_NEAR(delta, expected_delta, tol);
121}
122
123TEST_F(ServoCppFixture, PoseTest)
124{
125 moveit_servo::StatusCode status_curr, status_next, status_initial;
126 moveit_servo::PoseCommand zero_pose, non_zero_pose;
127 zero_pose.frame_id = "panda_link0";
128 zero_pose.pose = getCurrentPose("panda_link8");
129
130 non_zero_pose.frame_id = "panda_link0";
131 non_zero_pose.pose = getCurrentPose("panda_link8");
132 non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()));
133
134 servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE);
135 status_initial = servo_test_instance_->getStatus();
136 ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
137
138 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
139 moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_pose);
140 status_curr = servo_test_instance_->getStatus();
141 ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
142
143 moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, non_zero_pose);
144 status_next = servo_test_instance_->getStatus();
145 ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
146
147 // Check against manually verified value
148 constexpr double expected_delta = 0.057420;
149 double delta = next_state.positions[6] - curr_state.positions[6];
150 constexpr double tol = 0.00001;
151 ASSERT_NEAR(delta, expected_delta, tol);
152}
153
154} // namespace
155
156int main(int argc, char** argv)
157{
158 rclcpp::init(argc, argv);
159 ::testing::InitGoogleTest(&argc, argv);
160 int result = RUN_ALL_TESTS();
161 rclcpp::shutdown();
162 return result;
163}
moveit_servo::CommandType::TWIST
@ TWIST
moveit_servo::CommandType::JOINT_JOG
@ JOINT_JOG
moveit_servo::CommandType::POSE
@ POSE
moveit_servo::StatusCode
StatusCode
Definition datatypes.hpp:52
moveit_servo::StatusCode::NO_WARNING
@ NO_WARNING
servo_cpp_fixture.hpp
moveit_servo::JointJogCommand
Definition datatypes.hpp:89
moveit_servo::KinematicState
Definition datatypes.hpp:115
moveit_servo::KinematicState::positions
Eigen::VectorXd positions
Definition datatypes.hpp:117
moveit_servo::PoseCommand
Definition datatypes.hpp:105
moveit_servo::PoseCommand::frame_id
std::string frame_id
Definition datatypes.hpp:106
moveit_servo::PoseCommand::pose
Eigen::Isometry3d pose
Definition datatypes.hpp:107
moveit_servo::TwistCommand
Definition datatypes.hpp:97
TEST_F
TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
Continuous self collision checks are not supported yet by the bullet integration.
Definition test_bullet_continuous_collision_checking.cpp:226
main
int main(int argc, char **argv)
Definition test_integration.cpp:156
Generated by doxygen 1.9.8