Skip to content
This repository has been archived by the owner on Jul 27, 2024. It is now read-only.

Commit

Permalink
spin&move（#13）微调，机械臂（#22）优化
Browse files Browse the repository at this point in the history
  • Loading branch information
@HuaYuXiao
HuaYuXiao authored Dec 27, 2023
1 parent 314a992 commit 0293d61
Showing 1 changed file with 34 additions and 29 deletions.
63 changes: 34 additions & 29 deletions tb4_find_pickup_place/tb4_find_pickup_place/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -359,8 +359,8 @@ def __init__(self):
self.camera2base_Matrix = np.eye(4)
self.marker2base_Matrix = np.eye(4)

self.rot_matrix1 = np.array([[0, 0, 1, 0],
[0, 1, 0, 0],
self.rot_matrix1 = np.array([[0, 0, 1, 0.02],
[0, 1, 0, 0.032],
[-1, 0, 0, 0],
[0, 0, 0, 1]])

Expand Down Expand Up @@ -442,18 +442,17 @@ def ar_cb(self, msg):

def spin_cb(self):
print('spin_cb', self.marker2base_Matrix)
print(np.deg2rad(10.0))

# 调整云台俯仰角
self.pantil_deg_cmd.pitch = 20.0
self.pantil_deg_cmd.pitch = 10.0
self.pantil_deg_cmd.yaw = 0.0
self.pantil_deg_cmd.speed = 10
self.pantil_pub.publish(self.pantil_deg_cmd)

# 检查是否找到物块
if np.array_equal(self.marker2base_Matrix, np.eye(4)):
print('No marker detected!')
self.move_cmd.angular.z = 0.1
self.move_cmd.angular.z = 0.3
self.cmd_pub.publish(self.move_cmd)
else:
print('Marker detected!')
Expand All @@ -466,7 +465,7 @@ def spin_cb(self):
if abs(angle) < np.deg2rad(5.0):
self.move_cmd.angular.z = 0.0
self.cmd_pub.publish(self.move_cmd)
# self.place_timer.destroy()
self.spin_timer.destroy()
print(1/0)
return
else:
Expand All @@ -484,8 +483,8 @@ def __init__(self):
self.camera2base_Matrix = np.eye(4)
self.marker2base_Matrix = np.eye(4)

self.rot_matrix1 = np.array([[0, 0, 1, 0],
[0, 1, 0, 0],
self.rot_matrix1 = np.array([[0, 0, 1, 0.02],
[0, 1, 0, 0.032],
[-1, 0, 0, 0],
[0, 0, 0, 1]])

Expand Down Expand Up @@ -568,7 +567,7 @@ def move_cb(self):
print('move_cb', self.marker2base_Matrix)

# 调整云台俯仰角
self.pantil_deg_cmd.pitch = 15.0
self.pantil_deg_cmd.pitch = 10.0
self.pantil_deg_cmd.yaw = 0.0
self.pantil_deg_cmd.speed = 10
self.pantil_pub.publish(self.pantil_deg_cmd)
Expand All @@ -583,18 +582,20 @@ def move_cb(self):
relative = [self.marker2base_Matrix[0,3],
self.marker2base_Matrix[1,3]]
print(relative)

# 判断是否足够接近物块
if (relative[0] < 0.2) and (abs(relative[1]) < 0.2):
safe_diatance = 0.225
if (relative[0] < safe_diatance) and (abs(relative[1]) < safe_diatance):
self.move_cmd.linear.x = 0.0
self.move_cmd.linear.y = 0.0
self.cmd_pub.publish(self.move_cmd)
# self.place_timer.destroy()
self.move_timer.destroy()
print(1/0)
return
else:
# let's turn at 0 radians/s
self.move_cmd.linear.x = relative[0] - 0.2
self.move_cmd.linear.y = relative[1] - 0.2
self.move_cmd.linear.x = relative[0] - safe_diatance
self.move_cmd.linear.y = relative[1] - safe_diatance
self.cmd_pub.publish(self.move_cmd)


Expand All @@ -606,8 +607,8 @@ def __init__(self):
self.marker2base_Matrix = np.eye(4)

self.rot_matrix1 = np.array([[0, 0, 1, 0.02],
[0, 1, 0, 0.025],
[-1, 0, 0, -0.02],
[0, 1, 0, 0.032],
[-1, 0, 0, 0],
[0, 0, 0, 1]])

self.rot_matrix2 = np.array([[0, 0, 1, 0],
Expand Down Expand Up @@ -688,8 +689,8 @@ def ar_cb(self, msg):
position = np.array([position.x, position.y, position.z])
orientation = np.array([orientation.x, orientation.y, orientation.z, orientation.w])
self.camera2base_Matrix = self.quat2matrix(orientation, position)
self.aruco_update = False
# print("camera2base updated")
# self.aruco_update = False
print("aruco updated")
except :
print("pass")
pass
Expand Down Expand Up @@ -731,6 +732,7 @@ def pickup_cb(self):
self.pantil_deg_cmd.yaw = 0.0
self.pantil_deg_cmd.speed = 10
self.pantil_pub.publish(self.pantil_deg_cmd)
self.aruco_update = True
if self.go_sleep_pos() == True and self.release():
print('Go slep pos done!')
self.aruco_update = True
Expand All @@ -748,15 +750,17 @@ def pickup_cb(self):
# time.sleep(3.0)

case "NEXT0":
if self.set_group_pos([-1.0, 0.0, 0.8, -1.4]) == True:
if self.set_group_pos([-1.0, 0.0, 0.8, -1.3]) == True:
print('NEXT0 control done!')
# self.machine_state = "NEXT1"
self.machine_state = "NEXT1"
#self.machine_state = "TEST1"
print('aruco: ', self.marker2base_Matrix)
self.aruco_update = False

case "NEXT1":
angle = np.arctan2(self.marker2base_Matrix[1, 3], self.marker2base_Matrix[0, 3])
if self.set_group_pos([angle, -0.3, 0.8, -1.4]) == True:
if self.set_group_pos([angle, -0.3, 0.8, -1.3]) == True:
print('NEXT1 control done!')
self.machine_state = "NEXT2"

Expand Down Expand Up @@ -908,28 +912,32 @@ def matrix_control(self, T_sd, custom_guess: list[float]=None, execute: bool=Tru
maxiterations = 400
Tsb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, thetalist)
err = Tsb[0:3, 3] - T_sd[0:3, 3]
print(err)
len0 = 0.07
len = (err[0] ** 2 + err[1] ** 2) ** 0.5 * np.sign(err[0]) - len0
step = 0.005
while sum(abs(err)) > 0.01 and i < maxiterations:
if abs(err[0]) > 0.005:
if err[0] < 0:
while (err[2] > 0.005 or abs(len) > 0.005) and i < maxiterations:
print('err: ', err)
print('len: ', len)
if abs(len) > 0.002:
if len < 0:
if thetalist[2] < 0:
if thetalist[1] < 0.9:
thetalist = thetalist + np.array([0.0, step, -step, 0.0])
print('strech')
else:
thetalist = thetalist + np.array([0.0, 0.0, -step, step])
else:
thetalist = thetalist - np.array([0.0, 0.0, -step, step])
if abs(err[2]) > 0.005:
if abs(err[2]) > 0.002:
if err[2] < 0:
thetalist = thetalist + np.array([0.0, 0.0, -step, -step])
else:
thetalist = thetalist - np.array([0.0, 0.0, -step, -step])
Tsb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, thetalist)
err = Tsb[0:3, 3] - T_sd[0:3, 3]
len = (err[0] ** 2 + err[1] ** 2) ** 0.5 * np.sign(err[0]) - len0
i = i + 1
# print(i)
print('tlist', thetalist)
print(i)
return thetalist, True

# self.core.get_logger().warn('No valid pose could be found. Will not execute')
Expand Down Expand Up @@ -1057,6 +1065,3 @@ def _wrap_theta_list(self, theta_list: list[np.ndarray]) -> list[np.ndarray]:
elif round(theta_list[x], 3) > round(self.joint_upper_limits[x], 3):
theta_list[x] -= REV
return theta_list



0 comments on commit 0293d61

Please sign in to comment.