From 4afeefa8f7fc91d35f1d954becda420f87a8bc12 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Jun 2021 11:28:02 +0200 Subject: [PATCH 1/8] Add trajectory interface to URScript code --- resources/external_control.urscript | 88 +++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index eaa594e3..5fd2a2a1 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -6,6 +6,7 @@ steptime = get_steptime() textmsg("ExternalControl: steptime=", steptime) MULT_jointstate = {{JOINT_STATE_REPLACE}} +MULT_time = {{TIME_REPLACE}} #Constants SERVO_UNINITIALIZED = -1 @@ -17,6 +18,14 @@ MODE_UNINITIALIZED = -1 MODE_IDLE = 0 MODE_SERVOJ = 1 MODE_SPEEDJ = 2 +MODE_FORWARD = 3 + +TRAJECTORY_MODE_RECEIVE = 1 +TRAJECTORY_MODE_CANCEL = -1 + +TRAJECTORY_POINT_JOINT = 0 +TRAJECTORY_POINT_CARTESIAN = 1 +TRAJECTORY_DATA_DIMENSION = 7 #Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED @@ -26,6 +35,8 @@ global cmd_servo_q_last = get_actual_joint_positions() global extrapolate_count = 0 global extrapolate_max_count = 0 global control_mode = MODE_UNINITIALIZED +global trajectory_points_left = 0 + cmd_speedj_active = True def set_servo_setpoint(q): @@ -95,14 +106,72 @@ thread speedThread(): stopj(5.0) end +thread jointTrajectoryThread(): + textmsg("Executing trajectory") + textmsg(trajectory_points_left) + while trajectory_points_left > 0: + textmsg("Waiting to read point") + enter_critical + #reading trajectory point + blend radius + type of point (cartesian/joint based) + raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket") + trajectory_points_left = trajectory_points_left - 1 + exit_critical + if raw_point[0] > 0: + q = [ raw_point[1] / MULT_jointstate, raw_point[2] / MULT_jointstate, raw_point[3] / MULT_jointstate, raw_point[4] / MULT_jointstate, raw_point[5] / MULT_jointstate, raw_point[6] / MULT_jointstate] + tmptime = raw_point[7] / MULT_time + blend_radius = raw_point[8] / MULT_time + + textmsg("Executing Point") + textmsg(trajectory_points_left) + textmsg(q) + textmsg(tmptime) + textmsg(blend_radius) + textmsg(raw_point[9]) + + if trajectory_points_left > 0: + if raw_point[9] == TRAJECTORY_POINT_JOINT: + textmsg("Executing movej with blending") + movej(q, t=tmptime, r=blend_radius) + textmsg("done") + elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: + textmsg("Executing movel with blending") + movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius) + end + else: + if raw_point[9] == TRAJECTORY_POINT_JOINT: + textmsg("Executing movej without blending") + movej(q, t=tmptime, r=0.0) + textmsg("done") + elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: + textmsg("Executing movel without blending") + movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=0.0) + end + end + #add some form of error handling here + end + end + textmsg("Trajectory finished") +end + +def clear_remaining_trajectory_points(): + while trajectory_points_left > 0: + raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+2, "trajectory_socket") + trajectory_points_left = trajectory_points_left - 1 + end +end + # HEADER_END # NODE_CONTROL_LOOP_BEGINS socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") +trajectory_port = {{SERVER_PORT_REPLACE}}+10 +socket_open("{{SERVER_IP_REPLACE}}", trajectory_port, "trajectory_socket") control_mode = MODE_UNINITIALIZED thread_move = 0 +thread_trajectory = 0 +trajectory_points_left = 0 global keepalive = -2 params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0) textmsg("ExternalControl: External control active") @@ -113,12 +182,18 @@ while keepalive > 0 and control_mode > MODE_STOPPED: if params_mult[0] > 0: keepalive = params_mult[1] if control_mode != params_mult[8]: + if control_mode == MODE_FORWARD: + kill thread_trajectory + clear_remaining_trajectory_points() + end control_mode = params_mult[8] join thread_move if control_mode == MODE_SERVOJ: thread_move = run servoThread() elif control_mode == MODE_SPEEDJ: thread_move = run speedThread() + elif control_mode == MODE_FORWARD: + kill thread_move end end if control_mode == MODE_SERVOJ: @@ -127,6 +202,17 @@ while keepalive > 0 and control_mode > MODE_STOPPED: elif control_mode == MODE_SPEEDJ: qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] set_speed(qd) + elif control_mode == MODE_FORWARD: + if params_mult[2] == TRAJECTORY_MODE_RECEIVE: + kill thread_trajectory + clear_remaining_trajectory_points() + trajectory_points_left = params_mult[3] + thread_trajectory = run jointTrajectoryThread() + elif params_mult[2] == TRAJECTORY_MODE_CANCEL: + textmsg("cancel received") + kill thread_trajectory + clear_remaining_trajectory_points() + end end else: keepalive = keepalive - 1 @@ -137,7 +223,9 @@ end textmsg("ExternalControl: Stopping communication and control") control_mode = MODE_STOPPED join thread_move +join thread_trajectory textmsg("ExternalControl: All threads ended") socket_close("reverse_socket") +socket_close("trajectory_socket") # NODE_CONTROL_LOOP_ENDS From afc8ad3cc3b3068796573c1876980d1ab9c7dd29 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Jun 2021 10:37:50 +0200 Subject: [PATCH 2/8] Use MULT_JOINTSTATE from reverse_interface.h --- include/ur_client_library/comm/control_mode.h | 3 ++- include/ur_client_library/control/reverse_interface.h | 3 ++- src/ur/ur_driver.cpp | 4 ++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 039212d9..0cf75526 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -27,7 +27,6 @@ #ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED #define UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED -#endif // ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED namespace urcl { @@ -46,3 +45,5 @@ enum class ControlMode : int32_t }; } // namespace comm } // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index a88b40fe..86403cc2 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -47,6 +47,8 @@ namespace control class ReverseInterface { public: + static const int32_t MULT_JOINTSTATE = 1000000; + ReverseInterface() = delete; /*! * \brief Creates a ReverseInterface object including a TCPServer. @@ -92,7 +94,6 @@ class ReverseInterface int client_fd_; comm::TCPServer server_; - static const int32_t MULT_JOINTSTATE = 1000000; template size_t append(uint8_t* buffer, T& val) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 613db0cf..09acab65 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -40,7 +40,6 @@ namespace urcl { -static const int32_t MULT_JOINTSTATE = 1000000; static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}"); static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); @@ -86,7 +85,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ std::string prog = readScriptFile(script_file); while (prog.find(JOINT_STATE_REPLACE) != std::string::npos) { - prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE)); + prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), + std::to_string(control::ReverseInterface::MULT_JOINTSTATE)); } std::ostringstream out; From aae9a830e10dc2b3c5db27b21c0c7f7d755f8ff7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Jun 2021 10:40:22 +0200 Subject: [PATCH 3/8] Add trajectory_interface --- CMakeLists.txt | 1 + include/ur_client_library/comm/control_mode.h | 3 +- .../control/reverse_interface.h | 35 ++++-- .../control/trajectory_point_interface.h | 86 +++++++++++++ include/ur_client_library/ur/ur_driver.h | 25 ++++ src/control/reverse_interface.cpp | 35 ++++++ src/control/trajectory_point_interface.cpp | 114 ++++++++++++++++++ src/ur/ur_driver.cpp | 20 +++ 8 files changed, 310 insertions(+), 9 deletions(-) create mode 100644 include/ur_client_library/control/trajectory_point_interface.h create mode 100644 src/control/trajectory_point_interface.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1dfe9f69..e780848b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ add_library(urcl SHARED src/comm/tcp_server.cpp src/control/reverse_interface.cpp src/control/script_sender.cpp + src/control/trajectory_point_interface.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 0cf75526..c4be869f 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -41,7 +41,8 @@ enum class ControlMode : int32_t MODE_UNINITIALIZED = -1, ///< Startup default until another mode is sent to the script. MODE_IDLE = 0, ///< Set when no controller is currently active controlling the robot. MODE_SERVOJ = 1, ///< Set when servoj control is active. - MODE_SPEEDJ = 2 ///< Set when speedj control is active. + MODE_SPEEDJ = 2, ///< Set when speedj control is active. + MODE_FORWARD = 3 ///< Set when trajectory forwarding is active. }; } // namespace comm } // namespace urcl diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index 86403cc2..4f090768 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -40,6 +40,16 @@ namespace urcl { namespace control { +/*! + * \brief Control messages for forwarding and aborting trajectories. + */ +enum class TrajectoryControlMessage : int32_t +{ + TRAJECTORY_CANCEL = -1, ///< Represents command to cancel currently active trajectory. + TRAJECTORY_NOOP = 0, ///< Represents no new control command. + TRAJECTORY_START = 1, ///< Represents command to start a new trajectory. +}; + /*! * \brief The ReverseInterface class handles communication to the robot. It starts a server and * waits for the robot to connect via its URCaps program. @@ -61,7 +71,7 @@ class ReverseInterface /*! * \brief Disconnects possible clients so the reverse interface object can be safely destroyed. */ - ~ReverseInterface() = default; + virtual ~ReverseInterface() = default; /*! * \brief Writes needed information to the robot to be read by the URCaps program. @@ -72,29 +82,38 @@ class ReverseInterface * * \returns True, if the write was performed successfully, false otherwise. */ - bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE); + virtual bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE); + + /*! + * \brief Writes needed information to the robot to be read by the URScript program. + * + * \param trajectory_action 1 if a trajectory is to be started, -1 if it should be stopped + * \param point_number The number of points of the trajectory to be executed + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0); /*! * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot. * * \param count Number of allowed timeout reads on the robot. */ - void setKeepaliveCount(const uint32_t& count) + virtual void setKeepaliveCount(const uint32_t& count) { keepalive_count_ = count; } -private: - void connectionCallback(const int filedescriptor); +protected: + virtual void connectionCallback(const int filedescriptor); - void disconnectionCallback(const int filedescriptor); + virtual void disconnectionCallback(const int filedescriptor); - void messageCallback(const int filedescriptor, char* buffer); + virtual void messageCallback(const int filedescriptor, char* buffer); int client_fd_; comm::TCPServer server_; - template size_t append(uint8_t* buffer, T& val) { diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h new file mode 100644 index 00000000..1df40bf1 --- /dev/null +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -0,0 +1,86 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner mauch@fzi.de + * \date 2021-06-01 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED +#define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED + +#include "ur_client_library/control/reverse_interface.h" +#include "ur_client_library/comm/control_mode.h" +#include "ur_client_library/types.h" +#include "ur_client_library/log.h" + +namespace urcl +{ +namespace control +{ +/*! + * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full + * trajectories are forwarded to the robot controller and are executed there. + */ +class TrajectoryPointInterface : public ReverseInterface +{ +public: + static const int32_t MULT_TIME = 1000; + static const int32_t JOINT_POINT = 0; + static const int32_t CARTESIAN_POINT = 1; + + TrajectoryPointInterface() = delete; + /*! + * \brief Creates a TrajectoryPointInterface object including a TCPServer. + * + * \param port Port the Server is started on + */ + TrajectoryPointInterface(uint32_t port); + + /*! + * \brief Disconnects possible clients so the reverse interface object can be safely destroyed. + */ + virtual ~TrajectoryPointInterface() = default; + + /*! + * \brief Writes needed information to the robot to be read by the URScript program. + * + * \param positions A vector of joint or cartesian targets for the robot + * \param time The goal time to reach the target + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, + const bool cartesian); + +protected: + virtual void connectionCallback(const int filedescriptor) override; + + virtual void disconnectionCallback(const int filedescriptor) override; + + virtual void messageCallback(const int filedescriptor, char* buffer) override; +}; + +} // namespace control +} // namespace urcl + +#endif // UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index d2910a7a..5ec84753 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -31,6 +31,7 @@ #include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/control/reverse_interface.h" +#include "ur_client_library/control/trajectory_point_interface.h" #include "ur_client_library/control/script_sender.h" #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" @@ -180,6 +181,29 @@ class UrDriver */ bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode); + /*! + * \brief Writes a trajectory point onto the dedicated socket. + * + * \param values Desired joint or cartesian positions + * \param goal_time Time for the robot to reach this point + * \param cartesian True, if the point sent is cartesian, false if joint-based + * + * \returns True on successful write. + */ + bool writeTrajectoryPoint(const vector6d_t& values, const bool cartesian, const float goal_time = 0.0, + const float blend_radius = 0.052); + + /*! + * \brief Writes a control message in trajectory forward mode. + * + * \param trajectory_action The action to be taken, such as starting a new trajectory. + * \param point_number The number of points of a new trajectory to be sent. + * + * \returns True on successful write. + */ + bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, + const int point_number = 0); + /*! * \brief Write a keepalive signal only. * @@ -272,6 +296,7 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; + std::unique_ptr trajectory_interface_; std::unique_ptr script_sender_; std::unique_ptr> primary_stream_; std::unique_ptr> secondary_stream_; diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index d03159a4..008392c1 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -78,6 +78,41 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, + const int point_number) +{ + if (client_fd_ == -1) + { + return false; + } + uint8_t buffer[sizeof(int32_t) * 8]; + uint8_t* b_pos = buffer; + + // The first element is always the keepalive signal. + int32_t val = htobe32(1); + b_pos += append(b_pos, val); + + val = htobe32(toUnderlying(trajectory_action)); + b_pos += append(b_pos, val); + + val = htobe32(point_number); + b_pos += append(b_pos, val); + + // writing zeros to allow usage in control loop with other control messages + for (size_t i = 0; i < 4; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + + val = htobe32(toUnderlying(comm::ControlMode::MODE_FORWARD)); + b_pos += append(b_pos, val); + + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + void ReverseInterface::connectionCallback(const int filedescriptor) { if (client_fd_ < 0) diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp new file mode 100644 index 00000000..28fafd95 --- /dev/null +++ b/src/control/trajectory_point_interface.cpp @@ -0,0 +1,114 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner mauch@fzi.de + * \date 2021-06-01 + * + */ +//---------------------------------------------------------------------- + +#include + +namespace urcl +{ +namespace control +{ +TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInterface(port, [](bool foo) { return foo; }) +{ +} + +bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, + const float blend_radius, const bool cartesian) +{ + if (client_fd_ == -1) + { + return false; + } + uint8_t buffer[sizeof(int32_t) * 9]; + uint8_t* b_pos = buffer; + + if (positions != nullptr) + { + for (auto const& pos : *positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } + else + { + b_pos += 6 * sizeof(int32_t); + } + + int32_t val = static_cast(goal_time * MULT_TIME); + val = htobe32(val); + b_pos += append(b_pos, val); + + val = static_cast(blend_radius * MULT_TIME); + val = htobe32(val); + b_pos += append(b_pos, val); + + if (cartesian) + { + val = CARTESIAN_POINT; + } + else + { + val = JOINT_POINT; + } + + val = htobe32(val); + b_pos += append(b_pos, val); + + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + +void TrajectoryPointInterface::connectionCallback(const int filedescriptor) +{ + if (client_fd_ < 0) + { + URCL_LOG_DEBUG("Robot connected to trajectory interface."); + client_fd_ = filedescriptor; + } + else + { + URCL_LOG_ERROR("Connection request to TrajectoryPointInterface received while connection already established. Only " + "one " + "connection is allowed at a time. Ignoring this request."); + } +} + +void TrajectoryPointInterface::disconnectionCallback(const int filedescriptor) +{ + URCL_LOG_DEBUG("Connection to trajectory interface dropped.", filedescriptor); + client_fd_ = -1; +} + +void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer) +{ + // TODO: Send up. This will be things like "finished trajectory" + URCL_LOG_INFO("Received message %s on TrajectoryPointInterface", buffer); +} +} // namespace control +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 09acab65..185af308 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -42,6 +42,7 @@ namespace urcl { static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}"); static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); +static const std::string TIME_REPLACE("{{TIME_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); @@ -88,6 +89,11 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(control::ReverseInterface::MULT_JOINTSTATE)); } + while (prog.find(TIME_REPLACE) != std::string::npos) + { + prog.replace(prog.find(TIME_REPLACE), TIME_REPLACE.length(), + std::to_string(control::TrajectoryPointInterface::MULT_TIME)); + } std::ostringstream out; out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; @@ -148,6 +154,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state)); + // TODO swap to configurable or static port + trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10)); URCL_LOG_DEBUG("Initialization done"); } @@ -196,6 +204,18 @@ bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMo return reverse_interface_->write(&values, control_mode); } +bool UrDriver::writeTrajectoryPoint(const vector6d_t& values, const bool cartesian, const float goal_time, + const float blend_radius) +{ + return trajectory_interface_->writeTrajectoryPoint(&values, goal_time, blend_radius, cartesian); +} + +bool UrDriver::writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, + const int point_number) +{ + return reverse_interface_->writeTrajectoryControlMessage(trajectory_action, point_number); +} + bool UrDriver::writeKeepalive() { vector6d_t* fake = nullptr; From 8d0d03e5ebb6542cc83887afef89b538466b2bef Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Jun 2021 16:21:37 +0200 Subject: [PATCH 4/8] Create an interface for registering a trajectoryDone callback for passthrough --- examples/full_driver.cpp | 8 +++++-- include/ur_client_library/comm/tcp_server.h | 4 ++-- .../control/reverse_interface.h | 2 +- .../control/trajectory_point_interface.h | 21 ++++++++++++++++- include/ur_client_library/ur/ur_driver.h | 15 +++++++----- resources/external_control.urscript | 6 +++++ src/comm/tcp_server.cpp | 2 +- src/control/reverse_interface.cpp | 6 ++--- src/control/trajectory_point_interface.cpp | 23 ++++++++++++++++--- src/ur/ur_driver.cpp | 13 +++++++---- 10 files changed, 76 insertions(+), 24 deletions(-) diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 45213d4c..4e9f27b3 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -53,12 +53,16 @@ void handleRobotProgramState(bool program_running) // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; } +void trajectoryDoneCb(urcl::control::TrajectoryResult result) +{ + std::cout << "Trajectory finished in state " << toUnderlying(result) << std::endl; +} int main(int argc, char* argv[]) { std::unique_ptr tool_comm_setup; - g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, false, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, + &trajectoryDoneCb, false, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); // 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. diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h index 6e478717..20cda613 100644 --- a/include/ur_client_library/comm/tcp_server.h +++ b/include/ur_client_library/comm/tcp_server.h @@ -90,7 +90,7 @@ class TCPServer * \param func Function handling the event information. The file client's file_descriptor will be * passed to the function as well as the actual message received from the client. */ - void setMessageCallback(std::function func) + void setMessageCallback(std::function func) { message_callback_ = func; } @@ -184,7 +184,7 @@ class TCPServer std::function new_connection_callback_; std::function disconnect_callback_; - std::function message_callback_; + std::function message_callback_; }; } // namespace comm diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index 4f090768..618be2d4 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -109,7 +109,7 @@ class ReverseInterface virtual void disconnectionCallback(const int filedescriptor); - virtual void messageCallback(const int filedescriptor, char* buffer); + virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv); int client_fd_; comm::TCPServer server_; diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 1df40bf1..88a5c82f 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -37,6 +37,17 @@ namespace urcl { namespace control { +/*! + * \brief Types for encoding trajectory execution result + */ +enum class TrajectoryResult : int32_t +{ + + TRAJECTORY_RESULT_SUCCESS = 0, ///< Successful execution + TRAJECTORY_RESULT_CANCELED = 1, ///< Canceled by user + TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution +}; + /*! * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full * trajectories are forwarded to the robot controller and are executed there. @@ -72,12 +83,20 @@ class TrajectoryPointInterface : public ReverseInterface bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, const bool cartesian); + void setTrajectoryEndCallback(std::function callback) + { + handle_trajectory_end_ = callback; + } + protected: virtual void connectionCallback(const int filedescriptor) override; virtual void disconnectionCallback(const int filedescriptor) override; - virtual void messageCallback(const int filedescriptor, char* buffer) override; + virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override; + +private: + std::function handle_trajectory_end_; }; } // namespace control diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 5ec84753..43abeb8c 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -87,7 +87,8 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -116,7 +117,8 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -144,13 +146,14 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "") - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, + headless_mode, std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, + servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) { } diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 5fd2a2a1..8ed48208 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -27,6 +27,10 @@ TRAJECTORY_POINT_JOINT = 0 TRAJECTORY_POINT_CARTESIAN = 1 TRAJECTORY_DATA_DIMENSION = 7 +TRAJECTORY_RESULT_SUCCESS = 0 +TRAJECTORY_RESULT_CANCELED = 1 +TRAJECTORY_RESULT_FAILURE = 2 + #Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -150,6 +154,7 @@ thread jointTrajectoryThread(): #add some form of error handling here end end + socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") textmsg("Trajectory finished") end @@ -212,6 +217,7 @@ while keepalive > 0 and control_mode > MODE_STOPPED: textmsg("cancel received") kill thread_trajectory clear_remaining_trajectory_points() + socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") end end else: diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp index 3443c223..59aba63d 100644 --- a/src/comm/tcp_server.cpp +++ b/src/comm/tcp_server.cpp @@ -272,7 +272,7 @@ void TCPServer::readData(const int fd) { if (message_callback_) { - message_callback_(fd, input_buffer_); + message_callback_(fd, input_buffer_, nbytesrecv); } } else diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index 008392c1..ae1d0de1 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -35,8 +35,8 @@ ReverseInterface::ReverseInterface(uint32_t port, std::function hand : client_fd_(-1), server_(port), handle_program_state_(handle_program_state), keepalive_count_(1) { handle_program_state_(false); - server_.setMessageCallback( - std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2)); + server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1)); server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1)); server_.setMaxClientsAllowed(1); @@ -135,7 +135,7 @@ void ReverseInterface::disconnectionCallback(const int filedescriptor) handle_program_state_(false); } -void ReverseInterface::messageCallback(const int filedescriptor, char* buffer) +void ReverseInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) { URCL_LOG_WARN("Message on ReverseInterface received. The reverse interface currently does not support any message " "handling. This message will be ignored."); diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 28fafd95..f0d8559c 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -105,10 +105,27 @@ void TrajectoryPointInterface::disconnectionCallback(const int filedescriptor) client_fd_ = -1; } -void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer) +void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) { - // TODO: Send up. This will be things like "finished trajectory" - URCL_LOG_INFO("Received message %s on TrajectoryPointInterface", buffer); + if (nbytesrecv == 4) + { + int32_t* status = reinterpret_cast(buffer); + URCL_LOG_DEBUG("Received message %d on TrajectoryPointInterface", be32toh(*status)); + + if (handle_trajectory_end_) + { + handle_trajectory_end_(static_cast(be32toh(*status))); + } + else + { + URCL_LOG_DEBUG("Trajectory execution finished with result %d, but no callback was given"); + } + } + else + { + URCL_LOG_WARN("Received %d bytes on TrajectoryPointInterface. Expecting 4 bytes, so ignoring this message", + nbytesrecv); + } } } // namespace control } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 185af308..fbb1a373 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -49,7 +49,8 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, + std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) @@ -156,19 +157,21 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state)); // TODO swap to configurable or static port trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10)); + trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); URCL_LOG_DEBUG("Initialization done"); } urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, + std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, + headless_mode, std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, + servoj_lookahead_time, non_blocking_read, reverse_ip) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " From 158dfa0ecabc91d4a7609cba03961a79446f9746 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 4 Jun 2021 12:03:55 +0200 Subject: [PATCH 5/8] Move trajectory_done callback out of constructor This will increase backwards compatibility. --- examples/full_driver.cpp | 8 ++----- include/ur_client_library/ur/ur_driver.h | 29 ++++++++++++++++-------- src/ur/ur_driver.cpp | 13 ++++------- 3 files changed, 27 insertions(+), 23 deletions(-) diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 4e9f27b3..45213d4c 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -53,16 +53,12 @@ void handleRobotProgramState(bool program_running) // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; } -void trajectoryDoneCb(urcl::control::TrajectoryResult result) -{ - std::cout << "Trajectory finished in state " << toUnderlying(result) << std::endl; -} int main(int argc, char* argv[]) { std::unique_ptr tool_comm_setup; - g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - &trajectoryDoneCb, false, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, false, + std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); // 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. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 43abeb8c..bb4b339e 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -87,8 +87,7 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, - std::function trajectory_done_cb, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -117,8 +116,7 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, - std::function trajectory_done_cb, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -146,14 +144,13 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, - std::function trajectory_done_cb, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "") - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, - headless_mode, std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, - servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, + std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, + servoj_lookahead_time, non_blocking_read, reverse_ip) { } @@ -292,6 +289,20 @@ class UrDriver */ void setKeepaliveCount(const uint32_t& count); + /*! + * \brief Register a callback for the robot-based trajectory execution completion. + * + * One modes of robot control is to forward a complete trajectory to the robot for execution. + * When the execution is done, the callback function registered here, will be triggered. + * + * \param trajectory_done_cb Callback function that will be triggered in the event of finishing + * a trajectory execution. + */ + void registerTrajectoryDoneCallback(std::function trajectory_done_cb) + { + trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); + } + private: std::string readScriptFile(const std::string& filename); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index fbb1a373..185af308 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -49,8 +49,7 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, - std::function trajectory_done_cb, bool headless_mode, + std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) @@ -157,21 +156,19 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state)); // TODO swap to configurable or static port trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10)); - trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); URCL_LOG_DEBUG("Initialization done"); } urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, - std::function trajectory_done_cb, bool headless_mode, + std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, - headless_mode, std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, + std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, + non_blocking_read, reverse_ip) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " From bacc6c4f57c943cef22543474b67330d21567302 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 11 Jun 2021 11:29:08 +0200 Subject: [PATCH 6/8] Apply documentation suggestions from code review Thanks @t-schnell Co-authored-by: t-schnell --- .../control/trajectory_point_interface.h | 4 +++- include/ur_client_library/ur/ur_driver.h | 13 +++++++------ src/control/trajectory_point_interface.cpp | 5 ++--- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 88a5c82f..cfc6f6c5 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -38,7 +38,7 @@ namespace urcl namespace control { /*! - * \brief Types for encoding trajectory execution result + * \brief Types for encoding trajectory execution result. */ enum class TrajectoryResult : int32_t { @@ -77,6 +77,8 @@ class TrajectoryPointInterface : public ReverseInterface * * \param positions A vector of joint or cartesian targets for the robot * \param time The goal time to reach the target + * \param blend_radius The radius to be used for blending between control points + * \param cartesian True, if the written point is specified in cartesian space, false if in joint space * * \returns True, if the write was performed successfully, false otherwise. */ diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index bb4b339e..30f5ebda 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -185,8 +185,9 @@ class UrDriver * \brief Writes a trajectory point onto the dedicated socket. * * \param values Desired joint or cartesian positions - * \param goal_time Time for the robot to reach this point * \param cartesian True, if the point sent is cartesian, false if joint-based + * \param goal_time Time for the robot to reach this point + * \param blend_radius The radius to be used for blending between control points * * \returns True on successful write. */ @@ -196,8 +197,8 @@ class UrDriver /*! * \brief Writes a control message in trajectory forward mode. * - * \param trajectory_action The action to be taken, such as starting a new trajectory. - * \param point_number The number of points of a new trajectory to be sent. + * \param trajectory_action The action to be taken, such as starting a new trajectory + * \param point_number The number of points of a new trajectory to be sent * * \returns True on successful write. */ @@ -292,11 +293,11 @@ class UrDriver /*! * \brief Register a callback for the robot-based trajectory execution completion. * - * One modes of robot control is to forward a complete trajectory to the robot for execution. - * When the execution is done, the callback function registered here, will be triggered. + * One mode of robot control is to forward a complete trajectory to the robot for execution. + * When the execution is done, the callback function registered here will be triggered. * * \param trajectory_done_cb Callback function that will be triggered in the event of finishing - * a trajectory execution. + * a trajectory execution */ void registerTrajectoryDoneCallback(std::function trajectory_done_cb) { diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index f0d8559c..701a3d2a 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -94,8 +94,7 @@ void TrajectoryPointInterface::connectionCallback(const int filedescriptor) else { URCL_LOG_ERROR("Connection request to TrajectoryPointInterface received while connection already established. Only " - "one " - "connection is allowed at a time. Ignoring this request."); + "one connection is allowed at a time. Ignoring this request."); } } @@ -118,7 +117,7 @@ void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* b } else { - URCL_LOG_DEBUG("Trajectory execution finished with result %d, but no callback was given"); + URCL_LOG_DEBUG("Trajectory execution finished with result %d, but no callback was given."); } } else From 7d8236df187558098e1faa11d17615a685c3b3d3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 11 Jun 2021 11:37:12 +0200 Subject: [PATCH 7/8] Cleanup URScript output for trajectory control --- resources/external_control.urscript | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8ed48208..34eabc00 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -111,10 +111,8 @@ thread speedThread(): end thread jointTrajectoryThread(): - textmsg("Executing trajectory") - textmsg(trajectory_points_left) + textmsg("Executing trajectory. Number of points: ", trajectory_points_left) while trajectory_points_left > 0: - textmsg("Waiting to read point") enter_critical #reading trajectory point + blend radius + type of point (cartesian/joint based) raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket") @@ -125,33 +123,23 @@ thread jointTrajectoryThread(): tmptime = raw_point[7] / MULT_time blend_radius = raw_point[8] / MULT_time - textmsg("Executing Point") - textmsg(trajectory_points_left) - textmsg(q) - textmsg(tmptime) - textmsg(blend_radius) - textmsg(raw_point[9]) - if trajectory_points_left > 0: if raw_point[9] == TRAJECTORY_POINT_JOINT: - textmsg("Executing movej with blending") + # textmsg("Executing movej with blending") movej(q, t=tmptime, r=blend_radius) - textmsg("done") elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: - textmsg("Executing movel with blending") + # textmsg("Executing movel with blending") movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius) end else: if raw_point[9] == TRAJECTORY_POINT_JOINT: - textmsg("Executing movej without blending") + # textmsg("Executing movej without blending") movej(q, t=tmptime, r=0.0) - textmsg("done") elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: - textmsg("Executing movel without blending") + # textmsg("Executing movel without blending") movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=0.0) end end - #add some form of error handling here end end socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") From ca8fc298f6cd38a970dc73b0df3edea0fda24324 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 11 Jun 2021 12:17:00 +0200 Subject: [PATCH 8/8] Use configurable trajectory port setup We should not implicitly open a port that is not exposed in the API interface. With adding this to the API interface (inside the UrDriver's constructor) developers will explicitly know that this port will be opened. --- include/ur_client_library/ur/ur_driver.h | 13 ++++++++++--- resources/external_control.urscript | 3 +-- src/ur/ur_driver.cpp | 16 +++++++++++----- 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 30f5ebda..6ea2f6b0 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -85,12 +85,14 @@ class UrDriver * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP * address of the interface that is used for connecting to the robot's RTDE port will be used. + * \param trajectory_port Port used for sending trajectory points to the robot in case of + * trajectory forwarding. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, - bool non_blocking_read = false, const std::string& reverse_ip = ""); + bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003); /*! * \brief Constructs a new UrDriver object. @@ -114,12 +116,15 @@ class UrDriver * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP * address of the interface that is used for connecting to the robot's RTDE port will be used. + * \param trajectory_port Port used for sending trajectory points to the robot in case of + * trajectory forwarding. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, - double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); + double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", + const uint32_t trajectory_port = 50003); /*! * \brief Constructs a new UrDriver object. * @@ -142,12 +147,14 @@ class UrDriver * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP * address of the interface that is used for connecting to the robot's RTDE port will be used. + * \param trajectory_port Port used for sending trajectory points to the robot in case of + * trajectory forwarding. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, - bool non_blocking_read = false, const std::string& reverse_ip = "") + bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 34eabc00..3e109fd3 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -158,8 +158,7 @@ end # NODE_CONTROL_LOOP_BEGINS socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") -trajectory_port = {{SERVER_PORT_REPLACE}}+10 -socket_open("{{SERVER_IP_REPLACE}}", trajectory_port, "trajectory_socket") +socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") control_mode = MODE_UNINITIALIZED thread_move = 0 diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 185af308..9cf35fde 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -46,13 +46,14 @@ static const std::string TIME_REPLACE("{{TIME_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); +static const std::string TRAJECTORY_PORT_REPLACE("{{TRAJECTORY_SERVER_PORT_REPLACE}}"); urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip) + bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port) : servoj_time_(0.008) , servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) @@ -112,6 +113,11 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); } + while (prog.find(TRAJECTORY_PORT_REPLACE) != std::string::npos) + { + prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(), std::to_string(trajectory_port)); + } + robot_version_ = rtde_client_->getVersion(); std::stringstream begin_replace; @@ -154,8 +160,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state)); - // TODO swap to configurable or static port - trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10)); + trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port)); URCL_LOG_DEBUG("Initialization done"); } @@ -165,10 +170,11 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, - double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) + double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip, + const uint32_t trajectory_port) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip) + non_blocking_read, reverse_ip, trajectory_port) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This "