Skip to content

Enable force mode compatibility with various move types #230

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Dec 17, 2024
Merged

Enable force mode compatibility with various move types #230

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions resources/external_control.urscript
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,8 +68,8 @@ 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_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
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]
global extrapolate_count = 0
global extrapolate_max_count = 0
Expand Down Expand Up @@ -158,7 +158,7 @@ end
thread servoThread():
textmsg("ExternalControl: Starting servo thread")
state = SERVO_IDLE
local q_last = get_target_joint_positions()
local q_last = get_joint_positions()
while control_mode == MODE_SERVOJ:
enter_critical
q = cmd_servo_q
Expand Down Expand Up @@ -221,13 +221,13 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=Fals
local str = str_cat(end_q, str_cat(end_qd, time))
textmsg("Cubic spline arg: ", str)

local start_q = get_target_joint_positions()
local start_q = get_joint_positions()
# Zero time means infinite velocity to reach the target and is therefore impossible
if time <= 0.0:
if is_first_point and time == 0.0:
# If users specify the current joint position with time 0 that may be fine, in that case just
# ignore that point
local distance = norm(end_q - get_target_joint_positions())
local distance = norm(end_q - start_q)
if distance < 0.01:
local splineTimerTraveled = 0.0
# USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled)
Expand Down Expand Up @@ -263,13 +263,13 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first
local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time)))
textmsg("Quintic spline arg: ", str)

local start_q = get_target_joint_positions()
local start_q = get_joint_positions()
# Zero time means infinite velocity to reach the target and is therefore impossible
if time <= 0.0:
if is_first_point and time == 0.0:
# If users specify the current joint position with time 0 that may be fine, in that case just
# ignore that point
local distance = norm(end_q - get_target_joint_positions())
local distance = norm(end_q - start_q)
if distance < 0.01:
return False
end
Expand Down Expand Up @@ -553,7 +553,7 @@ end
thread servoThreadP():
textmsg("Starting pose servo thread")
state = SERVO_IDLE
local q_last = get_target_joint_positions()
local q_last = get_joint_positions()
while control_mode == MODE_POSE:
enter_critical
q = cmd_servo_q
Expand Down
Loading