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));

Why

因为觉得hexo post的图片排版太烂了，手打html插入图片又很烦，于是乎决定写一个hexo的插件，能够通过一些关键字，识别图片组，然后再把这些图片以照片墙的方式组合在一起。
顺带可以了解一下hexo的渲染管道和javascript。

How

Hexo 渲染顺序

1. 初始化阶段：

   • Hexo 读取配置文件和初始化插件。
   • 加载主题和其配置文件。

2. 内容处理阶段：

   • before_generate：生成前触发，可用于在生成前进行一些准备工作。
   • before_post_render：在文章渲染之前触发，可用于在文章内容渲染之前对其进行修改。
   • after_post_render：在文章渲染之后触发，可用于在文章内容渲染之后对其进行修改。
   • before_exit：Hexo 退出前触发，可用于在退出前进行一些清理工作。

3. 生成阶段：

   • 生成静态文件，包括 HTML、CSS、JavaScript 等。
   • 将内容写入 public 目录。

4. 部署阶段：

   • 部署生成的静态文件到目标服务器或平台

我们需要做的就是在内容处理阶段，重新生成图片墙部分的html代码。

Hexo Filter

过滤器的工作原理

过滤器是通过 hexo.extend.filter.register() 方法来注册的。每个过滤器都绑定在特定的生命周期事件上，当这些事件触发时，Hexo 会依次执行所有注册在该事件上的过滤器。

• before_generate：生成站点之前触发。
• before_post_render：在文章内容渲染之前触发。
• after_post_render：在文章内容渲染之后触发。
• before_exit：Hexo 进程退出之前触发。
• after_render：HTML 内容渲染之后触发。
• after_render：CSS 内容渲染之后触发。
• after_render：JavaScript 内容渲染之后触发。

  1 2 3 4 5

  hexo.extend.filter.register('after_post_render', function(data) { // 查找所有 img 标签并将 src 属性替换为 HTTPS 协议 data.content = data.content.replace(/<img src="http:\/\/(.*?)"/g, '<img src="https://$1"'); return data; });

过滤器的返回值

• 对于大多数过滤器函数，需要返回处理后的数据。例如，after_post_render 过滤器需要返回修改后的文章内容。
• 某些过滤器（如 before_exit）不需要返回值。

实现

首先创建一个存放插件的文件夹，我放在hexo_root/plugins/hexo-photo-wall
文件夹中创建一个js入口文件index.js
文件中注册filter并且实现替换功能

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
36
37
38
39
40
41
42

hexo.extend.filter.register('before_generate', () => {
  console.log('This is my custom Hexo plugin!');
});
hexo.extend.filter.register('before_post_render', function(data) {
  const startTag = '
';

  const regex = new RegExp(`${startTag}[\\s\\S]*?${endTag}`, 'g');

  data.content = data.content.replace(regex, match => {
    const pictures = match
      .replace(startTag, '')
      .replace(endTag, '')
      .trim();

    const pictureArray = pictures.split('\n').map(item => item.trim()).filter(item => item);

    pictureArray.forEach(picture => {
        const srcMatch = picture.match(/!\[.*?\]\((.*?)\s+\"(.*?)\"\)/);
        if (srcMatch && srcMatch[1]) {
        console.log(`Found image: ${srcMatch[1]}, title: ${srcMatch[2]}`);  // 输出匹配到的图片路径和标题
        }
    });

    let pictureWallHtml = '<div class="picture-wall">';

    pictureArray.forEach(picture => {
      const srcMatch = picture.match(/!\[.*?\]\((.*?)\)/);
      if (srcMatch && srcMatch[1]) {
        pictureWallHtml += `<div class="picture-wall-item"><img src="${srcMatch[1]}" alt=""></div>`;
      }
    });

    pictureWallHtml += '</div>';

    console.log(pictureWallHtml);

    return pictureWallHtml;
  });

  return data;
});

此处的照片墙的排版和原本应该并没有区别，只是处于同一个class="picture-wall"的div下。后续需要通过修改css或者直接更改html来实现照片墙的效果。

For this RC, we will go through Lab 4 in detail.

CubeMX

We need to generate PWM signal on PB14(embeded with an LED D1, or D2 if you like)

when the PB14 output LOW, D1 will light up.

In STM32, Input Capture and Output Compare is configured in Timer channels.

Let’s check which Timer is connected to PB14

It’s the CH2N channel of timer 1.

Then we configure timer 1.


Prescaler: 7200-1
Counter Period: 200-1 (50Hz)
Pulse: 180 (duty cycle of 90%; TIM_CNT 0179: HIGH, TIM_CNT 180199: LOW)
CHN Polarity: LOW (needed for CHN)

CHxN is the complementary channel of CHx in Timer1.
If the polarity of CHxN and CHx are different, the two channels will follow the same output pattern, otherwise they will be complementary

Also remember to configure PB12 (SW4) as GPIO_Input.

You can try to use Input Capture to capture the press action of the button (as indicated in lab manual, i.e. connecting PB12 to PA0).

Code

Init PWM in main():

1
2
3
4

/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
/* USER CODE END 2 */

Change PWM duty cycle:

1
2
3
4
5

void ChangeDutyCycle(int pulse_width)
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pulse_width);
    // htim1.Instance->CCR2 = pulse_width;
}

Get the press/release action of the button (PB12):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

int pressed = 0;
   /* USER CODE BEGIN WHILE */
   while (1) { // main loop
       /* USER CODE END WHILE */
       /* USER CODE BEGIN 3 */
       if (!HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) && pressed == 0) {
           pressed = 1;
           // what to do when pressed
       }
       else if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) && pressed == 1) {
           pressed = 0;
           // what to do when releassed
       }
   }
   /* USER CODE END 3 */

Input Compare Module

IC is a peripheral that can monitor the input signal change (pos/neg edge) independent of the processor (Core).

You can consider IC as a timer value recorder. It will record the timer value each time the capture condition is met.

For detailed configuration, please refer to Reference Manual.pdf on canvas, page 349-359, 382-385.

Some useful documentation on the configuration register: