openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加末端执行器角度规划

Browse Source
This commit is contained in:
shenchenyang 2026-02-06 17:48:58 +08:00
parent a2fc8c1faf
commit e3b7360b9a
2 changed files with 121 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -6,6 +6,7 @@ import numpy as np
from sensor_msgs.msg import Image from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import PoseStamped
from cv_bridge import CvBridge from cv_bridge import CvBridge
from sensor_msgs.msg import CameraInfo from sensor_msgs.msg import CameraInfo
import mediapipe as mp import mediapipe as mp
@ -71,7 +72,7 @@ class StereoHandDepthNode(Node):
) )
self.pub = self.create_publisher( self.pub = self.create_publisher(
PointStamped, PoseStamped,
'/hand_3d', '/hand_3d',
10 10
) )
@ -134,9 +135,9 @@ class StereoHandDepthNode(Node):
left = frame[:, :mid] left = frame[:, :mid]
right = frame[:, mid:] right = frame[:, mid:]
left_rect = cv2.remap(left, self.map1_l, self.map2_l, cv2.INTER_LINEAR) left_rect = cv2.remap(left, self.map1_l, self.map2_l, cv2.INTER_LINEAR)
right_rect = cv2.remap(right, self.map1_r, self.map2_r, cv2.INTER_LINEAR) # right_rect = cv2.remap(right, self.map1_r, self.map2_r, cv2.INTER_LINEAR)
# results = self.mp_hands.process(left_rect) # results = self.mp_hands.process(left_rect)
p = PointStamped() # p = PointStamped()
# if results.multi_hand_landmarks: # if results.multi_hand_landmarks:
# wrist = results.multi_hand_landmarks[0].landmark[0] # wrist = results.multi_hand_landmarks[0].landmark[0]
@ -148,29 +149,29 @@ class StereoHandDepthNode(Node):
# p.point.z = 0.0 # 暂时不用 # p.point.z = 0.0 # 暂时不用
# 转灰度 # 转灰度
left_gray = cv2.cvtColor(left_rect, cv2.COLOR_BGR2GRAY) # left_gray = cv2.cvtColor(left_rect, cv2.COLOR_BGR2GRAY)
right_gray = cv2.cvtColor(right_rect, cv2.COLOR_BGR2GRAY) # right_gray = cv2.cvtColor(right_rect, cv2.COLOR_BGR2GRAY)
# 计算视差 # 计算视差
disparity = self.stereo.compute(left_gray, right_gray).astype(np.float32) / 16.0 # disparity = self.stereo.compute(left_gray, right_gray).astype(np.float32) / 16.0
corners, ids, _ = cv2.aruco.detectMarkers( corners, ids, _ = cv2.aruco.detectMarkers(
left_rect, self.aruco_dict, parameters=self.aruco_params left_rect, self.aruco_dict, parameters=self.aruco_params
) )
msg_out = PoseStamped()
if ids is not None: if ids is not None:
cv2.aruco.drawDetectedMarkers(left_gray, corners, ids) cv2.aruco.drawDetectedMarkers(left_rect, corners, ids)
c = corners[0][0] # c = corners[0][0]
center = c.mean(axis=0) # center = c.mean(axis=0)
u, v = int(center[0]), int(center[1]) # u, v = int(center[0]), int(center[1])
d = disparity[v, u] # d = disparity[v, u]
cv2.circle(left_gray, (u, v), 5, (0, 0, 255), -1) # cv2.circle(left_gray, (u, v), 5, (0, 0, 255), -1)
cv2.putText( # cv2.putText(
left_gray, f"disp={d:.2f}", (u+5, v-5), # left_gray, f"disp={d:.2f}", (u+5, v-5),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1 # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1
) # )
for e in corners: for e in corners:
ret, rvec, tvec = cv2.solvePnP( ret, rvec, tvec = cv2.solvePnP(
objectPoints=np.array([ objectPoints=np.array([
@ -184,57 +185,73 @@ class StereoHandDepthNode(Node):
distCoeffs=self.D1 distCoeffs=self.D1
) )
if(ret == True): if(ret == True):
cv2.drawFrameAxes(left_gray, self.K1, self.D1, rvec, tvec, 0.1, 2) cv2.drawFrameAxes(left_rect, self.K1, self.D1, rvec, tvec, 0.1, 2)
R_ca, _ = cv2.Rodrigues(rvec) R_cv, _ = cv2.Rodrigues(rvec)
rot = R.from_matrix(R_ca) R_cv_to_ros = np.array([
[ 0, 0, -1],
[ 1, 0, 0],
[ 0, -1, 0]
])
R_ros = R_cv_to_ros @ R_cv
t_ros = R_cv_to_ros @ tvec.reshape(3)
# t_ros = tvec.reshape(3)
rot = R.from_matrix(R_ros)
qx, qy, qz, qw = rot.as_quat() qx, qy, qz, qw = rot.as_quat()
roll, pitch, yaw = rot.as_euler('xyz') roll, pitch, yaw = rot.as_euler('xyz')
self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})") self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})")
self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})") self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})")
msg_out.pose.orientation.x = float(qx)
msg_out.pose.orientation.y = float(qy)
msg_out.pose.orientation.z = float(qz)
msg_out.pose.orientation.w = float(qw)
msg_out.pose.position.x = float(t_ros[0]) + 0.7
msg_out.pose.position.y = float(t_ros[1])
msg_out.pose.position.z = float(t_ros[2])
# u = int(u * (w / 2)) # 注意这里是左图宽度 # u = int(u * (w / 2)) # 注意这里是左图宽度
# v = int(v * h) # v = int(v * h)
p.header = msg.header # p.header = msg.header
p.point.x = float(u) # p.point.x = float(u)
p.point.y = float(v) # p.point.y = float(v)
p.point.z = 0.0 # 暂时不用 # p.point.z = 0.0 # 暂时不用
cv2.imshow("left", left_gray) cv2.imshow("left", left_rect)
# ----------- 这里先用“假手点”（画面中心）----------- # ----------- 这里先用“假手点”（画面中心）-----------
x = int(p.point.x) # x = int(p.point.x)
y = int(p.point.y) # y = int(p.point.y)
# cv2.circle(left_gray, (x, y), 5, (255, 0, 0), -1) # cv2.circle(left_gray, (x, y), 5, (255, 0, 0), -1)
# print("left_gray.shape:", left_gray.shape) # print("left_gray.shape:", left_gray.shape)
# cv2.imshow('left', left_gray) # cv2.imshow('left', left_gray)
# print(f'Wrist pixel: ({x}, {y})') # print(f'Wrist pixel: ({x}, {y})')
# ROI 取中值，稳很多 # ROI 取中值，稳很多
roi = disparity[y-3:y+3, x-3:x+3] # roi = disparity[y-3:y+3, x-3:x+3]
d = np.nanmedian(roi) # d = np.nanmedian(roi)
# print(f'Disparity at wrist: {roi}') # print(f'Disparity at wrist: {roi}')
if d <= 0: # if d <= 0:
return # return
# 深度 # 深度
Z = (self.fx * self.baseline) / d # Z = (self.fx * self.baseline) / d
X = (x - self.cx) * Z / self.fx # X = (x - self.cx) * Z / self.fx
Y = (y - self.cy) * Z / self.fy # Y = (y - self.cy) * Z / self.fy
# 发布 # 发布
msg_out = PointStamped()
msg_out.header.stamp = self.get_clock().now().to_msg() msg_out.header.stamp = self.get_clock().now().to_msg()
msg_out.header.frame_id = 'camera_link' msg_out.header.frame_id = 'camera_link'
msg_out.point.x = float(Z) # msg_out.pose.position.x = float(Z)
msg_out.point.y = float(X) # msg_out.pose.position.y = float(X)
msg_out.point.z = -float(Y) # msg_out.pose.position.z = -float(Y)
self.get_logger().info(f"X:{X:.3f}, Y:{Y:.3f}, Z:{Z:.3f}") # self.get_logger().info(f"X:{X:.3f}, Y:{Y:.3f}, Z:{Z:.3f}")
self.pub.publish(msg_out) self.pub.publish(msg_out)
# ----------- 可视化（调试用）----------- # ----------- 可视化（调试用）-----------
disp_vis = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX) # disp_vis = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX)
disp_vis = disp_vis.astype(np.uint8) # disp_vis = disp_vis.astype(np.uint8)
cv2.circle(disp_vis, (x, y), 5, 255, -1) # cv2.circle(disp_vis, (x, y), 5, 255, -1)
disp_vis = disp_vis[:, self.numDisparities:] # disp_vis = disp_vis[:, self.numDisparities:]
cv2.imshow('disparity', disp_vis) # cv2.imshow('disparity', disp_vis)
cv2.waitKey(1) cv2.waitKey(1)

105
src/openarm_moveit_control/openarm_moveit_control/follow_pose_goal.py
View File

@ -39,11 +39,11 @@ class HandFollowMoveIt(Node):
self.p_prev = None self.p_prev = None
self.plan_period = 1.0 # 秒（规划频率） self.plan_period = 0.1 # 秒（规划频率）
self.min_delta = 0.005 # m，手移动超过才重新规划 self.min_delta = 0.005 # m，手移动超过才重新规划
self.z_min = 0.15 # 防止撞桌 self.z_min = 0.15 # 防止撞桌
self.z_max = 0.60 self.z_max = 0.60
self.latest_hand = None self.latest_hand_pos = None
self.last_target = None self.last_target = None
self.base_frame = 'openarm_body_link0' self.base_frame = 'openarm_body_link0'
# Create node for this example # Create node for this example
@ -51,7 +51,7 @@ class HandFollowMoveIt(Node):
# Declare parameters for position and orientation # Declare parameters for position and orientation
self.declare_parameter("position", [0.5, 0.0, 0.25]) self.declare_parameter("position", [0.5, 0.0, 0.25])
self.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0]) self.declare_parameter("quat_xyzw", [0.723, 0.0, 0.69, 0.0])
self.declare_parameter("synchronous", True) self.declare_parameter("synchronous", True)
# If non-positive, don't cancel. Only used if synchronous is False # If non-positive, don't cancel. Only used if synchronous is False
self.declare_parameter("cancel_after_secs", 0.0) self.declare_parameter("cancel_after_secs", 0.0)
@ -83,7 +83,7 @@ class HandFollowMoveIt(Node):
# Spin the node in background thread(s) and wait a bit for initialization # Spin the node in background thread(s) and wait a bit for initialization
# self.executor = rclpy.executors.MultiThreadedExecutor(2) # self.executor = rclpy.executors.MultiThreadedExecutor(2)
# self.executor.add_node(self) # self.executor.add_node(self)
# self.executor_thread = Thread(target=self.executor.spin, daemon=True, args=()) # self.executor_thread = Thread(target_pos=self.executor.spin, daemon=True, args=())
# self.executor_thread.start() # self.executor_thread.start()
# self.create_rate(1.0).sleep() # self.create_rate(1.0).sleep()
@ -125,12 +125,12 @@ class HandFollowMoveIt(Node):
# ====== TF ====== # ====== TF ======
self.tf_buffer = tf2_ros.Buffer() self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.send_goal([0.0, 0.15, 0.3]) self.zero_hand()
time.sleep(3) time.sleep(3)
# ====== 订阅 ====== # ====== 订阅 ======
self.sub = self.create_subscription( self.sub = self.create_subscription(
PointStamped, PoseStamped,
'/hand_3d', '/hand_3d',
self.hand_cb, self.hand_cb,
10 10
@ -148,76 +148,89 @@ class HandFollowMoveIt(Node):
) )
except Exception: except Exception:
return return
print(f"手坐标（{self.base_frame}):{hand.point.x:.3f}, {hand.point.y:.3f}, {hand.point.z:.3f}") self.get_logger().info(f"手坐标（{self.base_frame}):{hand.pose.position.x:.3f}, {hand.pose.position.y:.3f}, {hand.pose.position.z:.3f}")
self.latest_hand = np.array([ self.get_logger().info(f"手四元数（{self.base_frame}):{hand.pose.orientation.x:.3f}, {hand.pose.orientation.y:.3f}, {hand.pose.orientation.z:.3f}, {hand.pose.orientation.w:.3f}")
hand.point.x, self.latest_hand_pos = np.array([
hand.point.y, hand.pose.position.x,
hand.point.z hand.pose.position.y,
hand.pose.position.z
]) ])
self.quat_xyzw = np.array([
hand.pose.orientation.x,
hand.pose.orientation.y,
hand.pose.orientation.z,
hand.pose.orientation.w
])
def zero_hand(self):
self.send_goal([0.0, 0.15, 0.3], [1.0, 0.0, 0.0, 0.0])
# def update_hand(self, p):
# """ p: np.array([x,y,z]) in base_link """
def update_hand(self, p): # # 1️⃣ 限幅（按你机械臂改）
""" p: np.array([x,y,z]) in base_link """ # p[0] = np.clip(p[0], -0.1, 0.1)
# p[1] = np.clip(p[1], -0.20, 0.20)
# p[2] = np.clip(p[2], 0.0, 0.30)
# 1️⃣ 限幅（按你机械臂改） # # 2️⃣ 低通滤波
p[0] = np.clip(p[0], -0.1, 0.1) # if self.p_prev is None:
p[1] = np.clip(p[1], -0.20, 0.20) # self.p_prev = p
p[2] = np.clip(p[2], 0.0, 0.30) # return
# 2️⃣ 低通滤波 # alpha = 0.2
if self.p_prev is None: # p = alpha * p + (1 - alpha) * self.p_prev
self.p_prev = p
return
alpha = 0.2 # # 3️⃣ 死区
p = alpha * p + (1 - alpha) * self.p_prev # if np.linalg.norm(p - self.p_prev) < 0.015:
# return
# 3️⃣ 死区 # # 4️⃣ 控制频率
if np.linalg.norm(p - self.p_prev) < 0.015: # if time.time() - self.last_sent < self.send_period:
return # return
# 4️⃣ 控制频率 # self.send_goal(p)
if time.time() - self.last_sent < self.send_period: # self.p_prev = p
return # self.last_sent = time.time()
self.send_goal(p) def send_goal(self, p, quat):
self.p_prev = p
self.last_sent = time.time()
def send_goal(self, p):
# Move to pose # Move to pose
self.get_logger().info( self.get_logger().info(
f"Moving to {{position: {list(p)}, quat_xyzw: {list(self.quat_xyzw)}}}" f"Moving to {{position: {list(p)}, quat_xyzw: {list(self.quat_xyzw)}}}"
) )
self.moveit2.move_to_pose( self.moveit2.move_to_pose(
position=p, position=p,
quat_xyzw=self.quat_xyzw, quat_xyzw=quat,
cartesian=self.cartesian, cartesian=self.cartesian,
cartesian_max_step=self.cartesian_max_step, cartesian_max_step=self.cartesian_max_step,
cartesian_fraction_threshold=self.cartesian_fraction_threshold, cartesian_fraction_threshold=self.cartesian_fraction_threshold,
) )
def plan_timer(self): def plan_timer(self):
if self.latest_hand is None:
if self.latest_hand_pos is None:
self.zero_hand()
return return
# 限制 workspace # 限制 workspace
target = self.latest_hand.copy() target_pos = self.latest_hand_pos.copy()
if np.linalg.norm(target_pos) == 0:
self.zero_hand()
return
target[0] = 0.6 - np.clip(target[0], 0, 0.8) target_pos[0] = np.clip(target_pos[0], 0, 0.8)
target[1] = np.clip(target[1], -0.2, 0.2) + 0.15 target_pos[1] = np.clip(target_pos[1], -0.2, 0.2) + 0.15
target[2] = np.clip(target[2], -0.3, 0.3) + 0.3 target_pos[2] = np.clip(target_pos[2], -0.3, 0.3) + 0.3
# target[0] = 0.12 # target_pos[0] = 0.12
# target[2] = 0.32 # target_pos[2] = 0.32
# 判断是否值得重新规划 # 判断是否值得重新规划
if self.last_target is not None: if self.last_target is not None:
if np.linalg.norm(target - self.last_target) < self.min_delta: if np.linalg.norm(target_pos - self.last_target) < self.min_delta:
return return
self.last_target = target.copy() self.last_target = target_pos.copy()
self.send_goal(target) self.send_goal(target_pos, self.quat_xyzw)
print(f"发送新目标：{target[0]:.3f}, {target[1]:.3f}, {target[2]:.3f}") self.get_logger().info(f"发送新目标：{target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}")
def main(): def main():
rclpy.init() rclpy.init()