Constrained freedrive implementation

examples/freedrive_example.cpp

@@ -53,14 +53,44 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 
 std::unique_ptr<ExampleRobotWrapper> g_my_robot;
 
-void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
+void runFreedrive(UrDriver& urdriver, std::chrono::seconds duration)
 {
-  bool ret = g_my_robot->getUrDriver()->writeFreedriveControlMessage(freedrive_action);
-  if (!ret)
+  URCL_LOG_INFO("Starting freedrive mode");
+
+  urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_START);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < duration || duration.count() == 0)
+  {
+    // Keeping the robot in freedrive by sending NOOP messages
+    urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP);
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+  }
+
+  urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_STOP);
+  URCL_LOG_INFO("Stopping freedrive mode");
+}
+
+void runConstrainedFreedrive(UrDriver& urdriver, std::array<int32_t, 6>& free_axes, std::array<double, 6>& feature_pose,
+                             std::chrono::seconds duration)
+{
+  URCL_LOG_INFO("Starting constrained freedrive mode");
+
+  urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_START, free_axes,
+                                                   feature_pose);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < duration || duration.count() == 0)
   {
-    URCL_LOG_ERROR("Could not send joint command. Is there an external_control program running on the robot?");
-    exit(1);
+    // Keeping the robot in constrained freedrive by sending NOOP messages
+    urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, free_axes,
+                                                     feature_pose);
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
   }
+
+  urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_STOP, free_axes,
+                                                   feature_pose);
+  URCL_LOG_INFO("Stopping constrained freedrive mode");
 }
 
 int main(int argc, char* argv[])
@@ -73,15 +103,26 @@ int main(int argc, char* argv[])
     robot_ip = std::string(argv[1]);
   }
 
+  // Select the freedrive mode
+  bool constrained = false;
+  if (argc > 2)
+  {
+    std::string arg = argv[2];
+    constrained = (arg == "true" || arg == "1");
+  }
+
   // Parse how many seconds to run
   auto second_to_run = std::chrono::seconds(0);
-  if (argc > 2)
+  if (argc > 3)
   {
-    second_to_run = std::chrono::seconds(std::stoi(argv[2]));
+    second_to_run = std::chrono::seconds(std::stoi(argv[3]));
   }
 
-  bool headless_mode = true;
+  std::array<int32_t, 6> free_axes = { 1, 0, 1, 1, 0, 1 };
+  std::array<double, 6> feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
+  // Initialize robot connection
+  bool headless_mode = true;
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
 
@@ -91,24 +132,13 @@ int main(int argc, char* argv[])
     return 1;
   }
 
-  URCL_LOG_INFO("Starting freedrive mode");
-  sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START);
-
-  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)
+  // Execute selected freedrive mode
+  if (constrained)
   {
-    sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP);
-
-    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));
+    runConstrainedFreedrive(*g_my_robot->getUrDriver(), free_axes, feature_pose, second_to_run);
+  }
+  else
+  {
+    runFreedrive(*g_my_robot->getUrDriver(), second_to_run);
   }
-
-  URCL_LOG_INFO("Stopping freedrive mode");
-  sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP);
 }

include/ur_client_library/control/reverse_interface.h

@@ -140,12 +140,18 @@ class ReverseInterface
    * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external
    * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off()
    * function to create the RobotReceiveTimeout object
+   * \param free_axes A 6-dimensional vector (x, y, z, rx, ry, rz) defining which axes are compliant.
+   * Use 1 to enable movement in an axis and 0 to lock it.
+   * \param feature_pose A pose vector [x, y, z, rx, ry, rz] defining the freedrive frame relative to the base frame.
+   * Position values (x, y, z) should be in meters, and orientation values (rx, ry, rz) in radians.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
   bool
   writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action,
-                               const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
+                               const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200),
+                               const std::array<int32_t, 6>& free_axes = { 1, 1, 1, 1, 1, 1 },
+                               const std::array<double, 6>& feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
 
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
@@ -228,7 +234,7 @@ class ReverseInterface
     return s;
   }
 
-  static const int MAX_MESSAGE_LENGTH = 8;
+  static const int MAX_MESSAGE_LENGTH = 15;
 
   std::function<void(bool)> handle_program_state_;
   std::chrono::milliseconds step_time_;

include/ur_client_library/ur/ur_driver.h

@@ -611,6 +611,26 @@ class UrDriver
   writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action,
                                const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
 
+  /*!
+   * \brief Writes a control message in freedrive mode.
+   *
+   * \param freedrive_action The action to be taken, such as starting or stopping freedrive
+   * \param free_axes A 6-dimensional vector (x, y, z, rx, ry, rz) defining which axes are compliant.
+   * Use 1 to enable movement in an axis and 0 to lock it.
+   * \param feature_pose A pose vector [x, y, z, rx, ry, rz] defining the freedrive frame relative to the base frame.
+   * Position values (x, y, z) should be in meters, and orientation values (rx, ry, rz) in radians.
+   * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external
+   * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off()
+   * function to create the RobotReceiveTimeout object
+   *
+   * \returns True on successful write.
+   */
+  bool writeConstrainedFreedriveControlMessage(
+      const control::FreedriveControlMessage freedrive_action,
+      const std::array<int32_t, 6>& free_axes = { 1, 1, 1, 1, 1, 1 },
+      const std::array<double, 6>& feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+      const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
+
   /*!
    * \brief Zero the force torque sensor (only availbe on e-Series). Note:  It requires the external control script to
    * be running or the robot to be in headless mode

resources/external_control.urscript

@@ -29,7 +29,7 @@ MODE_FREEDRIVE = 6
 MODE_TOOL_IN_CONTACT = 7
 MODE_TORQUE = 8
 # Data dimensions of the message received on the reverse interface
-REVERSE_INTERFACE_DATA_DIMENSION = 8
+REVERSE_INTERFACE_DATA_DIMENSION = 15
 
 TRAJECTORY_MODE_RECEIVE = 1
 TRAJECTORY_MODE_CANCEL = -1
@@ -1142,7 +1142,9 @@ while control_mode > MODE_STOPPED:
     elif control_mode == MODE_FREEDRIVE:
       if params_mult[2] == FREEDRIVE_MODE_START:
         textmsg("Entering freedrive mode")
-        freedrive_mode()
+        free_axes = [params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7], params_mult[8]]
+        feature_pose = p[params_mult[9]/MULT_jointstate, params_mult[10]/MULT_jointstate, params_mult[11]/MULT_jointstate, params_mult[12]/MULT_jointstate, params_mult[13]/MULT_jointstate, params_mult[14]/MULT_jointstate]
+        freedrive_mode(free_axes, feature_pose)
       elif params_mult[2] == FREEDRIVE_MODE_STOP:
         textmsg("Leaving freedrive mode")
         end_freedrive_mode()

src/control/reverse_interface.cpp

@@ -169,9 +169,11 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess
 }
 
 bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action,
-                                                    const RobotReceiveTimeout& robot_receive_timeout)
+                                                    const RobotReceiveTimeout& robot_receive_timeout,
+                                                    const std::array<int32_t, 6>& free_axes,
+                                                    const std::array<double, 6>& feature_pose)
 {
-  const int message_length = 2;
+  const int message_length = 14;
   if (client_fd_ == INVALID_SOCKET)
   {
     return false;
@@ -197,6 +199,20 @@ bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessag
   val = htobe32(toUnderlying(freedrive_action));
   b_pos += append(b_pos, val);
 
+  // Add allowed axes for movement
+  for (int32_t axis : free_axes)
+  {
+    val = htobe32(axis);
+    b_pos += append(b_pos, val);
+  }
+
+  // Add feature pose scaled to microns microradians to avoid precision loss in integer conversion
+  for (double p : feature_pose)
+  {
+    val = htobe32(static_cast<int32_t>(round(p * 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 - 1; i++)
   {

src/ur/ur_driver.cpp

@@ -266,6 +266,15 @@ bool UrDriver::writeFreedriveControlMessage(const control::FreedriveControlMessa
   return reverse_interface_->writeFreedriveControlMessage(freedrive_action, robot_receive_timeout);
 }
 
+bool UrDriver::writeConstrainedFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action,
+                                                       const std::array<int32_t, 6>& free_axes,
+                                                       const std::array<double, 6>& feature_pose,
+                                                       const RobotReceiveTimeout& robot_receive_timeout)
+{
+  return reverse_interface_->writeFreedriveControlMessage(freedrive_action, robot_receive_timeout, free_axes,
+                                                          feature_pose);
+}
+
 bool UrDriver::zeroFTSensor()
 {
   if (getVersion().major < 5)

tests/test_reverse_interface.cpp

@@ -43,6 +43,8 @@ std::condition_variable g_connection_condition;
 class TestableReverseInterface : public control::ReverseInterface
 {
 public:
+  using control::ReverseInterface::MAX_MESSAGE_LENGTH;
+
   TestableReverseInterface(const control::ReverseInterfaceConfig& config) : control::ReverseInterface(config)
   {
   }
@@ -85,11 +87,13 @@ class ReverseInterfaceTest : public ::testing::Test
 
     void readMessage(int32_t& read_timeout, vector6int32_t& pos, int32_t& control_mode)
     {
+      constexpr size_t MAX_MESSAGE_LENGTH = TestableReverseInterface::MAX_MESSAGE_LENGTH;
+
       // Read message
-      uint8_t buf[sizeof(int32_t) * 8];
+      uint8_t buf[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
       uint8_t* b_pos = buf;
       size_t read = 0;
-      size_t remainder = sizeof(int32_t) * 8;
+      size_t remainder = sizeof(int32_t) * MAX_MESSAGE_LENGTH;
       while (remainder > 0)
       {
         if (!TCPSocket::read(b_pos, remainder, read))
@@ -116,6 +120,8 @@ class ReverseInterfaceTest : public ::testing::Test
         b_pos += sizeof(int32_t);
       }
 
+      b_pos = buf + (sizeof(int32_t) * (MAX_MESSAGE_LENGTH - 1));
+
       // Decode control mode
       std::memcpy(&val, b_pos, sizeof(int32_t));
       control_mode = be32toh(val);
@@ -478,6 +484,22 @@ TEST_F(ReverseInterfaceTest, write_freedrive_control_message)
   EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
 }
 
+TEST_F(ReverseInterfaceTest, write_constrained_freedrive_message)
+{
+  // Wait for the client to connect to the server
+  EXPECT_TRUE(waitForProgramState(1000, true));
+  control::FreedriveControlMessage written_freedrive_message = control::FreedriveControlMessage::FREEDRIVE_START;
+
+  std::array<int32_t, 6> free_axes = { 1, 0, 1, 0, 1, 0 };
+  std::array<double, 6> feature_pos = { 0.1, -0.2, 0.5, 1.1, 0.0, 3.14 };
+
+  reverse_interface_->writeFreedriveControlMessage(written_freedrive_message, RobotReceiveTimeout::millisec(200),
+                                                   free_axes, feature_pos);
+
+  int32_t received_freedrive_message = client_->getFreedriveControlMode();
+  EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
+}
+
 TEST_F(ReverseInterfaceTest, deprecated_set_keep_alive_count)
 {
   // Wait for the client to connect to the server

tests/test_ur_driver.cpp

@@ -155,8 +155,11 @@ TEST_F(UrDriverTest, read_existing_script_file)
 TEST_F(UrDriverTest, robot_receive_timeout)
 {
   // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important
-  vector6d_t zeros = { 0, 0, 0, 0, 0, 0 };
-  g_my_robot->getUrDriver()->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200));
+  std::array<int32_t, 6> zeros_int = { 0, 0, 0, 0, 0, 0 };
+  std::array<double, 6> zeros_double = { 0, 0, 0, 0, 0, 0 };
+
+  g_my_robot->getUrDriver()->writeJointCommand(zeros_double, comm::ControlMode::MODE_IDLE,
+                                               RobotReceiveTimeout::millisec(200));
   EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400));
 
   // Start robot program
@@ -172,6 +175,15 @@ TEST_F(UrDriverTest, robot_receive_timeout)
   g_my_robot->resendRobotProgram();
   EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000));
 
+  // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important
+  g_my_robot->getUrDriver()->writeConstrainedFreedriveControlMessage(
+      control::FreedriveControlMessage::FREEDRIVE_NOOP, zeros_int, zeros_double, RobotReceiveTimeout::millisec(200));
+  EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400));
+
+  // Start robot program
+  g_my_robot->resendRobotProgram();
+  EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000));
+
   // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important
   g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                                            RobotReceiveTimeout::millisec(200));
@@ -197,6 +209,11 @@ TEST_F(UrDriverTest, robot_receive_timeout_off)
                                                           RobotReceiveTimeout::off());
   EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000));
 
+  g_my_robot->getUrDriver()->writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP,
+                                                                     { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 },
+                                                                     RobotReceiveTimeout::off());
+  EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000));
+
   // Program should keep running when setting receive timeout off
   g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                                            RobotReceiveTimeout::off());