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 039212d9..c4be869f 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 { @@ -42,7 +41,10 @@ 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 + +#endif // ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED 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 a88b40fe..618be2d4 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. @@ -47,6 +57,8 @@ namespace control class ReverseInterface { public: + static const int32_t MULT_JOINTSTATE = 1000000; + ReverseInterface() = delete; /*! * \brief Creates a ReverseInterface object including a TCPServer. @@ -59,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. @@ -70,30 +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 nbytesrecv); 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/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..cfc6f6c5 --- /dev/null +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -0,0 +1,107 @@ +// 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 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. + */ +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 + * \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. + */ + 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, int nbytesrecv) override; + +private: + std::function handle_trajectory_end_; +}; + +} // 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..6ea2f6b0 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" @@ -84,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. @@ -113,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. * @@ -141,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) @@ -180,6 +188,30 @@ 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 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. + */ + 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. * @@ -265,6 +297,20 @@ class UrDriver */ void setKeepaliveCount(const uint32_t& count); + /*! + * \brief Register a callback for the robot-based trajectory execution completion. + * + * 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 + */ + void registerTrajectoryDoneCallback(std::function trajectory_done_cb) + { + trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); + } + private: std::string readScriptFile(const std::string& filename); @@ -272,6 +318,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/resources/external_control.urscript b/resources/external_control.urscript index eaa594e3..3e109fd3 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,18 @@ 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 + +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 @@ -26,6 +39,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 +110,60 @@ thread speedThread(): stopj(5.0) end +thread jointTrajectoryThread(): + textmsg("Executing trajectory. Number of points: ", trajectory_points_left) + while trajectory_points_left > 0: + 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 + + if trajectory_points_left > 0: + if raw_point[9] == TRAJECTORY_POINT_JOINT: + # textmsg("Executing movej with blending") + movej(q, t=tmptime, r=blend_radius) + 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) + 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 + end + end + socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") + 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") +socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "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 +174,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 +194,18 @@ 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() + socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") + end end else: keepalive = keepalive - 1 @@ -137,7 +216,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 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 d03159a4..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); @@ -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) @@ -100,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 new file mode 100644 index 00000000..701a3d2a --- /dev/null +++ b/src/control/trajectory_point_interface.cpp @@ -0,0 +1,130 @@ +// 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, int nbytesrecv) +{ + 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 613db0cf..9cf35fde 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -40,19 +40,20 @@ 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 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) @@ -86,7 +87,13 @@ 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)); + } + 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; @@ -106,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; @@ -148,6 +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)); + trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port)); URCL_LOG_DEBUG("Initialization done"); } @@ -157,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 " @@ -196,6 +210,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;