137 lines
5.4 KiB
C++
137 lines
5.4 KiB
C++
// Copyright (c) 2023 Franka Emika GmbH
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <memory>
|
|
#include <vector>
|
|
|
|
#include "franka_example_controllers/gravity_compensation_example_controller.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include "hardware_interface/handle.hpp"
|
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
|
#include "rclcpp/utilities.hpp"
|
|
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
|
|
|
using hardware_interface::CommandInterface;
|
|
using hardware_interface::HW_IF_EFFORT;
|
|
using hardware_interface::LoanedCommandInterface;
|
|
|
|
class TestGravityCompensationExample : public ::testing::Test {
|
|
public:
|
|
static void SetUpTestSuite();
|
|
static void TearDownTestSuite();
|
|
|
|
void SetUp();
|
|
void TearDown();
|
|
|
|
void SetUpController();
|
|
|
|
protected:
|
|
std::unique_ptr<franka_example_controllers::GravityCompensationExampleController> controller_;
|
|
|
|
// dummy joint state values used for tests
|
|
const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3", "joint4",
|
|
"joint5", "joint6", "joint7"};
|
|
std::vector<double> joint_commands_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
|
|
|
|
CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
|
|
CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
|
|
CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
|
|
CommandInterface joint_4_pos_cmd_{joint_names_[3], HW_IF_EFFORT, &joint_commands_[3]};
|
|
CommandInterface joint_5_pos_cmd_{joint_names_[4], HW_IF_EFFORT, &joint_commands_[4]};
|
|
CommandInterface joint_6_pos_cmd_{joint_names_[5], HW_IF_EFFORT, &joint_commands_[5]};
|
|
CommandInterface joint_7_pos_cmd_{joint_names_[6], HW_IF_EFFORT, &joint_commands_[6]};
|
|
};
|
|
|
|
void TestGravityCompensationExample::SetUpTestSuite() {
|
|
rclcpp::init(0, nullptr);
|
|
}
|
|
|
|
void TestGravityCompensationExample::TearDownTestSuite() {
|
|
rclcpp::shutdown();
|
|
}
|
|
|
|
void TestGravityCompensationExample::SetUp() {
|
|
controller_ =
|
|
std::make_unique<franka_example_controllers::GravityCompensationExampleController>();
|
|
}
|
|
|
|
void TestGravityCompensationExample::TearDown() {
|
|
controller_.reset(nullptr);
|
|
}
|
|
|
|
void TestGravityCompensationExample::SetUpController() {
|
|
const auto result = controller_->init("test_gravitiy_compensation_example");
|
|
ASSERT_EQ(result, controller_interface::return_type::OK);
|
|
std::vector<LoanedCommandInterface> command_ifs;
|
|
command_ifs.emplace_back(joint_1_pos_cmd_);
|
|
command_ifs.emplace_back(joint_2_pos_cmd_);
|
|
command_ifs.emplace_back(joint_3_pos_cmd_);
|
|
command_ifs.emplace_back(joint_4_pos_cmd_);
|
|
command_ifs.emplace_back(joint_5_pos_cmd_);
|
|
command_ifs.emplace_back(joint_6_pos_cmd_);
|
|
command_ifs.emplace_back(joint_7_pos_cmd_);
|
|
|
|
controller_->assign_interfaces(std::move(command_ifs), {});
|
|
}
|
|
|
|
TEST_F(TestGravityCompensationExample, JointsParameterNotSet) {
|
|
GTEST_SKIP() << "Skipping joint parameters not set test";
|
|
SetUpController();
|
|
|
|
// TODO(baris) configure must fail, 'joints' parameter not set
|
|
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
|
|
}
|
|
|
|
TEST_F(TestGravityCompensationExample, JointsParameterIsEmpty) {
|
|
GTEST_SKIP() << "Skipping joints parameter is empty test";
|
|
SetUpController();
|
|
controller_->get_node()->set_parameter({"joints", std::vector<std::string>()});
|
|
|
|
// Should return ERROR!!
|
|
// TODO(baris) params_.joints can't be empty add a check
|
|
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
|
|
}
|
|
|
|
TEST_F(TestGravityCompensationExample, given_correct_number_of_joints_configure_returns_success) {
|
|
SetUpController();
|
|
controller_->get_node()->set_parameter({"joints", joint_names_});
|
|
|
|
// configure successful
|
|
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
|
|
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
|
|
}
|
|
|
|
TEST_F(TestGravityCompensationExample, given_joints_and_interface_when_update_expect_zero_values) {
|
|
SetUpController();
|
|
controller_->get_node()->set_parameter({"joints", joint_names_});
|
|
|
|
// configure successful
|
|
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
|
|
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
|
|
// update successful though no command has been send yet
|
|
ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
|
|
controller_interface::return_type::OK);
|
|
|
|
// check joint commands are updated to zero torque value
|
|
ASSERT_EQ(joint_1_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_2_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_3_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_4_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_5_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_6_pos_cmd_.get_value(), 0.0);
|
|
ASSERT_EQ(joint_7_pos_cmd_.get_value(), 0.0);
|
|
}
|