第 5 部：檢測人員和玩 Fetch - Detecting People and Playing Fetch

星期一, 9月 13, 2021

前言大綱

恭喜，您距離功能齊全、可玩的 Spot 僅一步之遙！最後一步是教 Spot 檢測人。

雖然我們可以訓練一個新的 ML 模型，就像我們在 第 2 部分 中所做的那樣，但我們將使用工作量更少的 現成off-the-shelf 模型，並且可能會產生更好的性能。

加載現成的模型

我們將使用我們用於轉換學習的相同模型。這有許多優點：

它具有足夠的人員檢測性能。
結構是相同的，所以我們不必改變我們調用模型的方式。
獎勵：你已經擁有了！

將標籤文件複製到模型目錄中：

cp ~/fetch/models-with-protos/research/object_detection/data/mscoco_label_map.pbtxt ~/fetch/dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/

要加載模型，請使用附加的 -m 參數調用 network_compute_server.py。

如果您仍在運行 network_compute_server.py 用 ^C 停止它

使用新模型及其標籤重新啟動它：

cd ~/fetch
python network_compute_server.py -m dogtoy/exported-models/dogtoy-model/saved_model dogtoy/annotations/label_map.pbtxt -m dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt --username user --password YOUR_ROBOTS_PASSWORD 192.168.80.3

哎呀！讓我們分解一下：

python network_compute_server.py 調用我們的腳本。
-m dogtoy/exported-models/dogtoy-model/saved_model dogtoy/annotations/label_map.pbtxt 是我們之前運行的自定義模型及其標籤。
-m dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt 是預先訓練模型及其標籤。
--username user --password YOUR_ROBOTS_PASSWORD 192.168.80.3 是機器人的用戶名、密碼和IP地址

成功後，您會在 TensorFlow 和 CUDA 啟動時看到一堆輸出。最終，您將看到兩個模型都已加載的指示，如下所示：

[... lots of text ...]
Service fetch_server running on port: 50051
Loaded models:
    dogtoy-model
    ssd_resnet50_v1_fpn_640x640_coco17_tpu-8

測試人員檢測

隨著新模型的運行，我們可以回到平板電腦上試用一下。

1. 選擇 Hamburger Menu > Utilities > ML Model Viewer

2. 選擇 ssd_resnet50_v1_fpn_640x640_coco17_tpu-8 型號，然後按開始。

成功後，您會在視頻中看到人物周圍的邊界框。確保與機器人保持 2 米的距離。

給 gif 愛好者：

把它放在一起

我們現在擁有了所有需要獲取的部分：

狗玩具檢測
抓玩具
人物檢測

為了把它放在一起，我們將添加到我們的 fetch.py 並使用人員檢測來玩 fetch！

（或下載：fetch.py）

首先，刪除 fetch.py 末尾附近的以下幾行

        # For now, we'll just exit...
        print('')
        print('Done for now, returning control to tablet in 5 seconds...')
        time.sleep(5.0)
        break

在這些行之後添加新代碼以直接替換它：

        # Wait for the carry command to finish
        time.sleep(0.75)

        person = None
        while person is None:
            # Find a person to deliver the toy to
            person, image, vision_tform_person = get_obj_and_img(
                network_compute_client, options.ml_service,
                options.person_model, options.confidence_person, kImageSources,
                'person')

與上面相同的調用，但這次我們使用模型 options.person_model 並蒐索帶有“person”標籤的物件。

如果您不熟悉 Spot 的幀框約定，請查看我們的 概念文件。

        # We now have found a person to drop the toy off near.
        drop_position_rt_vision, heading_rt_vision = compute_stand_location_and_yaw(
            vision_tform_person, robot_state_client, distance_margin=2.0)

        wait_position_rt_vision, wait_heading_rt_vision = compute_stand_location_and_yaw(
            vision_tform_person, robot_state_client, distance_margin=3.0)

計算我們要放下玩具的位置和我們要備份並等待的位置。我們將在下面填寫這個函數。

        # Tell the robot to go there

        # Limit the speed so we don't charge at the person.
        move_cmd = RobotCommandBuilder.trajectory_command(
            goal_x=drop_position_rt_vision[0],
            goal_y=drop_position_rt_vision[1],
            goal_heading=heading_rt_vision,
            frame_name=frame_helpers.VISION_FRAME_NAME,
            params=get_walking_params(0.5, 0.5))
        end_time = 5.0
        cmd_id = command_client.robot_command(command=move_cmd,
                                              end_time_secs=time.time() +
                                              end_time)

設置行走時的速度限制
命令機器人移動到我們的 drop position and yaw

        # Wait until the robot reports that it is at the goal.
        block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True)

        print('Arrived at goal, dropping object...')

等待機器人行走。這個輔助函數使用 cmd_id 輪詢機器人以獲得反饋，並在我們到達時返回。

       # Do an arm-move to gently put the object down.
        # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame).
        x = 0.75
        y = 0
        z = -0.25
        hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z)

        # Point the hand straight down with a quaternion.
        qw = 0.707
        qx = 0
        qy = 0.707
        qz = 0
        flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)

        flat_body_tform_hand = geometry_pb2.SE3Pose(
            position=hand_ewrt_flat_body, rotation=flat_body_Q_hand)

        robot_state = robot_state_client.get_robot_state()
        vision_tform_flat_body = frame_helpers.get_a_tform_b(
            robot_state.kinematic_state.transforms_snapshot,
            frame_helpers.VISION_FRAME_NAME,
            frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME)

        vision_tform_hand_at_drop = vision_tform_flat_body * math_helpers.SE3Pose.from_obj(
            flat_body_tform_hand)

計算掉落位置。由於我們已經站在正確的位置，我們可以做一個相對於身體的移動，將手指向 Spot 的前面。

注意：我們使用 vision_tform_flat_body * ... 轉換為視覺框架，因為我們不希望機器人相對於它的身體移動（我們想要世界上的一個固定點）。

通常這無關緊要，但如果機器人受到干擾，它會將手臂保持在原位，而不是相對於機器人保持手臂

        # duration in seconds
        seconds = 1

        arm_command = RobotCommandBuilder.arm_pose_command(
            vision_tform_hand_at_drop.x, vision_tform_hand_at_drop.y,
            vision_tform_hand_at_drop.z, vision_tform_hand_at_drop.rot.w,
            vision_tform_hand_at_drop.rot.x, vision_tform_hand_at_drop.rot.y,
            vision_tform_hand_at_drop.rot.z, frame_helpers.VISION_FRAME_NAME,
            seconds)

        # Keep the gripper closed.
        gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
            0.0)

        # Combine the arm and gripper commands into one RobotCommand
        command = RobotCommandBuilder.build_synchro_command(
            gripper_command, arm_command)

        # Send the request
        cmd_id = command_client.robot_command(command)

構建並發送 手臂 arm 命令。我們使用 同步命令 synchronized command 來命令手臂和抓手。我們不會為 Spot 的身體傳遞任何東西，所以它會繼續站立。

        # Wait until the arm arrives at the goal.
        block_until_arm_arrives(command_client, cmd_id)

        # Open the gripper
        gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
            1.0)
        command = RobotCommandBuilder.build_synchro_command(gripper_command)
        cmd_id = command_client.robot_command(command)

        # Wait for the dogtoy to fall out
        time.sleep(1.5)

        # Stow the arm.
        stow_cmd = RobotCommandBuilder.arm_stow_command()
        command_client.robot_command(stow_cmd)

        time.sleep(1)

等到手臂就位以進行下降
打開抓手
收起手臂

        print('Backing up and waiting...')

        # Back up one meter and wait for the person to throw the object again.
        move_cmd = RobotCommandBuilder.trajectory_command(
            goal_x=wait_position_rt_vision[0],
            goal_y=wait_position_rt_vision[1],
            goal_heading=wait_heading_rt_vision,
            frame_name=frame_helpers.VISION_FRAME_NAME,
            params=get_walking_params(0.5, 0.5))
        end_time = 5.0
        cmd_id = command_client.robot_command(command=move_cmd,
                                              end_time_secs=time.time() +
                                              end_time)

        # Wait until the robot reports that it is at the goal.
        block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True)

最後一部分！倒退讓用戶安全地拿起玩具並循環。

小心！此行已在 第 4 部分 的文件中。不要復制兩次。

    lease_client.return_lease(lease)

現在我們將定義一些輔助函數：

def compute_stand_location_and_yaw(vision_tform_target, robot_state_client,
                                   distance_margin):
    # Compute drop-off location:
    #   Draw a line from Spot to the person
    #   Back up 2.0 meters on that line
    vision_tform_robot = frame_helpers.get_a_tform_b(
        robot_state_client.get_robot_state(
        ).kinematic_state.transforms_snapshot, frame_helpers.VISION_FRAME_NAME,
        frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME)

此函數計算玩具掉落的位置。它需要：

目標位置
客戶端獲取機器人狀態
偏移距離

我們要做的是：

畫一條從目標位置到我們當前位置的線。
忽略該行的長度。
從目標沿線的方向移動，直到我們沿線移動 2.0 米。

    # Compute vector between robot and person
    robot_rt_person_ewrt_vision = [
        vision_tform_robot.x - vision_tform_target.x,
        vision_tform_robot.y - vision_tform_target.y,
        vision_tform_robot.z - vision_tform_target.z
    ]

這兩個向量相減計算位置之間的向量。

    # Compute the unit vector.
    if np.linalg.norm(robot_rt_person_ewrt_vision) < 0.01:
        robot_rt_person_ewrt_vision_hat = vision_tform_robot.transform_point(1, 0, 0)
    else:
        robot_rt_person_ewrt_vision_hat = robot_rt_person_ewrt_vision / np.linalg.norm(
            robot_rt_person_ewrt_vision)

我們不關心向量有多長，所以我們將其更改為 1.0 米（計算單位向量）。

請注意，我們使用機器人的當前方向來防止除以零

    # Starting at the person, back up meters along the unit vector.
    drop_position_rt_vision = [
        vision_tform_target.x +
        robot_rt_person_ewrt_vision_hat[0] * distance_margin,
        vision_tform_target.y +
        robot_rt_person_ewrt_vision_hat[1] * distance_margin,
        vision_tform_target.z +
        robot_rt_person_ewrt_vision_hat[2] * distance_margin
    ]

現在我們有一個長度正好為 1.0 的向量，我們可以將它乘以我們想要的任何長度並得到我們的放置位置。

   # We also want to compute a rotation (yaw) so that we will face the person when dropping.
    # We'll do this by computing a rotation matrix with X along
    #   -robot_rt_person_ewrt_vision_hat (pointing from the robot to the person) and Z straight up:
    xhat = -robot_rt_person_ewrt_vision_hat
    zhat = [0.0, 0.0, 1.0]
    yhat = np.cross(zhat, xhat)
    mat = np.matrix([xhat, yhat, zhat]).transpose()
    heading_rt_vision = math_helpers.Quat.from_matrix(mat).to_yaw()

    return drop_position_rt_vision, heading_rt_vision

我們希望機器人在從狗玩具上掉下來時面向人。我們將通過構建一個旋轉矩陣並從中提取偏航來做到這一點。

回想一下線性代數，我們可以從三個正交向量構造一個旋轉矩陣：

用數字：

我們可以構建一個新的旋轉矩陣，其中 xhat 是我們希望機器人面對的方向。

我們已經計算了一個單位向量，robot_rt_person_ewrt_vision_hat，它從人指向機器人。如果我們否定該向量，它將從機器人指向人：

xhat = -robot_rt_person_ewrt_vision_hat

對於 zhat，我們將使用重力矢量：[0, 0, 1]。

視覺上：

yhat 很容易，因為它總是與 xhat 和 zhat 正交，所以我們可以使用叉積找到它。

使用 xhat、yhat 和 zhat，我們可以構造一個旋轉矩陣： mat = np.matrix([xhat, yhat, zhat]).transpose()
然後我們可以輕鬆地從旋轉矩陣通過四元數轉換為偏航。Heading_rt_vision = math_helpers.Quat.from_matrix(mat).to_yaw()

為什麼不使用歐拉角？
“如果你使用歐拉角，你的代碼是錯誤的”——作者
顯然這不是嚴格正確的，但是歐拉角很難用 3D 角正確處理。當涉及到數學 + 機器人時，他們有最糟糕的屬性：它看起來是正確的，但事實並非如此*。* 這意味著它會在最糟糕的時候崩潰歐拉角對於輸出顯示效果很好，但對於幀數學來說太容易出錯了。

def pose_dist(pose1, pose2):
    diff_vec = [pose1.x - pose2.x, pose1.y - pose2.y, pose1.z - pose2.z]
    return np.linalg.norm(diff_vec)

計算兩幀之間的距離：

def get_walking_params(max_linear_vel, max_rotation_vel):
    max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel)
    max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear,
                                           angular=max_rotation_vel)
    vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2)
    params = RobotCommandBuilder.mobility_params()
    params.vel_limit.CopyFrom(vel_limit)
    return params

設置 Spot 的移動性參數以限制步行速度。

為 Pickup 啟用更遠距離的步行

在上一節中，我們有 一個被註釋掉的部分 part that was commented out，開頭是：

# NOTE: we'll enable this code in Part 5, when we understand it.

這與我們走到上面的人時所做的基本相同。取消註釋此部分以允許機器人走得更遠到狗玩具。

玩取物

運行代碼：

python fetch.py --username user --password YOUR_ROBOTS_PASSWORD -s fetch-server -m dogtoy-model -p ssd_resnet50_v1_fpn_640x640_coco17_tpu-8 192.168.80.3

參數是：

用戶名和密碼（和以前一樣）
我們的機器學習服務器的名稱
狗玩具模型的名稱
我們的人物檢測模型的名稱
機器人的IP地址

成功後，機器人應該撿起玩具，拿給你，放下，等你再扔！

下一步

恭喜，您已成功完成教程並構建 fetch！您所學到的知識為您提供了可以擴展的基礎。我們希望您玩得開心，我們期待看到您建立的令人興奮的新行為！

嘗試更改代碼，使用 關節角度 API ( the joint angle API) 將玩具放入背面的桶中。這就是我們製作 Spot's Got an Arm 視頻的 手套和洗衣店部分 glove and laundry-pickup 的方式。
在其他環境中收集更多數據以調整您的模型。

搜尋此網誌

先機致勝 AI Advantage