From e1797f3d760b0efee28cf4fcf88201ebdd43187e Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 3 Jul 2024 11:03:34 +0000 Subject: [PATCH] Implement force mode arguments in urscript And in client library --- examples/force_mode_example.cpp | 28 +++- .../control/script_command_interface.h | 13 +- include/ur_client_library/ur/ur_driver.h | 111 ++++++++++++- resources/external_control.urscript | 60 +++---- src/control/script_command_interface.cpp | 11 +- src/ur/ur_driver.cpp | 150 ++++++++++++------ 6 files changed, 283 insertions(+), 90 deletions(-) diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 4afe6fd1..409ff590 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -130,7 +130,7 @@ 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))); // 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 +141,27 @@ 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 + for (int i = 0; i < 100; i++) + { + g_my_driver->writeKeepalive(); + g_my_driver->getDataPackage(); + } // 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 = 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 + , + 0.8, 0.8); // 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 2a5befde..07c17255 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -109,10 +109,19 @@ class ScriptCommandInterface : public ReverseInterface * 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 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/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index c4970e3c..e6cf099b 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -92,16 +92,51 @@ 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 +169,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, @@ -321,6 +356,67 @@ class UrDriver */ bool setToolVoltage(const ToolVoltage voltage); + /*! + * \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. + * + * \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); + + /*! + * \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. * @@ -342,6 +438,8 @@ class UrDriver * * \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); @@ -500,6 +598,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_; std::chrono::milliseconds step_time_; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index d132651c..2cf2e6a8 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 @@ -113,11 +113,11 @@ thread servoThread(): end q = extrapolate() - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + servoj(q, t = steptime, {{SERVO_J_REPLACE}}) elif state == SERVO_RUNNING: extrapolate_count = 0 - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + servoj(q, t = steptime, {{SERVO_J_REPLACE}}) else: extrapolate_count = 0 sync() @@ -144,7 +144,7 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end -def cubicSplineRun(end_q, end_qd, time, is_last_point=False): +def cubicSplineRun(end_q, end_qd, time, is_last_point = False): local str = str_cat(end_q, str_cat(end_qd, time)) textmsg("Cubic spline arg: ", str) @@ -163,7 +163,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False): end end -def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): +def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point = False): local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time))) textmsg("Quintic spline arg: ", str) @@ -244,7 +244,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 @@ -261,7 +261,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 @@ -341,11 +341,11 @@ thread trajectoryThread(): enter_critical while trajectory_points_left > 0: #reading trajectory point + blend radius + type of point (cartesian/joint based) - local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime()) + local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 1 + 1, "trajectory_socket", get_steptime()) 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 @@ -355,34 +355,34 @@ 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] + 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] cubicSplineRun(q, qd, tmptime, is_last_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] + 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] quinticSplineRun(q, qd, qdd, tmptime, is_last_point) else: textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) @@ -400,7 +400,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 @@ -443,11 +443,11 @@ thread servoThreadP(): end q = extrapolate() - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + servoj(q, t = steptime, {{SERVO_J_REPLACE}}) elif state == SERVO_RUNNING: extrapolate_count = 0 - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + servoj(q, t = steptime, {{SERVO_J_REPLACE}}) else: extrapolate_count = 0 sync() @@ -513,6 +513,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() @@ -536,9 +543,6 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") socket_open("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket") -force_mode_set_damping({{FORCE_MODE_SET_DAMPING_REPLACE}}) -force_mode_set_gain_scaling({{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}) - control_mode = MODE_UNINITIALIZED thread_move = 0 thread_trajectory = 0 @@ -551,7 +555,7 @@ while control_mode > MODE_STOPPED: params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout) if params_mult[0] > 0: # Convert to read timeout from milliseconds to seconds - read_timeout = params_mult[1] / 1000.0 + 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: @@ -559,7 +563,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() @@ -590,10 +594,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 771e1c7c..63c48eab 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 9292681b..ed67b475 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) , step_time_(std::chrono::milliseconds(8)) @@ -129,45 +129,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; @@ -181,9 +142,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } begin_replace << "set_tool_voltage(" << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; - begin_replace << "set_tool_communication(" - << "True" - << ", " << tool_comm_setup->getBaudRate() << ", " + begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", " << static_cast::type>(tool_comm_setup->getParity()) << ", " << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " << tool_comm_setup->getTxIdleChars() << ")"; @@ -217,6 +176,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, @@ -378,10 +352,75 @@ 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) + { + URCL_LOG_ERROR("Force mode gain scaling factor cannot be set on a CB3 robot."); + return false; + } + // 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()); + return false; + } + 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; + } + } + + if (damping_factor > 1 || damping_factor < 0) + { + URCL_LOG_ERROR("The force mode damping factor should be between 0 and 1, both inclusive."); + return false; + } + + if (gain_scaling_factor < 0 || gain_scaling_factor > 2) + { + URCL_LOG_ERROR("The force mode gain scaling factor should be between 0 and 2, both inclusive."); + return false; + } + + 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) + { + URCL_LOG_ERROR("You should also specify a force mode gain scaling factor to activate force mode on a e-series " + "robot."); + return false; + } // Test that the type is either 1, 2 or 3. switch (type) { @@ -406,9 +445,16 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_ } } + if (damping_factor > 1 || damping_factor < 0) + { + URCL_LOG_ERROR("The force mode damping factor should be between 0 and 1, both inclusive."); + return false; + } + 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 { @@ -417,6 +463,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())