From 2adb339a40654da3f67a376d38a2dca853d65189 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Thu, 20 Feb 2025 07:54:22 +0100 Subject: [PATCH 1/4] feat: add torque control to urscript --- include/ur_client_library/comm/control_mode.h | 9 +++++--- resources/external_control.urscript | 23 +++++++++++++++++++ 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 62dd706f..c4889cc3 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -52,6 +52,7 @@ enum class ControlMode : int32_t MODE_FREEDRIVE = 6, ///< Set when freedrive mode is active. MODE_TOOL_IN_CONTACT = 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() + MODE_TORQUE = 8, ///< Set when torque control is active. END ///< This is not an actual control mode, but used internally to get the number of control modes }; @@ -62,9 +63,11 @@ class ControlModeTypes { public: // Control modes that require realtime communication - static const inline std::vector REALTIME_CONTROL_MODES = { - ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE - }; + static const inline std::vector REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ, + ControlMode::MODE_SPEEDJ, + ControlMode::MODE_SPEEDL, + ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE }; // Control modes that doesn't require realtime communication static const inline std::vector NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE, diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 266b8c8b..e89d2643 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -25,6 +25,7 @@ MODE_SPEEDL = 4 MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 +MODE_TORQUE = 8 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -70,6 +71,7 @@ JOINT_IGNORE_SPEED = 20.0 global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +global cmd_tau = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_servo_q = get_joint_positions() global cmd_servo_q_last = cmd_servo_q global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -225,6 +227,22 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end +# Helpers for torque control +def set_torque(tau): + cmd_tau = tau + control_mode = MODE_TORQUE +end + +thread torqueThread(): + textmsg("ExternalControl: Starting torque thread") + while control_mode == MODE_TORQUE: + tau = cmd_tau + torque_command(tau, friction_comp=True) + end + textmsg("ExternalControl: torque thread ended") + stopj(STOPJ_ACCELERATION) +end + # Function return value (bool) determines whether the robot is moving after this spline segment or # not. def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=False): @@ -748,6 +766,8 @@ while control_mode > MODE_STOPPED: thread_move = run servoThread() elif control_mode == MODE_SPEEDJ: thread_move = run speedThread() + elif control_mode == MODE_TORQUE: + thread_move = run torqueThread() elif control_mode == MODE_FORWARD: kill thread_move stopj(STOPJ_ACCELERATION) @@ -766,6 +786,9 @@ while 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_TORQUE: + tau = [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_torque(tau) elif control_mode == MODE_FORWARD: if params_mult[2] == TRAJECTORY_MODE_RECEIVE: kill thread_trajectory From a97a51ea49cb09fd0af97a7e8ba3ebe0dda1e435 Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Tue, 20 May 2025 10:52:23 +0200 Subject: [PATCH 2/4] fix: use torque instead of tau Co-authored-by: Felix Exner --- resources/external_control.urscript | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index e89d2643..89c72e5f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -71,7 +71,7 @@ JOINT_IGNORE_SPEED = 20.0 global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -global cmd_tau = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +global cmd_torque = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_servo_q = get_joint_positions() global cmd_servo_q_last = cmd_servo_q global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -228,16 +228,16 @@ thread speedThread(): end # Helpers for torque control -def set_torque(tau): - cmd_tau = tau +def set_torque(target_torque): + cmd_torque = torque control_mode = MODE_TORQUE end thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: - tau = cmd_tau - torque_command(tau, friction_comp=True) + torque = cmd_torque + torque_command(torque, friction_comp=friction_compensation_enabled) end textmsg("ExternalControl: torque thread ended") stopj(STOPJ_ACCELERATION) @@ -787,8 +787,8 @@ while control_mode > MODE_STOPPED: 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_TORQUE: - tau = [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_torque(tau) + torque = [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_torque(torque) elif control_mode == MODE_FORWARD: if params_mult[2] == TRAJECTORY_MODE_RECEIVE: kill thread_trajectory From ec55620e7cc7f54d25b048db5cad3830fb97f159 Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Tue, 20 May 2025 10:53:39 +0200 Subject: [PATCH 3/4] fix: typo --- resources/external_control.urscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 89c72e5f..b5081ff7 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -229,7 +229,7 @@ end # Helpers for torque control def set_torque(target_torque): - cmd_torque = torque + cmd_torque = target_torque control_mode = MODE_TORQUE end From c0a936cd7b49976711083a59c77aed7e70788730 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 May 2025 11:01:27 +0200 Subject: [PATCH 4/4] Code formatting --- include/ur_client_library/comm/control_mode.h | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index c4889cc3..51156da6 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -51,9 +51,9 @@ enum class ControlMode : int32_t MODE_POSE = 5, ///< Set when cartesian pose control is active. MODE_FREEDRIVE = 6, ///< Set when freedrive mode is active. MODE_TOOL_IN_CONTACT = - 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() - MODE_TORQUE = 8, ///< Set when torque control is active. - END ///< This is not an actual control mode, but used internally to get the number of control modes + 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() + MODE_TORQUE = 8, ///< Set when torque control is active. + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -63,11 +63,10 @@ class ControlModeTypes { public: // Control modes that require realtime communication - static const inline std::vector REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ, - ControlMode::MODE_SPEEDJ, - ControlMode::MODE_SPEEDL, - ControlMode::MODE_POSE, - ControlMode::MODE_TORQUE }; + static const inline std::vector REALTIME_CONTROL_MODES = { + ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE + }; // Control modes that doesn't require realtime communication static const inline std::vector NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE,