ur_driver.h

Go to the documentation of this file.

// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
 
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// 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 ------------------------------------------------
 
//----------------------------------------------------------------------
//----------------------------------------------------------------------
#ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
#define UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
 
#include <chrono>
#include <functional>
#include <memory>
 
#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_command_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"
#include "ur_client_library/ur/robot_receive_timeout.h"
#include "ur_client_library/primary/primary_client.h"
#include "ur_client_library/primary/robot_message/version_message.h"
#include "ur_client_library/rtde/rtde_writer.h"
 
namespace urcl
{
struct UrDriverConfiguration
{
  std::string robot_ip;            
  std::string script_file;         
  std::string output_recipe_file;  
  std::string input_recipe_file;   
 
  std::function<void(bool)> handle_program_state;
  bool headless_mode;  
 
  std::unique_ptr<ToolCommSetup> tool_comm_setup = nullptr;  
 
  uint32_t reverse_port = 50001;
 
  uint32_t script_sender_port = 50002;
 
  uint32_t trajectory_port = 50003;
 
  uint32_t script_command_port = 50004;
 
  std::string reverse_ip = "";
 
  int servoj_gain = 2000;
 
  double servoj_lookahead_time = 0.03;
 
  size_t socket_reconnect_attempts = 0;
 
  std::chrono::milliseconds socket_reconnection_timeout = std::chrono::seconds(10);
 
  size_t rtde_initialization_attempts = 3;
 
  std::chrono::milliseconds rtde_initialization_timeout = std::chrono::seconds(5);
 
  bool non_blocking_read = false;
 
  // TODO: Remove on 2027-05
  // The following parameters are considered deprecated and will be removed in May 2027.
  std::string calibration_checksum = "";
  double force_mode_damping = 0.025;
  double force_mode_gain_scaling = 0.5;
};
 
class UrDriver
{
public:
  explicit UrDriver(const UrDriverConfiguration& config)
  {
    init(config);
  }
 
  // Called sigA in tests
  [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
               "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
  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<void(bool)> handle_program_state, bool headless_mode,
           std::unique_ptr<ToolCommSetup> 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 = "", const uint32_t trajectory_port = 50003,
           const uint32_t script_command_port = 50004)
  {
    UrDriverConfiguration config;
    config.robot_ip = robot_ip;
    config.script_file = script_file;
    config.output_recipe_file = output_recipe_file;
    config.input_recipe_file = input_recipe_file;
    config.handle_program_state = handle_program_state;
    config.headless_mode = headless_mode;
    config.tool_comm_setup = std::move(tool_comm_setup);
    config.reverse_port = reverse_port;
    config.script_sender_port = script_sender_port;
    config.servoj_gain = servoj_gain;
    config.servoj_lookahead_time = servoj_lookahead_time;
    config.non_blocking_read = non_blocking_read;
    config.reverse_ip = reverse_ip;
    config.trajectory_port = trajectory_port;
    config.script_command_port = script_command_port;
    init(config);
  }
 
  // Called sigB in tests
  [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
               "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
  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<void(bool)> handle_program_state, bool headless_mode,
           std::unique_ptr<ToolCommSetup> 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, const uint32_t trajectory_port, const uint32_t script_command_port,
           double force_mode_damping, double force_mode_gain_scaling = 0.5)
  {
    UrDriverConfiguration config;
    config.robot_ip = robot_ip;
    config.script_file = script_file;
    config.output_recipe_file = output_recipe_file;
    config.input_recipe_file = input_recipe_file;
    config.handle_program_state = handle_program_state;
    config.headless_mode = headless_mode;
    config.tool_comm_setup = std::move(tool_comm_setup);
    config.reverse_port = reverse_port;
    config.script_sender_port = script_sender_port;
    config.servoj_gain = servoj_gain;
    config.servoj_lookahead_time = servoj_lookahead_time;
    config.non_blocking_read = non_blocking_read;
    config.reverse_ip = reverse_ip;
    config.trajectory_port = trajectory_port;
    config.script_command_port = script_command_port;
    config.force_mode_damping = force_mode_damping;
    config.force_mode_gain_scaling = force_mode_gain_scaling;
    init(config);
  }
 
  // Called sigC in tests
  [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
               "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
  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<void(bool)> handle_program_state, bool headless_mode,
           std::unique_ptr<ToolCommSetup> 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 = "",
           const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004,
           double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5)
  {
    UrDriverConfiguration config;
    config.robot_ip = robot_ip;
    config.script_file = script_file;
    config.output_recipe_file = output_recipe_file;
    config.input_recipe_file = input_recipe_file;
    config.handle_program_state = handle_program_state;
    config.headless_mode = headless_mode;
    config.calibration_checksum = calibration_checksum;
    config.tool_comm_setup = std::move(tool_comm_setup);
    config.reverse_port = reverse_port;
    config.script_sender_port = script_sender_port;
    config.servoj_gain = servoj_gain;
    config.servoj_lookahead_time = servoj_lookahead_time;
    config.non_blocking_read = non_blocking_read;
    config.reverse_ip = reverse_ip;
    config.trajectory_port = trajectory_port;
    config.script_command_port = script_command_port;
    config.force_mode_damping = force_mode_damping;
    config.force_mode_gain_scaling = force_mode_gain_scaling;
    init(config);
  }
  // Called sigD in tests
  [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
               "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
  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<void(bool)> 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 = "", const uint32_t trajectory_port = 50003,
           const uint32_t script_command_port = 50004, double force_mode_damping = 0.025,
           double force_mode_gain_scaling = 0.5)
  {
    UrDriverConfiguration config;
    config.robot_ip = robot_ip;
    config.script_file = script_file;
    config.output_recipe_file = output_recipe_file;
    config.input_recipe_file = input_recipe_file;
    config.handle_program_state = handle_program_state;
    config.headless_mode = headless_mode;
    config.calibration_checksum = calibration_checksum;
    config.reverse_port = reverse_port;
    config.script_sender_port = script_sender_port;
    config.servoj_gain = servoj_gain;
    config.servoj_lookahead_time = servoj_lookahead_time;
    config.non_blocking_read = non_blocking_read;
    config.reverse_ip = reverse_ip;
    config.trajectory_port = trajectory_port;
    config.script_command_port = script_command_port;
    config.force_mode_damping = force_mode_damping;
    config.force_mode_gain_scaling = force_mode_gain_scaling;
    init(config);
  }
 
  virtual ~UrDriver() = default;
 
  std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
 
  uint32_t getControlFrequency() const
  {
    return rtde_client_->getTargetFrequency();
  }
 
  bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode,
                         const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(20));
 
  bool writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time = 0.0,
                            const float blend_radius = 0.052);
 
  bool writeTrajectoryPoint(const vector6d_t& positions, float acceleration, float velocity, const bool cartesian,
                            const float goal_time = 0.0, const float blend_radius = 0.052);
 
  bool writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities,
                                  const vector6d_t& accelerations, const float goal_time = 0.0);
 
  bool writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities,
                                  const float goal_time = 0.0);
 
  bool writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time = 0.0);
 
  bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> motion_instruction);
 
  bool
  writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, const int point_number = 0,
                                const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
 
  bool
  writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action,
                               const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
 
  bool zeroFTSensor();
 
  bool setPayload(const float mass, const vector3d_t& cog);
 
  bool setToolVoltage(const ToolVoltage voltage);
 
  bool startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench,
                      const unsigned int type, const vector6d_t& limits, double damping_factor);
 
  bool startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench,
                      const unsigned int type, const vector6d_t& limits, double damping_factor,
                      double gain_scaling_factor);
 
  [[deprecated("Starting force mode without specifying the force mode damping factor and gain scale factor has been "
               "deprecated. These values should be given with each function call.")]] bool
  startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench,
                 const unsigned int type, const vector6d_t& limits);
 
  bool endForceMode();
 
  bool startToolContact();
 
  bool endToolContact();
 
  bool writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(1000));
 
  void startRTDECommunication();
 
  bool stopControl();
 
  bool checkCalibration(const std::string& checksum);
 
  std::deque<urcl::primary_interface::ErrorCode> getErrorCodes();
 
  rtde_interface::RTDEWriter& getRTDEWriter();
 
  bool sendScript(const std::string& program);
 
  bool sendRobotProgram();
 
  const VersionInformation& getVersion()
  {
    return robot_version_;
  }
 
  std::vector<std::string> getRTDEOutputRecipe();
 
  [[deprecated("Set keepaliveCount is deprecated, instead set the robot receive timeout directly in the write "
               "commands.")]] void
  setKeepaliveCount(const uint32_t count);
 
  void registerTrajectoryDoneCallback(std::function<void(control::TrajectoryResult)> trajectory_done_cb)
  {
    trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb);
  }
 
  void registerToolContactResultCallback(std::function<void(control::ToolContactResult)> tool_contact_result_cb)
  {
    script_command_interface_->setToolContactResultCallback(tool_contact_result_cb);
  }
 
  void resetRTDEClient(const std::vector<std::string>& output_recipe, const std::vector<std::string>& input_recipe,
                       double target_frequency = 0.0, bool ignore_unavailable_outputs = false);
 
  void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
                       double target_frequency = 0.0, bool ignore_unavailable_outputs = false);
 
  void startPrimaryClientCommunication();
 
  void stopPrimaryClientCommunication()
  {
    primary_client_->stop();
  }
 
  void registerTrajectoryInterfaceDisconnectedCallback(std::function<void(const int)> fun)
  {
    trajectory_interface_->registerDisconnectionCallback(fun);
  }
 
  static std::string readScriptFile(const std::string& filename);
 
  bool isReverseInterfaceConnected() const
  {
    return reverse_interface_->isConnected();
  }
 
  bool isTrajectoryInterfaceConnected() const
  {
    return trajectory_interface_->isConnected();
  }
 
  std::shared_ptr<urcl::primary_interface::PrimaryClient> getPrimaryClient()
  {
    return primary_client_;
  }
 
private:
  void init(const UrDriverConfiguration& config);
 
  void initRTDE();
  void setupReverseInterface(const uint32_t reverse_port);
 
  comm::INotifier notifier_;
  std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
  std::shared_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
  std::unique_ptr<control::ReverseInterface> reverse_interface_;
  std::unique_ptr<control::TrajectoryPointInterface> trajectory_interface_;
  std::unique_ptr<control::ScriptCommandInterface> script_command_interface_;
  std::unique_ptr<control::ScriptSender> script_sender_;
 
  size_t socket_connection_attempts_ = 0;
  std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000);
 
  size_t rtde_initialization_attempts_ = 0;
  std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::milliseconds(10000);
 
  double force_mode_gain_scale_factor_ = 0.5;
  double force_mode_damping_factor_ = 0.025;
 
  uint32_t servoj_gain_;
  double servoj_lookahead_time_;
 
  std::function<void(bool)> handle_program_state_;
 
  std::string robot_ip_;
  bool in_headless_mode_;
  std::string full_robot_program_;
 
  int get_packet_timeout_;
  bool non_blocking_read_;
 
  VersionInformation robot_version_;
};
}  // namespace urcl
#endif  // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED