Added command to set friction compensation for the torque command

Author: urmahp

include/ur_client_library/control/script_command_interface.h

@@ -147,6 +147,17 @@ class ScriptCommandInterface : public ReverseInterface
    */
   bool endToolContact();
 
+  /*!
+   * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
+   * if false it will not.
+   *
+   * \param friction_compesation Will set a friction_compensation variable in urscript, which will be used when calling
+   * torque_command
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setFrictionCompensation(const bool friction_compensation);
+
   /*!
    * \brief  Returns whether a client/robot is connected to this server.
    *
@@ -177,13 +188,14 @@ class ScriptCommandInterface : public ReverseInterface
   enum class ScriptCommand : int32_t
   {
 
-    ZERO_FTSENSOR = 0,       ///< Zero force torque sensor
-    SET_PAYLOAD = 1,         ///< Set payload
-    SET_TOOL_VOLTAGE = 2,    ///< Set tool voltage
-    START_FORCE_MODE = 3,    ///< Start force mode
-    END_FORCE_MODE = 4,      ///< End force mode
-    START_TOOL_CONTACT = 5,  ///< Start detecting tool contact
-    END_TOOL_CONTACT = 6,    ///< End detecting tool contact
+    ZERO_FTSENSOR = 0,              ///< Zero force torque sensor
+    SET_PAYLOAD = 1,                ///< Set payload
+    SET_TOOL_VOLTAGE = 2,           ///< Set tool voltage
+    START_FORCE_MODE = 3,           ///< Start force mode
+    END_FORCE_MODE = 4,             ///< End force mode
+    START_TOOL_CONTACT = 5,         ///< Start detecting tool contact
+    END_TOOL_CONTACT = 6,           ///< End detecting tool contact
+    SET_FRICTION_COMPENSATION = 7,  ///< Set friction compensation
   };
 
   bool client_connected_;

include/ur_client_library/ur/ur_driver.h

@@ -714,6 +714,17 @@ class UrDriver
    */
   bool endToolContact();
 
+  /*!
+   * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
+   * if false it will not.
+   *
+   * \param friction_compesation Will set a friction_compensation variable in urscript, which will be used when calling
+   * torque_command
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setFrictionCompensation(const bool friction_compensation);
+
   /*!
    * \brief Write a keepalive signal only.
    *

resources/external_control.urscript

@@ -49,6 +49,7 @@ START_FORCE_MODE = 3
 END_FORCE_MODE = 4
 START_TOOL_CONTACT = 5
 END_TOOL_CONTACT = 6
+SET_FRICTION_COMPENSATION = 7
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -81,6 +82,7 @@ global spline_qdd = [0, 0, 0, 0, 0, 0]
 global spline_qd = [0, 0, 0, 0, 0, 0]
 global tool_contact_running = False
 global trajectory_result = 0
+global friction_compensation = True
 
 # Global thread variables
 thread_move = 0
@@ -691,6 +693,12 @@ thread script_commands():
           socket_send_int(UNTIL_TOOL_CONTACT_RESULT_CANCELED, "script_command_socket")
         end
         tool_contact_running = False
+      elif command == SET_FRICTION_COMPENSATION:
+        if raw_command[2] == 0:
+          friction_compensation = False
+        else:
+          friction_compensation = True
+        end
       end
     end
   end

src/control/script_command_interface.cpp

@@ -222,6 +222,29 @@ bool ScriptCommandInterface::endToolContact()
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation)
+{
+  const int message_length = 2;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_FRICTION_COMPENSATION));
+  b_pos += append(b_pos, val);
+
+  val = htobe32(friction_compensation);
+  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++)
+  {
+    val = htobe32(0);
+    b_pos += append(b_pos, val);
+  }
+  size_t written;
+
+  return server_.write(client_fd_, buffer, sizeof(buffer), written);
+}
+
 bool ScriptCommandInterface::clientConnected()
 {
   return client_connected_;

src/ur/ur_driver.cpp

@@ -527,6 +527,19 @@ bool UrDriver::endToolContact()
   }
 }
 
+bool UrDriver::setFrictionCompensation(const bool friction_compensation)
+{
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setFrictionCompensation(friction_compensation);
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script command interface is not running. Unable to set friction compensation.");
+    return 0;
+  }
+}
+
 bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout)
 {
   vector6d_t* fake = nullptr;

tests/test_script_command_interface.cpp

@@ -382,6 +382,45 @@ TEST_F(ScriptCommandInterfaceTest, test_tool_contact_callback)
   EXPECT_EQ(toUnderlying(received_result_), toUnderlying(send_result));
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  script_command_interface_->setFrictionCompensation(true);
+
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 7 is set friction compensation
+  int32_t expected_command = 7;
+  EXPECT_EQ(command, expected_command);
+
+  int32_t expected_friction_compensation = 1;
+  EXPECT_EQ(message[0], expected_friction_compensation);
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0);
+  int32_t expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+
+  script_command_interface_->setFrictionCompensation(false);
+
+  message.clear();
+  client_->readMessage(command, message);
+
+  EXPECT_EQ(command, expected_command);
+
+  expected_friction_compensation = 0;
+  EXPECT_EQ(message[0], expected_friction_compensation);
+
+  // The rest of the message should be zero
+  message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0);
+  expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);