diff --git a/CMakeLists.txt b/CMakeLists.txt index 7910df03..265fb717 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,7 +119,6 @@ endif() target_include_directories(franka PUBLIC $ - $ $ ) @@ -173,13 +172,6 @@ install(EXPORT FrankaTargets DESTINATION ${INSTALL_CMAKE_CONFIG_DIR} ) -set(ROBOT_MODELS_PATH ${CMAKE_INSTALL_PREFIX}/share/robot_models) -configure_file(cmake/config.h.in config.h) - -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/robot_models - DESTINATION share -) - ## Subprojects # Ignore find_package(Franka) in subprojects. diff --git a/cmake/config.h.in b/cmake/config.h.in deleted file mode 100644 index ddd4008c..00000000 --- a/cmake/config.h.in +++ /dev/null @@ -1 +0,0 @@ -#define ROBOT_MODELS_PATH "@ROBOT_MODELS_PATH@" diff --git a/common b/common index dd768c88..22b08375 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit dd768c882855801b64b37ab82a05a4db11fa8166 +Subproject commit 22b083750c45469ac22ed29f97a84ffd755ef8ce diff --git a/include/franka/commands/get_robot_model_command.hpp b/include/franka/commands/get_robot_model_command.hpp new file mode 100644 index 00000000..678ab51f --- /dev/null +++ b/include/franka/commands/get_robot_model_command.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include + +namespace franka { + +struct GetRobotModelResult { + std::string robot_model_urdf; +}; + +} // namespace franka diff --git a/include/franka/model.h b/include/franka/model.h index 5d9730b2..8b00507c 100644 --- a/include/franka/model.h +++ b/include/franka/model.h @@ -5,7 +5,6 @@ #include #include -#include #include #include #include @@ -63,7 +62,7 @@ class Model { * * @throw ModelException if the model library cannot be loaded. */ - explicit Model(franka::Network& network); + explicit Model(franka::Network& network, const std::string& urdf_model); /** * Creates a new Model instance only for the tests. @@ -299,7 +298,6 @@ class Model { private: std::unique_ptr library_; std::unique_ptr robot_model_; - std::string k_urdf_path_ = std::string(ROBOT_MODELS_PATH) + "/fr3.urdf"; }; } // namespace franka diff --git a/include/franka/robot.h b/include/franka/robot.h index 20d42dd7..94c640a9 100644 --- a/include/franka/robot.h +++ b/include/franka/robot.h @@ -13,6 +13,7 @@ #include #include #include +#include /** * @file robot.h @@ -455,6 +456,14 @@ class Robot { * @{ */ + /** + * @throw CommandException if the Control reports an error. + * @throw NetworkException if the connection is lost, e.g. after a timeout. + * + * @return std::string Provides the URDF model of the attached robot arm as json string + */ + auto getRobotModel() -> std::string; + /** * Changes the collision behavior. * diff --git a/include/franka/robot_model.h b/include/franka/robot_model.h index 716ddd0b..fe5e86df 100644 --- a/include/franka/robot_model.h +++ b/include/franka/robot_model.h @@ -11,6 +11,8 @@ #include "franka/robot_model_base.h" +namespace franka { + /** * Implements RobotModelBase using Pinocchio. */ @@ -57,4 +59,6 @@ class RobotModel : public RobotModelBase { pinocchio::Inertia initial_last_link_inertia_; pinocchio::FrameIndex last_link_frame_index_; pinocchio::JointIndex last_joint_index_; -}; \ No newline at end of file +}; + +} // namespace franka diff --git a/robot_models/fr3.urdf b/robot_models/fr3.urdf deleted file mode 100644 index daf4d6f0..00000000 --- a/robot_models/fr3.urdf +++ /dev/null @@ -1,230 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/model.cpp b/src/model.cpp index 86c57b42..b4ffb1ca 100644 --- a/src/model.cpp +++ b/src/model.cpp @@ -14,20 +14,6 @@ #include "model_library.h" #include "network.h" -std::string readURDFFromFile(const std::string& filename) { - std::ifstream file(filename); - if (!file.is_open()) { - std::cerr << "Error opening file: " << filename << std::endl; - return ""; - } - - std::ostringstream oss; - oss << file.rdbuf(); - file.close(); - - return oss.str(); -} - using namespace std::string_literals; // NOLINT(google-build-using-namespace) namespace franka { @@ -38,10 +24,9 @@ Frame operator++(Frame& frame, int /* dummy */) noexcept { return original; } -Model::Model(Network& network) : library_{new ModelLibrary(network)} { - // TODO(baris) workaround for now, get the robot specific urdf from the robot in the future - auto urdf_string = readURDFFromFile(k_urdf_path_); - robot_model_ = std::make_unique(urdf_string); +Model::Model(Network& network, const std::string& urdf_model) + : library_{new ModelLibrary(network)} { + robot_model_ = std::make_unique(urdf_model); } // for the tests diff --git a/src/network.h b/src/network.h index dce98ae4..9509c5a6 100644 --- a/src/network.h +++ b/src/network.h @@ -18,8 +18,12 @@ #include +#include + namespace franka { +constexpr std::chrono::milliseconds kTimeout{10}; + class Network { public: Network(const std::string& franka_address, @@ -195,7 +199,13 @@ uint32_t Network::tcpSendRequest(TArgs&&... args) try { sizeof(typename T::template Message)), typename T::Request(std::forward(args)...)); - tcp_socket_.sendBytes(&message, sizeof(message)); + // NOLINTNEXTLINE + if constexpr (std::is_same_v) { + auto serialized_request = message.serialize(); + tcp_socket_.sendBytes(serialized_request.data(), serialized_request.size()); + } else { // NOLINT(readability-misleading-indentation) + tcp_socket_.sendBytes(&message, sizeof(message)); + } return message.header.command_id; } catch (const Poco::Exception& e) { @@ -235,7 +245,7 @@ typename T::Response Network::tcpBlockingReceiveResponse(uint32_t command_id, decltype(received_responses_)::const_iterator it; do { lock.lock(); - tcpReadFromBuffer(10ms); + tcpReadFromBuffer(kTimeout); it = received_responses_.find(command_id); lock.unlock(); std::this_thread::yield(); @@ -258,6 +268,31 @@ typename T::Response Network::tcpBlockingReceiveResponse(uint32_t command_id, return message.getInstance(); } +template <> +inline research_interface::robot::GetRobotModel::Response +Network::tcpBlockingReceiveResponse( + uint32_t command_id, + std::vector* /*vl_buffer*/) { + using namespace std::literals::chrono_literals; // NOLINT(google-build-using-namespace) + std::unique_lock lock(tcp_mutex_, std::defer_lock); + decltype(received_responses_)::const_iterator it; + do { + lock.lock(); + tcpReadFromBuffer(kTimeout); + it = received_responses_.find(command_id); + lock.unlock(); + std::this_thread::yield(); + } while (it == received_responses_.end()); + + auto get_robot_model = + research_interface::robot::GetRobotModel::Message< + research_interface::robot::GetRobotModel::Response>::deserialize(it->second) + .getInstance(); + + received_responses_.erase(it); + return get_robot_model; +} + template void connect(Network& network, uint16_t* ri_version) { uint32_t command_id = network.tcpSendRequest(network.udpPort()); diff --git a/src/robot.cpp b/src/robot.cpp index a8cf037d..572257fe 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -192,6 +192,12 @@ RobotState Robot::readOnce() { return impl_->readOnce(); } +auto Robot::getRobotModel() -> std::string { + auto get_robot_model = + impl_->executeCommand(); + return get_robot_model.robot_model_urdf; +} + void Robot::setCollisionBehavior(const std::array& lower_torque_thresholds_acceleration, const std::array& upper_torque_thresholds_acceleration, const std::array& lower_torque_thresholds_nominal, @@ -306,7 +312,7 @@ void Robot::stop() { } Model Robot::loadModel() { - return impl_->loadModel(); + return impl_->loadModel(getRobotModel()); } Model Robot::loadModel(std::unique_ptr robot_model) { diff --git a/src/robot_impl.cpp b/src/robot_impl.cpp index 6ad17951..7b1ae412 100644 --- a/src/robot_impl.cpp +++ b/src/robot_impl.cpp @@ -409,8 +409,8 @@ void Robot::Impl::cancelMotion(uint32_t motion_id) { current_move_controller_mode_ = research_interface::robot::ControllerMode::kOther; } -Model Robot::Impl::loadModel() const { - return Model(*network_); +Model Robot::Impl::loadModel(const std::string& urdf_model) const { + return Model(*network_, urdf_model); } // for the tests diff --git a/src/robot_impl.h b/src/robot_impl.h index df9caebf..0b8d8279 100644 --- a/src/robot_impl.h +++ b/src/robot_impl.h @@ -14,6 +14,7 @@ #include #include +#include "franka/commands/get_robot_model_command.hpp" #include "logger.h" #include "network.h" #include "robot_control.h" @@ -83,10 +84,10 @@ class Robot::Impl : public RobotControl { */ void finishMotion(uint32_t motion_id, const Torques& control_input); - template - uint32_t executeCommand(TArgs... /* args */); + template + ReturnType executeCommand(TArgs... /* args */); - Model loadModel() const; + Model loadModel(const std::string& urdf_model) const; // for the unit tests Model loadModel(std::unique_ptr robot_model) const; @@ -178,6 +179,11 @@ class Robot::Impl : public RobotControl { } } + template >> + ReturnType handleCommandResponse(const typename T::Response& response) const; + research_interface::robot::RobotCommand sendRobotCommand( const research_interface::robot::MotionGeneratorCommand* motion_command, const research_interface::robot::ControllerCommand* control_command) const; @@ -201,6 +207,35 @@ class Robot::Impl : public RobotControl { uint64_t message_id_; }; +template <> +inline GetRobotModelResult +Robot::Impl::handleCommandResponse( + const research_interface::robot::GetRobotModel::Response& response) const { + using namespace std::string_literals; // NOLINT(google-build-using-namespace) + + switch (response.status) { + case research_interface::robot::GetRobotModel::Status::kSuccess: + return GetRobotModelResult{.robot_model_urdf = response.robot_model}; + case research_interface::robot::GetRobotModel::Status::kCommandNotPossibleRejected: + throw CommandException("libfranka: "s + + research_interface::robot::CommandTraits< + research_interface::robot::GetRobotModel>::kName + + commandNotPossibleMsg()); + case research_interface::robot::GetRobotModel::Status:: + kCommandRejectedDueToActivatedSafetyFunctions: + throw CommandException("libfranka: "s + + research_interface::robot::CommandTraits< + research_interface::robot::GetRobotModel>::kName + + " command rejected due to activated safety function! Please disable " + "all safety functions."); + default: + throw ProtocolException("libfranka: Unexpected response while handling "s + + research_interface::robot::CommandTraits< + research_interface::robot::GetRobotModel>::kName + + " command!"); + } +} + template <> inline void Robot::Impl::handleCommandResponse( const research_interface::robot::Move::Response& response) const { @@ -368,12 +403,24 @@ inline void Robot::Impl::handleCommandResponse -uint32_t Robot::Impl::executeCommand(TArgs... args) { +template +ReturnType Robot::Impl::executeCommand(TArgs... args) { uint32_t command_id = network_->tcpSendRequest(args...); typename T::Response response = network_->tcpBlockingReceiveResponse(command_id); handleCommandResponse(response); return command_id; } +template <> +inline GetRobotModelResult +Robot::Impl::executeCommand() { + uint32_t command_id = network_->tcpSendRequest(); + research_interface::robot::GetRobotModel::Response response = + network_->tcpBlockingReceiveResponse(command_id); + auto get_robot_model_result = + handleCommandResponse( + response); + return get_robot_model_result; +} + } // namespace franka diff --git a/src/robot_model.cpp b/src/robot_model.cpp index 229180a3..fb31d73d 100644 --- a/src/robot_model.cpp +++ b/src/robot_model.cpp @@ -2,6 +2,8 @@ // Use of this source code is governed by the Apache-2.0 license, see LICENSE #include "franka/robot_model.h" +namespace franka { + RobotModel::RobotModel(const std::string& urdf) { pinocchio::urdf::buildModelFromXML(urdf, pinocchio_model_); @@ -101,4 +103,6 @@ void RobotModel::mass(const std::array& q, data.M.transpose().triangularView(); std::copy(data.M.data(), data.M.data() + data.M.size(), m_ne.begin()); -} \ No newline at end of file +} + +} // namespace franka diff --git a/test/mock_server.h b/test/mock_server.h index 53ed0314..274b04d1 100644 --- a/test/mock_server.h +++ b/test/mock_server.h @@ -215,7 +215,11 @@ template typename T::Request MockServer::receiveRequest(Socket& tcp_socket, typename T::Header* header_ptr) { typename T::template Message request_message; - tcp_socket.receiveBytes(&request_message, sizeof(request_message)); + if constexpr (std::is_same::value) { + tcp_socket.receiveBytes(&request_message.header, sizeof(request_message.header)); + } else { + tcp_socket.receiveBytes(&request_message, sizeof(request_message)); + } if (header_ptr != nullptr) { *header_ptr = request_message.header; } @@ -231,6 +235,19 @@ void MockServer::sendResponse(Socket& tcp_socket, tcp_socket.sendBytes(&response_message, sizeof(response_message)); } +template <> +template <> +inline void MockServer::sendResponse( + Socket& tcp_socket, + const research_interface::robot::GetRobotModel::Header& header, + const research_interface::robot::GetRobotModel::Response& response) { + research_interface::robot::GetRobotModel::Message< + research_interface::robot::GetRobotModel::Response> + response_message(header, response); + auto byte_vector = response_message.serialize(); + tcp_socket.sendBytes(byte_vector.data(), byte_vector.size()); +} + template template void MockServer::handleCommand( diff --git a/test/model_tests.cpp b/test/model_tests.cpp index 4baa1f84..cc83c429 100644 --- a/test/model_tests.cpp +++ b/test/model_tests.cpp @@ -114,6 +114,12 @@ TEST(InvalidModel, ThrowsIfNoModelReceived) { RobotMockServer server; franka::Robot robot("127.0.0.1"); + server + .waitForCommand([this](const typename GetRobotModel::Request& /*request*/) { + return GetRobotModel::Response(GetRobotModel::Status::kSuccess); + }) + .spinOnce(); + server .waitForCommand([&](const LoadModelLibrary::Request&) { return LoadModelLibrary::Response(LoadModelLibrary::Status::kError); diff --git a/test/robot_command_tests.cpp b/test/robot_command_tests.cpp index 38c66dee..f91fb486 100644 --- a/test/robot_command_tests.cpp +++ b/test/robot_command_tests.cpp @@ -18,6 +18,7 @@ using franka::Torques; using research_interface::robot::AutomaticErrorRecovery; using research_interface::robot::Connect; +using research_interface::robot::GetRobotModel; using research_interface::robot::LoadModelLibrary; using research_interface::robot::Move; using research_interface::robot::SetCartesianImpedance; @@ -29,6 +30,8 @@ using research_interface::robot::SetLoad; using research_interface::robot::SetNEToEE; using research_interface::robot::StopMove; +const std::string kExpectedModelString = "test_string"; + template class Command : public ::testing::Test { public: @@ -137,6 +140,11 @@ bool Command::compare(const AutomaticErrorRecovery::Requ return true; } +template <> +bool Command::compare(const GetRobotModel::Request&, const GetRobotModel::Request&) { + return true; +} + template <> bool Command::compare(const StopMove::Request&, const StopMove::Request&) { return true; @@ -210,6 +218,11 @@ AutomaticErrorRecovery::Request Command::getExpected() { return AutomaticErrorRecovery::Request(); } +template <> +GetRobotModel::Request Command::getExpected() { + return GetRobotModel::Request(); +} + template <> StopMove::Request Command::getExpected() { return StopMove::Request(); @@ -226,7 +239,15 @@ typename T::Response Command::createResponse(const typename T::Request&, return typename T::Response(status); } -using CommandTypes = ::testing::Types +typename GetRobotModel::Response Command::createResponse( + const typename GetRobotModel::Request&, + const typename GetRobotModel::Status status) { + return typename GetRobotModel::Response(status, kExpectedModelString); +} + +using CommandTypes = ::testing::Types) { + EXPECT_NO_THROW((robot.executeCommand())); + } else { + EXPECT_NO_THROW(TestFixture::executeCommand(robot)); + } } TYPED_TEST(Command, CanSendAndReceiveRejected) { @@ -271,7 +296,12 @@ TYPED_TEST(Command, CanSendAndReceiveRejected) { }) .spinOnce(); - EXPECT_THROW(TestFixture::executeCommand(robot), CommandException); + if constexpr (std::is_same_v) { + EXPECT_THROW((robot.executeCommand()), + CommandException); + } else { + EXPECT_THROW(TestFixture::executeCommand(robot), CommandException); + } } TYPED_TEST(Command, ThrowsProtocolExceptionIfInvalidResponseReceived) { @@ -289,7 +319,12 @@ TYPED_TEST(Command, ThrowsProtocolExceptionIfInvalidResponseReceived) { }) .spinOnce(); - EXPECT_THROW(TestFixture::executeCommand(robot), ProtocolException); + if constexpr (std::is_same_v) { + EXPECT_THROW((robot.executeCommand()), + ProtocolException); + } else { + EXPECT_THROW(TestFixture::executeCommand(robot), ProtocolException); + } } INSTANTIATE_TEST_CASE_P( diff --git a/test/robot_model_tests.cpp b/test/robot_model_tests.cpp index 567af400..2be9e11a 100644 --- a/test/robot_model_tests.cpp +++ b/test/robot_model_tests.cpp @@ -37,7 +37,7 @@ std::string readFileToString(const std::string& filename) { class RobotModelTest : public ::testing::Test { protected: - std::unique_ptr model; + std::unique_ptr model; std::array q = {0, 0, 0, 0, 0, 0, 0}; std::array dq = {0, 0, 0, 0, 0, 0, 0}; std::array unit_dq = {1, 1, 1, 1, 1, 1, 1}; @@ -56,7 +56,7 @@ class RobotModelTest : public ::testing::Test { RobotModelTest() { auto urdf_string = readFileToString(urdf_path); - model = std::make_unique(urdf_string); + model = std::make_unique(urdf_string); } }; @@ -231,4 +231,4 @@ TEST_F(RobotModelTest, givenNoLoad_whenComputingInertia_thenCloseToBaseline) { for (size_t i = 0; i < test_inertia.size(); ++i) { ASSERT_NEAR(test_inertia[i], expected_inertia[i], kDeltaInertia); } -} \ No newline at end of file +}