Added commands to set pd controller gains and maximum joint torques (#342)

Co-authored-by: Felix Exner <[email protected]>

Author: urmahp

doc/architecture/script_command_interface.rst

@@ -22,6 +22,8 @@ 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.
+- ``setPDControllerGains()``: Set PD gains for the PD control loop running in the external control script.
+- ``setMaxJointTorques()``: Set Max joint torques for the PD control loop running in the external control script.
 
 Communication protocol
 ----------------------
@@ -52,6 +54,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 6: endToolContact
            - 7: setFrictionCompensation
            - 8: ftRtdeInputEnable
+           - 9: setPDControllerGains
+           - 10: setMaxJointTorques
    1-27   data fields specific to the command
    =====  =====
 
@@ -144,6 +148,24 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    2      sensor_mass in kg (floating point)
    3-5    sensor_mesurement_offset in m, displacement from the tool flange (3d floating point)
    6-9    sensor_cog in m, displacement from the tool flange (3d floating point)
+
+.. table:: With setPDControllerGains command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1-6    Kp gains for each joint, used in the PD control loop running in the external control script (floating point)
+   7-12   Kd gains for each joint, used in the PD control loop running in the external control script (floating point)
+   =====  =====
+
+.. table:: With setMaxJointTorques command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1-6    max_joint_torques for each joint, used to clamp the torques between +-max_joint_torques before applying them to the robot in the PD control loop running in the external control script (floating point)
    =====  =====
 
 .. note::

examples/script_command_interface.cpp

@@ -74,6 +74,12 @@ void sendScriptCommands()
     run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); });
     run_cmd("Setting friction_compensation variable to `true`",
             []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); });
+    run_cmd("Setting PD controller gains", []() {
+      g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 },
+                                                      { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 });
+    });
+    run_cmd("Setting max joint torques",
+            []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); });
   }
   URCL_LOG_INFO("Script command thread finished.");
 }

include/ur_client_library/control/script_command_interface.h

@@ -158,11 +158,11 @@ 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.
+   * \brief Set friction compensation for the direct_torque command. If true the torque command will compensate for
+   * friction, if false it will not.
    *
    * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be
-   * used when calling torque_command
+   * used when calling direct_torque.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
@@ -188,6 +188,32 @@ class ScriptCommandInterface : public ReverseInterface
                          const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 },
                          const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 });
 
+  /*!
+   * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint
+   * torques based on either tcp poses or joint poses and applies the torques to the robot using the direct_torque
+   * function. The gains can be used to change the response of the controller. Be aware that changing the controller
+   * response can make it unstable.
+   * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for
+   * each robot model.
+   *
+   * \param kp A vector6d of proportional gains for each of the joints in the robot.
+   * \param kd A vector6d of derivative gains for each of the joints in the robot.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd);
+
+  /*!
+   * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD
+   * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the
+   * direct_torque function.
+   *
+   * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setMaxJointTorques(const urcl::vector6d_t* max_joint_torques);
+
   /*!
    * \brief  Returns whether a client/robot is connected to this server.
    *
@@ -227,6 +253,8 @@ 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_PD_CONTROLLER_GAINS = 9,    ///< Set PD controller gains
+    SET_MAX_JOINT_TORQUES = 10,     ///< Set max joint torques
   };
 
   /*!

include/ur_client_library/ur/ur_driver.h

@@ -716,11 +716,11 @@ 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.
+   * \brief Set friction compensation for the direct_torque command. If true the torque command will compensate for
+   * friction, if false it will not.
    *
    * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be
-   * used when calling torque_command
+   * used when calling direct_torque.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
@@ -746,6 +746,32 @@ class UrDriver
                          const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 },
                          const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 });
 
+  /*!
+   * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint
+   * torques based on either tcp poses or joint poses and applies the torques to the robot using the direct_torque
+   * function. The gains can be used to change the response of the controller. Be aware that changing the controller
+   * response can make it unstable.
+   * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for
+   * each robot model.
+   *
+   * \param kp A vector6d of proportional gains for each of the joints in the robot.
+   * \param kd A vector6d of derivative gains for each of the joints in the robot.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd);
+
+  /*!
+   * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD
+   * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the
+   * direct_torque function.
+   *
+   * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setMaxJointTorques(const urcl::vector6d_t& max_joint_torques);
+
   /*!
    * \brief Write a keepalive signal only.
    *

resources/external_control.urscript

@@ -57,6 +57,8 @@ START_TOOL_CONTACT = 5
 END_TOOL_CONTACT = 6
 SET_FRICTION_COMPENSATION = 7
 FT_RTDE_INPUT_ENABLE = 8
+SET_PD_CONTROLLER_GAINS = 9
+SET_MAX_JOINT_TORQUES = 10
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -857,6 +859,11 @@ thread script_commands():
         # This has a known error that the resulting torques are computed with opposite sign.
         enable_external_ft_sensor(enabled, sensor_mass, sensor_offset, sensor_cog)
         {% endif %}
+      elif command == SET_PD_CONTROLLER_GAINS:
+        pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate]
+        pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate]
+      elif command == SET_MAX_JOINT_TORQUES:
+        max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] 
       end
     end
   end

src/control/script_command_interface.cpp

@@ -293,6 +293,64 @@ bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd)
+{
+  const int message_length = 13;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PD_CONTROLLER_GAINS));
+  b_pos += append(b_pos, val);
+
+  for (auto const& p_gain : *kp)
+  {
+    val = htobe32(static_cast<int32_t>(round(p_gain * MULT_JOINTSTATE)));
+    b_pos += append(b_pos, val);
+  }
+
+  for (auto const& d_gain : *kd)
+  {
+    val = htobe32(static_cast<int32_t>(round(d_gain * 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::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques)
+{
+  const int message_length = 7;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_MAX_JOINT_TORQUES));
+  b_pos += append(b_pos, val);
+
+  for (auto const& max_torque : *max_joint_torques)
+  {
+    val = htobe32(static_cast<int32_t>(round(max_torque * 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::clientConnected()
 {
   return client_connected_;
@@ -360,4 +418,4 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo
 }
 
 }  // namespace control
-}  // namespace urcl
+}  // namespace urcl

src/ur/ur_driver.cpp

@@ -571,6 +571,54 @@ bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass,
   }
 }
 
+bool UrDriver::setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd)
+{
+  if (!std::all_of(kp.begin(), kp.end(), [](double v) { return v >= 0.0; }))
+  {
+    throw InvalidRange("kp should be larger than zero");
+  }
+
+  if (!std::all_of(kd.begin(), kd.end(), [](double v) { return v >= 0.0; }))
+  {
+    throw InvalidRange("kd should be larger than zero");
+  }
+
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setPDControllerGains(&kp, &kd);
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script command interface is not running. Unable to set PD Controller gains.");
+    return 0;
+  }
+}
+
+bool UrDriver::setMaxJointTorques(const urcl::vector6d_t& max_joint_torques)
+{
+  const urcl::vector6d_t max_torques_for_robot_type =
+      control::getMaxTorquesFromRobotType(primary_client_->getRobotType());
+  if (!std::equal(max_joint_torques.begin(), max_joint_torques.end(), max_torques_for_robot_type.begin(),
+                  [](double v1, double v2) { return v1 <= v2 && v1 >= 0.0; }))
+  {
+    std::stringstream ss;
+    ss << "The max joint torques should be smaller or equal to the maximum joint torques for the robot type and larger "
+          "than zero. Provided max joint torques "
+       << max_joint_torques << " and maximum joint torques for the robot type " << max_torques_for_robot_type;
+    throw InvalidRange(ss.str().c_str());
+  }
+
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setMaxJointTorques(&max_joint_torques);
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script command interface is not running. Unable to set max joint torques.");
+    return 0;
+  }
+}
+
 bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout)
 {
   vector6d_t* fake = nullptr;

tests/test_script_command_interface.cpp

@@ -431,7 +431,6 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable)
 {
   // Wait for the client to connect to the server
   waitForClientConnection();
-
   double sensor_mass = 1.42;
   vector3d_t sensor_measuring_offset = { 0.1, 0.2, 0.3 };
   vector3d_t sensor_cog = { 0.01, 0.02, 0.03 };
@@ -482,6 +481,76 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable)
   EXPECT_EQ(received_enabled, false);
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_set_pd_controller_gains)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  urcl::vector6d_t kp = { 220.2, 220.2, 300.0, 10.32, 10.32, 10.32 };
+  urcl::vector6d_t kd = { 29.68, 29.68, 35.0, 6.4, 6.4, 6.4 };
+  script_command_interface_->setPDControllerGains(&kp, &kd);
+
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 8 is set PD controller gains
+  int32_t expected_command = 9;
+  EXPECT_EQ(command, expected_command);
+
+  int32_t message_idx = 0;
+
+  for (auto& p_gain : kp)
+  {
+    const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(p_gain, received_gain);
+    message_idx = message_idx + 1;
+  }
+
+  for (auto& d_gain : kd)
+  {
+    const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(d_gain, received_gain);
+    message_idx = message_idx + 1;
+  }
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0);
+  int32_t expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+}
+
+TEST_F(ScriptCommandInterfaceTest, test_set_max_joint_torques)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  urcl::vector6d_t max_joint_torques = { 100.0, 150.0, 21.2, 10.32, 10.32, 10.32 };
+  script_command_interface_->setMaxJointTorques(&max_joint_torques);
+
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 9 is set max joint torques
+  int32_t expected_command = 10;
+  EXPECT_EQ(command, expected_command);
+
+  int32_t message_idx = 0;
+
+  for (auto& max_torque : max_joint_torques)
+  {
+    const double received_max_torque = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(max_torque, received_max_torque);
+    message_idx = message_idx + 1;
+  }
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + message_idx, 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);