diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 62dd706f..51156da6 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -51,8 +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() - 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,7 +64,8 @@ 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_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE }; // Control modes that doesn't require realtime communication diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 266b8c8b..b5081ff7 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_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] @@ -225,6 +227,22 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end +# Helpers for torque control +def set_torque(target_torque): + cmd_torque = target_torque + control_mode = MODE_TORQUE +end + +thread torqueThread(): + textmsg("ExternalControl: Starting torque thread") + while control_mode == MODE_TORQUE: + torque = cmd_torque + torque_command(torque, friction_comp=friction_compensation_enabled) + 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: + 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