Giving force mode parameters as arguments when calling 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);
@@ -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/ur/ur_driver.h

@@ -92,16 +92,61 @@ class UrDriver
    * trajectory forwarding.
    * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be
    * executed locally on the robot.
-   * \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)
    */
   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<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port = 50001,
            const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
            bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003,
-           const uint32_t script_command_port = 50004, double force_mode_damping = 0.025,
-           double force_mode_gain_scaling = 0.5);
+           const uint32_t script_command_port = 50004);
+
+  /*!
+   * \brief Constructs a new UrDriver object.
+   * \param robot_ip IP-address under which the robot is reachable.
+   * \param script_file URScript file that should be sent to the robot.
+   * \param output_recipe_file Filename where the output recipe is stored in.
+   * \param input_recipe_file Filename where the input recipe is stored in.
+   * \param handle_program_state Function handle to a callback on program state changes. For this to
+   * work, the URScript program will have to send keepalive signals to the \p reverse_port. I no
+   * keepalive signal can be read, program state will be false.
+   * \param headless_mode Parameter to control if the driver should be started in headless mode.
+   * \param tool_comm_setup Configuration for using the tool communication.
+   * calibration reported by the robot.
+   * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver
+   * and the robot controller.
+   * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. If
+   * the robot cannot connect to this port, `External Control` will stop immediately.
+   * \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
+   * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
+   * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
+   * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP
+   * address of the interface that is used for connecting to the robot's RTDE port will be used.
+   * \param trajectory_port Port used for sending trajectory points to the robot in case of
+   * trajectory forwarding.
+   * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be
+   * executed locally on the robot.
+   * \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<void(bool)> handle_program_state,
+                                                                         bool headless_mode,
+                                                                         std::unique_ptr<ToolCommSetup> 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.
@@ -134,7 +179,7 @@ class UrDriver
    */
   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<void(bool)> handle_program_state, bool headless_mode,
-           std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
+           std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
            const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
            double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "",
            const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004,
@@ -340,10 +385,73 @@ class UrDriver
    * 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
+   * 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.
+   *
    * \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);
+
+  /*!
+   * \brief Start the robot to be controlled in force mode.
+   *
+   * \param task_frame A pose vector that defines the force frame relative to the base frame
+   * \param selection_vector A 6d vector of 0s and 1s. 1 means that the robot will be compliant in the corresponding
+   * axis of the task frame
+   * \param wrench 6d vector of forces/torques [x,y,z,rotX,rotY,rotZ] that the robot will apply to its environment. The
+   * robot adjusts its position along/about compliant axis in order to achieve the specified force/torque. Values have
+   * no effect for non-compliant axes.
+   * \param type An integer [1;3] specifying how the robot interprets the force frame.
+   *  1: The force frame is transformed in a way such that its y-axis is aligned with a vector pointing from the robot
+   *  tcp towards the origin of the force frame
+   *  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 (double) 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
+   * 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 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, double damping_factor,
+                      double gain_scaling_factor);
+
+  /*!
+   * \brief Start the robot to be controlled in force mode.
+   *
+   * \param task_frame A pose vector that defines the force frame relative to the base frame
+   * \param selection_vector A 6d vector of 0s and 1s. 1 means that the robot will be compliant in the corresponding
+   * axis of the task frame
+   * \param wrench 6d vector of forces/torques [x,y,z,rotX,rotY,rotZ] that the robot will apply to its environment. The
+   * robot adjusts its position along/about compliant axis in order to achieve the specified force/torque. Values have
+   * no effect for non-compliant axes.
+   * \param type An integer [1;3] specifying how the robot interprets the force frame.
+   *  1: The force frame is transformed in a way such that its y-axis is aligned with a vector pointing from the robot
+   *  tcp towards the origin of the force frame
+   *  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 (double) 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
+   *
+   * \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);
 
   /*!
    * \brief Stop force mode and put the robot into normal operation mode.
@@ -500,6 +608,9 @@ class UrDriver
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> primary_stream_;
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> secondary_stream_;
 
+  double force_mode_gain_scale_factor_ = 0.5;
+  double force_mode_damping_factor_ = 0.025;
+
   uint32_t servoj_gain_;
   double servoj_lookahead_time_;
   std::chrono::milliseconds step_time_;