Add documentation for force_mode example

Author: urfeex

doc/examples.rst

@@ -11,4 +11,5 @@ starting point for your own applications.
    :maxdepth: 1
 
    examples/dashboard_client
+   examples/force_mode
    examples/ur_driver

doc/examples/force_mode.rst

@@ -0,0 +1,63 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/force_mode.rst
+
+Force Mode example
+==================
+
+The ``ur_client_library`` supports leveraging the robot's force mode directly. An example on how to
+use it can be found in `force_mode_example.cpp <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/force_mode_example.cpp>`_.
+
+In order to utilize force mode, we'll have to create and initialize a full ``UrDriver`` object
+first:
+
+.. literalinclude:: ../../examples/force_mode_example.cpp
+   :language: c++
+   :caption: examples/force_mode_example.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Now the robot is ready to receive a program
+   :end-at: // End of initialization
+
+Start force mode
+----------------
+
+After that, we can start force mode by calling the ``startForceMode()`` function:
+
+.. literalinclude:: ../../examples/force_mode_example.cpp
+   :language: c++
+   :caption: examples/force_mode_example.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Start force mode
+   :end-at: if (!success)
+
+All parameters for the force mode are included into the ``startForceMode()`` function call. If you
+want to change the parameters, e.g. change the forces applied, you can simply call
+``startForceMode()`` again with the new parameters.
+
+.. note::
+   CB3 robots don't support specifying force_mode's ``gain_scaling``, so there are two different
+   functions available.
+
+Once force mode is started successfully, we'll have to send keepalive messages to the robot in
+order to keep the communication active:
+
+.. literalinclude:: ../../examples/force_mode_example.cpp
+   :language: c++
+   :caption: examples/force_mode_example.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: std::chrono::duration<double> time_done(0);
+   :end-at: URCL_LOG_INFO("Timeout reached.");
+
+Stop force mode
+---------------
+
+Once finished, force_mode can be stopped by calling ``endForceMode()``.
+
+.. literalinclude:: ../../examples/force_mode_example.cpp
+   :language: c++
+   :caption: examples/force_mode_example.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: endForceMode()
+   :end-at: endForceMode()

examples/force_mode_example.cpp

@@ -162,72 +162,55 @@ int main(int argc, char* argv[])
                    "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.
-  g_my_driver->startRTDECommunication();
-
-  std::chrono::duration<double> time_done(0);
-  std::chrono::duration<double> timeout(second_to_run);
-  auto stopwatch_last = std::chrono::steady_clock::now();
-  auto stopwatch_now = stopwatch_last;
   // Make sure that external control script is running
   if (!waitForProgramRunning())
   {
     URCL_LOG_ERROR("External Control script not running.");
     return 1;
   }
+  // End of initialization -- We've started the external control program, which means we have to
+  // write keepalive signals from now on. Otherwise the connection will be dropped.
+
+  // Start force mode
   // 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;
   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
+    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, -2, 0, 0, 0 },  // Press in -z direction
+                                          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.
+                                          0.005);  // 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
+    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, -2, 0, 0, 0 },  // Press in -z direction
+                                          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.
+                                          0.005,                               // damping_factor
+                                          1.0);  // gain_scaling. See ScriptManual for details.
   }
   if (!success)
   {
     URCL_LOG_ERROR("Failed to start force mode.");
     return 1;
   }
 
-  while (true)
+  std::chrono::duration<double> time_done(0);
+  std::chrono::duration<double> timeout(second_to_run);
+  auto stopwatch_last = std::chrono::steady_clock::now();
+  auto stopwatch_now = stopwatch_last;
+  while (time_done < timeout || second_to_run.count() == 0)
   {
-    // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
-    // robot will effectively be in charge of setting the frequency of this loop.
-    // In a real-world application this thread should be scheduled with real-time priority in order
-    // to ensure that this is called in time.
-    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
-    if (data_pkg)
-    {
-      g_my_driver->writeKeepalive();
-
-      if (time_done > timeout && second_to_run.count() != 0)
-      {
-        URCL_LOG_INFO("Timeout reached.");
-        break;
-      }
-    }
-    else
-    {
-      URCL_LOG_WARN("Could not get fresh data package from robot");
-    }
+    g_my_driver->writeKeepalive();
 
     stopwatch_now = std::chrono::steady_clock::now();
     time_done += stopwatch_now - stopwatch_last;
     stopwatch_last = stopwatch_now;
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
   }
+  URCL_LOG_INFO("Timeout reached.");
   g_my_driver->endForceMode();
 }