はじめに

こんにちは，ロボットなどを制作している高校生です．
今回は先日投稿したデモの解説を行います．

色々改善して動作が割と安定しました pic.twitter.com/fW6V9UJteX
— basalte(ばざると) (@basalte1199) September 25, 2024

軽くこの記事↓で説明したのですが，コードなどを追加して説明していきたいと思います．

ハンドトラッキング

RealSenseでMediapipeを動かして手の座標を取得します．
mediapipeから取得できるデータから必要な箇所だけ取り出し，手の中央座標を計算してdepthをもとに3次元座標を計算しています．RealSenseからのRGBD topicのcallback関数内です．

    def listener_callback(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg.rgb, "bgr8")
        depth = self.bridge.imgmsg_to_cv2(msg.depth, "passthrough")
        results = hands.process(frame)

        if results.multi_hand_landmarks: # 手が検出された場合
            landmarks = []
            for hand_lms in results.multi_hand_landmarks:
                landmark = []
                for index, lm in enumerate(hand_lms.landmark):
                    ih, iw, _ = frame.shape
                    x, y = int(lm.x * iw), int(lm.y * ih)
                    landmark.append([x, y])
            landmarks.append(landmark)

            x_list = []
            y_list = []
            for i in range(21):
                x_list.append(landmarks[0][i][0])
                y_list.append(landmarks[0][i][1])

            most_right_x = max(x_list)
            most_right_index = x_list.index(most_right_x)
            most_right_y = y_list[most_right_index]

            most_left_x = min(x_list)
            most_left_index = x_list.index(most_left_x)
            most_left_y = y_list[most_left_index]

            most_up_y = max(y_list)
            most_up_index = y_list.index(most_up_y)
            most_up_x = x_list[most_up_index]

            most_down_y = min(y_list)
            most_down_index = y_list.index(most_down_y)
            most_down_x = x_list[most_down_index]

            depth_list = []
            depth_list.append([landmarks[0][5][0],landmarks[0][5][1]])
            depth_list.append([landmarks[0][9][0],landmarks[0][9][1]])
            depth_list.append([landmarks[0][13][0],landmarks[0][13][1]])
            depth_list.append([landmarks[0][17][0],landmarks[0][17][1]])
            distance_list = []
            for f in depth_list:
                if f[1] >= 480 or f[0] >= 640:
                    continue
                distance = depth[int(f[1]),int(f[0])]
                if distance != 0:
                    distance_list.append(distance)
            try:
                distance_average = sum(distance_list)/len(distance_list)
                print(distance_average/1000)
                hand_width = int(math.tan(math.radians((most_right_x - most_left_x) / 640 * 85.2 )) * distance_average)
                hand_high = int(math.tan(math.radians((most_up_y - most_down_y) / 480 * 65 )) * distance_average)
                center_x = math.sin(math.radians((320 - landmarks[0][9][0]) / 640 * 85.2)) * distance_average/1000
                center_y = math.sin(math.radians((240 - landmarks[0][9][1]) / 480 * 65)) * distance_average/1000

逆運動学

ロボットアームを動かします．テレオペの制御にはいろいろな方法がありますが，今回はエンドエフェクタの座標を軸に処理をしています．(ほかの方法だと同じ軸構成のロボットアームをもう一つ用意して軸ごとに反映させる方法などもあります．)

下のようなコードで，エンドエフェクタの座標からそれぞれの軸の角度を計算します．自分が使っているロボットアームは5軸なのでだいぶ簡単に計算することができます．違う構成，違う軸数のロボットアームを使っている場合はそれに適したプログラムに変更する必要があります．

SetPositionFourMotor()という独自のTopic型に，position_1~4の４つのモーターの取るべき角度を代入してこの下でpublishします．(計算式のコードが汚すぎますがご容赦ください)

 def end_effector(self,position):
        output = SetPositionFourMotor()
        try:
            position_1 = math.atan2(position[1],position[0])
            output.position_1 = int(math.degrees(position_1)*4096/360 + self.base_angle[0])
            raw_l = math.sqrt(position[0]**2 + position[1]**2)
            ik_l = raw_l - self.length[3] * math.sin(position[3])
            ik_z = position[2] - self.length[0] - self.length[3] * math.cos(position[3])
            v = math.sqrt(ik_l**2 + ik_z**2)

            position_2 = math.pi/2.0 - math.acos((self.length[1]**2 + v**2 - self.length[2]**2)/(2*self.length[1]*v)) - math.atan2(ik_z,ik_l)
            output.position_2 = int(self.base_angle[1] - math.degrees(position_2)*4096/360)
            position_3 = math.pi - math.acos((self.length[1]**2 + self.length[2]**2 - v**2)/(2*self.length[1]*self.length[2]))
            output.position_3 = int(self.base_angle[2] - math.degrees(position_3)*4096/360)
            position_4 = position[3] - (position_2 +position_3)
            output.position_4 = int(self.base_angle[3] - math.degrees(position_4)*4096/360)

ロボットアームを動かすプログラムのループの一番最初に，現在のロボットアームのエンドエフェクタの座標を求める処理を挟んでいます(順運動学といいます)．これは絶対座標で最初から指令を出してしまうと最初だけ予想だにしない角度に向かってしまう可能性があるためです．

まとめ

私はROS2を使っているため，いくつかのプログラムを同時に走らせ，その間をros2 topicで通信することで動作しています．上で紹介したプログラムのほかにRealSenseとDynamixelをBring upするノードを立ち上げています．
これより詳しく書くのは難しい部分もあるため，質問等あればこちらに連絡ください．