Skip to content
New issue

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

#

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

Already on GitHub? # to your account

Axis Movement Fixes from RoboCup 2024 + FH Fixes #709

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Axis Movement Fixes from RoboCup 2024 + FH Fixes #709

Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
ddf98a3
manipulate_wp: change tolerances to config
MatteoTschesche Jul 18, 2024
f33cf95
manipulate_wp: fix typo
MatteoTschesche Jul 18, 2024
17ad609
manipulate_wp: add higher fine-tune tolerances and debug prints
MatteoTschesche Jul 20, 2024
0cf4b95
manipulate_wp: fix debug log
MatteoTschesche Jul 21, 2024
acdd8cb
manipulate_wp: fix debug log fr fr
MatteoTschesche Jul 21, 2024
3e89fc6
manipulate_wp: only check z axis when aligning
MatteoTschesche Jul 21, 2024
6e18128
manipulate_wp: allow for more z tolerance
MatteoTschesche Jul 21, 2024
dc21f30
lua: wait before gripping to remove camera img lag
TarikViehmann Aug 14, 2024
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
77 changes: 56 additions & 21 deletions src/lua/skills/robotino/manipulate_wp.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ Parameters:

local LASER_BASE_OFFSET = 0.5 -- distance between robotino middle point and laser-line
-- used for DRIVE_TO_LASER_LINE
local GRIPPER_TOLERANCE = {x = 0.005, y = 0.001, z = 0.001} -- accuracy
local GRIPPER_TOLERANCE = {x = 0.0075, y = 0.0015, z = 0.002} -- accuracy
local MISSING_MAX = 5 -- limit for missing object detections in a row while fine-tuning gripper
local MIN_VIS_HIST_LINE = 5 -- minimum visibility history for laser-line before considering it
local MIN_ACTUAL_DIST = 1.8 -- minimum distance b/w bot and laser center
Expand Down Expand Up @@ -181,13 +181,34 @@ function gripper_aligned()
z = object_tracking_if:gripper_frame(2),
ori = fawkes.tf.create_quaternion_from_yaw(0)
}, "base_link", "end_effector_home")

return within_tolerance(arduino:x_position(), 0, 0.0001) and
within_tolerance(arduino:y_position() - y_max / 2, y_max / 2,
0.0001) and
within_tolerance(
math.max(0.01, math.min(gripper_target.z, z_max)),
arduino:z_position(), GRIPPER_TOLERANCE.z)
print_debug("fine-tune gripper pose aligned:")
print_debug(
"within_tolerance(arduino:x_position(), 0, GRIPPER_TOLERANCE.x): " ..
tostring(
within_tolerance(arduino:x_position(), 0, GRIPPER_TOLERANCE.x)))
print_debug(
"within_tolerance(arduino:y_position() - y_max / 2, y_max / 2, GRIPPER_TOLERANCE.y, y): " ..
tostring(
within_tolerance(arduino:y_position() - y_max / 2, y_max / 2,
GRIPPER_TOLERANCE.y, y)))
print_debug(
"math.max(0.01, math.min(gripper_target.z, z_max)), arduino:z_position(), GRIPPER_TOLERANCE.z: " ..
tostring(
within_tolerance(
math.max(0.01, math.min(gripper_target.z, z_max)),
arduino:z_position(), GRIPPER_TOLERANCE.z * 1.1)))
local result = within_tolerance(math.max(0.01,
math.min(gripper_target.z, z_max)),
arduino:z_position(),
GRIPPER_TOLERANCE.z * 1.1)
if result == true then
if fsm.vars.img_wait > 10 then return result end
fsm.vars.img_wait = fsm.vars.img_wait + 1
return false
else
fsm.vars.img_wait = 0
return result
end
end

function set_gripper(x, y, z)
Expand Down Expand Up @@ -218,20 +239,15 @@ function set_gripper(x, y, z)
end

if (not arduino:is_final() and
(math.abs(fsm.vars.gripper_target_pos_y - y_clipped) >
GRIPPER_TOLERANCE.y * 1 or
math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
(math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
GRIPPER_TOLERANCE.z * 1.5)) or
(arduino:is_final() and fsm.vars.target == "SLIDE" and
(math.abs(fsm.vars.gripper_target_pos_y - y_clipped) >
GRIPPER_TOLERANCE.y * 1.3 or
math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
(math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
GRIPPER_TOLERANCE.z * 1.3)) or (arduino:is_final() and
(math.abs(fsm.vars.gripper_target_pos_y - y_clipped) >
GRIPPER_TOLERANCE.y or
math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
(math.abs(fsm.vars.gripper_target_pos_z - z_clipped) >
GRIPPER_TOLERANCE.z)) then
fsm.vars.gripper_wait = 0
fsm.vars.img_wait = 0
fsm.vars.gripper_target_pos_x = x_clipped
fsm.vars.gripper_target_pos_y = y_clipped
fsm.vars.gripper_target_pos_z = z_clipped
Expand Down Expand Up @@ -331,10 +347,29 @@ function within_tolerance(value, target, margin)
end

function ready_for_gripper_movement()
return within_tolerance(arduino:x_position(), default_x, 0.0001) and
within_tolerance(arduino:y_position() - y_max / 2, default_y,
0.0001) and
within_tolerance(arduino:z_position(), default_z, 0.0001)

print_debug("default gripper ready for movement:")
print_debug(
"within_tolerance(arduino:x_position(), default_x, GRIPPER_TOLERANCE.x): " ..
tostring(
within_tolerance(arduino:x_position(), default_x,
GRIPPER_TOLERANCE.x)))
print_debug(
"within_tolerance(arduino:y_position() - y_max / 2, default_y, GRIPPER_TOLERANCE.y): " ..
tostring(
within_tolerance(arduino:y_position() - y_max / 2, default_y,
GRIPPER_TOLERANCE.y)))
print_debug(
"within_tolerance(arduino:z_position(), default_z, GRIPPER_TOLERANCE.z) :" ..
tostring(
within_tolerance(arduino:z_position(), default_z,
GRIPPER_TOLERANCE.z)))
return
within_tolerance(arduino:x_position(), default_x, GRIPPER_TOLERANCE.x) and
within_tolerance(arduino:y_position() - y_max / 2, default_y,
GRIPPER_TOLERANCE.y) and
within_tolerance(arduino:z_position(), default_z,
GRIPPER_TOLERANCE.z)
end

function dry_expected_object_found()
Expand Down