Первая статья – Управляем роботами из VR

Вторая статья – Управляем роботами из VR. Продолжение 1.

В прошлой статье мы реализовали основные сетевые функции, настроили DataChannel и работу с медиатраками из JS/Unity/Python. В этой статье мы реализуем управление камерой и манипулятором(Части 5, 6).

Часть 5. Настройка управления камерой

Начинать нужно с малого, поэтому начнем с управления LED и поворотами головы, этим у нас занимаются 2 серво SG90 на кронштейне. Для этого будем отправлять простое текстовое сообщение в datachannel с указанием статуса LED и позиции, на которой должен быть выставлен угол сервомотора, а на стороне Python скрипта их обрабатывать.

Серверный компонент

Добавим в созданную разметку новые элементы для управления:

main.html

...
  <div class="row-md-6 mt-3 mb-3">
    <form class="form-inline">
      <div class="form-group">
        <div class="custom-control custom-switch">
          <input type="checkbox" class="custom-control-input" id="ledSwitch" disabled>
          <label class="custom-control-label" for="ledSwitch" id="ledSwitchLabel">LED control (LED Off)</label>
        </div>
      </div>
    </form>
  </div>
  <div class="row-md-6 mt-3 mb-3">
    <form class="form-inline">
      <div class="form-group">
        <label for="servoRangeX" class="form-label">SG90 X-angle:</label>
        <input type="range" class="form-range" min="0" max="180" value="90" step="1" style="width: 500px;" id="servoRangeX" disabled>
        <span id="servoRangeXInfo"></span>
      </div>
    </form>
  </div>
  <div class="row-md-6 mt-3 mb-3">
    <form class="form-inline">
      <div class="form-group">
        <label for="servoRangeY" class="form-label">SG90 Y-angle:</label>
        <input type="range" class="form-range" min="0" max="180" value="90" step="1" style="width: 500px;" id="servoRangeY" disabled>
        <span id="servoRangeYInfo"></span>
      </div>
    </form>
  </div>
...

Доработаем наш скрипт следующим образом:

robo.js

// Добавим активацию элементов при открытии datachannel:
// open handling
dataChannel.onopen = function(){
  console.log("Data channel is open!");
  $( "#ledSwitch" ).prop( "disabled", false );
  $( "#servoRangeX" ).prop( "disabled", false );
  $( "#servoRangeY" ).prop( "disabled", false );
}

// И функции, которые делают передачу инфо:
function switchLed(){
  if(dataChannel != null){
    if(blink == "ON"){
      dataChannel.send("BLINK_OFF");
      console.log("LED is OFF");
      blink = "OFF";
      $("#ledSwitchLabel").text("LED control (LED OFF)");
    } else{
      dataChannel.send("BLINK_ON");
      console.log("LED is ON");
      blink = "ON";
      $("#ledSwitchLabel").text("LED control (LED ON)");
    }
  }
}

function servoRangeX(){
  if(dataChannel != null){
    servoAngleX = $("#servoRangeX").val();
    dataChannel.send("X-ANG#"+servoAngleX);
    console.log("Sending new servo X angle = "+ servoAngleX);
    $("#servoRangeXInfo").text("" + servoAngleX);
  }
}

function servoRangeY(){
  if(dataChannel != null){
    servoAngleY = $("#servoRangeY").val();
    dataChannel.send("Y-ANG#"+servoAngleY);
    console.log("Sending new servo Y angle = "+ servoAngleY);
    $("#servoRangeYInfo").text("" + servoAngleY);
  }
}
...
$("#ledSwitch").change(function() {switchLed(); });
$("#servoRangeX").change(function() {servoRangeX(); });
$("#servoRangeY").change(function() {servoRangeY(); });

Итого, при изменении элементов в datachannel отправляется сообщение с новым углом установки серво или вкл/выкл LED.

Исполнительный компонент(Python-скрипт на RPI)

Со стороны скрипта настроим обработку этих сообщений.

part4.py


## Вспомним нашу подготовительную часть и добавим нужные зависимости:
import RPi.GPIO as GPIO
import busio
from board import SCL, SDA
from adafruit_motor import servo
from adafruit_pca9685 import PCA9685

## Инициируем все при загрузке скрипта глобально:
GPIO.setmode(GPIO.BCM)
GPIO.setup(21, GPIO.OUT)
i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c)
pca.frequency = 60
servoX = servo.Servo(pca.channels[0], min_pulse=500, max_pulse=2400)
servoY = servo.Servo(pca.channels[1], min_pulse=500, max_pulse=2400)
servoX.angle = 90
servoY.angle = 90

## Добавим новую функцию on message:
@channel.on("message")
async def on_message(rtc_message):
  if isinstance(rtc_message, str):
  print("New message from datachannel " + rtc_message)
  #channel.send("Reply from PyPi - " + rtc_message)
  await consume_message(rtc_message)

## И реализуем ее:
async def consume_message(rtc_message: str) -> None:
  if rtc_message.startswith("BLINK_ON"):
  GPIO.output(21,True)
  if rtc_message.startswith("BLINK_OFF"):
  GPIO.output(21,False)
  if rtc_message.startswith("X-ANG"):
  xServoAngle = int(rtc_message.split("#")[1])
  servoX.angle = xServoAngle
  if rtc_message.startswith("Y-ANG"):
  yServoAngle = int(rtc_message.split("#")[1])
  servoY.angle = yServoAngle

Теперь можно сделать проверку между браузером и скриптом:

проверка:

как видим, LED и серво отрабатывают успешно, можно переходить к VR части.

Управляющий компонент(Unity VR)

Для реализации поворота головы нужно сначала сделать подготовку.

Давайте сделаем отдельной пустой объект, создадим в нем новую камеру Robo Camera(тэг RoboCamera) и перенесем наш Panel в объект Robo Camera. Причем разместим их на высоте 3 над ground. И немного отдалим Image от камеры.(+ у Robo Camera отключаем AudioListener). Так же в Robo Camera добавим компонент «Tracked Pose Driver(Input System)» и переносим его настройки Position Input/ Rotation Input с Main Camera.

Должно получиться примерно вот так:

Потом нам нужно сделать переключение между камерами Main и Robo. Для этого нам нужно реализовать обработку нажатия кнопок на правом контроллере Oculus. Сделаем это создав скрипт RightControllerButtonWatcher.cs

RightControllerButtonWatcher.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.XR;
public class RightControllerButtonWatcher : MonoBehaviour
{
  public static bool triggerValue;
  private List<InputDevice> rightHandDevices;
  InputDevice rightHandCantroller;

  void Awake()
  {
      rightHandDevices = new List<InputDevice>();
  }
  
  void Update()
  {
      if (rightHandDevices.Count == 0)
      {
          InputDevices.GetDevicesAtXRNode(XRNode.RightHand, rightHandDevices);
          Debug.Log("Try to get Right controller!");
      }
  
      if (rightHandDevices.Count > 0)
      {
          rightHandCantroller = rightHandDevices[0];
          if (rightHandCantroller.TryGetFeatureValue(CommonUsages.secondaryButton, out triggerValue) && triggerValue)
          {
              gameObject.SendMessage("MakeVideoInFullScreen", triggerValue);
          }
      }
  }

}

И приаттачим его к тому же GameObject, на котором установлен скрипт Connection.cs.

В самом Connection.cs обработаем нажатие на кнопку с помощью простого накопителя.

Connection.cs

...
private bool fullScreenOn = false;
private short fullScreenCounter = 0;
private Camera mainCamera;
private Camera roboCamera;
...
void Start()
{
mainCamera = GameObject.FindGameObjectWithTag("MainCamera").GetComponent<Camera>();
roboCamera = GameObject.FindGameObjectWithTag("RoboCamera").GetComponent<Camera>();
roboCamera.enabled = false;
CameraTrackingChanger(roboCamera, false);
CameraAudioChanger(roboCamera, false);
...
...
void CameraTrackingChanger(Camera camera, bool status)
{
  Component[] components = camera.GetComponents(typeof(MonoBehaviour));
  foreach (Component comp in components)
  {
    if (comp.ToString().Contains("TrackedPoseDriver"))
    {
      MonoBehaviour mb = (MonoBehaviour)comp;
      mb.enabled = status;
    }
  }
}

void CameraAudioChanger(Camera camera, bool status)
{
  var audio = camera.GetComponent<AudioListener>();
  audio.enabled = status;
}
  
// И создадим метод  MakeVideoInFullScreen
public void MakeVideoInFullScreen(bool trigger)
{
  if (trigger && !fullScreenOn)
  {
    fullScreenCounter++;
    if (fullScreenCounter >= 100)
    {
      fullScreenOn = true;
      mainCamera.enabled = false;
      CameraTrackingChanger(mainCamera, false);
      CameraAudioChanger(mainCamera, false);
      roboCamera.enabled = true;
      CameraTrackingChanger(roboCamera, true);
      CameraAudioChanger(roboCamera, true);
      Debug.Log("Robo cam switch On");
    }
  }
  else if (trigger &amp;&amp; fullScreenOn)
  {
  fullScreenCounter--;
  if (fullScreenCounter &lt;= 0)
  {
    fullScreenOn = false;
    roboCamera.enabled = false;
    CameraTrackingChanger(roboCamera, false);
    CameraAudioChanger(roboCamera, false);
    mainCamera.enabled = true;
    CameraTrackingChanger(mainCamera, true);
    CameraAudioChanger(mainCamera, true);
    Debug.Log("Robo cam switch Off");
  }
  }
}

Можем сделать промежуточную проверку. При запуске приложения у нас активна Main Camera. При зажатии кнопки «B» (SecondaryButton) правого контроллера нас перебрасывает на Robo Camera. И обратно.

Потом сделаем скрипт, который при повороте головы будет отслеживать угол поворота шлема. Для этого создадим скрипт по получению значений углов и приведению их к значениям, которые приемлемы для наших серво:

HeadRotation.cs

public class HeadRotation : MonoBehaviour
{
  public static int valHeadX;
  public static int valHeadY;
  void Update()
  {
      if (Camera.current != null)
      {
          valHeadX = (int) Camera.current.transform.localEulerAngles.x;
          valHeadY = (int) Camera.current.transform.localEulerAngles.y;
  
          // angle casting
          if (valHeadX &gt;= 0 &amp;&amp; valHeadX &lt; 180) valHeadX = 90 - valHeadX;
          else if (valHeadX &gt; 180) valHeadX = 360 - valHeadX + 90;
  
          if (valHeadY &gt;= 0 &amp;&amp; valHeadY &lt; 180) valHeadY = 90 - valHeadY;
          else if (valHeadY &gt; 180) valHeadY = 360 - valHeadY + 90;
      }
  }
}

и в скрипте Connection.cs будем просто получать эти значения(т.к. Переменные valHeadX valHeadY – public static) и отправлять в datachannel 20 раз в секунду.

Connection.cs

...
private int headX;
private int headY;
private float elapsed = 0f;
...

void Update()
{
  if (dataChannel != null)
  {
  elapsed += Time.deltaTime;
  if (elapsed >= 0.05f)
  {
    elapsed = 0f;
    if (headX != HeadRotation.valHeadX)
    {
      headX = HeadRotation.valHeadX;
      var message = "X-ANG#" + headX;
      dataChannel.Send(message);
    }
    if (headY != HeadRotation.valHeadY)
    {
      headY = HeadRotation.valHeadY;
      var message = "Y-ANG#" + headY;
      dataChannel.Send(message);
    }
  }
}
...

Не забываем особенность – поворот головы в Unity идет в диапазоне 0-360 по каждой из оси ротации, а нам нужно привести это значение к диапазону вращения сервомотора 0-180, поэтому реализован каст от 0-360 к 0-180, сделан он грубо, но этого вполне хватит.

Теперь, после установления соединения по ws и webrtc, нажатием клавиши на контроллере нас перебрасывает на новую камеру, в которой мы видим изображение с камеры, а HeadRotation.cs отслеживает изменение поворота головы по осям и кастует их к необходимому диапазону.

Проверка взаимодействия компонентов

Запускаем Python скрипт и Unity, устанавливаем соединение, переключаемся между камерами и видим, что при повороте головы серво соответственно поворачивают камеру, можем вертеть головой и наслаждаться.

Часть 6. Настройка управления манипулятором

Управление манипулятором сводится к 4 этапам:

Инициацию управления, это мы реализуем с помощью Grip Button правого контроллера и накопителя, так же как и при переключении камеры, т. е. если зажать кнопку Grip на контроллере – то происходит присоединение контроллера к управлению рукой. Если зажать еще раз – то происходит отключение от управления.
Управление захватом(клешней). Этот функционал будем реализовывать с помощью джойстика на правом контроллере курсор лево-право.
Управление поворотом клешни. Этот функционал будем реализовывать с помощью захвата ротации правого контроллера.
Управление положением клешни в пространстве. Тут будем использовать отслеживание перемещение контроллера по осям X-Y-Z в unity.

И последний пункт является самым сложным из 4х. Мы хотим из Unity выдавать только Vector3 с координатами положения захвата манипулятора(их относительным изменением), а сервомоторы должны повернуться на нужный угол в нужной последовательности, и решение этой задачи относится к инверсной кинематике.

Реализовать этот процесс можно было бы несколькими путями:

Использовать IK solver из Unity – т. е. реализовать аналогичную модель нашей роборуки в Unity и использовать эту модель для передачи углов на реальную роборуку. Я его отказался от этого варианта, т. к. это жестко привязывает приложение к конкретному манипулятору.
Использовать доступные IK solver из пакетов Python, например tinyik, pybotics или ikpy, т. е. Инициировать виртуальный манипулятор, выставить его в default position, аналогично реальному, и при поступлении новых координат EndEffector получать актуальные углы смещения серво. Я попробовал tinyik, но на RPI он работает достаточно медленно, 0,3s-0,5s на расчет, заниматься профилированием мне не хотелось, как и пробовать другие, т.к. текущее решение просто PoC
Реализовать примитивное решение самостоятельно, после установки манипулятора в default position можно провести серию изменений углов на сервоприводах, зафиксировать изменения положения EndEffector на осях X-Y-Z, и примерно составить простое свое решение(из глины и палок, только в этот раз без палок). Так я и поступил.

в ROS задачи ИК решаются гораздо элегантнее, с предварительным моделированием и составлением UDRF-модели, в дальнейшем мы это увидим.

Мы определились с инструментами и можем приступить к сборке манипулятора. Толковых инструкций по этому процессу не много, например тут или тут, но как всегда есть индивидуальные детали. Всегда помните, что у сервомотора есть угол поворота, на который он может поворачиваться, для всех наших серв – SG-90, MG996R и YF-6125MG он стандартный 180 градусов. Сборку компонентов нужно проводить с учетом границы работы сервомоторов и желаемого положения «по-умолчниию», той позиции с которой начнет работу наша рука и, в которую придет после завершения работы, а исходя из границ работы сервомоторов будет определяться рабочая зона для манипулятора.

Теперь можем приступить собственно к сборке и напишем простой скрипт, который нам поможет при сборке и выставлении углов:

test.py

import time
import busio
from board import SCL, SDA
from adafruit_motor import servo
from adafruit_pca9685 import PCA9685

i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c)
pca.frequency = 50
servoR1 = servo.Servo(pca.channels[2], min_pulse=500, max_pulse=2500)
servoR2 = servo.Servo(pca.channels[3], min_pulse=500, max_pulse=2500)
servoS1 = servo.Servo(pca.channels[4], min_pulse=550, max_pulse=2300)
servoS2 = servo.Servo(pca.channels[5], min_pulse=550, max_pulse=2300)
servoS3 = servo.Servo(pca.channels[6], min_pulse=550, max_pulse=2300)
servoS4 = servo.Servo(pca.channels[7], min_pulse=550, max_pulse=2300)

print("Assembling test")

try:
    # R1 calibration
    servoR1.angle = 180
    time.sleep(10)
    servoR1.angle = 0
    
    # R2 calibration
    #servoR2.angle = 180
    #time.sleep(10)
    #servoR2.angle = 0
    
    # S1 calibration
    #servoS1.angle = 180
    #time.sleep(10)
    #servoS1.angle = 0
    
    # S2 calibration
    #servoS2.angle = 180
    #time.sleep(10)
    #servoS2.angle = 0
    
    # S3 calibration
    #servoS3.angle = 180
    #time.sleep(10)
    #servoS3.angle = 0
    
    # S4 calibration
    #servoS4.angle = 180
    #time.sleep(10)
    #servoS4.angle = 110
    
    # set Def position
    #servoS4.angle = 150
    #time.sleep(0.5)
    #servoS3.angle = 60
    #time.sleep(0.5)
    #servoS2.angle = 90
    #time.sleep(0.5)
    #servoS1.angle = 100
    #time.sleep(0.5)
    #servoR2.angle = 100
    #time.sleep(0.5)
    #servoR1.angle = 90
    #print("Def position setted")
    
except KeyboardInterrupt:
    pca.deinit()

print("Assembling done")
pca.deinit()

Запускаем его каждый раз при сборке очередного компонента манипулятора снимая комменты с соответствующей части.

Мы по очереди выставляем на серво угол 0 и 180 градусов во время сборки начиная с основания, учитываем рабочие границы сервомоторов, определяем min-max положение остальных элементов исходя из рабочего угла серво. В конце делаем монтаж манипулятора на основание и пробуем занять default position.

Теперь делаем новый тестовый файл для реализации ИК — testKin.py

testKin.py

import time
import busio
from board import SCL, SDA
from adafruit_motor import servo
from adafruit_pca9685 import PCA9685


i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c)
pca.frequency = 50
servoR1 = servo.Servo(pca.channels[2], min_pulse=500, max_pulse=2500)
servoR2 = servo.Servo(pca.channels[3], min_pulse=500, max_pulse=2500)
servoS1 = servo.Servo(pca.channels[4], min_pulse=550, max_pulse=2300)
servoS2 = servo.Servo(pca.channels[5], min_pulse=550, max_pulse=2300)
servoS3 = servo.Servo(pca.channels[6], min_pulse=550, max_pulse=2300)
servoS4 = servo.Servo(pca.channels[7], min_pulse=550, max_pulse=2300)
ik_servos = [servoR1, servoR2, servoS1, servoS2]
servo_ang = [90, 100, 100, 90]
ee_x = 0
ee_y = 5
ee_z = 0


def set_home_position():
  global servo_ang
  servoR1.angle = 90
  time.sleep(0.1)
  servoR2.angle = 100
  time.sleep(0.1)
  servoS1.angle = 100
  time.sleep(0.1)
  servoS2.angle = 90
  time.sleep(0.1)
  servoS3.angle = 60
  time.sleep(0.1)
  servoS4.angle = 150
  time.sleep(0.1)
  print('Home position setted')
  servo_ang = [90, 100, 100, 90]

  
def add_arm_pos_x():
  global ee_x
  global ee_y
  global servo_ang
  if ee_x != 0 and ee_x % 10 == 0 and ee_y < 20:
    correction_add_y()
  if ee_x != 0 and ee_x % 10 == 0 and ee_y > 20:
    correction_sub_y()
  if ee_x <= 80:
    servo_ang[3] += 1
    servoS2.angle = servo_ang[3]
    servo_ang[2] -= 1
    servoS1.angle = servo_ang[2]
  if ee_x < 50:
    servo_ang[1] += 1
    servoR2.angle = servo_ang[1]
  else:
    servo_ang[1] += 2
    servoR2.angle = servo_ang[1]
    ee_x += 1

def correction_add_x():
  global ee_x
  global servo_ang
  servo_ang[3] += 1
  servoS2.angle = servo_ang[3]
  servo_ang[2] -= 1
  servoS1.angle = servo_ang[2]
  if ee_x < 50:
    servo_ang[1] += 1
    servoR2.angle = servo_ang[1]
  else:
    servo_ang[1] += 2
    servoR2.angle = servo_ang[1]
  
def sub_arm_pos_x():
  global ee_x
  global ee_y
  global servo_ang
  if ee_x != 0 and ee_x % 10 == 0 and ee_y < 20:
    correction_sub_y()
  if ee_x != 0 and ee_x % 10 == 0 and ee_y > 20:
    correction_add_y()
  if ee_x <= 80:
    servo_ang[3] -= 1
    servoS2.angle = servo_ang[3]
    servo_ang[2] += 1
    servoS1.angle = servo_ang[2]
  if ee_x < 50:
    servo_ang[1] -= 1
    servoR2.angle = servo_ang[1]
  else:
    servo_ang[1] -= 2
    servoR2.angle = servo_ang[1]
    ee_x -= 1

  
def correction_sub_x():
  global ee_x
  global servo_ang
  servo_ang[3] -= 1
  servoS2.angle = servo_ang[3]
  servo_ang[2] += 1
  servoS1.angle = servo_ang[2]
  if ee_x < 50:
    servo_ang[1] -= 1
    servoR2.angle = servo_ang[1]
  else:
    servo_ang[1] -= 2
    servoR2.angle = servo_ang[1]

  
def add_arm_pos_y():
  global ee_x
  global ee_y
  global servo_ang
  if ee_y % 10 == 0:
    correction_add_x()
  if ee_y <= 50:
    servo_ang[1] -= 1
    servoR2.angle = servo_ang[1]
  if ee_y <= 25:
    servo_ang[2] -= 1
    servoS1.angle = servo_ang[2]
    servo_ang[3] -= 1
    servoS2.angle = servo_ang[3]
    ee_y += 1


def correction_add_y():
  global ee_y
  global servo_ang
  servo_ang[1] -= 1
  servoR2.angle = servo_ang[1]
  servo_ang[2] -= 1
  servoS1.angle = servo_ang[2]
  servo_ang[3] -= 1
  servoS2.angle = servo_ang[3]


def sub_arm_pos_y():
  global ee_x
  global ee_y
  global servo_ang
  if ee_y % 10 == 0:
    correction_sub_x()
  if ee_y <= 50:
    print(f'short ee_y={ee_y}')
    servo_ang[1] += 1
    servoR2.angle = servo_ang[1]
  if ee_y <= 25:
    servo_ang[2] += 1
    servoS1.angle = servo_ang[2]
    servo_ang[3] += 1
    servoS2.angle = servo_ang[3]
    ee_y -= 1

  
def correction_sub_y():
  global ee_y
  global servo_ang
  servo_ang[1] += 1
  servoR2.angle = servo_ang[1]
  servo_ang[2] += 1
  servoS1.angle = servo_ang[2]
  servo_ang[3] += 1
  servoS2.angle = servo_ang[3]


def add_arm_pos_z():
  global ee_z
  global servo_ang
  servo_ang[0] += 1
  servoR1.angle = servo_ang[0]
  ee_z += 1

  
def sub_arm_pos_z():
  global ee_z
  global servo_ang
  servo_ang[0] -= 1
  servoR1.angle = servo_ang[0]
  ee_z -= 1


try:
  print('Servo Kinematics test')
  set_home_position()
  print(f'IK servo angles:{servo_ang}')
  print(f'EE position: X:{ee_x}, Y:{ee_y}, Z:{ee_z}')
  time.sleep(2)
  for i in range(20):
      add_arm_pos_y()
      time.sleep(0.1)
      
  time.sleep(2)
  
  for i in range(60):
      add_arm_pos_x()
      time.sleep(0.1)
      
  time.sleep(2)
  
  for i in range(30):
      add_arm_pos_z()
      time.sleep(0.1)
      
  time.sleep(2)
  
  for i in range(60):
      sub_arm_pos_x()
      time.sleep(0.1)
      
  time.sleep(2)
  
  for i in range(30):
      sub_arm_pos_z()
      time.sleep(0.1)
      
  time.sleep(2)
  
  for i in range(20):
      sub_arm_pos_y()
      time.sleep(0.1)
      
  time.sleep(2)
  
  print(f'IK servo angles:{servo_ang}')
  print(f'EE position: X:{ee_x}, Y:{ee_y}, Z:{ee_z}')
  
  set_home_position()

except KeyboardInterrupt:
  pca.deinit()
  print("Kinematics test done")
  pca.deinit()

Это результат интуитивной импровизации в чистом виде, проводим тесты, видим, что все в принципе работает, хотя конечно же работает плохо, погрешности у нашего манипулятора на 100мм +/-5-10мм, совсем плохо, что нет никакой проверки на столкновение с плоскостью пола(collision detection), проверку на допустимое взаимное расположение углов на серво (self-collision detection). Опять таки вспоминаем про ROS... Главное – реализовали кое-какой IK, он быстрый, и можем использовать шаблон из теста в итоговом скрипте.

И так сойдет...

Серверный компонент

Добавим элементы для степени захвата, угла поворота и положения манипулятора в пространстве. Для этого для начала нужно выставить минимальные и максимальные допустимые положения сервомоторов. Для захвата манипулятора – минимальное 150 это положение максимального открытия(потом изменим) и положение захвата 180. Для ротации - минимальное 0, максимальное 180. Для положения захвата в пространстве будем передавать вектор сдвига EE. Так же немного изменим сообщения отправляемые в datachannel, теперь будем отправлять туда в виде JSON:

main.html

...
<div class="row-md-6 mt-3 mb-3">
  <form class="form-inline">
    <div class="form-group">
      <label for="servoS4" class="form-label">Grab:</label>
      <input type="range" class="form-range" min="90" max="180" value="150" step="1" style="width: 500px;" id="servoS4" disabled>
      <span id="servoS4Info"></span>
    </div>
  </form>
</div>

<div class="row-md-6 mt-3 mb-3">
  <form class="form-inline">
    <div class="form-group">
      <label for="servoS3" class="form-label">EE rotation:</label>
      <input type="range" class="form-range" min="0" max="180" value="0" step="1" style="width: 500px;" id="servoS3" disabled>
      <span id="servoS3Info"></span>
    </div>
  </form>
</div>

<div class="row-md-6 mt-3 mb-3">
  <form class="form-inline">
    <div class="input-group">
      <span class="input-group-text">EE changer:</span>
      <input type="text" aria-label="ee-x-pos" class="form-control" placeholder="X (mm)" id="eePosX">
      <input type="text" aria-label="ee-y-pos" class="form-control" placeholder="Y (mm)" id="eePosY">
      <input type="text" aria-label="ee-z-pos" class="form-control" placeholder="Z (mm)" id="eePosZ">
      <button id="changeEE" class="btn btn-primary" type="submit" disabled>Change EE position</button>
    </div>
  </form>
</div>
...

robo.js

...
var servoH1angle = 0;
var servoH2angle = 0;
var servoS4angle = 0;
var servoS3angle = 0;
...
function servoEEgrab(){
	if(dataChannel != null){
		servoS4angle = $("#servoS4").val();
		dataChannel.send(JSON.stringify({'grabEE' : servoS4angle}));
		console.log("Sending EE grab angle = "+ servoS4angle);
		$("#servoS4Info").text("" + servoS4angle);
	}
}

function servoEErotate(){
	if(dataChannel != null){
		servoS3angle = $("#servoS3").val();
		dataChannel.send(JSON.stringify({'rotateEE' : servoS3angle}));
		console.log("Sending EE rotate angle = "+ servoS3angle);
		$("#servoS3Info").text("" + servoS3angle);
	}
}

function changeEEposition(){
	if(dataChannel != null){
		var xPosition = $("#eePosX").val();
		var yPosition = $("#eePosY").val();
		var zPosition = $("#eePosZ").val();
		dataChannel.send(JSON.stringify({'positionEE' : [xPosition, yPosition, zPosition]}));
		console.log("Sending EE position = x:" + xPosition + " y:" + yPosition +" z:"+ zPosition);
	}
}
...

Исполнительный компонент(Python-скрипт на RPI)

Дополняем наш основной скрипт:

part5.py

# добавляем методы из testIK.py
# add_arm_pos_x() / correction_add_x()
# sub_arm_pos_x() / correction_sub_x()
# add_arm_pos_y() / correction_add_y():
# sub_arm_pos_y() / correction_add_y()
# add_arm_pos_z()
# sub_arm_pos_z()

# обрабатываем сообщения из datachannel
async def consume_message(rtc_message: str) -> None:
    rtc_msg = json.loads(rtc_message)
    if rtc_msg.get("blink"):
        if rtc_msg.get("blink").startswith("ON"):
            GPIO.output(21,True)
        if rtc_msg.get("blink").startswith("OFF"): 
            GPIO.output(21,False)
    if rtc_msg.get("headX"):
        xServoAngle = int(rtc_msg.get("headX"))
        servoX.angle = xServoAngle
    if rtc_msg.get("headY"):
        yServoAngle = int(rtc_msg.get("headY"))
        servoY.angle = yServoAngle
    if rtc_msg.get("grabEE"):
        eeGrabAngle = int(rtc_msg.get("grabEE"))
        servoS4.angle = eeGrabAngle
    if rtc_msg.get("rotateEE"):
        eeRotateAngle = int(rtc_msg.get("rotateEE"))
        servoS3.angle = eeRotateAngle
    if rtc_msg.get("positionEE"):
        xyz = [int(i or 0) for i in list(rtc_msg.get("positionEE"))]
        if xyz[0] > 0:
            while xyz[0] > 0:
                add_arm_pos_x()
                xyz[0] -= 1
        if xyz[0] < 0:
            while xyz[0] < 0:
                sub_arm_pos_x()
                xyz[0] += 1
        if xyz[1] > 0:
            while xyz[1] > 0:
                add_arm_pos_y()
                xyz[1] -= 1
        if xyz[1] < 0:
            while xyz[1] < 0:
                sub_arm_pos_y()
                xyz[1] += 1
        if xyz[2] > 0:
            while xyz[2] > 0:
                add_arm_pos_z()
                xyz[2] -= 1
        if xyz[2] < 0:
            while xyz[2] < 0:
                sub_arm_pos_z()
                xyz[2] += 1

Если кратко – то мы перенесли логику работы из скрипта теста IK и немного изменили обработку сообщений в datachannel(теперь они извлекаются из JSON).

Все, можем проводить тест и видим, что мы можем управлять положением EE в пространстве.

Управляющий компонент(Unity VR)

Для дальнейших тестов мне понадобится статистика по RTT/Jitter. Поэтому сначала реализую ее, добавим на Robo Video Panel два текстовых элемента с тэгами, и обработаем добавление статистики в скрипте

Connection.cs

...
private Text rttVal;
private Text jtrVal;
private double rtt;
private double jtr;
...
  
void Start()
{
  ...
    rttVal = GameObject.FindGameObjectWithTag("rttVal").GetComponent<Text>();
    jtrVal = GameObject.FindGameObjectWithTag("jtrVal").GetComponent<Text>();
  ...
  
void Update()
{
...
  if (webrtcConnection != null)
  {
    elapsedStatistics += Time.deltaTime;
    if (elapsedStatistics >= 1f)
    {
      elapsedStatistics = 0f;
      StartCoroutine(GetStatistics());
    }
  }
  rttVal.text = "RTT=" + rtt * 1000 + " ms";
  jtrVal.text = "Jitter=" + jtr * 1000 + " ms";

...
}

IEnumerator GetStatistics()
{
  var statsOperation = webrtcConnection.GetStats();
  yield return statsOperation;
  var statsReport = statsOperation.Value;
  
  foreach (var x in statsReport.Stats)
  {
  if (x.Value is RTCInboundRTPStreamStats)
  {
    x.Value.Dict.TryGetValue("jitter", out object jitter);
    if (jitter != null) jtr = (double) jitter;
  } 
  if (x.Value is RTCIceCandidatePairStats)
  {
    x.Value.Dict.TryGetValue("currentRoundTripTime", out object currentRoundTripTime);
    if (currentRoundTripTime != null) rtt = (double) currentRoundTripTime;
  }
  }
}

Тут мы каждую секунду в апдейте вызываем GetStatistics, в котором получаем статистику по WebRTC, и используем значения jitter из RTCInboundRTPStreamStats и currentRoundTripTime из RTCIceCandidatePairStats. Теперь при приеме видеотрака у нас будет отражаться RTT/Jitter внизу экрана VR.

Для реализации переключения на отслеживание контроллера будем действовать аналогично обработке переключения камеры. В скрипте Connection.ws

Connection.ws

...

private bool controllerTrackingOn = false;
private short controllerTrackingCounter = 0;
private int grabEE;
private int rotateEE;
public Vector3 positionEE;
private float elapsedStatistics = 0f;

...
  
public void ControllerTracking(bool trigger)
{
    if (trigger && !controllerTrackingOn)
    {
        controllerTrackingCounter++;
        if (controllerTrackingCounter >= 100)
        {
            controllerTrackingOn = true;
            Debug.Log("Controller tracking enable!");
        }
    }
    else if (trigger && controllerTrackingOn)
    {
        controllerTrackingCounter--;
        if (controllerTrackingCounter <= 0)
        {
            controllerTrackingOn = false;
            Debug.Log("Controller tracking disable!");
        }
    }
}
...

А в скрипте RightControllerButtonWatcher.сs нам нужно добавить обработку нажатия GripButton, 2D вектор для отслеживания джойстика, кватернион для отслеживания ротации и 3D вектор для отслеживания перемещения контроллера в пространстве:

RightControllerButtonWatcher.сs

...
public static Vector2 rightController2DAxisPosition;
public static Quaternion rightControllerRotation;
public static Vector3 rightControllerPosition;
...
  
void Update()
{
  ...
  if ((rightHandCantroller.TryGetFeatureValue(CommonUsages.gripButton, out gripValue) && gripValue))
  {
    gameObject.SendMessage("ControllerTracking", gripValue);
  }
  
  rightHandCantroller.TryGetFeatureValue(CommonUsages.primary2DAxis, out rightController2DAxisPosition);
  rightHandCantroller.TryGetFeatureValue(CommonUsages.deviceRotation, out rightControllerRotation);
  rightHandCantroller.TryGetFeatureValue(CommonUsages.devicePosition, out rightControllerPosition);
  ...
}

Далее, нам необходимо только добавить отправку сообщения при изменении значений вращения головы/накопителя захвата манипулятора/поворота манипулятора и смены положения контроллера.

Connection.cs

void Update()
{
  if (dataChannel != null)
  {
    elapsed += Time.deltaTime;
    if (elapsed >= 0.1f)
    {
      elapsed = 0f;
      int xheadbuf = HeadRotation.valHeadX;
      int yheadbuf = HeadRotation.valHeadY;
      float joystickbuf = RightControllerButtonWatcher.rightController2DAxisPosition.x;
      float rotbuff = RightControllerButtonWatcher.rightControllerRotation.z;
      Vector3 posbuf = RightControllerButtonWatcher.rightControllerPosition;

      var message = new DcMessage();

      if (fullScreenOn)
      {
        if (headX != xheadbuf)
        {
          message.headX = xheadbuf.ToString();
          headX = xheadbuf;
        }
  
        if (headY != yheadbuf)
        {
          message.headY = yheadbuf.ToString();
          headY = yheadbuf;
        }
      }

      if (controllerTrackingOn)
      {
        if (joystickbuf > 0.3 && grabEE < 180)
        {
            grabEE += 5;
            message.grabEE = grabEE.ToString();
        }
        if (joystickbuf < -0.3 && grabEE > 60)
        {
          grabEE -= 5;
          message.grabEE = grabEE.ToString();
        }
  
        if (rotbuff > 0.3 && rotationEE > 0)
        {
          rotationEE -= 5;
          message.rotateEE = rotationEE.ToString();
        }
        if (rotbuff < -0.3 && rotationEE < 180)
        {
          rotationEE += 5;
          message.rotateEE = rotationEE.ToString();
        }
        if (posbuf != positionEE)
        {
          var delta = posbuf - positionEE;
          message.positionEE = new List<int>() { (int)(delta.z * 125), (int)(delta.y * 125), (int)(delta.x * 125) };
          positionEE = posbuf;
        }
      }
      else
      {
        positionEE = posbuf;
      }
      
      dataChannel.Send(JsonUtility.ToJson(message));
      
      }
}
...

...
[Serializable]
public class DcMessage
{
    public string headX;
    public string headY;
    public string grabEE;
    public string rotateEE;
    public List<int> positionEE;
}

Проверка взаимодействия компонентов

Итак, теперь мы можем проверить как управляется наш манипулятор из Unity.

проверка:

Установка приложения на Oculus

Ранее мы проводили все тесты через Oculus Link + Unity, но в целевой картинке управлять нашим манипулятором хочется сидя на диване, поэтому нам нужно собрать отдельное приложение и установить его, но для начала, есть одна интересная особенность с портом 9000 - на нем не заработает(у меня смутное ощущение, что я сталкивался с такой особенностью в Android, но хоть убей не смог вспомнить детали), по этому сменим порт websocket на :9090 во всех компонентах!

Теперь сделаем настройку сборки в Unity

И все, по нажатию Build&Run Unity сделает сборку проекта, установит его на Oculus и запустит приложение. Можете устраиваться поудобнее в кресле и привыкать у управлению девайсом из VR, именно таким образом сделано видео в начале первой статьи.

Привыкнуть к управлению можно примерно за 10-15 минут использования, однако низкое разрешение камеры изрядно портит впечатление, так же чувствуется недостаток стерео восприятия для определения расстояний до объектов и маленькая рабочая область манипулятора.

Тестирование на задержки

Еще один вопрос, который меня очень интересовал – как это решение будет работать с задержками/джиттером в сети, и как это скажется на управляемости и вообще впечатление от удаленного управления. Дабы не разворачивать все получившееся добро в паблик сейчас, будем использовать роутер с OWRT с пакетами tc+netem, которые позволяют изменять значения RTT/Loss/Jitter.

RPI подключим к этому роутеру с OWRT, роутер же wan-ом подключим к домашней сети, в которой будет находиться Quest и сервер сигнализации. Теперь можем изменять настройки на wan owrt и таким образом эмулировать задержки. Ну или можно поставить netem на RPI.

Сами тесты

За идеал я брал значения локальной сети, без дополнительных задержек. RTT = 5ms. Ощущения от управления замечательные, задержка практически не чувствуется отзывчивость великолепная, любые манипуляции с предметами выполняются вполне уверенно.

tc qdisc add dev eth0.2 root netem delay 50ms

или

tc qdisc change dev eth0.2 root netem delay 75ms

На значении RTT = 100-150ms где-то середина по ощущениям, не сказать, что отлично, но вертеть головой, выкладывать кубики или проводить простые манипуляции с предметами достаточно просто.

tc qdisc change dev eth0.2 root netem delay 100ms

На значениях в RTT = 200ms задержка уже ощущается сильно, чувствуется лаг между самим действием и реакцией манипулятора, к управлению нужно привыкать, но еще на самом деле вполне можно управлять устройством, просто это выходит дольше и каждое движение нужно сильнее контролировать.

tc qdisc change dev eth0.2 root netem delay 150ms

На значениях в RTT = 300 я решил закончить, тут уже задержка на мой взгляд не приемлемая для использования как есть, скорее всего при таких сетевых параметрах придется использовать другие механики обработки клиентских действий, например отдельные команды манипулятору на изменение положения на какое-то расстояние.

Так же можем попробовать изменять jitter:

tc qdisc add dev eth0.2 root netem delay 50ms 10ms

На счет увеличения jitter ситуация печальнее, повышение значения сетевого jitter на каждые 10ms кратно ухудшает восприятие, при значении >40мс видео начинает знатно сыпаться и приходится приостанавливать манипуляции. Loss и реордеринг не тестировал, оставил на потом.

После перехода на ROS опубликую продолжение, для этого у меня специально припасен RPi4. Так же есть желание поставить этого «однорукого бандита» на колеса или ходули, пусть перемещается по комнате, будет приносить кофе (хотя с этим пунктом у меня пока большие сложности в выборе платформы для передвижения).

Управляем роботами из VR. Продолжение 2