moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
44 namespace
45 {
46 
47 TEST_F(ServoCppFixture, JointJogTest)
48 {
49  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
50  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
51 
52  moveit_servo::StatusCode status_curr, status_next, status_initial;
53  moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } };
54  moveit_servo::JointJogCommand zero_joint_jog;
55 
56  // Compute next state.
57  servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG);
58  status_initial = servo_test_instance_->getStatus();
59  ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
60 
61  moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_joint_jog);
62  status_curr = servo_test_instance_->getStatus();
63  ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
64 
65  moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, joint_jog_z);
66  status_next = servo_test_instance_->getStatus();
67  ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
68 
69  // Check against manually verified value
70  double delta = next_state.positions[6] - curr_state.positions[6];
71  constexpr double tol = 1.0e-5;
72  ASSERT_NEAR(delta, 0.01, tol);
73 }
74 
75 TEST_F(ServoCppFixture, TwistTest)
76 {
77  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
78  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
79 
80  moveit_servo::StatusCode status_curr, status_next, status_initial;
81  moveit_servo::TwistCommand twist_non_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
82  moveit_servo::TwistCommand twist_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
83 
84  servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
85  status_initial = servo_test_instance_->getStatus();
86  ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
87  moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, twist_zero);
88  status_curr = servo_test_instance_->getStatus();
89  ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
90 
91  moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, twist_non_zero);
92  status_next = servo_test_instance_->getStatus();
93  ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
94 
95  // Check against manually verified value
96  constexpr double expected_delta = -0.000338;
97  double delta = next_state.positions[6] - curr_state.positions[6];
98  constexpr double tol = 1.0e-5;
99  ASSERT_NEAR(delta, expected_delta, tol);
100 }
101 
102 TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
103 {
104  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
105  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
106 
107  moveit_servo::StatusCode status_curr, status_next, status_initial;
108  moveit_servo::TwistCommand twist_non_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
109  moveit_servo::TwistCommand twist_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
110 
111  servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
112  status_initial = servo_test_instance_->getStatus();
113  ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
114  moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, twist_zero);
115  status_curr = servo_test_instance_->getStatus();
116  ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
117 
118  moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, twist_non_zero);
119  status_next = servo_test_instance_->getStatus();
120  ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
121 
122  // Check against manually verified value
123  constexpr double expected_delta = 0.000338;
124  double delta = next_state.positions[6] - curr_state.positions[6];
125  constexpr double tol = 1.0e-5;
126  ASSERT_NEAR(delta, expected_delta, tol);
127 }
128 
129 TEST_F(ServoCppFixture, PoseTest)
130 {
131  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
132  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
133 
134  moveit_servo::StatusCode status_curr, status_next, status_initial;
135  moveit_servo::PoseCommand zero_pose, non_zero_pose;
136  zero_pose.frame_id = "panda_link0";
137  zero_pose.pose = getCurrentPose("panda_link8");
138 
139  non_zero_pose.frame_id = "panda_link0";
140  non_zero_pose.pose = getCurrentPose("panda_link8");
141  non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()));
142 
143  servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE);
144  status_initial = servo_test_instance_->getStatus();
145  ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
146 
147  moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_pose);
148  status_curr = servo_test_instance_->getStatus();
149  ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
150 
151  moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(robot_state, non_zero_pose);
152  status_next = servo_test_instance_->getStatus();
153  ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
154 
155  // Check against manually verified value
156  constexpr double expected_delta = 0.003364;
157  double delta = next_state.positions[6] - curr_state.positions[6];
158  constexpr double tol = 1.0e-5;
159  ASSERT_NEAR(delta, expected_delta, tol);
160 }
161 
162 } // namespace
163 
164 int main(int argc, char** argv)
165 {
166  rclcpp::init(argc, argv);
167  ::testing::InitGoogleTest(&argc, argv);
168  int result = RUN_ALL_TESTS();
169  rclcpp::shutdown();
170  return result;
171 }
planning_scene_monitor::LockedPlanningSceneRO
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
Definition: planning_scene_monitor.hpp:623
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:164
Generated by doxygen 1.9.1