From 83662d69da31cce941dd1cc4f992daa2b368d378 Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 2 Oct 2024 01:49:09 +0000 Subject: [PATCH] Helper function in example, exceptions in startForceMode --- examples/force_mode_example.cpp | 38 +++++++++++++++++-- .../control/script_command_interface.h | 6 +-- src/ur/ur_driver.cpp | 6 +-- 3 files changed, 40 insertions(+), 10 deletions(-) diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 4d72a745..29f35443 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -55,11 +55,21 @@ const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; +std::condition_variable g_program_running_cv_; +std::mutex g_program_running_mutex_; +bool g_program_running; + // We need a callback function to register. See UrDriver's parameters for details. void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex_); + g_program_running = program_running; + g_program_running_cv_.notify_one(); + } } void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) @@ -72,6 +82,17 @@ void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_ } } +bool waitForProgramRunning(int milliseconds = 100) +{ + std::unique_lock lk(g_program_running_mutex_); + if (g_program_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; +} + int main(int argc, char* argv[]) { urcl::setLogLevel(urcl::LogLevel::INFO); @@ -132,6 +153,15 @@ int main(int argc, char* argv[]) g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup))); + if (!g_my_driver->checkCalibration(CALIBRATION_CHECKSUM)) + { + URCL_LOG_ERROR("Calibration checksum does not match actual robot."); + URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into " + "the description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); + } + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main // loop. @@ -142,10 +172,10 @@ int main(int argc, char* argv[]) auto stopwatch_last = std::chrono::steady_clock::now(); auto stopwatch_now = stopwatch_last; // Make sure that external control script is running - for (int i = 0; i < 100; i++) + if (!waitForProgramRunning()) { - g_my_driver->writeKeepalive(); - g_my_driver->getDataPackage(); + URCL_LOG_ERROR("External Control script not running."); + return 1; } // 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 @@ -164,7 +194,7 @@ int main(int argc, char* argv[]) { 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 - 0.8, // damping_factor + 0.8, // damping_factor 0.8); // gain_scaling. See ScriptManual for details. } if (!success) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 07c17255..c6cb15f6 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -105,15 +105,15 @@ class ScriptCommandInterface : public ReverseInterface * 2: The force frame is not transformed. * 3: The force frame is transformed in a way such that its x-axis is the projection of the robot tcp velocity vector * onto the x-y plane of the force frame - * \param limits (Float) 6d vector. For compliant axes, these values are the maximum allowed tcp speed along/about the + * \param limits 6d vector. For compliant axes, these values are the maximum allowed tcp speed along/about the * axis. For non-compliant axes, these values are the maximum allowed deviation along/about an axis between the actual * tcp position and the one set by the program * - * \param damping_factor (Double) Sets the damping parameter in force mode. In range [0,1], default value is 0.025 + * \param damping_factor Sets the damping parameter in force mode. In range [0,1], default value is 0.025 * A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 * is no damping, here the robot will maintain the speed. * - * \param gain_scaling_factor (Double) Scales the gain in force mode. scaling parameter is in range [0,2], default + * \param gain_scaling_factor Scales the gain in force mode. scaling parameter is in range [0,2], default * is 0.5. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard * surfaces. * diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 2e2a5bfc..779251e9 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -377,7 +377,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ std::stringstream ss; ss << "The type should be 1, 2 or 3. The type is " << type; URCL_LOG_ERROR(ss.str().c_str()); - return false; + throw std::invalid_argument(ss.str().c_str()); } for (unsigned int i = 0; i < selection_vector.size(); ++i) { @@ -394,7 +394,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ return false; } - if (gain_scaling_factor < 0 || gain_scaling_factor > 2) + if (gain_scaling_factor > 2 || gain_scaling_factor < 0) { URCL_LOG_ERROR("The force mode gain scaling factor should be between 0 and 2, both inclusive."); return false; @@ -436,7 +436,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ std::stringstream ss; ss << "The type should be 1, 2 or 3. The type is " << type; URCL_LOG_ERROR(ss.str().c_str()); - return false; + throw std::invalid_argument(ss.str().c_str()); } for (unsigned int i = 0; i < selection_vector.size(); ++i) {