# 人机交互 - 想做一个丝滑handover

## 系统框架

调用API的方式，实现人机交互。

Decision模块可以根据感知API的结果，调用控制API，实现人机交互。

Decision先用决策树或者状态机，后面可以换上LLM

API有以下几种：

感知API：人类手部重建✅，人类手部接触分类✅，物体定位✅，物体重建，物体接触识别，物体affordance map

控制API：机械臂运动规划✅，抓夹开合✅

## 一些随想

### 要素1：机器人的感知

- basic: 机器人的规划是基于人的动作的，所以需要实时的人体姿态估计。
- advanced: 机器人的规划是基于人的动作，所以需要人体姿态预测。
- advanced: 机器人的规划是基于人的意图，所以需要人体行为理解。

### 要素2：机器人的规划

- basic: 基于准确的人体三维姿态估计、手眼标定、机器人的运动学来做，用传统的规划算法。
- advanced: 模仿学习，用强化学习来做。

### 要素3：机器人的执行

- basic: 人是否接触到物体，接触到了，机器人才能放手。需要做带有接触标记的人体姿态估计。然后用if-else来控制机器人的动作。
- advanced: 机器人的动作是连续的，需要用强化学习来做。

## 进度

### 10-25

- pick and place的demo跑起来了，那么控制完成，物体定位完成，运动规划完成，抓夹开合完成

<img src="./pic/pickplace.png" width="80%" />

<img src="./pic/objloc.png" width="80%" />

### 10-27

- 写了个错的坐标变换，关键点到处飞哈哈哈

<img src="./pic/hd_ori.gif" width="50%"/><img src="./pic/hd_err_trans.gif" width="50%" />

<img src="./pic/rgbd3dhpe.png" width="80%" />

### 11-10

- 做全身好像没什么必要，用单目估计深度也是有点太难了，先做RGBD估计手部把

- 控制完成，物体定位完成，手部重建完成，运动规划完成，抓夹开合完成

- 尝试以下手部重建方法

❎rtmlib【可运行】：实时但是效果一般，用的nano版本，没有物体的重建

❎hold：看着有点复杂，数据需要预处理https://github.com/zc-alexfan/hold

❎obman【可运行】：实时，效果一般，没有手的detection

❎frankmocap【可运行】：没有物体重建https://github.com/facebookresearch/frankmocap/blob/main/docs/INSTALL.md

❎hamer：没有物体重建，还在报错，暂时弃了https://github.com/geopavlakos/hamer

❎handobjectconsist：https://github.com/hassony2/handobjectconsist

❎contactopt：https://github.com/facebookresearch/ContactOpt/blob/main/docs/INSTALL.md

✅Hand-Motion-Capture【可运行】：实时，效果挺好，没有物体重建

❎minimal-hand【可运行】：实时，没有物体重建，效果不好

Hand-Motion-Capture就用这个了, 加上bstro做手部接触识别

<img src="./pic/handdet.png" width="80%" />

<img src="./pic/hand_det_in_rviz.png" width="80%" />

数据集
https://github.com/zc-alexfan/arctic


## 11-11

- 要做双向的handover

    - H2R：人手、object of interest重建，机器人从哪里抓取应该由dynamic object affordance决定

    - R2H: 人手、object of interest重建，机器人从哪里抓取应该由object affordance决定，此外，还需要判断人对物体的接触

H2R更难，先做R2H

signed-affordance: 正的affordance是可以抓取的，负的affordance是不可以抓取的，或者已经发生接触了

- 手的重建是完成了，但是有遮挡的时候效果一般

- 接触分类也完成了，但是效果一般

<img src="./pic/contact_cls.png" width="50%" /><img src="./pic/contact_cls2.png" width="50%" />

有了以上这些, 最基本，最粗糙的R2H就可以做了，往后继续研究affordance之类的方法

用graspnet生成抓取姿势

detection, request, affordance, grasp, handover

affordance subject: human hand and object of interest

envision the final handover pose and what the robot should do

## 11-17

- 用graspnet生成抓取姿势，在ROS中实现抓起积木并递出去

<img src="./pic/grasp.png" width="50%" />

选取Object of Interest

<img src="./pic/grasp2.png" width="50%" />


task oriented dynamic object affordance

- 对graspnet进行改进，增加任务分类输出：交接任务、操作任务。就是说，在不同的任务设定下，应该使用不同的抓取姿势

    - 如果是操作任务：生成最佳的抓取姿态，然后抓起积木，放到指定位置

    - 如果是交接任务：生成最佳的抓取姿态对

        - R2H: 人类手部检测，执行抓取姿态对中的第一个抓取姿态，然后规划轨迹让姿态对中的第二个抓取姿态与人类的手对齐

        - H2R: 人类手部检测，

- 处理数据集，用于训练graspnet

## 仿真中实现抓取

## 系统框架

<img src="./pic/framework.png" width="80%" />

## 数据集制作

使用GraspNet生成一系列抓取动作，从其中筛选指向相反的动作，作为一个交接姿势

生成一对交接动作

<img src="./pic/handover.png" width="40%" />

下采样，对于不同的任务有不同的接触图

抓取任务的接触图只有一个接触区域

<img src="./pic/bowl1.png" width="40%" /><img src="./pic/cup1.png" width="40%" />


交接任务的接触图有两个区域

<img src="./pic/bowl2.png" width="40%" /><img src="./pic/cup2.png" width="40%" />

## 网络训练

- 推理的结果对任务分类不敏感，不论是交接任务还是操作任务，都是类似的结果

- 似乎没有学习到有效的特征，得出的结果都是类似的

- 怀疑是数据太少，目前只有3000+数据

- 数据集比较粗糙，计算接触图的方法也比较简单，无法很好的表达接触的情况

## 数据集v2制作

### 使用biman grasp数据集生成接触图

<img src="./pic/biman.png" width="50%" />

现在数据集内有900+个物体，15000+个交接抓取姿势，9000+个操作抓取姿势

### 简单的欧式距离不适用于比较薄的物体，无法区分接触的方向

比如说这样一个抓取姿势，抓取比较薄的物体

<img src="./pic/handovergrasp.png" width="30%" />

欧式距离会在背面产生一个很大的接触区域，但是实际上这一面并没有接触（左）

使用align distance可以解决这个问题（右）

<img src="./pic/eucd.png" height="300" />  <img src="./pic/align_dis.png" height="300" />

生成的接触图大概是这样的，抓取动作（上）和交接动作（下）

<img src="./pic/banana.png" height="300" /><img src="./pic/cleanser.png" height="300" />

<img src="./pic/bowl.png" height="300" /><img src="./pic/box.png" height="300" />

## 网络训练v2

- 现在根据不同的输入点云，可以生成不同的接触图了

<img src="./pic/alarm.png" height="300" /><img src="./pic/mustard.png" height="300" /><img src="./pic/cube.png" height="300" />

- 但是仍然对任务分类不敏感，怀疑是数据分布不均匀，交接任务和操作任务的数据量差距太大

## 数据集v2.1制作和训练

- 使得数据集中交接任务和操作任务的数据量相等

<img src="./pic/bihand.png" height="300" /><img src="./pic/sinhand.png" height="300" />

- 对于不同的任务，同一个物体，生成不同的接触图, 左边是操作任务，右边是交接任务

<img src="./pic/manipv2.png" height="300" /><img src="./pic/handoverv2.png" height="300" />

- 但有一个问题是两只手的接触图容易混合在一起，难以分辨

## 数据集v3制作和训练

- 左手的接触图是正数，右手的接触图是负数

- 网络无法输出有效的数据，怀疑是数据在-1到1之间，不适合用sigmoid函数

## 数据集v3.1制作和训练

- 将接触图归一化到0到1之间

- 现在的数据集看起来是这样的，左边是分辨了左右手的接触图，右边是没有分辨左右手的接触图

<img src="./pic/pos_neg_contactmap.png" height="300" /><img src="./pic/bihand.png" height="300" />


<img src="./pic/ho_plane.png" height="300" /><img src="./pic/manip_plane.png" height="300" />

将网络接入ros中，使用yolo检测目标物体，获得mask，裁剪出物体的点云，分析点云的接触图

<img src="./pic/legomap.png" width="50%" />

## 数据集v4制作和训练

将接触图分为两层，一层为左手一层为右手

<img src="./pic/cmapdim1.png" height="300" /><img src="./pic/cmapdim2.png" height="300" /><img src="./pic/cmapallone.png" height="300" />

交接任务的两层接触图如下图，左手和右手接触图的高亮位置会有一定距离

<img src="./pic/homapv4_ho.png" height="300" />

操作任务的接触图如下图，一侧的接触图值全为0，另一侧有高亮的位置

<img src="./pic/homapv4_manip.png" height="300" />

得到推理的结果，非常杂乱，最后发现是网络的通道顺序有错

<img src="./pic/err_channle.png" height="300" />

修正后的网络推理结果如图

交接任务

<img src="./pic/v4result_ho_plane.png" height="300" /><img src="./pic/v4result_ho_shoe.png" height="300" />

操作任务

<img src="./pic/v4result_ho_plane.png" height="300" />

尝试加入与任务相关的loss，对于交接任务，鼓励第一层和第二层高亮位置的中心点尽量远，对于操作任务，鼓励仅在某一层生成接触图，另一层尽量输出0