Giving force mode parameters as arguments when calling startForceMode (#208)

Co-authored-by: Felix Exner <[email protected]>

Author: URJala

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);
@@ -130,7 +151,16 @@ int main(int argc, char* argv[])
   std::unique_ptr<ToolCommSetup> tool_comm_setup;
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
-                                 std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+                                 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
@@ -141,15 +171,37 @@ int main(int argc, char* argv[])
   std::chrono::duration<double> timeout(second_to_run);
   auto stopwatch_last = std::chrono::steady_clock::now();
   auto stopwatch_now = stopwatch_last;
-  g_my_driver->writeKeepalive();
+  // Make sure that external control script is running
+  if (!waitForProgramRunning())
+  {
+    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
-  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 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
+                                          0.025);  // damping_factor. See ScriptManual 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
+                                          0.025,                               // damping_factor
+                                          0.8);  // gain_scaling. See ScriptManual for details.
+  }
+  if (!success)
+  {
+    URCL_LOG_ERROR("Failed to start force mode.");
+    return 1;
+  }
 
   while (true)
   {

include/ur_client_library/control/script_command_interface.h

@@ -105,14 +105,23 @@ 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 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 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.
+   *
    * \returns True, if the write was performed successfully, false otherwise.
    */
   bool startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector, const vector6d_t* wrench,
-                      const unsigned int type, const vector6d_t* limits);
+                      const unsigned int type, const vector6d_t* limits, double damping_factor,
+                      double gain_scaling_factor);
 
   /*!
    * \brief Stop force mode and put the robot into normal operation mode.
@@ -178,7 +187,7 @@ class ScriptCommandInterface : public ReverseInterface
   };
 
   bool client_connected_;
-  static const int MAX_MESSAGE_LENGTH = 26;
+  static const int MAX_MESSAGE_LENGTH = 28;
 
   std::function<void(ToolContactResult)> handle_tool_contact_result_;
 };

include/ur_client_library/exceptions.h

@@ -32,6 +32,7 @@
 #include <chrono>
 #include <stdexcept>
 #include <sstream>
+#include "ur/version_information.h"
 
 namespace urcl
 {
@@ -124,5 +125,67 @@ class TimeoutException : public UrException
 private:
   std::string text_;
 };
+
+class IncompatibleRobotVersion : public UrException
+{
+public:
+  explicit IncompatibleRobotVersion() = delete;
+  explicit IncompatibleRobotVersion(const std::string& text, const VersionInformation& minimum_robot_version,
+                                    const VersionInformation& actual_robot_version)
+    : std::runtime_error(text)
+  {
+    std::stringstream ss;
+    ss << text << "\n"
+       << "The requested feature is incompatible with the connected robot. Minimum required Polyscope version: "
+       << minimum_robot_version << ", actual Polyscope version: " << actual_robot_version;
+    text_ = ss.str();
+  }
+  virtual const char* what() const noexcept override
+  {
+    return text_.c_str();
+  }
+
+private:
+  std::string text_;
+};
+
+class InvalidRange : public UrException
+{
+private:
+  std::string text_;
+
+public:
+  explicit InvalidRange() = delete;
+  explicit InvalidRange(std::string text) : std::runtime_error(text)
+  {
+    text_ = text;
+  }
+  virtual const char* what() const noexcept override
+  {
+    return text_.c_str();
+  }
+};
+
+class MissingArgument : public UrException
+{
+private:
+  std::string text_;
+
+public:
+  explicit MissingArgument() = delete;
+  explicit MissingArgument(std::string text, std::string function_name, std::string argument_name, float default_value)
+    : std::runtime_error("")
+  {
+    std::stringstream ss;
+    ss << text << "\nMissing argument when calling function: " << function_name
+       << ". \nArgument missing: " << argument_name
+       << ". \nSet to default value if not important, default value is: " << default_value;
+    text_ = ss.str();
+  }
+  virtual const char* what() const noexcept override
+  {
+    return text_.c_str();
+  }
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED