Welcome to DOFBOT-Ultra repository
12. Arm posture control robotic arm
Arm Posture Control Robotic Arm
If using a depth camera (DABAI_DCW2), make sure to execute
ros2 run dofbot_ultra_driver calibrate_6pointfor a six-point calibration before running this example. Otherwise, the arm cannot accurately locate the target.
Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!
Detect human body posture (arm angles) via the camera, recognize predefined body actions, and control the arm to perform corresponding actions. The image is automatically mirrored for ease of operation.
1、Posture and Action Mapping Table
| Body Posture | Action Description | Arm Response |
|---|---|---|
| AKIMBO | Hands on hips (upper arms down, forearms bent inward) | Arm performs "nodding" action, 3 round trips, then resets |
Tip:
Hold the same posture in front of the camera for about 1 second (8 consecutive successful frames) to trigger the action
The lower left corner of the screen shows the currently detected four joint angles (LA/RA/LE/RE) and the posture name
The number turns green at 8/8, indicating imminent trigger
Stand about 1-2 meters in front of the camera, ensuring the upper body is fully visible in the frame
2、Startup
Open the desktop terminal and enter the following command to start the program
xxxxxxxxxxros2 run dofbot_mediapipe 12_PoseArm.py
Press the q key in the image window or Ctrl+C in the terminal to exit the program.
2.1、Source Code
Raspberry Pi code path:
xxxxxxxxxx~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/12_PoseArm.py
RDK X5 code path:
xxxxxxxxxx~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/12_PoseArm.py
x#!/usr/bin/env python3# encoding: utf-8import threadingimport cv2 as cvimport numpy as npimport mediapipe as mpfrom dofbot_utils.robot_controller import Robot_Controllerfrom dofbot_utils.camera import create_camerafrom dofbot_utils.fps import FPSfrom time import sleep
POSE_NAMES = ['RIGHT_UP', 'LEFT_UP', 'ALL_HANDS_UP', 'AKIMBO', 'TRIANGLE']POSE_THRESHOLD = 8MIN_VISIBILITY = 0.3KEY_LANDMARKS = [11, 12, 13, 14, 15, 16]
class PoseArmController: def __init__(self): self.robot = Robot_Controller() self.moving = False self.fps = FPS() self._init_holistic() self.robot.arm_move_6(self.robot.P_POSE_INIT, 1000) self.pose_counts = {name: 0 for name in POSE_NAMES} self.debug_angles = "" self.debug_pose = "" sleep(1.5)
def _init_holistic(self): self.mpHolistic = mp.solutions.holistic self.mpHands = mp.solutions.hands self.mpPose = mp.solutions.pose self.mpDraw = mp.solutions.drawing_utils self.holistic = self.mpHolistic.Holistic( static_image_mode=False, smooth_landmarks=True, min_detection_confidence=0.5, min_tracking_confidence=0.5) self.lmDrawSpec = mp.solutions.drawing_utils.DrawingSpec( color=(0, 0, 255), thickness=-1, circle_radius=3) self.drawSpec = mp.solutions.drawing_utils.DrawingSpec( color=(0, 255, 0), thickness=2, circle_radius=2)
def _find_holistic(self, frame, draw=True): pose_pts = [] vis = [0.0] * 33 h, w, c = frame.shape img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB) results = self.holistic.process(img_RGB) if results.pose_landmarks: if draw: self.mpDraw.draw_landmarks( frame, results.pose_landmarks, self.mpPose.POSE_CONNECTIONS, self.lmDrawSpec, self.drawSpec) for id, lm in enumerate(results.pose_landmarks.landmark): pose_pts.append([id, lm.x * w, lm.y * h, lm.z]) vis[id] = lm.visibility if results.left_hand_landmarks and draw: self.mpDraw.draw_landmarks( frame, results.left_hand_landmarks, self.mpHands.HAND_CONNECTIONS, self.lmDrawSpec, self.drawSpec) if results.right_hand_landmarks and draw: self.mpDraw.draw_landmarks( frame, results.right_hand_landmarks, self.mpHands.HAND_CONNECTIONS, self.lmDrawSpec, self.drawSpec) return frame, pose_pts, vis
def _get_angle(self, v1, v2): dot = np.dot(v1, v2) norm = np.sqrt(np.sum(v1 * v1)) * np.sqrt(np.sum(v2 * v2)) if norm == 0: return 0 angle = np.arccos(np.clip(dot / norm, -1, 1)) / np.pi * 180 cross = v2[0] * v1[1] - v2[1] * v1[0] if cross < 0: angle = -angle return angle
def _get_pose(self, keypoints, vis): for kid in KEY_LANDMARKS: if vis[kid] < MIN_VISIBILITY: self.debug_angles = "low visibility" return ""
kp = np.array(keypoints) v1 = kp[12] - kp[11] v2 = kp[13] - kp[11] la = self._get_angle(v1, v2) v1 = kp[11] - kp[12] v2 = kp[14] - kp[12] ra = self._get_angle(v1, v2) v1 = kp[11] - kp[13] v2 = kp[15] - kp[13] le = self._get_angle(v1, v2) v1 = kp[12] - kp[14] v2 = kp[16] - kp[14] re = self._get_angle(v1, v2)
self.debug_angles = ( f"LA:{la:.0f} RA:{ra:.0f} LE:{le:.0f} RE:{re:.0f}")
if (130 < la < 150 and -150 < ra < -130 and 90 < le < 120 and -120 < re < 90): return "AKIMBO" if (-150 < la < -120 and 120 < ra < 150 and -85 < le < -55 and 55 < re < 85): return "TRIANGLE"
if 10 < la < 130 and ra < -100: return "LEFT_UP" if -60 < la < 25 and 50 < ra < 170: return "RIGHT_UP" if 10 < la < 130 and 50 < ra < 170: return "ALL_HANDS_UP"
return ""
def process(self, frame): frame = cv.flip(frame, 1) frame, pose_pts, vis = self._find_holistic(frame) h_frame = frame.shape[0]
pose_name = "" if len(pose_pts) >= 17: keypoints = [(0, 0)] * 33 for pt in pose_pts: keypoints[pt[0]] = (pt[1], pt[2]) pose_name = self._get_pose(keypoints, vis)
for k in self.pose_counts: if k == pose_name: self.pose_counts[k] += 1 else: self.pose_counts[k] = 0
cv.putText(frame, self.debug_angles, (10, h_frame - 50), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 1) if pose_name: cnt = self.pose_counts[pose_name] color = (0, 255, 0) if cnt >= POSE_THRESHOLD else (0, 200, 255) cv.putText(frame, f"{pose_name} ({cnt}/{POSE_THRESHOLD})", (10, h_frame - 20), cv.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
if (not self.moving and pose_name and self.pose_counts[pose_name] >= POSE_THRESHOLD): self.moving = True self.pose_counts[pose_name] = 0 threading.Thread( target=self._arm_ctrl, args=(pose_name,), daemon=True).start()
self.fps.update_fps() self.fps.show_fps(frame) return frame
def _arm_ctrl(self, pose_name): if pose_name == "RIGHT_UP": self.robot.arm_move_6(self.robot.P_RIGHT_UP, 1000) sleep(3) elif pose_name == "LEFT_UP": self.robot.arm_move_6(self.robot.P_LEFT_UP, 1000) sleep(3) elif pose_name == "ALL_HANDS_UP": self.robot.arm_move_6(self.robot.P_HANDS_UP, 1000) sleep(3) elif pose_name == "TRIANGLE": self.robot.arm_move_6([90, 131, 52, 0, 90, 180], 1500) sleep(1.5) self.robot.arm_move_6([45, 180, 0, 0, 90, 180], 1500) sleep(2) self.robot.arm_move_6([135, 180, 0, 0, 90, 180], 1500) sleep(2) self.robot.arm_move_6([90, 131, 52, 0, 90, 180], 1500) sleep(2) elif pose_name == "AKIMBO": for _ in range(3): self.robot.arm_move_6(self.robot.P_ACTION_3, 1200) sleep(1.2) self.robot.arm_move_6(self.robot.P_LOOK_AT, 1000) sleep(1) self.robot.arm_move_6(self.robot.P_POSE_INIT, 1000) sleep(1.5) self.moving = False
def main(): controller = PoseArmController()
cap = create_camera() if not cap.isOpened(): raise RuntimeError("Failed to open camera")
try: while cap.isOpened(): ret, frame = cap.read() if not ret: continue frame = controller.process(frame) cv.imshow('PoseArm', frame) if cv.waitKey(1) & 0xFF == ord('q'): break finally: cap.release() cv.destroyAllWindows()
if __name__ == '__main__': main()