diff --git a/CMakeLists.txt b/CMakeLists.txt index 23681747..a2366ac9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(urcl src/primary/robot_message/error_code_message.cpp src/primary/robot_state/kinematics_info.cpp src/primary/robot_state/robot_mode_data.cpp + src/primary/robot_state/configuration_data.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.cpp diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 32a692fb..7093d921 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -35,6 +35,7 @@ #include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" #include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/configuration_data.h" namespace urcl { @@ -77,6 +78,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(KinematicsInfo& pkg) = 0; virtual bool consume(ErrorCodeMessage& pkg) = 0; virtual bool consume(RobotModeData& pkg) = 0; + virtual bool consume(ConfigurationData& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index b292dee8..fea1efdc 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -218,6 +218,25 @@ class PrimaryClient return robot_mode_data->is_protective_stopped_; } + /*! + * \brief Get the latest configuration data + * + * The configuration data will be updated in the background. This will always show the latest + * received configuration data independent of the time that has passed since receiving it. If no + * configuration data has been received yet, this will return a nullptr. + */ + std::shared_ptr getConfigurationData() + { + return consumer_->getConfigurationData(); + } + + /*! + * \brief Get the Robot type + * + * If no robot type data has been received yet, this will return UNDEFINED. + */ + RobotType getRobotType(); + private: /*! * \brief Reconnects the primary stream used to send program to the robot. diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 65445230..b167c653 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -170,6 +170,13 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return true; } + virtual bool consume(ConfigurationData& pkg) override + { + std::scoped_lock lock(configuration_data_mutex_); + configuration_data_ = std::make_shared(pkg); + return true; + } + /*! * \brief Set callback function which will be triggered whenever error code messages are received * @@ -216,6 +223,19 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return version_information_; } + /*! + * \brief Get the latest configuration data. + * + * The configuration data will be updated in the background. This will always show the latest + * received configuration data independent of the time that has passed since receiving it. If no + * configuration data has been received yet, this will return a nullptr. + */ + std::shared_ptr getConfigurationData() + { + std::scoped_lock lock(configuration_data_mutex_); + return configuration_data_; + } + private: std::function error_code_message_callback_; std::shared_ptr kinematics_info_; @@ -223,6 +243,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer std::shared_ptr robot_mode_; std::mutex version_information_mutex_; std::shared_ptr version_information_; + std::shared_ptr configuration_data_; + std::mutex configuration_data_mutex_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index cb73e55c..d707f758 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -30,6 +30,7 @@ #include "ur_client_library/primary/robot_message/version_message.h" #include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/configuration_data.h" namespace urcl { @@ -159,6 +160,8 @@ class PrimaryParser : public comm::Parser // return new MBD;*/ case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); + case RobotStateType::CONFIGURATION_DATA: + return new ConfigurationData(type); default: return new RobotState(type); } diff --git a/include/ur_client_library/primary/robot_state/configuration_data.h b/include/ur_client_library/primary/robot_state/configuration_data.h new file mode 100644 index 00000000..063da8b2 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/configuration_data.h @@ -0,0 +1,123 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#pragma once + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +#include + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The ConfigurationData class handles the configuration data sent via the primary UR interface. + */ +class ConfigurationData : public RobotState +{ +public: + struct JointPositionLimits + { + double joint_min_limit; + double joint_max_limit; + }; + + struct JointMotionLimits + { + double joint_max_speed; + double joint_max_acceleration; + }; + + ConfigurationData() = delete; + + /*! + * \brief Creates a new ConfigurationData object. + * + * \param type The type of RobotState message received + */ + ConfigurationData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a ConfigurationData object. + * + * \param pkg The ConfigurationData object to be copied + */ + ConfigurationData(const ConfigurationData& pkg); + + virtual ~ConfigurationData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + std::array joint_position_limits_; + std::array joint_motion_limits_; + double v_joint_default_; + double a_joint_default_; + double v_tool_default_; + double a_tool_default_; + double eq_radius_; + urcl::vector6d_t dh_a_; + urcl::vector6d_t dh_d_; + urcl::vector6d_t dh_alpha_; + urcl::vector6d_t dh_theta_; + int32_t masterboard_version_; + int32_t controller_box_type_; + int32_t robot_type_; + int32_t robot_sub_type_; +}; + +} // namespace primary_interface +} // namespace urcl diff --git a/include/ur_client_library/ur/datatypes.h b/include/ur_client_library/ur/datatypes.h index af6fecd2..c547a928 100644 --- a/include/ur_client_library/ur/datatypes.h +++ b/include/ur_client_library/ur/datatypes.h @@ -29,6 +29,7 @@ #pragma once #include +#include "ur_client_library/log.h" namespace urcl { @@ -86,6 +87,18 @@ enum class AnalogOutputType : int8_t VOLTAGE = 1 }; +enum class RobotType : int8_t +{ + UNDEFINED = -128, // This is not defined by UR but only inside this driver + UR5 = 1, + UR10 = 2, + UR3 = 3, + UR16 = 4, + UR20 = 7, + UR30 = 8, + UR15 = 9 +}; + inline std::string robotModeString(const RobotMode& mode) { switch (mode) @@ -186,4 +199,31 @@ inline std::string safetyStatusString(const SafetyStatus& status) throw std::invalid_argument(ss.str()); } } + +inline std::string robotTypeString(const RobotType& type) +{ + switch (type) + { + case RobotType::UR3: + return "UR3"; + case RobotType::UR5: + return "UR5"; + case RobotType::UR10: + return "UR10"; + case RobotType::UR15: + return "UR15"; + case RobotType::UR16: + return "UR16"; + case RobotType::UR20: + return "UR20"; + case RobotType::UR30: + return "UR30"; + default: + std::stringstream ss; + ss << "Unknown Robot Type: " << static_cast(type); + URCL_LOG_WARN(ss.str().c_str()); + return "UNDEFINED"; + } +} + } // namespace urcl diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 9671eabf..ade5b47a 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -279,5 +279,15 @@ std::shared_ptr PrimaryClient::getRobotVersion(bool blocking return consumer_->getVersionInformation(); } +RobotType PrimaryClient::getRobotType() +{ + std::shared_ptr configuration_data = consumer_->getConfigurationData(); + if (configuration_data == nullptr) + { + return RobotType::UNDEFINED; + } + return static_cast(configuration_data->robot_type_); +} + } // namespace primary_interface } // namespace urcl diff --git a/src/primary/robot_state/configuration_data.cpp b/src/primary/robot_state/configuration_data.cpp new file mode 100644 index 00000000..76e773d6 --- /dev/null +++ b/src/primary/robot_state/configuration_data.cpp @@ -0,0 +1,150 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/primary/robot_state/configuration_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ + +ConfigurationData::ConfigurationData(const ConfigurationData& pkg) : RobotState(RobotStateType::CONFIGURATION_DATA) +{ + joint_position_limits_ = pkg.joint_position_limits_; + joint_motion_limits_ = pkg.joint_motion_limits_; + v_joint_default_ = pkg.v_joint_default_; + a_joint_default_ = pkg.a_joint_default_; + v_tool_default_ = pkg.v_tool_default_; + a_tool_default_ = pkg.a_tool_default_; + eq_radius_ = pkg.eq_radius_; + dh_a_ = pkg.dh_a_; + dh_d_ = pkg.dh_d_; + dh_alpha_ = pkg.dh_alpha_; + dh_theta_ = pkg.dh_theta_; + masterboard_version_ = pkg.masterboard_version_; + controller_box_type_ = pkg.controller_box_type_; + robot_type_ = pkg.robot_type_; + robot_sub_type_ = pkg.robot_sub_type_; +} + +bool ConfigurationData::parseWith(comm::BinParser& bp) +{ + for (auto& joint_limits : joint_position_limits_) + { + bp.parse(joint_limits.joint_min_limit); + bp.parse(joint_limits.joint_max_limit); + } + for (auto& motion_limits : joint_motion_limits_) + { + bp.parse(motion_limits.joint_max_speed); + bp.parse(motion_limits.joint_max_acceleration); + } + bp.parse(v_joint_default_); + bp.parse(a_joint_default_); + bp.parse(v_tool_default_); + bp.parse(a_tool_default_); + bp.parse(eq_radius_); + bp.parse(dh_a_); + bp.parse(dh_d_); + bp.parse(dh_alpha_); + bp.parse(dh_theta_); + bp.parse(masterboard_version_); + bp.parse(controller_box_type_); + bp.parse(robot_type_); + bp.parse(robot_sub_type_); + + return true; +} + +bool ConfigurationData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ConfigurationData::toString() const +{ + std::stringstream os; + os << "ConfigurationData:" << std::endl; + os << "Joint position limits: [" << std::endl; + for (auto& limits : joint_position_limits_) + { + os << " {min: " << limits.joint_min_limit << ", max " << limits.joint_max_limit << "} " << std::endl; + } + os << "]" << std::endl; + os << "Joint motion limits: [" << std::endl; + for (auto& limits : joint_motion_limits_) + { + os << " {max joint speed: " << limits.joint_max_speed + << ", max joint acceleration: " << limits.joint_max_acceleration << "} " << std::endl; + } + os << "]" << std::endl; + os << "Velocity joint default: " << v_joint_default_ << std::endl; + os << "Acceleration joint default: " << a_joint_default_ << std::endl; + os << "Velocity tool default: " << v_tool_default_ << std::endl; + os << "Acceleration tool default: " << a_tool_default_ << std::endl; + os << "Equivalent radius: " << eq_radius_ << std::endl; + os << "dh_a: ["; + for (size_t i = 0; i < dh_a_.size(); ++i) + { + os << dh_a_[i] << " "; + } + os << "]" << std::endl; + + os << "dh_d: ["; + for (size_t i = 0; i < dh_d_.size(); ++i) + { + os << dh_d_[i] << " "; + } + os << "]" << std::endl; + + os << "dh_alpha: ["; + for (size_t i = 0; i < dh_alpha_.size(); ++i) + { + os << dh_alpha_[i] << " "; + } + os << "]" << std::endl; + os << "dh_theta: ["; + for (size_t i = 0; i < dh_theta_.size(); ++i) + { + os << dh_theta_[i] << " "; + } + os << "]" << std::endl; + + os << "Masterboard version: " << masterboard_version_ << std::endl; + os << "Controller box type: " << controller_box_type_ << std::endl; + os << "Robot type: " << robot_type_ << std::endl; + os << "Robot sub type: " << robot_sub_type_ << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index dda0cd11..6dd4ab2c 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -129,6 +129,8 @@ TEST_F(PrimaryClientTest, test_uninitialized_primary_client) // The client is not started yet, so the robot mode should be UNKNOWN ASSERT_EQ(client_->getRobotMode(), RobotMode::UNKNOWN); ASSERT_THROW(client_->isRobotProtectiveStopped(), UrException); + // The client is not started yet, so the robot type should be UNDEFINED + ASSERT_EQ(client_->getRobotType(), RobotType::UNDEFINED); } TEST_F(PrimaryClientTest, test_stop_command) @@ -177,6 +179,27 @@ TEST_F(PrimaryClientTest, test_stop_command) std::chrono::seconds(5))); } +TEST_F(PrimaryClientTest, test_configuration_data) +{ + EXPECT_NO_THROW(client_->start()); + + // Once we connect to the primary client we should receive configuration data + auto start_time = std::chrono::system_clock::now(); + const auto timeout = std::chrono::seconds(1); + auto config_data = client_->getConfigurationData(); + while (config_data == nullptr && std::chrono::system_clock::now() - start_time < timeout) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + config_data = client_->getConfigurationData(); + } + + // We should have received a configuration data package + EXPECT_NE(config_data, nullptr); + + // Robot type should no longer be undefined once we have received configuration data. + EXPECT_NE(client_->getRobotType(), RobotType::UNDEFINED); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);