Life goes on and throb lasts forever

先放两首诗

泰戈尔的《生如夏花》

我听见回声，
来自山谷和心间
以寂寞的镰刀收割空旷的灵魂
不断地重复决绝，
又重复幸福
终有绿洲摇曳在沙漠
我相信自己
生来如同璀璨的夏日之花
不凋不败，
妖冶如火
承受心跳的负荷和呼吸的累赘
乐此不疲
——
我听见音乐，
来自月光和胴体
辅极端的诱饵捕获飘渺的唯美
一生充盈着激烈，
又充盈着纯然
总有回忆贯穿于世间
我相信自己
死时如同静美的秋日落叶
不盛不乱，
姿态如烟
即便枯萎也保留丰肌清骨的傲然
玄之又玄
——
我听见爱情，
我相信爱情
爱情是一潭挣扎的蓝藻
如同一阵凄微的风
穿过我失血的静脉
驻守岁月的信念
——
我相信一切能够听见
甚至预见离散，
遇见另一个自己
而有些瞬间无法把握
任凭东走西顾，
逝去的必然不返
请看我头置簪花，
一路走来一路盛开
频频遗漏一些，
又深陷风霜雨雪的感动
——
般若波罗蜜，
一声一声
生如夏花之绚烂，
死如秋叶之静美
还在乎拥有什么

莎士比亚十四行诗其18

Shall I compare thee to a summer’s day?
Thou art more lovely and more temperate:
Rough winds do shake the darling buds of May,
And summer’s lease hath all too short a date:
——
Sometime too hot the eye of heaven shines,
And often is his gold complexion dimm’d;
And every fair from fair sometime declines,
By chance or nature’s changing course untrimm’d
——
But thy eternal summer shall not fade
Nor lose possession of that fair thou owest;
Nor shall Death brag thou wander’st in his shade,
When in eternal lines to time thou growest:
——
So long as men can breathe or eyes can see,
So long lives this and this gives life to thee
——
译：
可否把你比作夏天？
你比夏天更可爱温婉
烈风吹散了五月的花蕾
且相约有期，夏日苦短
有时盛夏之目光灼烤
金色的面容失去了光彩
一切美丽终将凋零
偶然，或自然规律的安排
可你的夏季永不衰败
天生丽质，娇颜不改
死的阴影也无法将你遮蔽
永恒的诗中你将常在
只要人呼吸，眼睛能阅读，
此诗长存，而你，青春永驻

L2 (Embedded system overview)

Take a look at the differences between Microprocessor (in our personal computer or phone) and Microcontroller

In embedded system, harvard architecture is widely used.
Our board (STM32F103C8T6) use harvard architecture on physical level (refer to the block diagram in reference manual)

However, in the software level, we treat the instruction memory and data memory as a whole block of memory (therefore, it is more accurate to say that the stm32 uses a mixed Harvard and von Neumann architecture.).

In stm32, instruction memory, data memory, registers of peripherals/IO are all mapped to memory.

L3 (Programming)

Type Qualifiers

const

• implies that value not supposed to be written by program (read only) during run-time. Can be modified by others like hardware.

  If you want to save the limited RAM spaces (data memory) for other variables, you can use this keyword to store this variable in ROM (program memory). It’s important for harvard architecture.

volatile

• indicate the value can be changed by something other than program so it should be reexamined frequently.

  This means two things:

  • The compiler will not try to optimize the variable with volatile. See the two examples on slides.
  • Each time the program reads the volatile variable, the processor will not look into cached data memory, meaning that the program can always get the newest updated data in memory (which is very important when external hardware change the variable). However, this case is not relevant with STM32 MCU since it didn’t have cache.

What about const volatile ?

• The combination of the above two concepts. Usually used to declare pointers
  Example:
  const volatile char *a declares a pointer pointing to a value that cannot be changed by the program through *a, but the value of a can be changed (pointing to another value). *a = 0 is not allowed, a = &b is allowed.

Generally, we use const volatile to declare pointers that points to hardware registers or memory-mapped Input ports(read only).

Basic Program Structure

1
2
3
4
5
6
7
8
9
10
11

// import header file for the board (containing the declaration of SFR)
// declare global variables
int main(void){
	// initialization (system clock, peripherals)
	while(true) // super loop, the the program alive
	{
		// interact with peripherals
	}

}

How to interact with peripherals

We need to use C code to set the value of SFR.

SFR (special function registers)

These are registers that are embedded in peripherals, used for configuration and control of peripherals.

VERBOSE~
If we want to get the status of a peripheral, we read the value of SFR.
If we want to send something to peripheral, we write value to SFR.

Let’s take timer as an example:
SFR in block diagram of timer:

SFR declaration in code:

We change operate with these registers through Bit Operation.

L4 (IO)

All the modes of GPIO:

You may see an unfamiliar unit

Input

After we configured the GPIO to be input ports, the output driver is disabled (disconnected).

Pull down

Pull up

Floating

General Purpose Output

The input driver part is still enabled so that we can read the output status.

Open Drain

Can “generate” voltage higher than VDD at IO pin.

Push Pull

Most common one.

Alternative Function Output

Not covered.

L5 (Interrupts)

Why interrupt?

Peripherals inform the processor through external interrupt.

Where do interrupts come from

We mainly deal with the interrupts from peripherals.

How to handle interrupts

Through interrupt service routine (ISR)

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

| Vector Number | Interrupt Number | Description                    | Vector Address   |
|---------------|------------------|--------------------------------|------------------|
| 0             | -                | Initial Stack Pointer          | 0x0800 0000      |
| 1             | -                | Reset Handler                  | 0x0800 0004      |
....
| 27            | 11               | DMA1 Channel1 global Interrupt | 0x0800 006C      |
| 28            | 12               | DMA1 Channel2 global Interrupt | 0x0800 0070      |
| 29            | 13               | DMA1 Channel3 global Interrupt | 0x0800 0074      |
| 30            | 14               | DMA1 Channel4 global Interrupt | 0x0800 0078      |
| 31            | 15               | DMA1 Channel5 global Interrupt | 0x0800 007C      |
| 32            | 16               | DMA1 Channel6 global Interrupt | 0x0800 0080      |
| 33            | 17               | DMA1 Channel7 global Interrupt | 0x0800 0084      |
...
| 41            | 25               | TIM1 Break Interrupt           | 0x0800 00A4      |
| 42            | 26               | TIM1 Update Interrupt          | 0x0800 00A8      |
| 43            | 27               | TIM1 Trigger and Commutation   | 0x0800 00AC      |
| 44            | 28               | TIM1 Capture Compare Interrupt | 0x0800 00B0      |
...

This is an arbitrary IVT that depicts the pattern of IVT, for detailed IVT of STM32, please refer to the reference manual.

What to do inside ISR

• Always remember to clear the interrupt flag in ISR.
• Because there is often more interrupt sources than interrupt vectors, you need to judge which source triggered this interrupt based on interrupt status register.
• Customized operation… (but be short, because if the operation take too many clock cycles, it may be interrupted by another interrupt source, which may not be on purpose)

Nested interrupts

L6 (Timer)

Let’s split Timer peripheral into 3 parts:

The reset frequency of the counter is $$\frac{f_{input}}{(Prescaler+1)\times(Counter Period+1)}$$

Red part: Timer-channels unit

The timer channels are the working elements of the timer.
They are the means by which a timer peripheral interacts with its external environment (through input capture or output compare).

How to use multiple timer for more counting digits (32-bit)

L7 (LCD)

Just refer to RC2_LCD. I think it’s not the focus of final exam.

L9 & L10 (Input Capture & Output Compare)

IC is a peripheral that can monitor the input signal changes (pos/neg edge) independent of the processor (Core).
OC is a peripheral that can generate precise output signal independent of the processor (Core).

In STM32, it is embedded in timer peripheral (together with output compare).

IC

You can consider IC as a timer value recorder. It will record the timer value each time the capture condition is met (you can see from the diagram, the timer value is from CNT counter).

These conditions can be:

• rising edge
• falling edge
• both
  With a prescaler, we trigger capture events every few edges.
  Similar for interrupt, we can trigger an interrupt every few captures.

Note:
The IC does not capture the edge immediately when a rising or falling edge happened. The capture event needs to be sync with PB_clk.
Further more, the module will capture the timer counter value that is valid 2-3 PB_clk cycles after the capture event.

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

OC

Just refer to RC2_Output_Campare and RC3_Lab4 for the concepts and PWM configuration.
Also, the solution of hw2 has been uploaded to canvas, please take a look.

项目框架

以下为项目布局示意图

（左下角为深度相机）

以下为项目的架构图

具体解释一下这张图

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

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

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

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

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

程序接口定义

Python->Unity

代码仓库：

https://github.com/Chen-Yulin/Unity-Python-UDP-Communication

传输的数据为字符串：

1
2
3

def SendData(self, strToSend):
    # Use this function to send string to C#
    self.udpSock.sendto(bytes(strToSend,'utf-8'), (self.udpIP, self.udpSendPort))

物体识别信息

需要包含的信息：物体种类，物体的三轴方位，三轴旋转，三轴尺寸。

格式：

1
2
3
4
5
6
7
8
9
10
11

{Object Detection}
category
position.x
position.y
position.z
eulerRotation.x
eulerRotation.y
eulerRotation.z
scale.x
scale.y
scale.z

示例：

1
2
3
4
5
6
7
8
9
10
11

{Object Detection}
Handle
0.1
0.052
0.026
0
90
0
0.5
0.3
1

机械臂实时角度

需要包含的信息：6 个 joint 角度(单位为度)

格式：

1
2
3
4
5
6
7
8

{Current Joint}
j0
j1
j2
j3
j4
j5
j6

Unity->Python

机械臂需要旋转的角度

需要包含的信息：6 个 joint 角度(单位为度)

格式：

1
2
3
4
5
6
7
8

{Target Joint}
j0
j1
j2
j3
j4
j5
j6

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

来控制