Webot通过RPC读取机械臂数据

介绍

为Webot中的模型编写一个Controller即可控制模型的仿真运动，在这个例子中通过我们编写一个Python的Controller，Controller通过RPC获取机械臂的关节数据

步骤

1. 引入需要的库

import time
from threading import Thread

import grpc
from controller import Robot

import robot_pb2
import robot_pb2_grpc

1. 创建RPC Clent

channel = grpc.insecure_channel("192.168.1.167:50051")
stub = robot_pb2_grpc.RobotServiceStub(channel)

1. 实例化Webot中的模型，并获取机械臂的各个关节，用于控制机械臂运动

robot = Robot()
joints = []
for i in range(1,7):
    joints.append(robot.getMotor(f"xb4_joint_{i}"))

1. 通过RPC实时获取机械臂的关节数据，这里需要注意由于实时获取机械臂关节数据的TStreamRobotJoint是流式传输，因此要在子线程中运行避免阻塞Webot仿真的主线程

def get_robot_joint():
    """
    获取机械臂关节
    以线程的方式启动，避免阻塞Webot的仿真线程
    """
    print("线程已经启动")
    stub.MoveStart(robot_pb2.ReqVoid())
    resp = stub.TStreamRobotJoint(robot_pb2.ReqVoid())
    for r in resp:
        print(r.data)
        for i in range(6):
            joints[i].setPosition(-r.data[i])

1. 启动获取关节数据的线程

get_joint_thread = Thread(target=get_robot_joint)
get_joint_thread.start()

1. 启动Webot仿真

timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1:
    time.sleep(0.01)

验证

为了验证结果，使用机器人IDE控制机械臂运动

完整代码

import time
from threading import Thread

import grpc
from controller import Robot

import robot_pb2
import robot_pb2_grpc

def get_robot_joint():
    """
    获取机械臂关节
    以线程的方式启动，避免阻塞Webot的仿真线程
    """
    print("线程已经启动")
    stub.MoveStart(robot_pb2.ReqVoid())
    resp = stub.TStreamRobotJoint(robot_pb2.ReqVoid())
    for r in resp:
        print(r.data)
        for i in range(6):
            joints[i].setPosition(-r.data[i])

channel = grpc.insecure_channel("192.168.1.167:50051")
stub = robot_pb2_grpc.RobotServiceStub(channel)

get_joint_thread = Thread(target=get_robot_joint)
get_joint_thread.start()
robot = Robot()
joints = []
for i in range(1,7):
    joints.append(robot.getMotor(f"xb4_joint_{i}"))
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1:
    time.sleep(0.01)