UGA-Innovation-Factory
/
UR10-Sentry
mirror of https://github.com/UGA-Innovation-Factory/UR10-Sentry.git
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Adjusted vertical speed, set up limits, lowered bbox up bias

This commit is contained in:
Tomás L 2024-07-24 14:25:23 -04:00
parent 761b1026eb
commit 0c3e6c49b7
2 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
BBoxProcessor.py
View File

@ -73,7 +73,7 @@ class BBoxProcessor:
closest_distance = 9999
for box in box_list:
distance = math.sqrt(
(box[0] + box[2] / 2 - 500) ** 2 + (box[1] + box[3] / 2 - 300) ** 2
(box[0] + box[2] / 2 - 500) ** 2 + (box[1] + box[3] / 2 - 400) ** 2
)
if distance < closest_distance:
closest = box

10
URSentry.py
View File

@ -108,7 +108,7 @@ class URSentry:
# If the joystick is None, we return to sentry mode after a certain amount of ticks
# We only do this if we have detected something at least once, as the camera gets buggy after fast movements
ticks_for_return_to_sentry = 50
ticks_for_return_to_sentry = 150
if joystick_pos is None:
if self.has_detected_once and not self.is_on_sentry_mode:
self.none_input_ticks += 1
@ -247,11 +247,13 @@ class URSentry:
joint_limit = self.looking_down_pose if direction > 0 else self.imposing_pose
#print("Joint limit: ", joint_limit, "Direction: ", direction)
for i in range(1, 4):
joint_difference = self.looking_down_pose[i] - self.imposing_pose[i]
joint_direction = math.copysign(1, self.looking_down_pose[i] - self.imposing_pose[i]) * direction
difference_from_limit = (joint_limit[i] - current_pose[i])
difference_from_limit = difference_from_limit if difference_from_limit * joint_direction > 0 else 0
self.robot_speed[i] = round(difference_from_limit * vertical_speed * abs(joystick_pos_y), 5)
print("Joint: ", i, " limit:", joint_limit[i], " current:" , current_pose[i] , " diff: ", difference_from_limit, " Speed: ", self.robot_speed[i])
# difference_from_limit = difference_from_limit if difference_from_limit * joint_direction > 0 else 0
# self.robot_speed[i] = round(difference_from_limit * vertical_speed * abs(joystick_pos_y), 5)
self.robot_speed[i] = round(joint_difference * vertical_speed * joystick_pos_y * 0.3, 5) if difference_from_limit * joint_direction > 0 else 0
print("Joint: ", i, " limit:", joint_limit[i], " current:" , current_pose[i] , " reachedlimit: ", not (difference_from_limit * joint_direction > 0), " Speed: ", self.robot_speed[i])
print("")
#self.set_neck_speed(neck_speed)