diff --git a/CHANGELOG.md b/CHANGELOG.md index e7ff0117..a01991d6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,11 @@ # CHANGELOG + +## 0.13.0 - 2023-11-16 + + * Add abstract ActiveControlBase class for the ActiveControllers to implement. + * **BREAKING** Fix function naming errors in robot class. + * **BREAKING** ActiveController initializer functions return ActiveControlBase + ## 0.12.1 - 2023-09-20 * Fix install common/include typo diff --git a/CMakeLists.txt b/CMakeLists.txt index a49dd64c..5d4d90e0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.4) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -set(libfranka_VERSION 0.12.1) +set(libfranka_VERSION 0.13.0) project(libfranka VERSION ${libfranka_VERSION} diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index 93b7082a..4a7d0da2 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -743,7 +743,7 @@ WARN_NO_PARAMDOC = YES # a warning is encountered. # The default value is: NO. -WARN_AS_ERROR = @DOXYGEN_WARN_AS_ERROR@ +WARN_AS_ERROR = NO # The WARN_FORMAT tag determines the format of the warning messages that doxygen # can produce. The string should contain the $file, $line, and $text tags, which diff --git a/examples/generate_cartesian_pose_motion_external_control_loop.cpp b/examples/generate_cartesian_pose_motion_external_control_loop.cpp index 2d853cc5..216e03bc 100644 --- a/examples/generate_cartesian_pose_motion_external_control_loop.cpp +++ b/examples/generate_cartesian_pose_motion_external_control_loop.cpp @@ -74,7 +74,7 @@ int main(int argc, char** argv) { }; bool motion_finished = false; - std::unique_ptr active_control = robot.startCartesianPositionControl( + auto active_control = robot.startCartesianPoseControl( research_interface::robot::Move::ControllerMode::kJointImpedance); while (!motion_finished) { auto read_once_return = active_control->readOnce(); diff --git a/examples/generate_cartesian_velocity_motion_external_control_loop.cpp b/examples/generate_cartesian_velocity_motion_external_control_loop.cpp index 4382befa..af2f4f3e 100644 --- a/examples/generate_cartesian_velocity_motion_external_control_loop.cpp +++ b/examples/generate_cartesian_velocity_motion_external_control_loop.cpp @@ -84,7 +84,7 @@ int main(int argc, char** argv) { }; bool motion_finished = false; - std::unique_ptr active_control = robot.startCartesianVelocityControl( + auto active_control = robot.startCartesianVelocityControl( research_interface::robot::Move::ControllerMode::kJointImpedance); while (!motion_finished) { auto read_once_return = active_control->readOnce(); diff --git a/examples/generate_joint_position_motion_external_control_loop.cpp b/examples/generate_joint_position_motion_external_control_loop.cpp index cd8e7d2b..ce3cbcbc 100644 --- a/examples/generate_joint_position_motion_external_control_loop.cpp +++ b/examples/generate_joint_position_motion_external_control_loop.cpp @@ -70,7 +70,7 @@ int main(int argc, char** argv) { }; bool motion_finished = false; - std::unique_ptr active_control = robot.startJointPositionControl( + auto active_control = robot.startJointPositionControl( research_interface::robot::Move::ControllerMode::kJointImpedance); while (!motion_finished) { auto read_once_return = active_control->readOnce(); diff --git a/examples/generate_joint_velocity_motion_external_control_loop.cpp b/examples/generate_joint_velocity_motion_external_control_loop.cpp index 35a7c329..e2417753 100644 --- a/examples/generate_joint_velocity_motion_external_control_loop.cpp +++ b/examples/generate_joint_velocity_motion_external_control_loop.cpp @@ -67,7 +67,7 @@ int main(int argc, char** argv) { }; bool motion_finished = false; - std::unique_ptr active_control = robot.startJointVelocityControl( + auto active_control = robot.startJointVelocityControl( research_interface::robot::Move::ControllerMode::kJointImpedance); while (!motion_finished) { auto read_once_return = active_control->readOnce(); diff --git a/examples/motion_with_control_external_control_loop.cpp b/examples/motion_with_control_external_control_loop.cpp index 230ddf53..9d352166 100644 --- a/examples/motion_with_control_external_control_loop.cpp +++ b/examples/motion_with_control_external_control_loop.cpp @@ -177,7 +177,7 @@ int main(int argc, char** argv) { }; bool motion_finished = false; - std::unique_ptr active_control = robot.startJointVelocityControl( + auto active_control = robot.startJointVelocityControl( research_interface::robot::Move::ControllerMode::kExternalController); while (!motion_finished) { auto read_once_return = active_control->readOnce(); diff --git a/include/franka/active_control.h b/include/franka/active_control.h index fa495e1b..9a73ebd5 100644 --- a/include/franka/active_control.h +++ b/include/franka/active_control.h @@ -2,147 +2,65 @@ // Use of this source code is governed by the Apache-2.0 license, see LICENSE #pragma once -#include -#include -#include - -#include +#include #include -#include #include "robot.h" /** * @file active_control.h - * Contains the `franka::ActiveControl`, `franka::ActiveTorqueControl` and - * `franka::ActiveMotionGenerator` type. + * Implements the ActiveControlBase abstract class. Contains the `franka::ActiveControl`, + * `franka::ActiveTorqueControl` and `franka::ActiveMotionGenerator` type. */ namespace franka { /** - * Allows the user to read the state of a Robot and to send new control commands after starting a - * control process of a Robot. - * - * hint: To create an ActiveControl, see franka::ActiveTorqueControl or - * franka::ActiveMotionGenerator - * + * Documented in ActiveControlBase */ -class ActiveControl { +class ActiveControl : public ActiveControlBase { public: - virtual ~ActiveControl(); + ~ActiveControl() override; - /** - * Waits for a robot state update and returns it. - * - * @return Current robot state & time since last read operation - * - * @throw NetworkException if the connection is lost, e.g. after a timeout. - * @throw ProtocolException if robot returns an unexpected message. - * @throw ControlException if robot is in an error state. - */ - std::pair readOnce(); + std::pair readOnce() override; - /** - * Updates torque commands of an active control - * - * hint: implemented in ActiveTorqueControl - * - * @return void - */ - virtual void writeOnce(const Torques& /* control_input */) { + void writeOnce(const Torques& /* control_input */) override { throw franka::ControlException(wrong_write_once_method_called); }; - /** - * Updates the joint position and torque commands of an active control - * - * hint: implemented in ActiveMotionGenerator - * - * @return void - */ - virtual void writeOnce(const JointPositions& /* motion_generator_input */, - const std::optional& /*control_input*/) { + void writeOnce(const JointPositions& /* motion_generator_input */, + const std::optional& /*control_input*/) override { throw franka::ControlException(wrong_write_once_method_called); }; - /** - * Updates the joint velocity and torque commands of an active control - * - * hint: implemented in ActiveMotionGenerator - * - * @return void - */ - virtual void writeOnce(const JointVelocities& /* motion_generator_input */, - const std::optional& /* control_input */) { + void writeOnce(const JointVelocities& /* motion_generator_input */, + const std::optional& /* control_input */) override { throw franka::ControlException(wrong_write_once_method_called); }; - /** - * Updates the cartesian position and torque commands of an active control - * - * hint: implemented in ActiveMotionGenerator - * - * @return void - */ - virtual void writeOnce(const CartesianPose& /* motion_generator_input */, - const std::optional& /* control_input */) { + void writeOnce(const CartesianPose& /* motion_generator_input */, + const std::optional& /* control_input */) override { throw franka::ControlException(wrong_write_once_method_called); }; - /** - * Updates the cartesian velocity and torque commands of an active control - * - * hint: implemented in ActiveMotionGenerator - * - * @return void - */ - virtual void writeOnce(const CartesianVelocities& /* motion_generator_input */, - const std::optional& /* control_input */) { + void writeOnce(const CartesianVelocities& /* motion_generator_input */, + const std::optional& /* control_input */) override { throw franka::ControlException(wrong_write_once_method_called); }; - /** - * Updates the joint position commands of an active control, with internal controller - * - * @param motion_generator_input the new motion generator input - * - * @return void - */ - virtual void writeOnce(const JointPositions& motion_generator_input) { + void writeOnce(const JointPositions& motion_generator_input) override { writeOnce(motion_generator_input, std::optional()); }; - /** - * Updates the joint velocity commands of an active control, with internal controller - * - * @param motion_generator_input the new motion generator input - * - * @return void - */ - virtual void writeOnce(const JointVelocities& motion_generator_input) { + void writeOnce(const JointVelocities& motion_generator_input) override { writeOnce(motion_generator_input, std::optional()); }; - /** - * Updates the cartesian position commands of an active control, with internal controller - * - * @param motion_generator_input the new motion generator input - * - * @return void - */ - virtual void writeOnce(const CartesianPose& motion_generator_input) { + void writeOnce(const CartesianPose& motion_generator_input) override { writeOnce(motion_generator_input, std::optional()); }; - /** - * Updates the cartesian velocity commands of an active control, with internal controller - * - * @param motion_generator_input the new motion generator input - * - * @return void - */ - virtual void writeOnce(const CartesianVelocities& motion_generator_input) { + void writeOnce(const CartesianVelocities& motion_generator_input) override { writeOnce(motion_generator_input, std::optional()); }; diff --git a/include/franka/active_control_base.h b/include/franka/active_control_base.h new file mode 100644 index 00000000..3b896a8d --- /dev/null +++ b/include/franka/active_control_base.h @@ -0,0 +1,119 @@ +// Copyright (c) 2023 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include +#include +#include +#include +#include + +/** + * @file active_control_base.h + * Abstract interface class as the base of the active controllers. + */ + +namespace franka { + +/** + * Allows the user to read the state of a Robot and to send new control commands after starting a + * control process of a Robot. + * + * hint: To create an ActiveControlBase, see franka::ActiveTorqueControl or + * franka::ActiveMotionGenerator + * + */ +class ActiveControlBase { + public: + virtual ~ActiveControlBase() = default; + + /** + * Waits for a robot state update and returns it. + * + * @return Current robot state & time since last read operation + * + * @throw NetworkException if the connection is lost, e.g. after a timeout. + * @throw ProtocolException if robot returns an unexpected message. + * @throw ControlException if robot is in an error state. + */ + virtual std::pair readOnce() = 0; + + /** + * Updates torque commands of an active control + * + * hint: implemented in ActiveTorqueControl + * + */ + virtual void writeOnce(const Torques& /* control_input */) = 0; + + /** + * Updates the joint position and torque commands of an active control + * + * hint: implemented in ActiveMotionGenerator + * + */ + virtual void writeOnce(const JointPositions& /* motion_generator_input */, + const std::optional& /*control_input*/) = 0; + + /** + * Updates the joint velocity and torque commands of an active control + * + * hint: implemented in ActiveMotionGenerator + * + */ + virtual void writeOnce(const JointVelocities& /* motion_generator_input */, + const std::optional& /* control_input */) = 0; + /** + * Updates the cartesian position and torque commands of an active control + * + * hint: implemented in ActiveMotionGenerator + * + */ + virtual void writeOnce(const CartesianPose& /* motion_generator_input */, + const std::optional& /* control_input */) = 0; + + /** + * Updates the cartesian velocity and torque commands of an active control + * + * hint: implemented in ActiveMotionGenerator + * + */ + virtual void writeOnce(const CartesianVelocities& /* motion_generator_input */, + const std::optional& /* control_input */) = 0; + /** + * Updates the joint position commands of an active control, with internal controller + * + * @param motion_generator_input the new motion generator input + * + */ + virtual void writeOnce(const JointPositions& motion_generator_input) = 0; + + /** + * Updates the joint velocity commands of an active control, with internal controller + * + * @param motion_generator_input the new motion generator input + * + */ + virtual void writeOnce(const JointVelocities& motion_generator_input) = 0; + /** + * Updates the cartesian pose commands of an active control, with internal controller + * + * @param motion_generator_input the new motion generator input + * + */ + virtual void writeOnce(const CartesianPose& motion_generator_input) = 0; + + /** + * Updates the cartesian velocity commands of an active control, with internal controller + * + * @param motion_generator_input the new motion generator input + * + */ + virtual void writeOnce(const CartesianVelocities& motion_generator_input) = 0; + + protected: + ActiveControlBase() = default; +}; + +} // namespace franka diff --git a/include/franka/active_motion_generator.h b/include/franka/active_motion_generator.h index fc45dd7d..cc09a90a 100644 --- a/include/franka/active_motion_generator.h +++ b/include/franka/active_motion_generator.h @@ -5,7 +5,7 @@ #include "active_control.h" /** - * @file active_control.h + * @file active_motion_generator.h * Contains the `franka::ActiveMotionGenerator` type. */ diff --git a/include/franka/robot.h b/include/franka/robot.h index 2da17a32..b45a62c5 100644 --- a/include/franka/robot.h +++ b/include/franka/robot.h @@ -22,10 +22,7 @@ namespace franka { class Model; -class ActiveTorqueControl; - -template -class ActiveMotionGenerator; +class ActiveControlBase; /** * Maintains a network connection to the robot, provides the current robot state, gives access to @@ -448,7 +445,7 @@ class Robot { * * @see Robot::read for a way to repeatedly receive the robot state. */ - RobotState readOnce(); + virtual RobotState readOnce(); /** * @name Commands @@ -653,7 +650,7 @@ class Robot { * @throw NetworkException if the connection is lost, e.g. after a timeout. * @throw std::invalid_argument if joint-level torque commands are NaN or infinity. */ - std::unique_ptr startTorqueControl(); + virtual std::unique_ptr startTorqueControl(); /** * Starts a new joint position motion generator @@ -668,7 +665,7 @@ class Robot { * @throw NetworkException if the connection is lost, e.g. after a timeout. * @throw std::invalid_argument if joint-level torque commands are NaN or infinity. */ - std::unique_ptr> startJointPositionControl( + virtual std::unique_ptr startJointPositionControl( const research_interface::robot::Move::ControllerMode& control_type); /** @@ -683,7 +680,7 @@ class Robot { * @throw NetworkException if the connection is lost, e.g. after a timeout. * @throw std::invalid_argument if joint-level torque commands are NaN or infinity. */ - std::unique_ptr> startJointVelocityControl( + virtual std::unique_ptr startJointVelocityControl( const research_interface::robot::Move::ControllerMode& control_type); /** @@ -698,7 +695,7 @@ class Robot { * @throw NetworkException if the connection is lost, e.g. after a timeout. * @throw std::invalid_argument if joint-level torque commands are NaN or infinity. */ - std::unique_ptr> startCartesianPositionControl( + virtual std::unique_ptr startCartesianPoseControl( const research_interface::robot::Move::ControllerMode& control_type); /** @@ -714,7 +711,7 @@ class Robot { * @throw NetworkException if the connection is lost, e.g. after a timeout. * @throw std::invalid_argument if joint-level torque commands are NaN or infinity. */ - std::unique_ptr> startCartesianVelocityControl( + virtual std::unique_ptr startCartesianVelocityControl( const research_interface::robot::Move::ControllerMode& control_type); /** @@ -764,6 +761,11 @@ class Robot { */ Robot(std::shared_ptr robot_impl); + /** + * Default constructor to enable mocking and testing. + */ + Robot() = default; + private: /** * Starts a new motion generator and controller @@ -780,7 +782,7 @@ class Robot { * @throw std::invalid_argument if joint - level torque commands are NaN or infinity. */ template - std::unique_ptr> startControl( + std::unique_ptr startControl( const research_interface::robot::Move::ControllerMode& controller_type); std::shared_ptr impl_; diff --git a/src/robot.cpp b/src/robot.cpp index 0e8d67e7..73479e5d 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -251,7 +251,7 @@ void Robot::automaticErrorRecovery() { } template -std::unique_ptr> Robot::startControl( +std::unique_ptr Robot::startControl( const research_interface::robot::Move::ControllerMode& controller_type) { std::unique_lock control_lock(control_mutex_, std::try_to_lock); assertOwningLock(control_lock); @@ -262,13 +262,11 @@ std::unique_ptr> Robot::startControl( uint32_t motion_id = impl_->startMotion(controller_type, motion_generator_mode, ControlLoop::kDefaultDeviation, ControlLoop::kDefaultDeviation); - - return std::unique_ptr>( - new ActiveMotionGenerator(impl_, motion_id, std::move(control_lock), - controller_type)); + return std::unique_ptr(new ActiveMotionGenerator( + impl_, motion_id, std::move(control_lock), controller_type)); } -std::unique_ptr Robot::startTorqueControl() { +std::unique_ptr Robot::startTorqueControl() { std::unique_lock control_lock(control_mutex_, std::try_to_lock); assertOwningLock(control_lock); @@ -279,26 +277,26 @@ std::unique_ptr Robot::startTorqueControl() { ControlLoop::kDefaultDeviation, ControlLoop::kDefaultDeviation); - return std::unique_ptr( + return std::unique_ptr( new ActiveTorqueControl(impl_, motion_id, std::move(control_lock))); } -std::unique_ptr> Robot::startJointPositionControl( +std::unique_ptr Robot::startJointPositionControl( const research_interface::robot::Move::ControllerMode& control_type) { return startControl(control_type); } -std::unique_ptr> Robot::startJointVelocityControl( +std::unique_ptr Robot::startJointVelocityControl( const research_interface::robot::Move::ControllerMode& control_type) { return startControl(control_type); } -std::unique_ptr> Robot::startCartesianPositionControl( +std::unique_ptr Robot::startCartesianPoseControl( const research_interface::robot::Move::ControllerMode& control_type) { return startControl(control_type); } -std::unique_ptr> Robot::startCartesianVelocityControl( +std::unique_ptr Robot::startCartesianVelocityControl( const research_interface::robot::Move::ControllerMode& control_type) { return startControl(control_type); } @@ -313,13 +311,13 @@ Model Robot::loadModel() { Robot::Robot(std::shared_ptr robot_impl) : impl_(std::move(robot_impl)){}; -template std::unique_ptr> Robot::startControl< - JointVelocities>(const research_interface::robot::Move::ControllerMode& controller_type); -template std::unique_ptr> Robot::startControl( +template std::unique_ptr Robot::startControl( + const research_interface::robot::Move::ControllerMode& controller_type); +template std::unique_ptr Robot::startControl( + const research_interface::robot::Move::ControllerMode& controller_type); +template std::unique_ptr Robot::startControl( const research_interface::robot::Move::ControllerMode& controller_type); -template std::unique_ptr> Robot::startControl( +template std::unique_ptr Robot::startControl( const research_interface::robot::Move::ControllerMode& controller_type); -template std::unique_ptr> Robot::startControl< - CartesianVelocities>(const research_interface::robot::Move::ControllerMode& controller_type); } // namespace franka diff --git a/test/active_motion_generator_tests.cpp b/test/active_motion_generator_tests.cpp index 8a9ae657..1228f7ac 100644 --- a/test/active_motion_generator_tests.cpp +++ b/test/active_motion_generator_tests.cpp @@ -29,7 +29,7 @@ class ActiveMotionGeneratorTest : public ::testing::Test { RealtimeConfig::kIgnore)), robot_(RobotMock(robot_impl_mock_)){}; - std::unique_ptr startControl( + std::unique_ptr startControl( research_interface::robot::Move::ControllerMode controller_mode); using CurrentMotionGeneratorType = MotionGeneratorType; @@ -49,7 +49,7 @@ class ActiveMotionGeneratorTest : public ::testing::Test { }; template <> -std::unique_ptr ActiveMotionGeneratorTest::startControl( +std::unique_ptr ActiveMotionGeneratorTest::startControl( research_interface::robot::Move::ControllerMode controller_mode) { EXPECT_CALL(*robot_impl_mock_, startMotion(testing::_, testing::_, testing::_, testing::_)) .Times(1) @@ -59,7 +59,7 @@ std::unique_ptr ActiveMotionGeneratorTest::startC } template <> -std::unique_ptr ActiveMotionGeneratorTest::startControl( +std::unique_ptr ActiveMotionGeneratorTest::startControl( research_interface::robot::Move::ControllerMode controller_mode) { EXPECT_CALL(*robot_impl_mock_, startMotion(testing::_, testing::_, testing::_, testing::_)) .Times(1) @@ -69,17 +69,17 @@ std::unique_ptr ActiveMotionGeneratorTest::start } template <> -std::unique_ptr ActiveMotionGeneratorTest::startControl( +std::unique_ptr ActiveMotionGeneratorTest::startControl( research_interface::robot::Move::ControllerMode controller_mode) { EXPECT_CALL(*robot_impl_mock_, startMotion(testing::_, testing::_, testing::_, testing::_)) .Times(1) .WillOnce(::testing::Return(default_motion_id)); - return robot_.startCartesianPositionControl(controller_mode); + return robot_.startCartesianPoseControl(controller_mode); } template <> -std::unique_ptr ActiveMotionGeneratorTest::startControl( +std::unique_ptr ActiveMotionGeneratorTest::startControl( research_interface::robot::Move::ControllerMode controller_mode) { EXPECT_CALL(*robot_impl_mock_, startMotion(testing::_, testing::_, testing::_, testing::_)) .Times(1) diff --git a/test/active_torque_control_tests.cpp b/test/active_torque_control_tests.cpp index 0a3953fc..c564b6df 100644 --- a/test/active_torque_control_tests.cpp +++ b/test/active_torque_control_tests.cpp @@ -28,7 +28,7 @@ class ActiveTorqueControlTest : public ::testing::Test { RealtimeConfig::kIgnore)), robot(RobotMock(robot_impl_mock)){}; - std::unique_ptr startTorqueControl() { + std::unique_ptr startTorqueControl() { EXPECT_CALL(*robot_impl_mock, startMotion(testing::_, testing::_, testing::_, testing::_)) .Times(1) .WillOnce(::testing::Return(100));