Tool Contact example

Univeral Robots’ tool contact mode allows detecting collisions between the robot’s tool and the environment and interrupting motions when that happens. This example demonstrates how to use the tool contact mode to detect collisions and stop the robot.

As a basic concept, we will move the robot linearly in the negative z axis until the tool hits something or the program’s timeout is hit.

At first, we create a initialize a driver as usual:

Listing 58 examples/tool_contact_example.cpp
80  bool headless_mode = true;
81  g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
82                                                     "external_control.urp");
83  if (!g_my_robot->isHealthy())
84  {
85    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
86    return 1;
87  }

We use a small helper function to make sure that the reverse interface is active and connected before proceeding.

When a tool contact is detected, a callback will be triggered. For that we define a function to handle this event:

Listing 59 examples/tool_contact_example.cpp
55void handleToolContactResult(control::ToolContactResult result)
56{
57  // Print the text in green so we see it better
58  std::cout << "\033[1;32mTool contact result: " << toUnderlying(result) << "\033[0m\n" << std::endl;
59  g_tool_contact_result = result;
60  g_tool_contact_result_triggered = true;
61}

This function is registered as a callback to the driver and then tool_contact mode is enabled:

Listing 60 examples/tool_contact_example.cpp
88  g_my_robot->getUrDriver()->registerToolContactResultCallback(&handleToolContactResult);
89  g_my_robot->getUrDriver()->startToolContact();

Once everything is initialized, we can start a control loop commanding the robot to move in the negative z direction until the program timeout is hit or a tool contact is detected:

Listing 61 examples/tool_contact_example.cpp
 91  // This will move the robot downward in the z direction of the base until a tool contact is detected or seconds_to_run
 92  // is reached
 93  const vector6d_t tcp_speed = { 0.0, 0.0, -0.02, 0.0, 0.0, 0.0 };
 94  auto start_time = std::chrono::system_clock::now();
 95  while (second_to_run.count() < 0 || (std::chrono::system_clock::now() - start_time) < second_to_run)
 96  {
 97    // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
 98    // reliable on non-realtime systems. Use with caution in productive applications.
 99    bool ret = g_my_robot->getUrDriver()->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL,
100                                                            RobotReceiveTimeout::millisec(100));
101    if (!ret)
102    {
103      URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
104      return 1;
105    }
106
107    if (g_tool_contact_result_triggered)
108    {
109      URCL_LOG_INFO("Tool contact detected");
110      break;
111    }
112  }
113  URCL_LOG_INFO("Timed out before reaching tool contact.");
114  g_my_robot->getUrDriver()->stopControl();
115  return 0;