Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions doc/examples.rst
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ may be running forever until manually stopped.
examples/primary_pipeline
examples/primary_pipeline_calibration
examples/rtde_client
examples/script_command_interface
examples/script_sender
examples/spline_example
examples/tool_contact_example
Expand Down
71 changes: 71 additions & 0 deletions doc/examples/script_command_interface.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/script_command_interface.rst

Script command interface
========================

The :ref:`script_command_interface` allows sending predefined commands to the robot while there is
URScript running that is connected to it.

An example to utilize the script command interface can be found in the `freedrive_example.cpp <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/script_command_interface.cpp>`_.

In order to use the ``ScriptCommandInterface``, there has to be a script code running on the robot
that connects to the ``ScriptCommandInterface``. This happens as part of the big
`external_control.urscript <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/resources/external_control.urscript>`_. In order to reuse that with this example, we will create a full
``UrDriver`` and leverage the ``ScriptCommandInterface`` through this.

At first, we create a ``ExampleRobotWrapper`` object in order to initialize communication with the
robot.

.. literalinclude:: ../../examples/script_command_interface.cpp
:language: c++
:caption: examples/script_command_interface.cpp
:linenos:
:lineno-match:
:start-at: g_my_robot =
:end-at: std::thread script_command_send_thread(sendScriptCommands);

The script commands will be sent in a separate thread which will be explained later.

Since the connection to the script command interface runs as part of the bigger external_control
script, we'll wrap the calls alongside a full ``ExampleRobotWrapper``. Hence, we'll have to send
keepalive signals regularly to keep the script running:

.. literalinclude:: ../../examples/script_command_interface.cpp
:language: c++
:caption: examples/script_command_interface.cpp
:linenos:
:lineno-match:
:start-at: std::chrono::duration<double> time_done(0);
:end-at: g_my_robot->getUrDriver()->stopControl();

Sending script commands
-----------------------

Once the script is running on the robot, a connection to the driver's script command interface
should be established. The ``UrDriver`` forwards most calls of the ``ScriptCommandInterface`` and
we will use that interface in this example. To send a script command, we can e.g. use
``g_my_robot->getUrDriver()->zeroFTSensor()``.

In the example, we have wrapped the calls into a lambda function that will wait a specific timeout,
print a log output what command will be sent and then call the respective command:

.. literalinclude:: ../../examples/script_command_interface.cpp
:language: c++
:caption: examples/script_command_interface.cpp
:linenos:
:lineno-match:
:start-at: run_cmd(
:end-before: URCL_LOG_INFO("Script command thread finished.");

The lambda itself looks like this:

.. literalinclude:: ../../examples/script_command_interface.cpp
:language: c++
:caption: examples/script_command_interface.cpp
:linenos:
:lineno-match:
:start-at: auto run_cmd =
:end-before: // Keep running all commands in a loop

For a list of all available script commands, please refer to the ``ScriptCommandInterface`` class
`here <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/include/ur_client_library/control/script_command_interface.h>`_.
4 changes: 4 additions & 0 deletions examples/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,10 @@ add_executable(script_sender_example
script_sender.cpp)
target_link_libraries(script_sender_example ur_client_library::urcl)

add_executable(script_command_interface_example
script_command_interface.cpp)
target_link_libraries(script_command_interface_example ur_client_library::urcl)

add_executable(trajectory_point_interface_example
trajectory_point_interface.cpp)
target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)
Expand Down
145 changes: 145 additions & 0 deletions examples/script_command_interface.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,145 @@
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2025 Universal Robots A/S
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the {copyright_holder} nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
// -- END LICENSE BLOCK ------------------------------------------------

#include <chrono>
#include <string>
#include "ur_client_library/ur/tool_communication.h"

#include <ur_client_library/log.h>
#include <ur_client_library/example_robot_wrapper.h>

using namespace urcl;

const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
const std::string SCRIPT_FILE = "resources/external_control.urscript";
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";

std::unique_ptr<ExampleRobotWrapper> g_my_robot;
bool g_HEADLESS = true;
bool g_running = false;

void sendScriptCommands()
{
auto run_cmd = [](const std::string& log_output, std::function<void()> func) {
const std::chrono::seconds timeout(3);
if (g_running)
{
// We wait a fixed time so that not each command is run directly behind each other.
// This is done for example purposes only, so users can follow the effect on the teach
// pendant.
std::this_thread::sleep_for(timeout);
URCL_LOG_INFO(log_output.c_str());
func();
}
};

// Keep running all commands in a loop until g_running is set to false
while (g_running)
{
run_cmd("Setting tool voltage to 24V",
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should call start and stop force mode as well

Copy link
Member Author

@urfeex urfeex May 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As force mode needs quite a lot of arguments and might complain about singularities if the robot is in the wrong pose, I would rather skip it here. It's got its own example. But I can also add freedrive mode. I'll skip freedrive mode, as well, as that expects a special keepalive signal. That also has its own example.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Make sense leaving it out then.

[]() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::_24V); });
run_cmd("Enabling tool contact mode", []() { g_my_robot->getUrDriver()->startToolContact(); });
run_cmd("Setting friction_compensation variable to `false`",
[]() { g_my_robot->getUrDriver()->setFrictionCompensation(false); });
run_cmd("Setting tool voltage to 0V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::OFF); });
run_cmd("Zeroing the force torque sensor", []() { g_my_robot->getUrDriver()->zeroFTSensor(); });
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); });
}
URCL_LOG_INFO("Script command thread finished.");
}

int main(int argc, char* argv[])
{
urcl::setLogLevel(urcl::LogLevel::INFO);
// Parse the ip arguments if given
std::string robot_ip = DEFAULT_ROBOT_IP;
if (argc > 1)
{
robot_ip = std::string(argv[1]);
}

// Parse how many seconds to run
auto second_to_run = std::chrono::seconds(0);
if (argc > 2)
{
second_to_run = std::chrono::seconds(std::stoi(argv[2]));
}

// Parse whether to run in headless mode
// When not using headless mode, the global variables can be watched on the teach pendant.
if (argc > 3)
{
g_HEADLESS = std::string(argv[3]) == "true" || std::string(argv[3]) == "1" || std::string(argv[3]) == "True" ||
std::string(argv[3]) == "TRUE";
}

g_my_robot =
std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "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;
}

// We will send script commands from a separate thread. That will stay active as long as
// g_running is true.
g_running = true;
std::thread script_command_send_thread(sendScriptCommands);

// We will need to keep the script running on the robot. As we use the "usual" external_control
// urscript, we'll have to send keepalive signals as long as we want to keep it active.
std::chrono::duration<double> time_done(0);
std::chrono::duration<double> timeout(second_to_run);
auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last;
while ((time_done < timeout || second_to_run.count() == 0) && g_my_robot->isHealthy())
{
g_my_robot->getUrDriver()->writeKeepalive();

stopwatch_now = std::chrono::steady_clock::now();
time_done += stopwatch_now - stopwatch_last;
stopwatch_last = stopwatch_now;
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int>(1.0 / g_my_robot->getUrDriver()->getControlFrequency())));
}

URCL_LOG_INFO("Timeout reached.");
g_my_robot->getUrDriver()->stopControl();

// Stop the script command thread
g_running = false;
script_command_send_thread.join();

return 0;
}
Loading