diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 4afe6fd14..1b275c249 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -55,11 +55,21 @@ const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; +std::condition_variable g_program_running_cv_; +std::mutex g_program_running_mutex_; +bool g_program_running; + // We need a callback function to register. See UrDriver's parameters for details. 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; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex_); + g_program_running = program_running; + g_program_running_cv_.notify_one(); + } } void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) @@ -72,6 +82,17 @@ void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_ } } +bool waitForProgramRunning(int milliseconds = 100) +{ + std::unique_lock lk(g_program_running_mutex_); + if (g_program_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; +} + int main(int argc, char* argv[]) { urcl::setLogLevel(urcl::LogLevel::INFO); @@ -130,7 +151,16 @@ int main(int argc, char* argv[]) std::unique_ptr tool_comm_setup; const bool HEADLESS = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + std::move(tool_comm_setup))); + + if (!g_my_driver->checkCalibration(CALIBRATION_CHECKSUM)) + { + URCL_LOG_ERROR("Calibration checksum does not match actual robot."); + URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into " + "the description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); + } // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main @@ -141,15 +171,37 @@ int main(int argc, char* argv[]) std::chrono::duration timeout(second_to_run); auto stopwatch_last = std::chrono::steady_clock::now(); auto stopwatch_now = stopwatch_last; - g_my_driver->writeKeepalive(); + // Make sure that external control script is running + if (!waitForProgramRunning()) + { + URCL_LOG_ERROR("External Control script not running."); + return 1; + } // Task frame at the robot's base with limits being large enough to cover the whole workspace // Compliance in z axis and rotation around z axis - g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all - { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }); // limits. See ScriptManual for - // details. + bool success; + if (g_my_driver->getVersion().major < 5) + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits + 0.025); // damping_factor. See ScriptManual for details. + else + { + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits + 0.025, // damping_factor + 0.8); // gain_scaling. See ScriptManual for details. + } + if (!success) + { + URCL_LOG_ERROR("Failed to start force mode."); + return 1; + } while (true) { diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 2a5befde0..c6cb15f6a 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -105,14 +105,23 @@ class ScriptCommandInterface : public ReverseInterface * 2: The force frame is not transformed. * 3: The force frame is transformed in a way such that its x-axis is the projection of the robot tcp velocity vector * onto the x-y plane of the force frame - * \param limits (Float) 6d vector. For compliant axes, these values are the maximum allowed tcp speed along/about the + * \param limits 6d vector. For compliant axes, these values are the maximum allowed tcp speed along/about the * axis. For non-compliant axes, these values are the maximum allowed deviation along/about an axis between the actual * tcp position and the one set by the program * + * \param damping_factor Sets the damping parameter in force mode. In range [0,1], default value is 0.025 + * A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 + * is no damping, here the robot will maintain the speed. + * + * \param gain_scaling_factor Scales the gain in force mode. scaling parameter is in range [0,2], default + * is 0.5. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard + * surfaces. + * * \returns True, if the write was performed successfully, false otherwise. */ bool startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector, const vector6d_t* wrench, - const unsigned int type, const vector6d_t* limits); + const unsigned int type, const vector6d_t* limits, double damping_factor, + double gain_scaling_factor); /*! * \brief Stop force mode and put the robot into normal operation mode. @@ -178,7 +187,7 @@ class ScriptCommandInterface : public ReverseInterface }; bool client_connected_; - static const int MAX_MESSAGE_LENGTH = 26; + static const int MAX_MESSAGE_LENGTH = 28; std::function handle_tool_contact_result_; }; diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 49a4fde7b..5145a75d8 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -32,6 +32,7 @@ #include #include #include +#include "ur/version_information.h" namespace urcl { @@ -124,5 +125,67 @@ class TimeoutException : public UrException private: std::string text_; }; + +class IncompatibleRobotVersion : public UrException +{ +public: + explicit IncompatibleRobotVersion() = delete; + explicit IncompatibleRobotVersion(const std::string& text, const VersionInformation& minimum_robot_version, + const VersionInformation& actual_robot_version) + : std::runtime_error(text) + { + std::stringstream ss; + ss << text << "\n" + << "The requested feature is incompatible with the connected robot. Minimum required Polyscope version: " + << minimum_robot_version << ", actual Polyscope version: " << actual_robot_version; + text_ = ss.str(); + } + virtual const char* what() const noexcept override + { + return text_.c_str(); + } + +private: + std::string text_; +}; + +class InvalidRange : public UrException +{ +private: + std::string text_; + +public: + explicit InvalidRange() = delete; + explicit InvalidRange(std::string text) : std::runtime_error(text) + { + text_ = text; + } + virtual const char* what() const noexcept override + { + return text_.c_str(); + } +}; + +class MissingArgument : public UrException +{ +private: + std::string text_; + +public: + explicit MissingArgument() = delete; + explicit MissingArgument(std::string text, std::string function_name, std::string argument_name, float default_value) + : std::runtime_error("") + { + std::stringstream ss; + ss << text << "\nMissing argument when calling function: " << function_name + << ". \nArgument missing: " << argument_name + << ". \nSet to default value if not important, default value is: " << default_value; + text_ = ss.str(); + } + virtual const char* what() const noexcept override + { + return text_.c_str(); + } +}; } // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index f0f9b0a3c..cf57963e3 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -92,16 +92,61 @@ class UrDriver * trajectory forwarding. * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be * executed locally on the robot. - * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] - * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ 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 = "", 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); + const uint32_t script_command_port = 50004); + + /*! + * \brief Constructs a new UrDriver object. + * \param robot_ip IP-address under which the robot is reachable. + * \param script_file URScript file that should be sent to the robot. + * \param output_recipe_file Filename where the output recipe is stored in. + * \param input_recipe_file Filename where the input recipe is stored in. + * \param handle_program_state Function handle to a callback on program state changes. For this to + * work, the URScript program will have to send keepalive signals to the \p reverse_port. I no + * keepalive signal can be read, program state will be false. + * \param headless_mode Parameter to control if the driver should be started in headless mode. + * \param tool_comm_setup Configuration for using the tool communication. + * calibration reported by the robot. + * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver + * and the robot controller. + * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. If + * the robot cannot connect to this port, `External Control` will stop immediately. + * \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw) + * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000] + * \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. + * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be + * executed locally on the robot. + * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] + * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) + */ + [[deprecated( + "Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has " + "been deprecated. Force mode parameters should be specified with each activiation of force mode, and " + "can be set in the function call to start force mode.")]] 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, + const uint32_t trajectory_port, + const uint32_t script_command_port, + double force_mode_damping, + double force_mode_gain_scaling = 0.5); /*! * \brief Constructs a new UrDriver object. @@ -134,7 +179,7 @@ class 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 std::string& calibration_checksum = "", + 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 = "", const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004, @@ -340,10 +385,73 @@ class UrDriver * along/about the axis. For non-compliant axes, these values are the maximum allowed deviation along/about an axis * between the actual tcp position and the one set by the program * + * \param damping_factor (double) Sets the damping parameter in force mode. In range [0,1], default value is 0.025 + * A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 + * is no damping, here the robot will maintain the speed. + * * \returns True, if the write was performed successfully, false otherwise. */ bool startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench, - const unsigned int type, const vector6d_t& limits); + const unsigned int type, const vector6d_t& limits, double damping_factor); + + /*! + * \brief Start the robot to be controlled in force mode. + * + * \param task_frame A pose vector that defines the force frame relative to the base frame + * \param selection_vector A 6d vector of 0s and 1s. 1 means that the robot will be compliant in the corresponding + * axis of the task frame + * \param wrench 6d vector of forces/torques [x,y,z,rotX,rotY,rotZ] that the robot will apply to its environment. The + * robot adjusts its position along/about compliant axis in order to achieve the specified force/torque. Values have + * no effect for non-compliant axes. + * \param type An integer [1;3] specifying how the robot interprets the force frame. + * 1: The force frame is transformed in a way such that its y-axis is aligned with a vector pointing from the robot + * tcp towards the origin of the force frame + * 2: The force frame is not transformed + * 3: The force frame is transformed in a way such that its x-axis is the projection of the robot tcp velocity vector + * onto the x-y plane of the force frame + * \param limits (double) 6d vector. For compliant axes, these values are the maximum allowed tcp speed + * along/about the axis. For non-compliant axes, these values are the maximum allowed deviation along/about an axis + * between the actual tcp position and the one set by the program + * + * \param damping_factor (double) Sets the damping parameter in force mode. In range [0,1], default value is 0.025 + * A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 + * is no damping, here the robot will maintain the speed. + * + * \param gain_scaling_factor (double) Scales the gain in force mode. scaling parameter in range [0,2], default is + * 0.5. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard + * surfaces. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + 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); + + /*! + * \brief Start the robot to be controlled in force mode. + * + * \param task_frame A pose vector that defines the force frame relative to the base frame + * \param selection_vector A 6d vector of 0s and 1s. 1 means that the robot will be compliant in the corresponding + * axis of the task frame + * \param wrench 6d vector of forces/torques [x,y,z,rotX,rotY,rotZ] that the robot will apply to its environment. The + * robot adjusts its position along/about compliant axis in order to achieve the specified force/torque. Values have + * no effect for non-compliant axes. + * \param type An integer [1;3] specifying how the robot interprets the force frame. + * 1: The force frame is transformed in a way such that its y-axis is aligned with a vector pointing from the robot + * tcp towards the origin of the force frame + * 2: The force frame is not transformed + * 3: The force frame is transformed in a way such that its x-axis is the projection of the robot tcp velocity vector + * onto the x-y plane of the force frame + * \param limits (double) 6d vector. For compliant axes, these values are the maximum allowed tcp speed + * along/about the axis. For non-compliant axes, these values are the maximum allowed deviation along/about an axis + * between the actual tcp position and the one set by the program + * + * \returns True, if the write was performed successfully, false otherwise. + */ + [[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); /*! * \brief Stop force mode and put the robot into normal operation mode. @@ -540,6 +648,9 @@ class UrDriver std::unique_ptr> primary_stream_; std::unique_ptr> secondary_stream_; + double force_mode_gain_scale_factor_ = 0.5; + double force_mode_damping_factor_ = 0.025; + uint32_t servoj_gain_; double servoj_lookahead_time_; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8624c4899..3b3fd53a1 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -34,7 +34,7 @@ TRAJECTORY_MODE_CANCEL = -1 TRAJECTORY_POINT_JOINT = 0 TRAJECTORY_POINT_CARTESIAN = 1 TRAJECTORY_POINT_JOINT_SPLINE = 2 -TRAJECTORY_DATA_DIMENSION = 3*6 + 1 +TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1 TRAJECTORY_RESULT_SUCCESS = 0 TRAJECTORY_RESULT_CANCELED = 1 @@ -47,7 +47,7 @@ START_FORCE_MODE = 3 END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 -SCRIPT_COMMAND_DATA_DIMENSION = 26 +SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 FREEDRIVE_MODE_STOP = -1 @@ -356,7 +356,7 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c scaled_step_time = get_steptime() * scaling_factor splineTimerTraveled = splineTimerTraveled + scaled_step_time - + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor, is_slowing_down) end scaling_factor = 0.0 @@ -373,7 +373,7 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, timeLeftToTravel, scaling_factor, is_slowing_down) end -def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor, is_slowing_down=False): +def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor, is_slowing_down = False): local last_spline_qd = spline_qd spline_qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 spline_qdd = 2.0 * coefficients2 + 6.0 * splineTimerTraveled * coefficients3 + 12.0 * pow(splineTimerTraveled, 2) * coefficients4 + 20.0 * pow(splineTimerTraveled, 3) * coefficients5 @@ -469,7 +469,7 @@ thread trajectoryThread(): trajectory_points_left = trajectory_points_left - 1 if raw_point[0] > 0: - local 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] + local 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] local tmptime = raw_point[INDEX_TIME] / MULT_time local blend_radius = raw_point[INDEX_BLEND] / MULT_time local is_last_point = False @@ -479,31 +479,32 @@ thread trajectoryThread(): end # MoveJ point if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: - movej(q, t=tmptime, r=blend_radius) + movej(q, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] - # Movel point + # Movel point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN: - movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius) + movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] - # Joint spline point + # Joint spline point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: # Cubic spline if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] is_robot_moving = cubicSplineRun(q, qd, tmptime, is_last_point, is_first_point) + # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] - # Quintic spline + # Quintic spline elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate] @@ -528,7 +529,7 @@ end def clear_remaining_trajectory_points(): while trajectory_points_left > 0: - raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+2, "trajectory_socket") + raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket") trajectory_points_left = trajectory_points_left - 1 end end @@ -646,6 +647,13 @@ thread script_commands(): wrench = [raw_command[14] / MULT_jointstate, raw_command[15] / MULT_jointstate, raw_command[16] / MULT_jointstate, raw_command[17] / MULT_jointstate, raw_command[18] / MULT_jointstate, raw_command[19] / MULT_jointstate] force_type = raw_command[20] / MULT_jointstate force_limits = [raw_command[21] / MULT_jointstate, raw_command[22] / MULT_jointstate, raw_command[23] / MULT_jointstate, raw_command[24] / MULT_jointstate, raw_command[25] / MULT_jointstate, raw_command[26] / MULT_jointstate] + force_mode_set_damping(raw_command[27] / MULT_jointstate) + # Check whether script is running on CB3 or e-series. Gain scaling can only be set on e-series robots. + # step time = 0.008: CB3 robot + # Step time = 0.002: e-series robot + if (get_steptime() < 0.008): + force_mode_set_gain_scaling(raw_command[28] / MULT_jointstate) + end force_mode(task_frame, selection_vector, wrench, force_type, force_limits) elif command == END_FORCE_MODE: end_force_mode() @@ -665,9 +673,6 @@ end # HEADER_END # NODE_CONTROL_LOOP_BEGINS -force_mode_set_damping({{FORCE_MODE_SET_DAMPING_REPLACE}}) -force_mode_set_gain_scaling({{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}) - socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") socket_open("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket") # This socket should be opened last as it tells the driver when it has control over the robot @@ -684,8 +689,10 @@ while control_mode > MODE_STOPPED: enter_critical params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout) if params_mult[0] > 0: + # Convert read timeout from milliseconds to seconds read_timeout = params_mult[1] / 1000.0 + if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]: # Clear remaining trajectory points if control_mode == MODE_FORWARD: @@ -693,7 +700,7 @@ while control_mode > MODE_STOPPED: clear_remaining_trajectory_points() stopj(STOPJ_ACCELERATION) socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") - # Stop freedrive + # Stop freedrive elif control_mode == MODE_FREEDRIVE: textmsg("Leaving freedrive mode") end_freedrive_mode() @@ -724,10 +731,10 @@ while control_mode > MODE_STOPPED: # Update the motion commands with new parameters if control_mode == MODE_SERVOJ: - q = [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] + q = [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_servo_setpoint(q) 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] + 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: diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 771e1c7cc..63c48eabd 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -108,9 +108,10 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) } bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector, - const vector6d_t* wrench, const unsigned int type, const vector6d_t* limits) + const vector6d_t* wrench, const unsigned int type, const vector6d_t* limits, + double damping_factor, double gain_scaling_factor) { - const int message_length = 26; + const int message_length = 28; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -144,6 +145,12 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const b_pos += append(b_pos, val); } + val = htobe32(static_cast(round(damping_factor * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + + val = htobe32(static_cast(round(gain_scaling_factor * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + // writing zeros to allow usage with other script commands for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 90a33c17d..c18cfa1f2 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -58,7 +58,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ 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, const uint32_t trajectory_port, - const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling) + const uint32_t script_command_port) : servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) , handle_program_state_(handle_program_state) @@ -123,45 +123,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ std::to_string(script_command_port)); } - while (prog.find(FORCE_MODE_SET_DAMPING_REPLACE) != std::string::npos) - { - if (force_mode_damping < 0 || force_mode_damping > 1) - { - std::stringstream ss; - ss << "Force mode damping, should be between 0 and 1, but it is " << force_mode_damping; - force_mode_damping = 0.025; - ss << " setting it to default " << force_mode_damping; - URCL_LOG_ERROR(ss.str().c_str()); - } - prog.replace(prog.find(FORCE_MODE_SET_DAMPING_REPLACE), FORCE_MODE_SET_DAMPING_REPLACE.length(), - std::to_string(force_mode_damping)); - } - - while (prog.find(FORCE_MODE_SET_GAIN_SCALING_REPLACE) != std::string::npos) - { - if (robot_version_.major < 5) - { - // force_mode_set_gain_scaling is only available for e-series and is therefore removed, if the robot is not - // e-series - std::stringstream ss; - ss << "force_mode_set_gain_scaling(" << FORCE_MODE_SET_GAIN_SCALING_REPLACE << ")"; - prog.replace(prog.find(ss.str()), ss.str().length(), ""); - } - else - { - if (force_mode_gain_scaling < 0 || force_mode_gain_scaling > 2) - { - std::stringstream ss; - ss << "Force mode gain scaling, should be between 0 and 2, but it is " << force_mode_gain_scaling; - force_mode_gain_scaling = 0.5; - ss << " setting it to default " << force_mode_gain_scaling; - URCL_LOG_ERROR(ss.str().c_str()); - } - prog.replace(prog.find(FORCE_MODE_SET_GAIN_SCALING_REPLACE), FORCE_MODE_SET_GAIN_SCALING_REPLACE.length(), - std::to_string(force_mode_gain_scaling)); - } - } - robot_version_ = rtde_client_->getVersion(); std::stringstream begin_replace; @@ -210,6 +171,21 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ 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::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, const uint32_t trajectory_port, + const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling) + : 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, trajectory_port, script_command_port) +{ + force_mode_damping_factor_ = force_mode_damping; + force_mode_gain_scale_factor_ = force_mode_gain_scaling; +} + 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, @@ -371,10 +347,85 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) return sendScript(cmd.str()); } } +// Function for e-series robots (Needs both damping factor and gain scaling factor) +bool UrDriver::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) +{ + if (robot_version_.major < 5) + { + std::stringstream ss; + ss << "Force mode gain scaling factor cannot be set on a CB3 robot."; + URCL_LOG_ERROR(ss.str().c_str()); + VersionInformation req_version = VersionInformation::fromString("5.0.0.0"); + throw IncompatibleRobotVersion(ss.str(), req_version, robot_version_); + } + // Test that the type is either 1, 2 or 3. + switch (type) + { + case 1: + break; + case 2: + break; + case 3: + break; + default: + std::stringstream ss; + ss << "The type should be 1, 2 or 3. The type is " << type; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); + } + for (unsigned int i = 0; i < selection_vector.size(); ++i) + { + if (selection_vector[i] > 1) + { + std::stringstream ss; + ss << "The selection vector should only consist of 0's and 1's"; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); + } + } + + if (damping_factor > 1 || damping_factor < 0) + { + std::stringstream ss; + ss << "The force mode damping factor should be between 0 and 1, both inclusive."; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); + } + + if (gain_scaling_factor > 2 || gain_scaling_factor < 0) + { + std::stringstream ss; + ss << "The force mode gain scaling factor should be between 0 and 2, both inclusive."; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, type, &limits, + damping_factor, gain_scaling_factor); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to start Force mode."); + return false; + } +} +// Function for CB3 robots (CB3 robots cannot use gain scaling) bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, - const vector6d_t& wrench, const unsigned int type, const vector6d_t& limits) + const vector6d_t& wrench, const unsigned int type, const vector6d_t& limits, + double damping_factor) { + if (robot_version_.major >= 5) + { + std::stringstream ss; + ss << "You should also specify a force mode gain scaling factor to activate force mode on an e-series robot."; + URCL_LOG_ERROR(ss.str().c_str()); + throw MissingArgument(ss.str(), "startForceMode", "gain_scaling_factor", 0.5); + } // Test that the type is either 1, 2 or 3. switch (type) { @@ -388,20 +439,31 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ std::stringstream ss; ss << "The type should be 1, 2 or 3. The type is " << type; URCL_LOG_ERROR(ss.str().c_str()); - return false; + throw InvalidRange(ss.str().c_str()); } for (unsigned int i = 0; i < selection_vector.size(); ++i) { if (selection_vector[i] > 1) { - URCL_LOG_ERROR("The selection vector should only consist of 0's and 1's"); - return false; + std::stringstream ss; + ss << "The selection vector should only consist of 0's and 1's"; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); } } + if (damping_factor > 1 || damping_factor < 0) + { + std::stringstream ss; + ss << "The force mode damping factor should be between 0 and 1, both inclusive."; + URCL_LOG_ERROR(ss.str().c_str()); + throw InvalidRange(ss.str().c_str()); + } + if (script_command_interface_->clientConnected()) { - return script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, type, &limits); + return script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, type, &limits, + damping_factor, 0); } else { @@ -410,6 +472,20 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ } } +bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, + const vector6d_t& wrench, const unsigned int type, const vector6d_t& limits) +{ + if (robot_version_.major < 5) + { + return startForceMode(task_frame, selection_vector, wrench, type, limits, force_mode_damping_factor_); + } + else + { + return startForceMode(task_frame, selection_vector, wrench, type, limits, force_mode_damping_factor_, + force_mode_gain_scale_factor_); + } +} + bool UrDriver::endForceMode() { if (script_command_interface_->clientConnected()) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 682adba6e..d834502a8 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -64,11 +64,11 @@ class ScriptCommandInterfaceTest : public ::testing::Test void readMessage(int32_t& command, std::vector& message) { - // Max message length is 26 - uint8_t buf[sizeof(int32_t) * 26]; + // Max message length is 28 + uint8_t buf[sizeof(int32_t) * 28]; uint8_t* b_pos = buf; size_t read = 0; - size_t remainder = sizeof(int32_t) * 26; + size_t remainder = sizeof(int32_t) * 28; while (remainder > 0) { if (!TCPSocket::read(b_pos, remainder, read)) @@ -252,7 +252,10 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode) urcl::vector6d_t wrench = { 20, 0, 40, 0, 0, 0 }; int32_t force_mode_type = 2; urcl::vector6d_t limits = { 0.1, 0.1, 0.1, 0.785, 0.785, 1.57 }; - script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits); + double damping = 0.8; + double gain_scaling = 0.8; + script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits, damping, + gain_scaling); int32_t command; std::vector message; @@ -298,8 +301,16 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode) EXPECT_EQ(received_limits[i], limits[i]); } + // Test damping return + double received_damping = (double)message[25] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_damping, damping); + + // Test Gain scaling return + double received_gain = (double)message[26] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_gain, gain_scaling); + // The rest of the message should be zero - int32_t message_sum = std::accumulate(std::begin(message) + 25, std::end(message), 0); + int32_t message_sum = std::accumulate(std::begin(message) + 27, std::end(message), 0); int32_t expected_message_sum = 0; EXPECT_EQ(message_sum, expected_message_sum);