项目框架

以下为项目布局示意图

（左下角为深度相机）

以下为项目的架构图

具体解释一下这张图

• 首先用户佩戴 hololens，hololens 会先把可交互的物体（物体数据源于物体检测算法）渲染在 UI 上，之后用户基于这些视觉信息，给出交互指令。

• 随后系统会根据给到的指令和可交互物体的信息，规划机械臂的运动路径。

• 路径信息会首先传到数字孪生系统中。该系统中有三部分孪生，分别是人体（实时数据来自人体检测算法），环境（实时数据来自 hololens 的环境感知系统） 和 机械臂（来自于前面提到的路径信息）的孪生。

• 随后该 DT 系统会根据路径信息解算在机械臂运动过程中是否会发生碰撞。

  • 如果发生碰撞
    • 会把碰撞发生的位发还给 hololens 显示，用户可以选择取消指令或者选择移走遮挡的物体（）。
  • 如果不发生碰撞
    • 会把机械臂的预计运动轨迹发回给 hololens 显示，用于警戒用户。
    • 将运动轨迹发给机械臂控制器，控制器控制机械臂运动。

Python-Unity Network

Python 和 Unity 间通讯

https://github.com/Siliconifier/Python-Unity-Socket-Communication

Demo 设计

一、需要实现的功能：

• 机械臂根据工人指示移动物体
• 机械臂运动意图在 Hololens 中的渲染
• 人体/机械臂轨迹的记录，用于评估（？）

二、具体 demo

机械臂移动物体到指定位置

首先 RGB-D 相机识别出桌面上的某一个物体，反馈到 hololens 的界面中，用户会看到对应物体上渲染出来了一个边界框和表示位姿的坐标轴：

随后用户可以通过手部的射线来移动这个目标框（物体上的检测框依然会存在）到某个位置，然后目标框上会显示一个 UI 询问是否要把物体移动到这里，如果点击确认，机械臂就会把对应的物体到目标框的位置。

可以在路径中设置一些障碍，展示机械臂障碍识别的功能。

机械臂移动物体到工人手上

用户凝视一个物体时显示一个界面，即是否要将操作员注视的物体移动到他的手上，如果手腕翻起，即为确认。随后机械臂会将该物体移动到手上。

自定义 Gen6d 物体

仓库：https://github.com/liuyuan-pal/Gen6D

手册：https://github.com/liuyuan-pal/Gen6D/blob/main/custom_object.md

步骤指令：

1
2
3
4
5
6
7
8
9

python prepare.py --action video2image --input data/custom/part1/ref.mp4 --output data/custom/part1/images --frame_inter 10 --image_size 960  

python prepare.py --action sfm --database_name custom/part1 --colmap D:\COLMAP\COLMAP-3.7-windows-cuda\COLMAP.bat        

# do some cloudcompare thing

python predict.py --cfg configs/gen6d_pretrain.yaml --database custom/part1 --video data/custom/part1/test.mp4 --resolution 460 --output data/custom/part1/test --ffmpeg ffmpeg

ffmpeg -framerate 30 -i %d-bbox.jpg -c:v libx264 -r 30 -pix_fmt yuv420p output.mp4

关于判定不准确怎么解决：https://github.com/liuyuan-pal/Gen6D/issues/29

6d pose -> unity coordinate

unity 使用左手坐标系，普遍的 6d 算法使用右手坐标系，所以得出[R;t]后需要做一步针对 y 轴的反射变换

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

def right_to_left_hand_pose_R(R):
    # 定义反射矩阵
    M = torch.tensor([
        [ 1,  0,  0],
        [ 0, -1,  0],
        [ 0,  0,  1]
    ], dtype=R.dtype)
    
    # 转换旋转矩阵
    R_prime = M @ R @ M
    
    return R_prime

def right_to_left_hand_pose_t(t):
    # 定义反射矩阵
    M = torch.tensor([
        [ 1,  0,  0],
        [ 0, -1,  0],
        [ 0,  0,  1]
    ], dtype=t.dtype)

    # 转换位移向量
    t_prime = M @ t

    return t_prime

可以看到效果很好：

6-D Pose Estimation Survey

Model based (CAD model)

State of The Art: Foundation Pose (https://github.com/NVlabs/FoundationPose)

RGB

CASAPose (https://github.com/fraunhoferhhi/casapose?tab=readme-ov-file)

MegaPose (https://github.com/megapose6d/megapose6d)

D-RGB

MegaPose (https://github.com/megapose6d/megapose6d)

OVE6D (https://github.com/dingdingcai/OVE6D-pose)

Non-model

Gen6D (https://github.com/liuyuan-pal/Gen6D)

Yolov5 (https://github.com/cviviers/YOLOv5-6D-Pose)

夹爪控制方式

使用 pyRobotiqGripper

但是只兼容 linux 电脑的串口，所以部署在笔记本上并且创建一个局域网服务器供台式机调用。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

import pyRobotiqGripper
import time
from flask import Flask, request

app = Flask(__name__)
gripper = pyRobotiqGripper.RobotiqGripper()
gripper.activate()

@app.route('/open_gripper', methods=['POST'])
def open_gripper():
    # 执行脚本
    # import subprocess
    # subprocess.call(['/path/to/your/script.sh'])
    gripper.open()
    return 'gripper open'
@app.route('/close_gripper', methods=['POST'])
def close_gripper():
    # 执行脚本
    # import subprocess
    # subprocess.call(['/path/to/your/script.sh'])
    gripper.close()
    return 'gripper closed'

if __name__ == '__main__':
    app.run(host='0.0.0.0', port=5000)

# gripper = pyRobotiqGripper.RobotiqGripper()
# gripper.activate()
# gripper.close()
# gripper.open()
# time.sleep(3)
# gripper.close()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

(base) cyl@arch ~/450> python gripper_test.py
Activation completed. Activation time :  1.9049019813537598
 * Serving Flask app 'gripper_test'
 * Debug mode: off
WARNING: This is a development server. Do not use it in a production deployment. Use a production 
WSGI server instead.
 * Running on all addresses (0.0.0.0)
 * Running on http://127.0.0.1:5000
 * Running on http://192.168.2.111:5000
Press CTRL+C to quit
192.168.2.103 - - [20/Jul/2024 16:26:01] "POST /open_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:05] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:07] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:10] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:13] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:16] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:18] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:21] "POST /close_gripper HTTP/1.1" 200 -
192.168.2.103 - - [20/Jul/2024 16:26:24] "POST /close_gripper HTTP/1.1" 200 -

台式机通过

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

# 定义要发送的命令和URL
laptop_ip = "192.168.2.111"
close_url = "http://"+laptop_ip+":5000/close_gripper"
open_url = "http://"+laptop_ip+":5000/open_gripper"

# 使用curl命令通过POST请求发送命令
curl_close_command = [
    'curl', 
    '-X', 'POST', 
    close_url, 
]

curl_open_command = [
    'curl', 
    '-X', 'POST', 
    open_url, 
]

print(subprocess.run(curl_open_command, capture_output=True, text=True))

来控制

Robotic Arm Digital Twin(DT)

机械臂 DT 需要的功能：

• 有和现实机械臂相似的模型外观
• 有碰撞箱
• 有基本的关节旋转控制脚本
• 接受路径规划单元传来的控制数据

模型外观

三个模型资产，分别从

导入。

碰撞箱

主要由 capsule collider 构成。整个 DT 的 layer 属于 RobotArm

关节控制脚本

挂载在 robotarm 物体上，用于控制各个关节旋转的角度

其中 RotateDir 为一次性配置，Joint 会自动检测进行配置

控制时只需要更改 Rotate 的值即可（runtime）

IK Path Planning

IK Solver

用于将关键点数据转化为关节动作（各关节旋转角度）

希望机械臂的末端始终竖直。方便夹取。

Unity IK: https://github.com/2000222/Robotic-Arm-IK-in-Unity

自定义自由度，非常好用，基于关节梯度迭代的方法

由这个 IK 算法负责机械臂前三个 joint 的旋转，确保 j4 的 anchor（红点）处于物体后上方的特定位置。

j4 位置的计算方法：

1
2
3
4
5
6
7
8
9
10
11

public Vector3 GetPositionForJ4(Vector3 pos)
    {
        Vector3 forward = pos - Joints[0].transform.position;
        forward.y = 0;
        float angle = Mathf.Acos(0.1148f / forward.magnitude);
        Vector3 left = Quaternion.AngleAxis(-angle * 180f/Mathf.PI, Vector3.up) * forward;
        left = left.normalized * 0.1148f;
        Vector3 offset = forward - left;
        offset = offset.normalized * 0.1175f;
        return pos + Vector3.up * 0.2446f - offset;
    }

之后由向量计算得出 j4 应该旋转的角度，以确保末端竖直。

j5 不旋转，j6 的旋转角度由物体朝向决定。

Router

Route

一条路径由若干个关键点构成，在构建路径时只需输入若干 turning point，就会自行生成路径中的关键点。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

public class KeyPoint
{
    public Vector3 pos = Vector3.zero;
    public bool grab = false;
    public bool wait = false;
    public KeyPoint(Vector3 pos, bool grab = false, bool wait = false)
    {
        this.pos = pos;
        this.grab = grab;
        this.wait = wait;
    }
}

public class Route
{
    public List<KeyPoint> routes = new List<KeyPoint>();
    public Route(List<KeyPoint> turningPoints)
    {
        if (turningPoints.Count < 2)
        {
            return;
        }
        for (int i = 1; i < turningPoints.Count; i++)
        {
            KeyPoint start = turningPoints[i-1];
            KeyPoint end = turningPoints[i];
            float dist = (start.pos - end.pos).magnitude;
            int sigment = (int)(dist * 20f);
            for (int j = 0; j <= sigment; j++)
            {
                routes.Add(new KeyPoint(start.pos + ((float)j) / sigment * (end.pos - start.pos), start.grab));
            }
        }
    }
}

IK 只需要获取关键点的坐标信息就可以生成各个关节旋转的数据。

1

InverseKinematics(GetPositionForJ4(position));

拟题：
基于混合现实的智能体交互

具体功能：
通过hololens2获取场景数据（识别小车/其他物体的位置，网格信息）。
通过hololens2的立体空间交互能力，指挥小车进行移动，执行任务（譬如抓取柱体并移动到相应位置）。

• 关于移动，可以是在眼镜中通过手部动作画一条路径
• 关于执行任务，可以是眼镜识别出物体的位置后规划小车移动和动作序列(A*or强化学习)->相关信息发给小车->小车收到后执行
  通过hololens2的虚拟物体放置功能，譬如可以放置小车的车库，柱体移动到的目标位置等
  可以通过hololens2显示小车将要移动的意图