Bug fixes and updates to tests

Author: URJala

examples/force_mode_example.cpp

@@ -149,14 +149,25 @@ int main(int argc, char* argv[])
   }
   // 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 = 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.
+  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. See ScriptManual
+                                          ,
+                                          0.8);  // 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. See ScriptManual
+                                          ,
+                                          0.8, 0.8);  // for details.
+  }
   if (!success)
   {
     URCL_LOG_ERROR("Failed to start force mode.");

include/ur_client_library/ur/ur_driver.h

@@ -128,15 +128,25 @@ class UrDriver
    * \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);
+  [[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.
@@ -439,9 +449,9 @@ 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);
+               "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.

src/ur/ur_driver.cpp

@@ -142,7 +142,9 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
     }
     begin_replace << "set_tool_voltage("
                   << static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";
-    begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", "
+    begin_replace << "set_tool_communication("
+                  << "True"
+                  << ", " << tool_comm_setup->getBaudRate() << ", "
                   << static_cast<std::underlying_type<Parity>::type>(tool_comm_setup->getParity()) << ", "
                   << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", "
                   << tool_comm_setup->getTxIdleChars() << ")";

tests/test_script_command_interface.cpp

@@ -64,11 +64,11 @@ class ScriptCommandInterfaceTest : public ::testing::Test
 
     void readMessage(int32_t& command, std::vector<int32_t>& message)
     {
-      // Max message length is 26
-      uint8_t buf[sizeof(int32_t) * 26];
+      // Max message length is 28
+      uint8_t buf[sizeof(int32_t) * 28];
       uint8_t* b_pos = buf;
       size_t read = 0;
-      size_t remainder = sizeof(int32_t) * 26;
+      size_t remainder = sizeof(int32_t) * 28;
       while (remainder > 0)
       {
         if (!TCPSocket::read(b_pos, remainder, read))
@@ -252,7 +252,10 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode)
   urcl::vector6d_t wrench = { 20, 0, 40, 0, 0, 0 };
   int32_t force_mode_type = 2;
   urcl::vector6d_t limits = { 0.1, 0.1, 0.1, 0.785, 0.785, 1.57 };
-  script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits);
+  double damping = 0.8;
+  double gain_scaling = 0.8;
+  script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits, damping,
+                                            gain_scaling);
 
   int32_t command;
   std::vector<int32_t> message;
@@ -298,8 +301,16 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode)
     EXPECT_EQ(received_limits[i], limits[i]);
   }
 
+  // Test damping return
+  double received_damping = (double)message[25] / script_command_interface_->MULT_JOINTSTATE;
+  EXPECT_EQ(received_damping, damping);
+
+  // Test Gain scaling return
+  double received_gain = (double)message[26] / script_command_interface_->MULT_JOINTSTATE;
+  EXPECT_EQ(received_gain, gain_scaling);
+
   // The rest of the message should be zero
-  int32_t message_sum = std::accumulate(std::begin(message) + 25, std::end(message), 0);
+  int32_t message_sum = std::accumulate(std::begin(message) + 27, std::end(message), 0);
   int32_t expected_message_sum = 0;
   EXPECT_EQ(message_sum, expected_message_sum);