From 62ac49b0750cbeae7246e723ffb356d3685a6bdc Mon Sep 17 00:00:00 2001 From: preet-dhir1-ocado Date: Fri, 27 Sep 2024 10:58:49 -0700 Subject: [PATCH] add limited extrapolation --- resources/external_control.urscript | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8624c4899..859491fbf 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -15,7 +15,8 @@ SERVO_UNINITIALIZED = -1 SERVO_IDLE = 0 SERVO_RUNNING = 1 -MODE_STOPPED = -2 +MODE_STOPPED = -3 +MODE_PAUSED = -2 MODE_UNINITIALIZED = -1 MODE_IDLE = 0 MODE_SERVOJ = 1 @@ -73,6 +74,7 @@ global cmd_servo_q_last = get_actual_joint_positions() global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global extrapolate_count = 0 global extrapolate_max_count = 0 +global max_consecutive_extrapolate_count = 50 global control_mode = MODE_UNINITIALIZED global trajectory_points_left = 0 global spline_qdd = [0, 0, 0, 0, 0, 0] @@ -570,6 +572,12 @@ thread servoThreadP(): if extrapolate_count > extrapolate_max_count: extrapolate_max_count = extrapolate_count end + + if extrapolate_count > max_consecutive_extrapolate_count: + textmsg("Exceeded max consecutive extrapolation count, stopping the arm") + stopj(STOPJ_ACCELERATION) + control_mode = MODE_PAUSED + end q = extrapolate() if targetWithinLimits(q_last, q, steptime):