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 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
75TEST_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
102TEST_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
129TEST_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
164int 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.h: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.8