diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 9d2da671..fd3765ad 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -78,6 +78,7 @@ global trajectory_points_left = 0 global spline_qdd = [0, 0, 0, 0, 0, 0] global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False +global trajectory_result = 0 # Global thread variables thread_move = 0 @@ -210,7 +211,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False): if time <= 0.0: error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion." textmsg(error_str) - socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") + trajectory_result = TRAJECTORY_RESULT_CANCELED popup(error_str, title="External Control error", blocking=False, error=True) elif targetWithinLimits(start_q, end_q, time): local start_qd = spline_qd @@ -223,7 +224,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False): local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point) else: - socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") + trajectory_result = TRAJECTORY_RESULT_CANCELED end end @@ -236,7 +237,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): if time <= 0.0: error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion." textmsg(error_str) - socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") + trajectory_result = TRAJECTORY_RESULT_CANCELED popup(error_str, title="External Control error", blocking=False, error=True) elif targetWithinLimits(start_q, end_q, time): local start_qd = spline_qd @@ -252,7 +253,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): local coefficients5 = (-12.0 * start_q + 12.0 * end_q - start_qdd * TIME2 + end_qdd * TIME2 - 6.0 * start_qd * time - 6.0 * end_qd * time) / (2.0 * pow(time, 5)) jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point) else: - socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") + trajectory_result = TRAJECTORY_RESULT_CANCELED end end @@ -412,63 +413,66 @@ thread trajectoryThread(): spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] 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()) - 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 tmptime = raw_point[INDEX_TIME] / MULT_time - local blend_radius = raw_point[INDEX_BLEND] / MULT_time - local is_last_point = False - if trajectory_points_left == 0: - blend_radius = 0.0 - is_last_point = True - end - # MoveJ point - if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: - 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 - 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) + trajectory_result = TRAJECTORY_RESULT_SUCCESS + while trajectory_result == TRAJECTORY_RESULT_SUCCESS: + 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()) + 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 tmptime = raw_point[INDEX_TIME] / MULT_time + local blend_radius = raw_point[INDEX_BLEND] / MULT_time + local is_last_point = False + if trajectory_points_left == 0: + blend_radius = 0.0 + is_last_point = True + end + # MoveJ point + if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: + 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] + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] - # Joint spline point - elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: + # 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) - # 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] - cubicSplineRun(q, qd, tmptime, is_last_point) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] - - # 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] - quinticSplineRun(q, qd, qdd, tmptime, is_last_point) - else: - textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) - clear_remaining_trajectory_points() - socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket") + spline_qd = [0, 0, 0, 0, 0, 0] + + # 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] + cubicSplineRun(q, qd, tmptime, is_last_point) + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + + # 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] + quinticSplineRun(q, qd, qdd, tmptime, is_last_point) + else: + textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) + clear_remaining_trajectory_points() + trajectory_result = TRAJECTORY_RESULT_FAILURE + end end end end end exit_critical stopj(STOPJ_ACCELERATION) - socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") - textmsg("Trajectory finished") + socket_send_int(trajectory_result, "trajectory_socket") + textmsg("Trajectory finished with result ", trajectory_result) end def clear_remaining_trajectory_points(): @@ -497,6 +501,7 @@ end thread servoThreadP(): textmsg("Starting pose servo thread") state = SERVO_IDLE + local q_last = get_target_joint_positions() while control_mode == MODE_POSE: enter_critical q = cmd_servo_q @@ -516,13 +521,13 @@ thread servoThreadP(): end q = extrapolate() - if targetWithinLimits(q, get_steptime()): + if targetWithinLimits(q_last, q, steptime): servoj(q, t=steptime, {{SERVO_J_REPLACE}}) end elif state == SERVO_RUNNING: extrapolate_count = 0 - if targetWithinLimits(q, get_steptime()): + if targetWithinLimits(q_last, q, steptime): servoj(q, t=steptime, {{SERVO_J_REPLACE}}) end else: