Skip to content

Commit 8b1af75

Browse files
urfeexurfeex
committed
Add Instruction executor
1 parent 423801a commit 8b1af75

File tree

10 files changed

+488
-5
lines changed

10 files changed

+488
-5
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ add_library(urcl SHARED
4242
src/ur/ur_driver.cpp
4343
src/ur/calibration_checker.cpp
4444
src/ur/dashboard_client.cpp
45+
src/ur/instruction_executor.cpp
4546
src/ur/tool_communication.cpp
4647
src/ur/robot_receive_timeout.cpp
4748
src/ur/version_information.cpp

‎examples/CMakeLists.txt‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,9 @@ project(ur_driver_examples)
77
# # Check C++11 support / enable global pedantic and Wall
88
# #
99
include(DefineCXX17CompilerFlag)
10-
DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
11-
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
10+
#DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
11+
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
12+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20")
1213

1314
add_executable(driver_example
1415
full_driver.cpp)
@@ -64,3 +65,7 @@ add_executable(trajectory_point_interface_example
6465
trajectory_point_interface.cpp)
6566
target_compile_options(trajectory_point_interface_example PUBLIC ${CXX17_FLAG})
6667
target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)
68+
69+
add_executable(instruction_executor
70+
instruction_executor.cpp)
71+
target_link_libraries(instruction_executor ur_client_library::urcl)

‎examples/instruction_executor.cpp‎

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
2+
// Copyright 2024 Universal Robots A/S
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
//
7+
// * Redistributions of source code must retain the above copyright
8+
// notice, this list of conditions and the following disclaimer.
9+
//
10+
// * Redistributions in binary form must reproduce the above copyright
11+
// notice, this list of conditions and the following disclaimer in the
12+
// documentation and/or other materials provided with the distribution.
13+
//
14+
// * Neither the name of the {copyright_holder} nor the names of its
15+
// contributors may be used to endorse or promote products derived from
16+
// this software without specific prior written permission.
17+
//
18+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
// POSSIBILITY OF SUCH DAMAGE.
29+
// -- END LICENSE BLOCK ------------------------------------------------
30+
31+
#include <chrono>
32+
#include <string>
33+
#include <thread>
34+
35+
#include "ur_client_library/types.h"
36+
#include "ur_client_library/ur/ur_driver.h"
37+
#include "ur_client_library/log.h"
38+
#include "ur_client_library/control/trajectory_point_interface.h"
39+
#include "ur_client_library/ur/dashboard_client.h"
40+
#include "ur_client_library/ur/instruction_executor.h"
41+
42+
const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
43+
const std::string SCRIPT_FILE = "resources/external_control.urscript";
44+
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
45+
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
46+
const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
47+
48+
std::unique_ptr<urcl::DashboardClient> g_my_dashboard;
49+
std::shared_ptr<urcl::UrDriver> g_my_driver;
50+
51+
// We need a callback function to register. See UrDriver's parameters for details.
52+
void handleRobotProgramState(bool program_running)
53+
{
54+
// Print the text in green so we see it better
55+
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
56+
}
57+
58+
int main(int argc, char* argv[])
59+
{
60+
urcl::setLogLevel(urcl::LogLevel::INFO);
61+
// Parse the ip arguments if given
62+
std::string robot_ip = DEFAULT_ROBOT_IP;
63+
if (argc > 1)
64+
{
65+
robot_ip = std::string(argv[1]);
66+
}
67+
68+
// --------------- INITIALIZATION BEGIN -------------------
69+
// Making the robot ready for the program by:
70+
// Connect the robot Dashboard
71+
g_my_dashboard.reset(new urcl::DashboardClient(robot_ip));
72+
// if (!g_my_dashboard->connect())
73+
// {
74+
// URCL_LOG_ERROR("Could not connect to dashboard");
75+
// return 1;
76+
// }
77+
//
78+
// // // Stop program, if there is one running
79+
// if (!g_my_dashboard->commandStop())
80+
// {
81+
// URCL_LOG_ERROR("Could not send stop program command");
82+
// return 1;
83+
// }
84+
//
85+
// // Power it off
86+
// if (!g_my_dashboard->commandPowerOff())
87+
// {
88+
// URCL_LOG_ERROR("Could not send Power off command");
89+
// return 1;
90+
// }
91+
//
92+
// // Power it on
93+
// if (!g_my_dashboard->commandPowerOn())
94+
// {
95+
// URCL_LOG_ERROR("Could not send Power on command");
96+
// return 1;
97+
// }
98+
//
99+
// // Release the brakes
100+
// if (!g_my_dashboard->commandBrakeRelease())
101+
// {
102+
// URCL_LOG_ERROR("Could not send BrakeRelease command");
103+
// return 1;
104+
// }
105+
106+
std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
107+
const bool headless = true;
108+
g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState,
109+
headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
110+
auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_driver);
111+
// --------------- INITIALIZATION END -------------------
112+
113+
URCL_LOG_INFO("Running motion");
114+
// Trajectory definition
115+
std::vector<std::shared_ptr<urcl::control::MotionPrimitive>> motion_sequence{
116+
std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1,
117+
std::chrono::seconds(5)),
118+
std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1,
119+
std::chrono::seconds(5)),
120+
121+
std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
122+
std::chrono::seconds(2)),
123+
std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1,
124+
std::chrono::seconds(2)),
125+
};
126+
instruction_executor->executeMotion(motion_sequence); // That could also receive velocity parametrization
127+
128+
instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 });
129+
instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 });
130+
instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 });
131+
instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 });
132+
133+
g_my_driver->stopControl();
134+
return 0;
135+
}

‎include/ur_client_library/control/motion_primitives.h‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
2+
// Copyright 2025 Universal Robots A/S
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
//
7+
// * Redistributions of source code must retain the above copyright
8+
// notice, this list of conditions and the following disclaimer.
9+
//
10+
// * Redistributions in binary form must reproduce the above copyright
11+
// notice, this list of conditions and the following disclaimer in the
12+
// documentation and/or other materials provided with the distribution.
13+
//
14+
// * Neither the name of the {copyright_holder} nor the names of its
15+
// contributors may be used to endorse or promote products derived from
16+
// this software without specific prior written permission.
17+
//
18+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
// POSSIBILITY OF SUCH DAMAGE.
29+
// -- END LICENSE BLOCK ------------------------------------------------
30+
31+
#ifndef UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
32+
#define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
33+
34+
#include <chrono>
35+
#include <ur_client_library/types.h>
36+
37+
namespace urcl
38+
{
39+
namespace control
40+
{
41+
42+
enum class MotionType : uint8_t
43+
{
44+
MOVEJ = 0,
45+
MOVEL = 1,
46+
MOVEP = 2,
47+
MOVEC = 3,
48+
SPLINE = 51,
49+
};
50+
struct MotionPrimitive
51+
{
52+
MotionType type;
53+
std::chrono::duration<double> duration;
54+
double acceleration;
55+
double velocity;
56+
double blend_radius;
57+
};
58+
59+
struct MoveJPrimitive : public MotionPrimitive
60+
{
61+
MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0,
62+
const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
63+
const double acceleration = 1.4, const double velocity = 1.04)
64+
{
65+
type = MotionType::MOVEJ;
66+
target_joint_configuration = target;
67+
this->duration = duration;
68+
this->acceleration = acceleration;
69+
this->velocity = velocity;
70+
this->blend_radius = blend_radius;
71+
}
72+
73+
urcl::vector6d_t target_joint_configuration;
74+
};
75+
76+
struct MoveLPrimitive : public MotionPrimitive
77+
{
78+
MoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0,
79+
const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
80+
const double acceleration = 1.4, const double velocity = 1.04)
81+
{
82+
type = MotionType::MOVEL;
83+
target_pose = target;
84+
this->duration = duration;
85+
this->acceleration = acceleration;
86+
this->velocity = velocity;
87+
this->blend_radius = blend_radius;
88+
}
89+
90+
urcl::Pose target_pose;
91+
};
92+
} // namespace control
93+
} // namespace urcl
94+
#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

‎include/ur_client_library/control/reverse_interface.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -141,13 +141,20 @@ class ReverseInterface
141141
"commands.")]] virtual void
142142
setKeepaliveCount(const uint32_t count);
143143

144+
void registerDisconnectionCallback(std::function<void(const int)> disconnection_fun)
145+
{
146+
disconnection_callback_ = disconnection_fun;
147+
}
148+
144149
protected:
145150
virtual void connectionCallback(const int filedescriptor);
146151

147152
virtual void disconnectionCallback(const int filedescriptor);
148153

149154
virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv);
150155

156+
std::function<void(const int)> disconnection_callback_ = nullptr;
157+
151158
int client_fd_;
152159
comm::TCPServer server_;
153160

‎include/ur_client_library/types.h‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,23 @@ using vector6d_t = std::array<double, 6>;
3131
using vector6int32_t = std::array<int32_t, 6>;
3232
using vector6uint32_t = std::array<uint32_t, 6>;
3333

34+
struct Pose
35+
{
36+
Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0)
37+
{
38+
}
39+
Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz)
40+
: x(x), y(y), z(z), rx(rx), ry(ry), rz(rz)
41+
{
42+
}
43+
double x;
44+
double y;
45+
double z;
46+
double rx;
47+
double ry;
48+
double rz;
49+
};
50+
3451
template <class T, std::size_t N>
3552
std::ostream& operator<<(std::ostream& out, const std::array<T, N>& item)
3653
{
@@ -59,4 +76,4 @@ constexpr typename std::underlying_type<E>::type toUnderlying(const E e) noexcep
5976
{
6077
return static_cast<typename std::underlying_type<E>::type>(e);
6178
}
62-
} // namespace urcl
79+
} // namespace urcl

0 commit comments

Comments
 (0)