From 535b083aa25ac46ec50681b92807bee9b097902d Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 9 Jul 2024 06:29:58 +0000 Subject: [PATCH] Bug fixes and updates to tests --- examples/force_mode_example.cpp | 27 +++++++++++++------ include/ur_client_library/ur/ur_driver.h | 34 +++++++++++++++--------- src/ur/ur_driver.cpp | 4 ++- tests/test_script_command_interface.cpp | 21 +++++++++++---- 4 files changed, 60 insertions(+), 26 deletions(-) diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 409ff590..fa3713bc 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -149,14 +149,25 @@ int main(int argc, char* argv[]) } // Task frame at the robot's base with limits being large enough to cover the whole workspace // Compliance in z axis and rotation around z axis - - bool success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all - { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual - , - 0.8, 0.8); // for details. + bool success; + if (g_my_driver->getVersion().major < 5) + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual + , + 0.8); // for details. + else + { + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual + , + 0.8, 0.8); // for details. + } if (!success) { URCL_LOG_ERROR("Failed to start force mode."); diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e6cf099b..02d4043a 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -128,15 +128,25 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ - [[deprecated("Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has " - "been deprecated. Force mode parameters should be specified with each activiation of force mode, and " - "can be set in the function call to start force mode.")]] - UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, - const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port, - double force_mode_damping, double force_mode_gain_scaling = 0.5); + [[deprecated( + "Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has " + "been deprecated. Force mode parameters should be specified with each activiation of force mode, and " + "can be set in the function call to start force mode.")]] UrDriver(const std::string& robot_ip, + const std::string& script_file, + const std::string& output_recipe_file, + const std::string& input_recipe_file, + std::function handle_program_state, + bool headless_mode, + std::unique_ptr tool_comm_setup, + const uint32_t reverse_port, + const uint32_t script_sender_port, + int servoj_gain, double servoj_lookahead_time, + bool non_blocking_read, + const std::string& reverse_ip, + const uint32_t trajectory_port, + const uint32_t script_command_port, + double force_mode_damping, + double force_mode_gain_scaling = 0.5); /*! * \brief Constructs a new UrDriver object. @@ -439,9 +449,9 @@ class UrDriver * \returns True, if the write was performed successfully, false otherwise. */ [[deprecated("Starting force mode without specifying the force mode damping factor and gain scale factor has been " - "deprecated. These values should be given with each function call.")]] - bool startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench, - const unsigned int type, const vector6d_t& limits); + "deprecated. These values should be given with each function call.")]] bool + startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench, + const unsigned int type, const vector6d_t& limits); /*! * \brief Stop force mode and put the robot into normal operation mode. diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index ed67b475..2e2a5bfc 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -142,7 +142,9 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } begin_replace << "set_tool_voltage(" << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; - begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", " + begin_replace << "set_tool_communication(" + << "True" + << ", " << tool_comm_setup->getBaudRate() << ", " << static_cast::type>(tool_comm_setup->getParity()) << ", " << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " << tool_comm_setup->getTxIdleChars() << ")"; diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 682adba6..d834502a 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -64,11 +64,11 @@ class ScriptCommandInterfaceTest : public ::testing::Test void readMessage(int32_t& command, std::vector& message) { - // Max message length is 26 - uint8_t buf[sizeof(int32_t) * 26]; + // Max message length is 28 + uint8_t buf[sizeof(int32_t) * 28]; uint8_t* b_pos = buf; size_t read = 0; - size_t remainder = sizeof(int32_t) * 26; + size_t remainder = sizeof(int32_t) * 28; while (remainder > 0) { if (!TCPSocket::read(b_pos, remainder, read)) @@ -252,7 +252,10 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode) urcl::vector6d_t wrench = { 20, 0, 40, 0, 0, 0 }; int32_t force_mode_type = 2; urcl::vector6d_t limits = { 0.1, 0.1, 0.1, 0.785, 0.785, 1.57 }; - script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits); + double damping = 0.8; + double gain_scaling = 0.8; + script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits, damping, + gain_scaling); int32_t command; std::vector message; @@ -298,8 +301,16 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode) EXPECT_EQ(received_limits[i], limits[i]); } + // Test damping return + double received_damping = (double)message[25] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_damping, damping); + + // Test Gain scaling return + double received_gain = (double)message[26] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_gain, gain_scaling); + // The rest of the message should be zero - int32_t message_sum = std::accumulate(std::begin(message) + 25, std::end(message), 0); + int32_t message_sum = std::accumulate(std::begin(message) + 27, std::end(message), 0); int32_t expected_message_sum = 0; EXPECT_EQ(message_sum, expected_message_sum);