update

Author: gialen

include/ur_client_library/comm/control_mode.h

@@ -45,7 +45,8 @@ enum class ControlMode : int32_t
   MODE_SPEEDJ = 2,          ///< Set when speedj control is active.
   MODE_FORWARD = 3,         ///< Set when trajectory forwarding is active.
   MODE_SPEEDL = 4,          ///< Set when cartesian velocity control is active.
-  MODE_POSE = 5             ///< Set when cartesian pose control is active.
+  MODE_POSE = 5,             ///< Set when cartesian pose control is active.
+  MODE_FREEDRIVE = 6        ///< Set when freedrive mode is active.
 };
 }  // namespace comm
 }  // namespace urcl

include/ur_client_library/control/reverse_interface.h

@@ -51,6 +51,16 @@ enum class TrajectoryControlMessage : int32_t
   TRAJECTORY_START = 1,    ///< Represents command to start a new trajectory.
 };
 
+/*!
+ * \brief Control messages for starting and stopping freedrive mode.
+ */
+enum class FreedriveControlMessage : int32_t
+{
+  FREEDRIVE_STOP = -1,  ///< Represents command to stop freedrive mode.
+  FREEDRIVE_NOOP = 0,   ///< Represents keep running in freedrive mode.
+  FREEDRIVE_START = 1,  ///< Represents command to start freedrive mode.
+};
+
 /*!
  * \brief The ReverseInterface class handles communication to the robot. It starts a server and
  * waits for the robot to connect via its URCaps program.
@@ -95,6 +105,15 @@ class ReverseInterface
    */
   bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0);
 
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program.
+   *
+   * \param freedrive_action 1 if freedrive mode is to be started, -1 if it should be stopped and 0 to keep it running
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action);
+
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
    *

include/ur_client_library/ur/ur_driver.h

@@ -222,6 +222,15 @@ class UrDriver
   bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action,
                                      const int point_number = 0);
 
+  /*!
+   * \brief Writes a control message in freedrive mode.
+   *
+   * \param freedrive_action The action to be taken, such as starting or stopping freedrive
+   *
+   * \returns True on successful write.
+   */
+  bool writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action);
+
   /*!
    * \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

@@ -21,6 +21,7 @@ MODE_SPEEDJ = 2
 MODE_FORWARD = 3
 MODE_SPEEDL = 4
 MODE_POSE = 5
+MODE_FREEDRIVE = 6
 
 TRAJECTORY_MODE_RECEIVE = 1
 TRAJECTORY_MODE_CANCEL = -1
@@ -38,6 +39,9 @@ SET_PAYLOAD = 1
 SET_TOOL_VOLTAGE = 2
 SCRIPT_COMMAND_DATA_DIMENSION = 5
 
+FREEDRIVE_MODE_START = 1
+FREEDRIVE_MODE_STOP = -1
+
 #Global variables are also showed in the Teach pendants variable list
 global cmd_servo_state = SERVO_UNINITIALIZED
 global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
@@ -265,6 +269,9 @@ while keepalive > 0 and control_mode > MODE_STOPPED:
       if control_mode == MODE_FORWARD:
         kill thread_trajectory
         clear_remaining_trajectory_points()
+      # Stop freedrive
+      elif control_mode == MODE_FREEDRIVE:
+        end_freedrive_mode()
       end
       control_mode = params_mult[8]
       join thread_move
@@ -304,6 +311,12 @@ while keepalive > 0 and control_mode > MODE_STOPPED:
     elif control_mode == MODE_POSE:
       pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
       set_servo_pose(pose)
+    elif control_mode == MODE_FREEDRIVE:
+      if params_mult[2] == FREEDRIVE_MODE_START:
+        freedrive_mode()
+      elif params_mult[2] == FREEDRIVE_MODE_STOP:
+        end_freedrive_mode()
+      end
     end
   else:
     keepalive = keepalive - 1

src/control/reverse_interface.cpp

@@ -114,6 +114,38 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action)
+{
+  const int message_length = 2;
+  if (client_fd_ == -1)
+  {
+    return false;
+  }
+  uint8_t buffer[sizeof(int32_t) * 8];
+  uint8_t* b_pos = buffer;
+
+  // The first element is always the keepalive signal.
+  int32_t val = htobe32(keepalive_count_);
+  b_pos += append(b_pos, val);
+
+  val = htobe32(toUnderlying(freedrive_action));
+  b_pos += append(b_pos, val);
+
+  // writing zeros to allow usage with other script commands
+  for (size_t i = message_length; i < 8 - 1; i++)
+  {
+    val = htobe32(0);
+    b_pos += append(b_pos, val);
+  }
+
+  val = htobe32(toUnderlying(comm::ControlMode::MODE_FREEDRIVE));
+  b_pos += append(b_pos, val);
+
+  size_t written;
+
+  return server_.write(client_fd_, buffer, sizeof(buffer), written);
+}
+
 void ReverseInterface::connectionCallback(const int filedescriptor)
 {
   if (client_fd_ < 0)

src/ur/ur_driver.cpp

@@ -233,6 +233,11 @@ bool UrDriver::writeTrajectoryControlMessage(const control::TrajectoryControlMes
   return reverse_interface_->writeTrajectoryControlMessage(trajectory_action, point_number);
 }
 
+bool UrDriver::writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action)
+{
+  return reverse_interface_->writeFreedriveControlMessage(freedrive_action);
+}
+
 bool UrDriver::zeroFTSensor()
 {
   if (getVersion().major < 5)

tests/test_reverse_interface.cpp

@@ -136,6 +136,16 @@ class ReverseIntefaceTest : public ::testing::Test
       return pos[1];
     }
 
+    int32_t getFreedriveControlMode()
+    {
+      // received_pos[0]=control::FreedriveControlMessage, when writing a trajectory control message
+      int32_t keep_alive_signal;
+      int32_t control_mode;
+      vector6int32_t pos;
+      readMessage(keep_alive_signal, pos, control_mode);
+      return pos[0];
+    }
+
   protected:
     virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
     {
@@ -360,6 +370,30 @@ TEST_F(ReverseIntefaceTest, write_control_mode)
   EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode);
 }
 
+TEST_F(ReverseIntefaceTest, write_freedrive_control_message)
+{
+  // Wait for the client to connect to the server
+  EXPECT_TRUE(waitForProgramState(1000, true));
+
+  control::FreedriveControlMessage written_freedrive_message = control::FreedriveControlMessage::FREEDRIVE_STOP;
+  reverse_interface_->writeFreedriveControlMessage(written_freedrive_message);
+  int32_t received_freedrive_message = client_->getFreedriveControlMode();
+
+  EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
+
+  written_freedrive_message = control::FreedriveControlMessage::FREEDRIVE_NOOP;
+  reverse_interface_->writeFreedriveControlMessage(written_freedrive_message);
+  received_freedrive_message = client_->getFreedriveControlMode();
+
+  EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
+
+  written_freedrive_message = control::FreedriveControlMessage::FREEDRIVE_START;
+  reverse_interface_->writeFreedriveControlMessage(written_freedrive_message);
+  received_freedrive_message = client_->getFreedriveControlMode();
+
+  EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);