Torque control example

In the examples subfolder you will find a minimal example for commanding torques to the robot. It applies a sinusoidal torque to the 6th joint (index 5) while reading the joint positions. To run it make sure to

• have an instance of a robot controller running at the configured IP address (or adapt the address to your needs). On URSim torque commands don’t have any effect.

• have PolyScope version 5.25.1 / 10.12.1 or later installed on the robot.

• run it from the package’s main folder (the one where the README.md file is stored), as for simplicity reasons it doesn’t use any sophisticated method to locate the required files.

This page will walk you through the direct_torque_control.cpp example.

Initialization

At first, we create a ExampleRobotWrapper object giving it the robot’s IP address, script file and RTDE recipes.

Listing 62 examples/direct_torque_control.cpp

  bool headless_mode = true;
  g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                           "external_control.urp");
  if (!g_my_robot->isHealthy())
  {
    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
    return 1;
  }

  // This example requires Software version 5.25.1 / 10.12.1 or higher. Skip gracefully on older
  // software versions so that CI (which runs all examples) does not fail.
  {
    auto robot_version = g_my_robot->getUrDriver()->getVersion();
    bool version_supported =
        ((robot_version.major == 5) && (robot_version >= urcl::VersionInformation::fromString("5.25.1"))) ||
        ((robot_version.major >= 10) && (robot_version >= urcl::VersionInformation::fromString("10.12.1")));
    if (!version_supported)
    {
      URCL_LOG_INFO("This direct_torque control example requires a robot with at least version 5.25.1 / 10.12.1. Your "
                    "robot has version %s. Skipping.",
                    robot_version.toString().c_str());
      return 0;
    }
  }
  // --------------- INITIALIZATION END -------------------

Note

This example requires PolyScope version 5.25.1 / 10.12.1 or later, as it uses setFrictionScales() which is only available in these versions and later. If you try to run it on an older software version, this example will skip gracefully.

Friction compensation setup

Before starting the control loop, friction compensation scales are configured using setFrictionScales(). Since this example only controls a single joint (JOINT_INDEX), friction compensation for the other joints is set to zero to keep them more steady. The controlled joint gets full compensation (scale factor 1.0) for both viscous and Coulomb friction.

Listing 63 examples/direct_torque_control.cpp

  // Scale each individual joint's friction compensation. This is supported from PolyScope 5.25.1 / PolyScope X 10.12.1
  // and upwards.
  urcl::vector6d_t viscous_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  urcl::vector6d_t coulomb_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

  // As this example only applies to JOINT_INDEX, we can set the other joints to 0.0 to disable friction compensation to
  // make them more steady.
  viscous_scale[JOINT_INDEX] = 1.0;
  coulomb_scale[JOINT_INDEX] = 1.0;
  g_my_robot->getUrDriver()->setFrictionScales(viscous_scale, coulomb_scale);

Move to start position

Before entering the torque control loop, the robot is moved to a known start position using the InstructionExecutor. This ensures a predictable initial state for the sinusoidal motion. First, RTDE communication is started and the current joint positions are read. Then, the target position for JOINT_INDEX is set to start_position and the robot is moved there using moveJ().

Listing 64 examples/direct_torque_control.cpp

  // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
  // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
  // loop.
  g_my_robot->getUrDriver()->startRTDECommunication();

  urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
  if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
  {
    URCL_LOG_ERROR("Could not get fresh data package from robot");
    return 1;
  }
  if (!data_pkg.getData("actual_q", g_joint_positions))
  {
    // This throwing should never happen unless misconfigured
    std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
    throw std::runtime_error(error_msg);
  }
  // Use motion primitives to move JOINT_INDEX to start position
  g_joint_positions[JOINT_INDEX] = START_POSITION;
  auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_robot->getUrDriver());
  instruction_executor->optimoveJ(g_joint_positions);

Robot control loop

The main control loop applies a sinusoidal torque to the controlled joint for 10 full periods. In each cycle, the latest RTDE data package is read using getDataPackage(). This call blocks until new data arrives from the robot (with an internal timeout), so the robot’s RTDE cycle effectively controls the timing of this loop. The current joint positions are then extracted from the received package:

Listing 65 examples/direct_torque_control.cpp

  // Run 10 periods of the sinusoidal
  while (time < 10 * 2 * M_PI / OMEGA)
  {
    time += timestep;
    // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
    // robot will effectively be in charge of setting the frequency of this loop.
    // In a real-world application this thread should be scheduled with real-time priority in order
    // to ensure that this is called in time.
    if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
    {
      URCL_LOG_WARN("Could not get fresh data package from robot");
      return 1;
    }
    // Read current joint positions from robot data
    if (!data_pkg.getData("actual_q", g_joint_positions))
    {
      // This throwing should never happen unless misconfigured
      std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
      throw std::runtime_error(error_msg);
    }

The target torque for the controlled joint is computed as a sinusoidal function of time:

Listing 66 examples/direct_torque_control.cpp

    // Open loop control. The target is incremented with a constant each control loop
    target_torques[JOINT_INDEX] = AMPLITUDE * std::sin(OMEGA * time);

To send the control command, the robot’s ReverseInterface is used via the writeJointCommand() function:

Listing 67 examples/direct_torque_control.cpp

    // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
    // reliable on non-realtime systems. Use with caution in productive applications. Having it
    // this high means that the robot will continue a motion for this given time if no new command
    // is sent / the connection is interrupted.
    bool ret = g_my_robot->getUrDriver()->writeJointCommand(target_torques, urcl::comm::ControlMode::MODE_TORQUE,
                                                            urcl::RobotReceiveTimeout::millisec(100));
    if (!ret)
    {
      URCL_LOG_ERROR("Could not send joint command. Make sure that the robot is in remote control mode and connected "
                     "with a network cable.");
      return 1;
    }