Skip to content

Commit 8dbe2a1

Browse files
urmahpFelix Exner
urmahp
and
Felix Exner
authored
Added support for force_mode, freedrive and tool contact (#138)
* Added support for force_mode, freedrive and tool contact This commit includes following changes: * New control mode for freedrive * New control mode for when the tool is in contact * Commands for stopping and starting tool contact * Commands for stopping and starting force mode * Added force_mode_damping and force_mode_gain_scaling to the constructor * Force mode gain scaling is only available for e-series and therefore it is removed from the control script, when the robot is not e-series * Tool contact example * Test for script command interface * Updated test for reverse interface * Added examples for force_mode and freedrive_mode * Added logging output for freedrive mode in urscript --------- Co-authored-by: Felix Exner <[email protected]>
1 parent 01b1fee commit 8dbe2a1

16 files changed

+1554
-30
lines changed

‎examples/CMakeLists.txt‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,3 +34,18 @@ add_executable(dashboard_example
3434
dashboard_example.cpp)
3535
target_compile_options(dashboard_example PUBLIC ${CXX17_FLAG})
3636
target_link_libraries(dashboard_example ur_client_library::urcl)
37+
38+
add_executable(tool_contact_example
39+
tool_contact_example.cpp)
40+
target_compile_options(tool_contact_example PUBLIC ${CXX17_FLAG})
41+
target_link_libraries(tool_contact_example ur_client_library::urcl)
42+
43+
add_executable(freedrive_example
44+
freedrive_example.cpp)
45+
target_compile_options(freedrive_example PUBLIC ${CXX17_FLAG})
46+
target_link_libraries(freedrive_example ur_client_library::urcl)
47+
48+
add_executable(force_mode_example
49+
force_mode_example.cpp)
50+
target_compile_options(force_mode_example PUBLIC ${CXX17_FLAG})
51+
target_link_libraries(force_mode_example ur_client_library::urcl)

‎examples/force_mode_example.cpp‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2022 Universal Robots A/S
5+
//
6+
// Licensed under the Apache License, Version 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
//
18+
// -- END LICENSE BLOCK ------------------------------------------------
19+
20+
// In a real-world example it would be better to get those values from command line parameters / a
21+
// better configuration system such as Boost.Program_options
22+
23+
#include <ur_client_library/ur/dashboard_client.h>
24+
#include <ur_client_library/ur/ur_driver.h>
25+
#include <ur_client_library/types.h>
26+
27+
#include <chrono>
28+
#include <cstdlib>
29+
#include <iostream>
30+
#include <memory>
31+
#include <thread>
32+
#include "ur_client_library/control/reverse_interface.h"
33+
34+
using namespace urcl;
35+
36+
const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
37+
const std::string SCRIPT_FILE = "resources/external_control.urscript";
38+
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
39+
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
40+
const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
41+
42+
std::unique_ptr<UrDriver> g_my_driver;
43+
std::unique_ptr<DashboardClient> g_my_dashboard;
44+
45+
// We need a callback function to register. See UrDriver's parameters for details.
46+
void handleRobotProgramState(bool program_running)
47+
{
48+
// Print the text in green so we see it better
49+
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
50+
}
51+
52+
void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
53+
{
54+
bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action);
55+
if (!ret)
56+
{
57+
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
58+
exit(1);
59+
}
60+
}
61+
62+
int main(int argc, char* argv[])
63+
{
64+
urcl::setLogLevel(urcl::LogLevel::INFO);
65+
// Parse the ip arguments if given
66+
std::string robot_ip = DEFAULT_ROBOT_IP;
67+
if (argc > 1)
68+
{
69+
robot_ip = std::string(argv[1]);
70+
}
71+
72+
// Parse how many seconds to run
73+
auto second_to_run = std::chrono::seconds(0);
74+
if (argc > 2)
75+
{
76+
second_to_run = std::chrono::seconds(std::stoi(argv[2]));
77+
}
78+
79+
// Making the robot ready for the program by:
80+
// Connect the robot Dashboard
81+
g_my_dashboard.reset(new DashboardClient(robot_ip));
82+
if (!g_my_dashboard->connect())
83+
{
84+
URCL_LOG_ERROR("Could not connect to dashboard");
85+
return 1;
86+
}
87+
88+
// // Stop program, if there is one running
89+
if (!g_my_dashboard->commandStop())
90+
{
91+
URCL_LOG_ERROR("Could not send stop program command");
92+
return 1;
93+
}
94+
95+
// Power it off
96+
// if (!g_my_dashboard->commandPowerOff())
97+
//{
98+
// URCL_LOG_ERROR("Could not send Power off command");
99+
// return 1;
100+
//}
101+
102+
// Power it on
103+
// if (!g_my_dashboard->commandPowerOn())
104+
//{
105+
// URCL_LOG_ERROR("Could not send Power on command");
106+
// return 1;
107+
//}
108+
109+
// Release the brakes
110+
// if (!g_my_dashboard->commandBrakeRelease())
111+
//{
112+
// URCL_LOG_ERROR("Could not send BrakeRelease command");
113+
// return 1;
114+
//}
115+
116+
// Now the robot is ready to receive a program
117+
std::unique_ptr<ToolCommSetup> tool_comm_setup;
118+
const bool HEADLESS = true;
119+
g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
120+
std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
121+
122+
// Once RTDE communication is started, we have to make sure to read from the interface buffer, as
123+
// otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
124+
// loop.
125+
g_my_driver->startRTDECommunication();
126+
127+
std::chrono::duration<double> time_done(0);
128+
std::chrono::duration<double> timeout(second_to_run);
129+
auto stopwatch_last = std::chrono::steady_clock::now();
130+
auto stopwatch_now = stopwatch_last;
131+
g_my_driver->writeKeepalive();
132+
// Task frame at the robot's base with limits being large enough to cover the whole workspace
133+
// Compliance in z axis and rotation around z axis
134+
g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
135+
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
136+
{ 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench
137+
2, // do not transform the force frame at all
138+
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }); // limits. See ScriptManual for
139+
// details.
140+
141+
while (true)
142+
{
143+
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
144+
// robot will effectively be in charge of setting the frequency of this loop.
145+
// In a real-world application this thread should be scheduled with real-time priority in order
146+
// to ensure that this is called in time.
147+
std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
148+
if (data_pkg)
149+
{
150+
g_my_driver->writeKeepalive();
151+
152+
if (time_done > timeout && second_to_run.count() != 0)
153+
{
154+
URCL_LOG_INFO("Timeout reached.");
155+
break;
156+
}
157+
}
158+
else
159+
{
160+
URCL_LOG_WARN("Could not get fresh data package from robot");
161+
}
162+
163+
stopwatch_now = std::chrono::steady_clock::now();
164+
time_done += stopwatch_now - stopwatch_last;
165+
stopwatch_last = stopwatch_now;
166+
}
167+
g_my_driver->endForceMode();
168+
}

‎examples/freedrive_example.cpp‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2022 Universal Robots A/S
5+
//
6+
// Licensed under the Apache License, Version 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
//
18+
// -- END LICENSE BLOCK ------------------------------------------------
19+
20+
// In a real-world example it would be better to get those values from command line parameters / a
21+
// better configuration system such as Boost.Program_options
22+
23+
#include <ur_client_library/ur/dashboard_client.h>
24+
#include <ur_client_library/ur/ur_driver.h>
25+
#include <ur_client_library/types.h>
26+
27+
#include <chrono>
28+
#include <cstdlib>
29+
#include <iostream>
30+
#include <memory>
31+
#include <thread>
32+
#include "ur_client_library/control/reverse_interface.h"
33+
34+
using namespace urcl;
35+
36+
const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
37+
const std::string SCRIPT_FILE = "resources/external_control.urscript";
38+
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
39+
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
40+
const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
41+
42+
std::unique_ptr<UrDriver> g_my_driver;
43+
std::unique_ptr<DashboardClient> g_my_dashboard;
44+
45+
// We need a callback function to register. See UrDriver's parameters for details.
46+
void handleRobotProgramState(bool program_running)
47+
{
48+
// Print the text in green so we see it better
49+
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
50+
}
51+
52+
void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
53+
{
54+
bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action);
55+
if (!ret)
56+
{
57+
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
58+
exit(1);
59+
}
60+
}
61+
62+
int main(int argc, char* argv[])
63+
{
64+
urcl::setLogLevel(urcl::LogLevel::INFO);
65+
// Parse the ip arguments if given
66+
std::string robot_ip = DEFAULT_ROBOT_IP;
67+
if (argc > 1)
68+
{
69+
robot_ip = std::string(argv[1]);
70+
}
71+
72+
// Parse how many seconds to run
73+
auto second_to_run = std::chrono::seconds(0);
74+
if (argc > 2)
75+
{
76+
second_to_run = std::chrono::seconds(std::stoi(argv[2]));
77+
}
78+
79+
// Making the robot ready for the program by:
80+
// Connect the robot Dashboard
81+
g_my_dashboard.reset(new DashboardClient(robot_ip));
82+
if (!g_my_dashboard->connect())
83+
{
84+
URCL_LOG_ERROR("Could not connect to dashboard");
85+
return 1;
86+
}
87+
88+
// // Stop program, if there is one running
89+
if (!g_my_dashboard->commandStop())
90+
{
91+
URCL_LOG_ERROR("Could not send stop program command");
92+
return 1;
93+
}
94+
95+
// Power it off
96+
if (!g_my_dashboard->commandPowerOff())
97+
{
98+
URCL_LOG_ERROR("Could not send Power off command");
99+
return 1;
100+
}
101+
102+
// Power it on
103+
if (!g_my_dashboard->commandPowerOn())
104+
{
105+
URCL_LOG_ERROR("Could not send Power on command");
106+
return 1;
107+
}
108+
109+
// Release the brakes
110+
if (!g_my_dashboard->commandBrakeRelease())
111+
{
112+
URCL_LOG_ERROR("Could not send BrakeRelease command");
113+
return 1;
114+
}
115+
116+
// Now the robot is ready to receive a program
117+
std::unique_ptr<ToolCommSetup> tool_comm_setup;
118+
const bool HEADLESS = true;
119+
g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
120+
std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
121+
122+
// Once RTDE communication is started, we have to make sure to read from the interface buffer, as
123+
// otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
124+
// loop.
125+
g_my_driver->startRTDECommunication();
126+
127+
std::chrono::duration<double> time_done(0);
128+
std::chrono::duration<double> timeout(second_to_run);
129+
auto stopwatch_last = std::chrono::steady_clock::now();
130+
auto stopwatch_now = stopwatch_last;
131+
g_my_driver->writeKeepalive();
132+
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START);
133+
134+
while (true)
135+
{
136+
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
137+
// robot will effectively be in charge of setting the frequency of this loop.
138+
// In a real-world application this thread should be scheduled with real-time priority in order
139+
// to ensure that this is called in time.
140+
std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
141+
if (data_pkg)
142+
{
143+
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP);
144+
145+
if (time_done > timeout && second_to_run.count() != 0)
146+
{
147+
URCL_LOG_INFO("Timeout reached.");
148+
break;
149+
}
150+
}
151+
else
152+
{
153+
URCL_LOG_WARN("Could not get fresh data package from robot");
154+
}
155+
156+
stopwatch_now = std::chrono::steady_clock::now();
157+
time_done += stopwatch_now - stopwatch_last;
158+
stopwatch_last = stopwatch_now;
159+
}
160+
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP);
161+
}

0 commit comments

Comments
 (0)