Helper function in example, exceptions in startForceMode

examples/force_mode_example.cpp

@@ -55,11 +55,21 @@ const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 std::unique_ptr<UrDriver> g_my_driver;
 std::unique_ptr<DashboardClient> 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<std::mutex> 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<std::mutex> 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)

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.
    *

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)
   {