Allow updating robot gravity (#415)

These changes expose the `set_gravity` URScript functionality through
the `Client Library`, allowing a user to update the gravity vector of
the robot during live operation.

---------

Signed-off-by: AdamPettinger <[email protected]>

Author: AdamPettinger

doc/architecture/script_command_interface.rst

@@ -22,6 +22,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
   for more information.
 - ``setFrictionCompensation()``: Set friction compensation for torque command.
 - ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing.
+- ``setGravity()``: Set the gravity vector for the robot.
 
 Communication protocol
 ----------------------
@@ -52,6 +53,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 6: endToolContact
            - 7: setFrictionCompensation
            - 8: ftRtdeInputEnable
+           - 9: setGravity
    1-27   data fields specific to the command
    =====  =====
 
@@ -146,6 +148,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    6-9    sensor_cog in m, displacement from the tool flange (3d floating point)
    =====  =====
 
+.. table:: With setGravity command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1-3    The gravity vector (towards the Earth's center), represented in robot's base frame (floating point)
+   =====  =====
+
 .. note::
    In URScript the ``socket_read_binary_integer()`` function is used to read the data from the
    script command socket. The first index in that function's return value is the number of integers read,

include/ur_client_library/control/script_command_interface.h

@@ -91,6 +91,16 @@ class ScriptCommandInterface : public ReverseInterface
    */
   bool setPayload(const double mass, const vector3d_t* cog);
 
+  /*!
+   * \brief Set the gravity vector
+   *
+   * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards
+   * the Earth's center) given in the robot's base frame
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setGravity(const vector3d_t* gravity);
+
   /*!
    * \brief Set the tool voltage.
    *
@@ -227,6 +237,7 @@ class ScriptCommandInterface : public ReverseInterface
     END_TOOL_CONTACT = 6,           ///< End detecting tool contact
     SET_FRICTION_COMPENSATION = 7,  ///< Set friction compensation
     FT_RTDE_INPUT_ENABLE = 8,       ///< Enable FT RTDE input
+    SET_GRAVITY = 9,                ///< Set gravity vector
   };
 
   /*!

include/ur_client_library/ur/ur_driver.h

@@ -596,6 +596,17 @@ class UrDriver
    */
   bool setPayload(const float mass, const vector3d_t& cog);
 
+  /*!
+   * \brief Set the gravity vector. Note: It requires the external control script to be running or
+   * the robot to be in headless mode.
+   *
+   * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards
+   * the Earth's center) given in the robot's base frame
+   *
+   * \returns True on successful write.
+   */
+  bool setGravity(const vector3d_t& gravity);
+
   /*!
    * \brief Set the tool voltage. Note: It requires the external control script to be running or the robot to be in
    * headless mode.

resources/external_control.urscript

@@ -56,6 +56,7 @@ START_TOOL_CONTACT = 5
 END_TOOL_CONTACT = 6
 SET_FRICTION_COMPENSATION = 7
 FT_RTDE_INPUT_ENABLE = 8
+SET_GRAVITY = 9
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -756,6 +757,9 @@ thread script_commands():
         mass = raw_command[2] / MULT_jointstate
         cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate]
         set_payload(mass, cog)
+      elif command == SET_GRAVITY:
+        gravity = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate]
+        set_gravity(gravity)
       elif command == SET_TOOL_VOLTAGE:
         tool_voltage = raw_command[2] / MULT_jointstate
         set_tool_voltage(tool_voltage)

src/control/script_command_interface.cpp

@@ -89,6 +89,31 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::setGravity(const vector3d_t* gravity)
+{
+  const int message_length = 4;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_GRAVITY));
+  b_pos += append(b_pos, val);
+
+  for (auto const& grav_component : *gravity)
+  {
+    val = htobe32(static_cast<int32_t>(round(grav_component * 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++)
+  {
+    val = htobe32(0);
+    b_pos += append(b_pos, val);
+  }
+  size_t written;
+
+  return server_.write(client_fd_, buffer, sizeof(buffer), written);
+}
+
 bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage)
 {
   const int message_length = 2;

src/ur/ur_driver.cpp

@@ -298,6 +298,26 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog)
   }
 }
 
+bool UrDriver::setGravity(const vector3d_t& gravity)
+{
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setGravity(&gravity);
+  }
+  else
+  {
+    URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series "
+                  "robots this will only work, if the robot is in remote_control mode.");
+    std::stringstream cmd;
+    cmd.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
+    cmd << "sec setup():" << std::endl
+        << " set_gravity([" << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << "])" << std::endl
+        << "end";
+    return sendScript(cmd.str());
+    return true;
+  }
+}
+
 bool UrDriver::setToolVoltage(const ToolVoltage voltage)
 {
   // Test that the tool voltage is either 0, 12 or 24.

tests/test_script_command_interface.cpp

@@ -482,6 +482,35 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable)
   EXPECT_EQ(received_enabled, false);
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_set_gravity)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  vector3d_t gravity = { 0.1, 0.2, -9.81 };
+  script_command_interface_->setGravity(&gravity);
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 9 is set gravity
+  int32_t expected_command = 9;
+  EXPECT_EQ(command, expected_command);
+
+  // Test gravity
+  vector3d_t received_gravity;
+  for (unsigned int i = 0; i < gravity.size(); ++i)
+  {
+    received_gravity[i] = (double)message[i] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_gravity[i], gravity[i]);
+  }
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + 3, std::end(message), 0);
+  int32_t expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);