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)

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

Let’s check which Timer is connected to PB14

Then we configure timer 1.

Also remember to configure PB12 (SW4) as GPIO_Input.

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

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

Some useful documentation on the configuration register:

Datasheet for HD44780: https://www.sparkfun.com/datasheets/LCD/HD44780.pdf

LCD Controller Pin

1. VSS (Pin 1): Ground (GND).
2. VDD (Pin 2): Power supply (usually +5V).
   Note: If you use J-link debugger to power the board, the output voltage at +5V pin will not be 5V, maybe around 2.8V, so remember to use the 5V power supply wire to power the board.
3. VO (Pin 3): Contrast adjustment pin. Usually connected to a potentiometer to adjust the contrast.
4. RS (Pin 4): Register select pin. Low for instruction register, high for data register.
5. RW (Pin 5): Read/Write pin. Low for write operation, high for read operation. Usually grounded (write mode).
6. E (Pin 6): Enable pin. The LCD controller only captures (grabs) the data presented at its register lines(D0-D7) only when the E pin “transitions” from high to low.
7. D0-D7 (Pins 7-14): Data bus pins. Used for communication with the microcontroller in either 4-bit (D4-D7) or 8-bit (D0-D7) mode.
8. A (Pin 15): For the backlight. Typically connected to +5V.
9. K (Pin 16): For the backlight. Typically grounded (GND).

Connection with STM32

Some Useful Charts

From datasheet

1. Four operation
   
   **IR: instruction register** **DR: data register**

In this Lab, we will mainly use the first and third operation

2. Instruction

   
   

3. DDRAM

   

4. CGRAM

   

Application Example

Send Command to LCD Controller

Recall the operation:

1. Set the pin of RS & R/W to low.
2. Set E to high, preparing for the data transfer.
3. Set D7-D0 to the desired instruction(8-bit).
4. Set E to low

Sample code:

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

void LCD_Write_Command(uchar Com) {
  Delay_ms(10);
  HAL_GPIO_WritePin(GPIOB, LCD_RS_Pin, GPIO_PIN_RESET); // LCD_RS = 0;
  HAL_GPIO_WritePin(GPIOB, LCD_RW_Pin, GPIO_PIN_RESET); //	LCD_RW = 0;
  HAL_GPIO_WritePin(GPIOB, LCD_E_Pin, GPIO_PIN_SET);    // LCD_E_Pin = 1;
  Delay_ms(1);                                          // wait for tpw;

  LCD_PORT = Com;// put command on the bus

  HAL_GPIO_WritePin(GPIOB, LCD_E_Pin, GPIO_PIN_RESET); // LCD_E_Pin =0;
  Delay_ms(1);                                         // wait for tpw;

}

Write Data To DDRAM

Recall the operation:

1. Set the pin of RS to high and R/W to low.

2. Set E to high, preparing for the data transfer.

3. Set D7-D0 to the desired address.

   

4. Set E to low

Sample code:

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

void LCD_Write_Data(uchar dat) {
  Delay_ms(1);

  HAL_GPIO_WritePin(GPIOB, LCD_RS_Pin, GPIO_PIN_SET);   // LCD_RS = 1;
  HAL_GPIO_WritePin(GPIOB, LCD_RW_Pin, GPIO_PIN_RESET); //	LCD_RW = 0;
  HAL_GPIO_WritePin(GPIOB, LCD_E_Pin, GPIO_PIN_SET);    // LCD_E_Pin = 1;
  // Delay_ms(1);

  LCD_PORT = dat; // put data on the bus

  HAL_GPIO_WritePin(GPIOB, LCD_E_Pin, GPIO_PIN_RESET); // Set LCD_E = 0;

  Delay_ms(1); // wait for tpw;
}

Init LCD

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

#define LCD_2_LINE_4_BITS 0x28
#define LCD_2_LINE_8_BITS 0x38
#define LCD_DSP_CSR 0x0c
#define LCD_CLR_DSP 0x01 
#define LCD_CSR_INC 0x06

void LCD_init(void)
{
  LCD_Write_Command(LCD_2_LINE_8_BITS); // function set -8 bit interface
  Delay_ms(5);
  LCD_Write_Command(LCD_2_LINE_8_BITS); 
  
  LCD_Write_Command(LCD_CLR_DSP); // clear display
  Delay_us(100);
  LCD_Write_Command(LCD_CSR_INC); // Set entry mode: increment
  Delay_us(100);
  LCD_Write_Command(LCD_DSP_CSR); // open display, close cursor
}

Display Charactor

First we need to set the cursor position

The cursor position is basically the DDRAM address that you want to write data in, which is corresponding to the display location on LCD screen.

1
2
3
4
5
6
7
8
9

void LCD_Set_Position(uchar x, uchar y) {
  if (y == 0) // first line
  {
    LCD_Write_Command(0x80 + x);
  } else if (y == 1) // second line
  {
    LCD_Write_Command(0xc0 + x);
  }
}

If we want to display a character at first line, first column:

1

LCD_Set_Position(0,0);

Then, we need to write data into DDRAM[addr] to tell the LCD which character you want to display, for example “J”.

1

LCD_Write_Data("J"); // send the ascii code

Putting them all together:

1
2
3
4
5

void LCD_Display_Char(uchar Char, uchar x, uchar y)
{
  LCD_Set_Position(x, y);
  LCD_Write_Data(Char);
}

Display String

The cursor will shift right (because we set increment entry mode) after each write_data operation, so displaying a string is quite simple.

1
2
3
4
5

void LCD_Display_Char(uchar Char, uchar x, uchar y) // 显示字符ASCII码
{
  LCD_Set_Position(x, y);
  LCD_Write_Data(Char);
}

Demo

…

Output Compare Module

OC is a peripheral that can generate precise output signal independent of the processor (Core).

Drive high, drive low, toggle

Dual Compare

Single pulse, continuous.

PWM