Give force mode parameters as arguments

Implement giving force mode damping and gain scaling as arguments in the call to start force mode in client library.

Author: URJala

examples/force_mode_example.cpp

@@ -130,7 +130,7 @@ 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)));
 
   // 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 +141,27 @@ 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
+  for (int i = 0; i < 100; i++)
+  {
+    g_my_driver->writeKeepalive();
+    g_my_driver->getDataPackage();
+  }
   // 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 = 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
+                                             ,
+                                             0.8, 0.8);  // 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

@@ -109,10 +109,19 @@ class ScriptCommandInterface : public ReverseInterface
    * 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 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,51 @@ 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 +169,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,
@@ -321,6 +356,67 @@ class UrDriver
    */
   bool setToolVoltage(const ToolVoltage voltage);
 
+  /*!
+   * \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.
+   *
+   * \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);
+
+  /*!
+   * \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.
    *
@@ -342,6 +438,8 @@ class UrDriver
    *
    * \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);
 
@@ -500,6 +598,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_;

src/control/script_command_interface.cpp

@@ -108,9 +108,10 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage)
 }
 
 bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector,
-                                            const vector6d_t* wrench, const unsigned int type, const vector6d_t* limits)
+                                            const vector6d_t* wrench, const unsigned int type, const vector6d_t* limits,
+                                            double damping_factor, double gain_scaling_factor)
 {
-  const int message_length = 26;
+  const int message_length = 28;
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
 
@@ -144,6 +145,12 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
     b_pos += append(b_pos, val);
   }
 
+  val = htobe32(static_cast<int32_t>(round(damping_factor * MULT_JOINTSTATE)));
+  b_pos += append(b_pos, val);
+
+  val = htobe32(static_cast<int32_t>(round(gain_scaling_factor * MULT_JOINTSTATE)));
+  b_pos += append(b_pos, val);
+
   // writing zeros to allow usage with other script commands
   for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++)
   {