├── .gitignore ├── .vscode ├── launch.json └── settings.json ├── README.assets ├── image-20210822213152685.png ├── image-20210822213301952.png ├── image-20210822213339895.png ├── image-20210822213507885.png ├── image-20210830104108635.png └── image-20210906143914883.png ├── README.md ├── check_good_meshes_for_gripper.py ├── copy_all_meshes.py ├── debug_tools ├── antipodal_grasp_sampler.py ├── generate-dataset-canny.py ├── group_mask_generate_REGNet.py ├── read_grasps_from_file.py ├── show_mesh_pc.py ├── show_raw_pc.py ├── test.py └── test1.py ├── doc ├── Dex-net典范抓取坐标系与求解.assets │ ├── image-20210828201622210-1630152983587.png │ ├── image-20210828201730737.png │ ├── image-20210828213543866.png │ ├── image-20210828213735005.png │ ├── image-20210829092109553.png │ ├── image-20210829100519836.png │ ├── image-20210829103319772.png │ ├── image-20210829112931824.png │ ├── image-20210829120857478.png │ ├── image-20210829155239115.png │ ├── image-20210829191441837.png │ ├── image-20210829192726091.png │ ├── image-20210829193625661.png │ ├── image-20210829193716537.png │ ├── image-20210829195830930.png │ ├── image-20210829200430654.png │ ├── image-20210829202625667.png │ ├── image-20210829205518316.png │ ├── image-20210829211317675.png │ ├── image-20210902110348277.png │ ├── image-20210904191110685.png │ ├── image-20210904192108478.png │ ├── image-20210906085839583.png │ └── image-20210907165347485.png ├── Dex-net典范抓取坐标系与求解.md ├── Markdown.md ├── sdf原理.md ├── 抓取典范坐标系.png ├── 点云与夹爪坐标变换.assets │ ├── image-20210831190104710.png │ ├── image-20210831190838330.png │ ├── image-20210831191151570.png │ └── image-20210831210923247.png ├── 点云与夹爪坐标变换.md └── 碰撞坐标系.png ├── generate_mujoco_xml.py ├── get_legal_grasps_with_score.py ├── group_mask_generate.py ├── pc_post_proceed.py ├── poses_simulation.py ├── raw_pc_generate.py ├── read_file_sdf.py ├── sample_grasps_from_meshes.py └── ycb_download.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .nox/ 42 | .coverage 43 | .coverage.* 44 | .cache 45 | nosetests.xml 46 | coverage.xml 47 | *.cover 48 | .hypothesis/ 49 | .pytest_cache/ 50 | 51 | # Translations 52 | *.mo 53 | *.pot 54 | 55 | # Django stuff: 56 | *.log 57 | local_settings.py 58 | db.sqlite3 59 | 60 | # Flask stuff: 61 | instance/ 62 | .webassets-cache 63 | 64 | # Scrapy stuff: 65 | .scrapy 66 | 67 | # Sphinx documentation 68 | docs/_build/ 69 | 70 | # PyBuilder 71 | target/ 72 | 73 | # Jupyter Notebook 74 | .ipynb_checkpoints 75 | 76 | # IPython 77 | profile_default/ 78 | ipython_config.py 79 | 80 | # pyenv 81 | .python-version 82 | 83 | # celery beat schedule file 84 | celerybeat-schedule 85 | 86 | # SageMath parsed files 87 | *.sage.py 88 | 89 | # Environments 90 | .env 91 | .venv 92 | env/ 93 | venv/ 94 | ENV/ 95 | env.bak/ 96 | venv.bak/ 97 | 98 | # Spyder project settings 99 | .spyderproject 100 | .spyproject 101 | 102 | # Rope project settings 103 | .ropeproject 104 | 105 | # mkdocs documentation 106 | /site 107 | 108 | # mypy 109 | .mypy_cache/ 110 | .dmypy.json 111 | dmypy.json 112 | 113 | # Pyre type checker 114 | .pyre/ 115 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // 使用 IntelliSense 了解相关属性。 3 | // 悬停以查看现有属性的描述。 4 | // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Python: 当前文件", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "console": "integratedTerminal" 13 | } 14 | ] 15 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "/home/wgk/anaconda3/bin/python" 3 | } -------------------------------------------------------------------------------- /README.assets/image-20210822213152685.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210822213152685.png -------------------------------------------------------------------------------- /README.assets/image-20210822213301952.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210822213301952.png -------------------------------------------------------------------------------- /README.assets/image-20210822213339895.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210822213339895.png -------------------------------------------------------------------------------- /README.assets/image-20210822213507885.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210822213507885.png -------------------------------------------------------------------------------- /README.assets/image-20210830104108635.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210830104108635.png -------------------------------------------------------------------------------- /README.assets/image-20210906143914883.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/README.assets/image-20210906143914883.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simulate Grasp Dataset Generation （未完成版） 2 | 3 | 本代码实现的主要功能是： 4 | 5 | 1. 针对指定夹爪，对YCB数据集中的mesh模型进行Antipodal抓取采样 6 | 7 | 2. 从YCB数据集中随机抽取指定数量的mesh模型，进行Mujoco物理仿真，获取各个物体的虚拟姿态 8 | 9 | 3. 将Antipodal采样出的抓取姿态旋转至虚拟场景中，然后进行碰撞检测等筛选剔除操作 10 | 11 | **注意：** 本代码是未完成版，只针对我的硬件环境完成了一些重点思路的编写和基本功能的实现， 由于本人课题更换，这个代码也写了一段时间了，为了保证精力我一般不会回答issue，有时间精力的同学可以随意简单看看，也许有些代码片段会对你有用； 最终生成效果还是有保证的，最后的效果图是旧的，实际的效果要好很多； 由于大部分是自己手撸出来的，里面的一些地方备注比较仔细，大家简单看看吧 12 | 13 | 14 | 15 | 16 | 17 | ## 数据集结构 18 | 19 | 本代码生成的数据集文件夹为`~/dataset/simulate_grasp_dataset/`，该文件夹下面的结构如下： 20 | 21 | ```bash 22 | . 23 | ├── panda #针对某个特定夹爪(panda)生成的数据集，每种夹爪都将会有自己的文件夹，以下以panda夹爪为例 24 | │   ├── antipodal_grasps#针对panda夹爪尺寸参数，对google_512k中的所有模型都进行Antipodal采样 25 | │   │   └── Readme.txt 26 | │   ├── gripper_params#panda 夹爪的尺寸参数 27 | │   ├── good_meshes.pickle#对于panda夹爪，具有足够多优质抓取的mesh集合 28 | │   └── scenes#虚拟的 29 | │   ├── 0#存放第0帧场景相关文件的文件夹， 30 | │   ├── 1 31 | │   └── ... 32 | └── ycb #存放仿真以及抓取采样需要的模型文件 33 | ├── all_16k_meshes#运行copy_all_meshes.py 脚本，将google_16k中的所有stl文件拷贝到该文件夹，将会作为模型库供mujoco仿真 34 | │   ├── 002_master_chef_can.stl#google_16k中的模型文件 35 | │   ├──... 36 | │   ├── bg_funnel_part.stl#mujoco世界背景模型文件 37 | │   └── ... 38 | ├── google_16k#将google_16k文件解压拷贝到这里，其中的stl文件将会被拷贝到all_16k_stls 39 | │   ├── 002_master_chef_can 40 | │   └── ... 41 | └── google_512k#将google_512k文件解压拷贝到这里，用于Antipodal抓取采样 42 | ├── 002_master_chef_can_google_512k 43 | └── ... 44 | 45 | 46 | ``` 47 | 48 | 49 | 50 | ## 安装 51 | 52 | 1. 安装修改后的dex-net 53 | 54 | 2. 安装mujoco 55 | 56 | 3. 安装blensor虚拟点云生成工具 57 | 58 | 4. 克隆本仓库代码到任意路径下 59 | 60 | ```bash 61 | git clone https://github.com/Hymwgk/simulate_dataset_generation.git 62 | ``` 63 | 64 | 65 | 66 | ## 使用 67 | 68 | 由于每种夹爪的尺寸是不同的，因此每种夹爪都需要生成特定的数据集，以下的教程以panda夹爪为例; 69 | 70 | 除了特别标注之外，其余默认使用python3 71 | 72 | 1. 创建`~/dataset/simulate_grasp_dataset/`文件夹，并创建 73 | 74 | 考虑，设置一个dataset_init.py脚本，来自动创建指定结构的目录 75 | 76 | 2. 下载[ycb数据集](http://ycb-benchmarks.s3-website-us-east-1.amazonaws.com/)中的google_512k以及google_16k两种分辨率的文件，之后将两个文件夹手动拷贝到`~/dataset/simulate_grasp_dataset/ycb/`路径下 77 | 78 | ```bash 79 | python ycb_download.py #python2 80 | ``` 81 | 82 | 3. 由于mujoco的场景xml文件，要求一个场景中所有的mesh文件都处于同一个文件夹中，所以为了方便mujoco读取模型，需要将仿真需要的16k分辨率文件拷贝到一个统一的`~/dataset/simulate_grasp_dataset/ycb/all_16k_meshes/`文件夹中 83 | 84 | ```bash 85 | python copy_all_meshes.py 86 | ``` 87 | 88 | 4. 将下载的桌子等背景文件拷贝到`all_16k_meshes`文件夹中 89 | 90 | 91 | 92 | 5. 为`~/dataset/simulate_grasp_dataset/ycb/google_512k/`文件夹下的模型生成sdf文件 93 | 94 | ```bash 95 | python read_file_sdf.py 96 | ``` 97 | 98 | 6. 为panda夹爪采样生成抓取，抓取结果将会被自动存放在`~/dataset/simulate_grasp_dataset/panda/antipodal_grasps/`路径下，此步骤执行时间较长 99 | 100 | 有两个py脚本，两种采样方法都是Antipodal，但是并行计算结构不同： 101 | 102 | - `sample_grasps_for_meshes.py` 单次只对一个物体进行并行多进程采样(优先使用该方法) 103 | 104 | `--gripper` 指定夹爪的型号 105 | 106 | `--mode` 指定代码运行模式： 107 | 108 | - `b` 断点采样模式，将会生成`original_.pickle`形式的未经过处理的抓取采样文件，会自动跳过已经生成的文件，支持断点运行 109 | - `r` 重新采样模式，将会忽略已经生成的文件，重新开始采样生成`original_.pickle` 110 | - `p` 处理模式，对已经生成好的`original_.pickle`文件作进一步的处理，初步筛选出较为优质的抓取 111 | 112 | `--rounds` 设定每个mesh模型采样几轮 113 | 114 | `--process_n` 设定每一轮采样使用多少个进程并行采样 115 | 116 | `--grasp_n` 设定每一个进程的采样目标是多少个抓取 117 | 118 | 以上的几个参数可以根据自己的电脑配置来选择，其中每个mesh模型总的目标采样数量的计算方式是： 119 | $$ 120 | target = rounds*process\_n*grasp\_n 121 | $$ 122 | 123 | ```bash 124 | #断点生成模式，进行一轮处理，此轮使用60个进程，每个采集200有效抓取 125 | python sample_grasps_for_meshes.py --gripper panda --mode b --rounds 1 --process_n 60 --grasp_n 200 126 | #生成结束后，进行打分等处理 127 | python sample_grasps_for_meshes.py --gripper panda --mode p 128 | ``` 129 | 130 | - `generate-dataset-canny.py` 旧版本的采样方法，同时对多个物体采样，每个物体只分配一个进程，并使用pointnetgpd的方法打分 131 | 132 | ```bash 133 | python generate-dataset-canny.py --gripper panda #夹爪名称 134 | ``` 135 | 136 | **TODO：**最好是，能将生成的所有抓取，进行一个重复性筛检，减少过多重复的抓取，因为，你想，对于小球，比如乒乓球，有效的抓取肯定很多，但是问题是，也必然存在大量的重复；后续如果我们希望对物体按照抓取数量进行排序，那小球会排的很高，但是显然，里面的太多抓取是重复的 137 | 138 | 139 | 140 | 7. 以交互界面形式查看已经采样出的抓取姿态 141 | 142 | ```bash 143 | python read_grasps_from_file.py --gripper panda 144 | ``` 145 | 146 | 147 | 148 | 8. 由于夹爪尺寸限制，有些模型采样得到的抓取较少，需要根据模型抓取采样结果的好坏多少，筛选出适合该特定夹爪的模型子集合用于场景仿真，它会在`~/dataset/simulate_grasp_dataset/panda/`文件夹下生成名为`good_meshes.pickle`的文件 **还未完善，需要等到上面的抓取生成后才行** 149 | 150 | - 没有高于某分数抓取的模型不要 151 | - 剔除掉人为设定的列表模型 152 | - 随机从合法模型库中抽取并进行复制 153 | 154 | 可以考虑：并不一定非要每种物体只有一个，可以把一些物体，重复几次 155 | 156 | ```bash 157 | python check_good_meshes_for_gripper.py --gripper panda #夹爪名称 158 | ``` 159 | 160 | 9. 从上一步筛选的合法模型子集中，随机抽取指定数量的模型，为Mujoco生成指定数量的模拟场景xml配置文件 161 | 162 | `--mesh_num` 每个模拟场景中包含的模型数量 163 | 164 | `--scene_num` 设定一共生成几个虚拟场景 165 | 166 | ```bash 167 | python generate_mujoco_xml.py --gripper panda --mesh_num 10 --scene_num 100 #夹爪名称 每个场景中包含10个物体 生成100个场景 168 | ``` 169 | 170 | 10. 读取各个场景的xml配置文件，利用Mujoco进行仿真，生成两类数据： 171 | 172 | 1)筛选出自由落体稳定后仍然存在于桌面上的物体列表（包括背景桌子）； 173 | 174 | 2)对应模型在空间中的位置姿态列表(平移向量+四元数) ； 175 | 176 | 这两类数据共同以`table_meshes_with_pose.pickle`的形式保存在各自的场景文件夹中，该文件将为后续使用BlenSor进行点云仿真提供 场景模型(.obj格式)的路径和对应姿态。 177 | 178 | **ToDo: **这一个步骤有一定的概率失败，并且最好是多进程共同仿真（已经做好了多进程并行仿真，利用周边的东西） 179 | 180 | ```bash 181 | python poses_simulation.py --gripper panda #夹爪名称 182 | ``` 183 | 184 | 11. 多进程渲染目标场景，这一步骤的夹爪需要在代码中改动，因为有个外部参数 `-P` 很麻烦； 185 | 186 | 默认选定同时渲染10个模拟场景的点云 187 | 188 | **ToDo:** 不能每一帧场景只有一个点云，这样太浪费了，从不同的视角观察，至少每帧场景渲染4帧点云（从桌子四周观察） 189 | 190 | ```bash 191 | ~/.blensor/./blender -P ~/code/simulate_dataset_generation/raw_pc_generate.py 192 | ``` 193 | 194 | 12. 查看刚刚渲染出的仿真场景点云 **（尝试包装一下，允许外部引用）** 195 | 196 | `--gripper` 指定夹爪名称，默认panda 197 | 198 | `--raw_pc` 出现该字样，则显示原生点云，否则显示处理后的点云 199 | 200 | `--show` 直接选择单独查看哪个场景，是文件夹的编号（非0），如果空白则从第0帧开始播放，直到所有点云播放完毕 201 | 202 | ```bash 203 | python show_raw_pc.py --gripper panda --show 5 #单独查看第5帧点云 204 | ``` 205 | 206 | 13. 多线程对场景中的候选抓取进行合法性检查，并得到最终的合理候选抓取(由于使用了显卡计算碰撞矩阵，导致计算有些慢，现在使用的是单线程计算的) 207 | 208 | 209 | 210 | 主要检测虚拟夹爪是否与点云碰撞、虚拟夹爪是否与桌面碰撞或者低于桌面、限制抓取approach轴与桌面垂直桌面的角度、设定夹爪内部点云最小数量以及场景点云嵌入夹爪的最小深度，参数为： 211 | 212 | `--gripper` 设定夹爪名称 213 | 214 | `--process_num` 设定同时多少个场景进行合法性检查 215 | 216 | 生成的合法抓取姿态以8维度向量（符合dex-net标准的7d位姿+1d 分数）的形式，保存在对应场景文件夹中，文件名统一为`legal_grasps_with_score.npy`，需要注意的是，最终的合法抓取姿态是与点云相互配准的，因此，夹爪姿态是相对于相机坐标系的。 217 | 218 | ```python 219 | python get_legal_grasps_with_score --gripper panda --process_num 30 220 | ``` 221 | 222 | 同时也有一个仅用于debug的单进程脚本 223 | 224 | `--load_npy` 出现该字眼则从外部读取结果，否则显示本次生成的结果. 225 | 226 | ```python 227 | python get_legal_grasps_with_score_single_thread --gripper panda --load_npy 228 | ``` 229 | 230 | 231 | 232 | 14. 对计算出的合法抓取作做拓展数据集合处理，主要包含如下处理 233 | 234 | 对场景中的抓取进行贪婪聚类，聚类出多个group； 235 | 236 | 计算每个group的抓取分数的总和，根据group分数总和来对所有group进行从高到低排序 237 | 238 | 设置数量阈值，每个场景仅保留最靠前的n个group(暂时先不对group进行剪切) 239 | 240 | 先计算每一个场景的合法抓取的扩大内部点云索引，比如2000个合法抓取，每个抓取对应一个index_list，并保存到文件夹中 241 | 242 | ​ 读取夹爪内部点索引，每一个group夹爪内部点进行计算并集区域，记录成为group point索引。 243 | 244 | ​ 从高到低，检测每个group之间的交并集区域，如果交集区域，已经占据了某个区域的80%以上，就把哪个group的mask剔除 245 | 246 | 247 | 248 | ```python 249 | python group_mask_generate.py 250 | ``` 251 | 252 | 253 | 254 | ​ 255 | 256 | 257 | 258 | 259 | 260 | 本周工作： 261 | 262 | 本周工作主要有两部分： 263 | 264 | 1.针对数据集生成代码中，耗时较多的矩阵运算，例如碰撞检测等环节，使用CUDA进行了优化加速。 265 | 266 | 2.编写了80%拓展数据集的生成算法代码； 267 | 268 | - 使用CUDA加速计算每个点云场景中，各个抓取之间的位置与姿态差异矩阵（已完成） 269 | 270 | - 对500帧点云场景（测试用）的每个场景中的抓取进行了贪婪聚类，得到多个group，根据group中各个抓取的分数总和，对group进行排序（已完成） 271 | 272 | - CUDA计算每个场景中的所有合法抓取的扩大内部点云索引，统计每个场景的周边点云集合并保存（已完成） 273 | 274 | - 读取每个场景夹爪内部点云索引，每一个group夹爪内部点进行计算并集区域，记录成为group point索引。（未完成） 275 | 276 | - 从高到低，检测每个group之间的交并集区域，如果交集区域，已经占据了某个区域的80%以上，就把哪个group的mask剔除（未完成） 277 | 278 | 从高分的簇，向下检索，如果 279 | 280 | - 将结果显示出来，设置，显示排名前5的mask，每种mask使用不同的颜色表示 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | ## 物理场景仿真结果示例 297 | 298 | 启动仿真器 299 | 300 | ```bash 301 | cd /home/wgk/.mujoco/mujoco200/bin 302 | ./simulate 303 | ``` 304 | 305 | ![image-20210906143914883](README.assets/image-20210906143914883.png) 306 | 307 | 308 | 309 | 310 | 311 | -------------------------------------------------------------------------------- /check_good_meshes_for_gripper.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | #读取采样后每个mesh上的抓取数量，筛选出满足某些要求的mesh，并把这些合法mesh的路径保存在 3 | # 某夹爪内部的legal_meshes_for_panda.txt中 4 | # 远程测试 5 | import os 6 | import sys 7 | import pickle 8 | import argparse 9 | import numpy as np 10 | import random 11 | 12 | #解析命令行参数 13 | parser = argparse.ArgumentParser(description='Check good meshes') 14 | parser.add_argument('--gripper', type=str, default='baxter') 15 | args = parser.parse_args() 16 | 17 | 18 | #返回all_16k_stls文件夹下除了背景模型的其他所有stl路径 19 | def get_stls_path(file_dir_): 20 | file_list = [] 21 | for root, dirs, files in os.walk(file_dir_): 22 | for file in files: 23 | #剔除掉背景模型以及非stl文件 24 | if file.find('bg_')==-1 and file.find('.stl')!=-1: 25 | file_list.append(os.path.join(root,file)) 26 | #排序 27 | #file_list.sort() 28 | return file_list 29 | 30 | #返回 31 | def get_grasp_files_path_name(file_dir_,method): 32 | file_list = [] 33 | names = [] 34 | for root, dirs, files in os.walk(file_dir_): 35 | for file in files: 36 | # 37 | if file.find(method)!=-1 and file.find('.npy')!=-1: 38 | file_list.append(os.path.join(root,file)) 39 | names.append(file.split('_'+method)[0]) 40 | #排序 41 | #file_list.sort() 42 | return file_list,names 43 | 44 | 45 | 46 | if __name__ == '__main__': 47 | 48 | gripper_name = args.gripper 49 | #可以手动指定一些需要删除的mesh 50 | manual_delet_list = ['059_chain'] 51 | 52 | print("Checking good meshes for {}.".format(gripper_name)) 53 | 54 | home_dir = os.environ['HOME'] 55 | meshes_16k_dir = home_dir+"/dataset/simulate_grasp_dataset/ycb/all_16k_meshes" 56 | #grasp 目录 57 | grasp_files_dir = home_dir+"/dataset/simulate_grasp_dataset/"+gripper_name+"/antipodal_grasps" 58 | #grasp files， name 59 | grasp_files_path,obj_names = get_grasp_files_path_name(grasp_files_dir,'pgpd') 60 | 61 | #获取{stl name:stl path} 62 | stls_path_list = get_stls_path(meshes_16k_dir) 63 | stl_name_path ={} 64 | for stl_path in stls_path_list: 65 | name = stl_path.split('/')[-1].split('.')[0] 66 | stl_name_path[name] = stl_path 67 | 68 | 69 | #获取{stl path: grasps} 70 | all_obj_grasps ={} 71 | #设定最小的分数阈值 72 | minimum_score = 2 73 | #首先是仅仅只将具有高分抓取的obj_name:grasps 保存起来 74 | for index,path in enumerate(grasp_files_path): 75 | grasp_with_score = np.load(path) #(-1,10+1) 76 | #是希望物体有高分，不仅仅是抓取多 77 | if np.sum(grasp_with_score[:,-1]>minimum_score)>5: 78 | obj_name = obj_names[index] 79 | try: 80 | all_obj_grasps[stl_name_path[obj_name]]=grasp_with_score #obj_name:grasps 81 | except: 82 | pass 83 | else: 84 | pass 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | #剔除指定名称 94 | for index,del_name in enumerate(manual_delet_list): 95 | for stl_path,_ in all_obj_grasps.items(): 96 | if stl_path.find(del_name)!=-1: 97 | break 98 | del all_obj_grasps[stl_path] 99 | 100 | 101 | #取出list 102 | final_paths=[] 103 | for stl_path,_ in all_obj_grasps.items(): 104 | final_paths.append(stl_path) 105 | 106 | 107 | #随即从合法模型库中抽取一半放在里面 108 | #final_paths = final_paths+random.sample(final_paths,int(len(final_paths)/2)) 109 | 110 | target = home_dir+"/dataset/simulate_grasp_dataset/"+gripper_name+"/good_meshes.pickle" 111 | #直接用pickel保存 112 | with open(target,'wb') as f: 113 | pickle.dump(final_paths, f) 114 | 115 | 116 | -------------------------------------------------------------------------------- /copy_all_meshes.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import os 3 | #复制文件 4 | import shutil 5 | import sys 6 | import glob 7 | 8 | #输入文件夹地址，返回一个列表，其中保存的是文件夹中的文件名称 9 | def get_file_name(file_dir_): 10 | file_list = [] 11 | for root, dirs, files in os.walk(file_dir_): 12 | #遍历出stl文件 13 | for file in files: 14 | if file=='nontextured.stl': 15 | file_list.append(os.path.join(root,file)) 16 | #排序 17 | file_list.sort() 18 | return file_list 19 | 20 | #把单一目标文件拷贝到指定的目录下 21 | def copy_stls(srcfile,dstpath): 22 | if not os.path.isfile(srcfile): 23 | print("%s not exit!" % (srcfile)) 24 | else: 25 | #fpath,fname=os.path.split(dstfile) 26 | #先找到模型的名字 27 | stl_name = srcfile.split('/')[-3] 28 | if not os.path.exists(dstpath): 29 | os.makedirs(dstpath) 30 | #将源文件拷贝到指定文件夹中，同时修改为指定的名称 31 | shutil.copyfile(srcfile,dstpath+stl_name+".stl") 32 | 33 | #把单一目标文件拷贝到指定的目录下 34 | def copy_objs(srcfile,dstpath): 35 | if not os.path.isfile(srcfile): 36 | print("%s not exit!" % (srcfile)) 37 | else: 38 | #fpath,fname=os.path.split(dstfile) 39 | #先找到模型的名字 40 | obj_name = srcfile.split('/')[-3] 41 | if not os.path.exists(dstpath): 42 | os.makedirs(dstpath) 43 | #将源文件拷贝到指定文件夹中，同时修改为指定的名称 44 | shutil.copyfile(srcfile,dstpath+obj_name+".obj") 45 | 46 | 47 | #把单一目标文件拷贝到指定的目录下 48 | def copy_maps(srcfile,dstpath): 49 | if not os.path.isfile(srcfile): 50 | print("%s not exit!" % (srcfile)) 51 | else: 52 | #fpath,fname=os.path.split(dstfile) 53 | #先找到模型的名字 54 | png_name = srcfile.split('/')[-3] 55 | if not os.path.exists(dstpath): 56 | os.makedirs(dstpath) 57 | #将源文件拷贝到指定文件夹中，同时修改为指定的名称 58 | shutil.copyfile(srcfile,dstpath+png_name+".png") 59 | 60 | #把单一目标文件拷贝到指定的目录下 61 | def copy_mtls(srcfile,dstpath): 62 | if not os.path.isfile(srcfile): 63 | print("%s not exit!" % (srcfile)) 64 | else: 65 | #fpath,fname=os.path.split(dstfile) 66 | #先找到模型的名字 67 | mtl_name = srcfile.split('/')[-3] 68 | if not os.path.exists(dstpath): 69 | os.makedirs(dstpath) 70 | #将源文件拷贝到指定文件夹中，同时修改为指定的名称 71 | shutil.copyfile(srcfile,dstpath+mtl_name+".mtl") 72 | 73 | 74 | 75 | if __name__ == '__main__': 76 | 77 | home_dir = os.environ['HOME'] 78 | #注意是16k分辨率的文件夹 79 | google_16k = home_dir + "/dataset/simulate_grasp_dataset/ycb/google_16k/" 80 | copy_to = home_dir + "/dataset/simulate_grasp_dataset/ycb/all_16k_meshes/" 81 | google_16k_stls = get_file_name(google_16k) #返回所有stl模型所处的文件夹的路径列表 82 | google_16k_objs=glob.glob(google_16k+'*/google_16k/*.obj') 83 | google_16k_texture_maps = glob.glob(google_16k+'*/google_16k/texture_map.png') 84 | google_16k_texture_mtls = glob.glob(google_16k+'*/google_16k/textured.mtl') 85 | 86 | #复制所有16k mesh模型到指定目录，并替换为规定的名字 87 | for google_16k_stl in google_16k_stls: 88 | copy_stls(google_16k_stl,copy_to) 89 | 90 | for google_16k_obj in google_16k_objs: 91 | copy_objs(google_16k_obj,copy_to) 92 | 93 | for google_16k_map in google_16k_texture_maps: 94 | copy_maps(google_16k_map,copy_to) 95 | for google_16k_mtl in google_16k_texture_mtls: 96 | copy_mtls(google_16k_mtl,copy_to) 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /debug_tools/antipodal_grasp_sampler.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | import pickle 4 | #from dexnet.grasping.quality import PointGraspMetrics3D 5 | #from dexnet.grasping import GaussianGraspSampler, AntipodalGraspSampler, UniformGraspSampler, GpgGraspSampler, grasp 6 | #from dexnet.grasping import RobotGripper, GraspableObject3D, GraspQualityConfigFactory, PointGraspSampler 7 | #import dexnet 8 | #from autolab_core import YamlConfig 9 | #from meshpy.obj_file import ObjFile 10 | #from meshpy.sdf_file import SdfFile 11 | import os 12 | import torch 13 | import math 14 | import multiprocessing 15 | #import matplotlib.pyplot as plt 16 | import open3d as o3d 17 | #from open3d import geometry 18 | #from open3d.geometry import voxel_down_sample, sample_points_uniformly, orient_normals_towards_camera_location, estimate_normals 19 | 20 | 21 | class AntipodalSampler: 22 | def __init__(self,ply_path,friction_coef): 23 | self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') 24 | self.friction_coef = friction_coef 25 | self.friction_cos_theta = torch.tensor(math.cos(math.atan(friction_coef)),dtype=torch.float64).cuda() 26 | 27 | 28 | 29 | 30 | #获取原始点云 31 | mesh = o3d.io.read_triangle_mesh(ply_path) 32 | #得到点云对象 33 | raw_pc = o3d.geometry.TriangleMesh.sample_points_uniformly(mesh, np.asarray(mesh.vertices).shape[0] * 10) 34 | #均匀降采样 35 | voxel_pc = o3d.geometry.PointCloud.voxel_down_sample(raw_pc, 0.0025) 36 | #估计点云的表面法向量 37 | o3d.geometry.PointCloud.estimate_normals(voxel_pc, search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) 38 | #将点云转换为np对象 39 | self.pc = np.asarray(voxel_pc.points) 40 | self.normals = np.asarray(voxel_pc.normals) 41 | 42 | 43 | 44 | 45 | def farthest_point_sample(self,xyz, npoint): 46 | """ 47 | Input: 48 | xyz: pointcloud data, [N, C] 49 | npoint: number of samples 50 | Return: 51 | centroids: sampled pointcloud index, [B, npoint] 52 | """ 53 | device = xyz.device 54 | N, C = xyz.shape 55 | centroids = torch.zeros(npoint, dtype=torch.long).to(device) 56 | distance = torch.ones(N,dtype=torch.float64).to(device) * 1e10 57 | farthest = torch.randint(0, N,(1,),dtype=torch.long).to(device) 58 | 59 | for i in range(npoint): 60 | # 更新第i个最远点 61 | centroids[i] = farthest 62 | # 取出这个最远点的xyz坐标 63 | centroid = xyz[farthest, :].view(1, 3) 64 | # 计算点集中的所有点到这个最远点的欧式距离 65 | dist = torch.sum((xyz - centroid) ** 2, -1) 66 | # 更新distances，记录样本中每个点距离所有已出现的采样点的最小距离 67 | mask = dist < distance 68 | distance[mask] = dist[mask] 69 | # 从更新后的distances矩阵中找出距离最远的点，作为最远点用于下一轮迭代 70 | farthest = torch.max(distance, -1)[1] 71 | return centroids 72 | 73 | def computeTangents(self,normals,max_samples=1000): 74 | """计算每个输入点的切线空间坐标系，类似于Darboux frame， 75 | 切线坐标系的z轴是表面法向量，y轴和x轴位于与点相切的平面内部 76 | """ 77 | N,C = normals.shape 78 | inner_normals = -normals#[n,3] 79 | 80 | U,_,_ = torch.svd(inner_normals.view(-1,3,1),some=False) 81 | print(U.shape) 82 | x,y = U[:,:, 1], U[:,:, 2]# 退化[n,3] 83 | # make sure t1 and t2 obey right hand rule 84 | z_hat = torch.cross(x, y) #[n,3] 85 | mask = torch.sum(z_hat.mul(inner_normals),dim=1)<0 86 | y[mask]=-y[mask] 87 | 88 | max_ip = torch.zeros([N,],dtype=torch.float64).cuda() 89 | max_theta = torch.zeros([N,],dtype=torch.float64).cuda() 90 | theta = torch.zeros([1,],dtype=torch.float64).cuda() 91 | target = torch.tensor([1, 0, 0]).unsqueeze(0).repeat(N,1).cuda()#[n,3] 92 | 93 | d_theta = torch.tensor([2 * math.pi / float(max_samples)]).cuda()#将2pi分解为1000份 94 | 95 | for i in range(max_samples): 96 | v = torch.cos(theta) * x + torch.sin(theta) * y#[n,3] 97 | mul = torch.sum(v.mul(target),dim=1)#[n,] 矩阵每行点乘值 98 | 99 | mask =mul>max_ip#[n,] 100 | 101 | max_ip[mask] = mul[mask]#更新 102 | max_theta[mask] = theta # 103 | theta += d_theta 104 | 105 | #新x轴 与 世界x轴配准 106 | v = torch.cos(max_theta).view(N,1) * x + torch.sin(max_theta).view(N,1) * y #[n,3] 107 | #叉乘出新的y轴 108 | w = torch.cross(inner_normals, v) 109 | 110 | return inner_normals, v, w 111 | 112 | 113 | def computeAxis(self,points,inner_noramls): 114 | #问题：如何处理夹爪对称的情况呢？ 115 | N,C = points.shape # 116 | c1_points = points.clone()#[N,3] 117 | c2_points = points.clone() 118 | c1_to_c2 = c2_points.view(N,1,C) - c1_points #[N,N,C] 119 | norm = torch.norm(c1_to_c2,dim=2,keepdim=True) 120 | #变成单位向量 121 | c1_to_c2_norm = c1_to_c2/torch.norm(c1_to_c2,dim=2,keepdim=True) 122 | 123 | #计算每个点的射线和inner_noramls之间的点乘，就是cos夹角 124 | angles = torch.sum(c1_to_c2_norm.mul(inner_noramls.view(N,1,3)),dim=2) #[N,N] 125 | 126 | mask = angles>self.friction_cos_theta #找到位于 127 | pass 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | def sample_grasps(self,): 145 | pc_cuda = torch.from_numpy(self.pc).cuda()#FIXME:这里的数据发生了截断 146 | normal_cuda = torch.from_numpy(self.normals).cuda() 147 | #抽取300个第一接触点 148 | c1_index = self.farthest_point_sample(pc_cuda,300) 149 | c1_points = pc_cuda[c1_index] 150 | c1_normals = normal_cuda[c1_index] 151 | self.inner_normals,self.tx,self.ty = self.computeTangents(c1_normals) 152 | self.computeAxis(c1_points,self.inner_normals) 153 | 154 | 155 | 156 | 157 | 158 | return True 159 | 160 | 161 | if __name__ == '__main__': 162 | home_dir = os.environ['HOME'] 163 | file = home_dir+"/dataset/simulate_grasp_dataset/ycb/google_512k/002_master_chef_can_google_512k/002_master_chef_can/google_512k/nontextured.ply" 164 | #sf = SdfFile(file) 165 | #sdf = sf.read() 166 | #ply = o3d.io.read_point_cloud(file) 167 | test = AntipodalSampler(file,2.0) 168 | test.sample_grasps() 169 | -------------------------------------------------------------------------------- /debug_tools/generate-dataset-canny.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # python3 4 | # Author : Hongzhuo Liang 5 | # E-mail : liang@informatik.uni-hamburg.de 6 | # Description: 7 | # Date : 20/05/2018 2:45 PM 8 | # File Name : generate-dataset-canny.py 9 | # 运行之前需要对各个cad文件生成sdf文件 10 | 11 | import numpy as np 12 | import sys 13 | import pickle 14 | from dexnet.grasping.quality import PointGraspMetrics3D 15 | from dexnet.grasping import GaussianGraspSampler, AntipodalGraspSampler, UniformGraspSampler, GpgGraspSampler, grasp 16 | from dexnet.grasping import RobotGripper, GraspableObject3D, GraspQualityConfigFactory, PointGraspSampler 17 | import dexnet 18 | from autolab_core import YamlConfig 19 | from meshpy.obj_file import ObjFile 20 | from meshpy.sdf_file import SdfFile 21 | import os 22 | 23 | import multiprocessing 24 | import matplotlib.pyplot as plt 25 | plt.switch_backend('agg') # for the convenient of run on remote computer 26 | 27 | 28 | import argparse 29 | 30 | #解析命令行参数 31 | parser = argparse.ArgumentParser(description='Old grasp sample method') 32 | parser.add_argument('--gripper', type=str, default='baxter') 33 | args = parser.parse_args() 34 | 35 | 36 | #sys.path() 37 | 38 | #输入文件夹地址，返回一个列表，其中保存的是文件夹中的文件名称 39 | def get_file_name(file_dir_): 40 | file_list = [] 41 | for root, dirs, files in os.walk(file_dir_): 42 | #将下一层子文件夹的地址保存到 file_list 中 43 | if root.count('/') == file_dir_.count('/') + 1: 44 | file_list.append(root) 45 | #排序 46 | file_list.sort() 47 | return file_list 48 | 49 | def do_job(i): #处理函数 处理index=i的模型 50 | """ 51 | 创建多线程，多线程调用worker函数，处理指定的模型， 52 | """ 53 | #根据id号，截取对应的目标模型名称 54 | object_name = file_list_all[i].split('/')[-1] 55 | #存放采样出的good抓取的列表，元素形式是二元素元组 (grasp pose, score) 56 | good_grasp=[] 57 | 58 | #对CAD模型进行Antipod采样，将采样的抓取结果放到good_grasp中 59 | #worker( 60 | # i, 处理第i个模型 61 | # sample_nums_per_round, 62 | # max_iter_per_round, 63 | # mini_grasp_amount_per_score, 64 | # max_rounds, good_grasp) 65 | #worker(i, 100, 5,20, 30,good_grasp) 66 | worker(i, 100, 5,20, 30,good_grasp) 67 | 68 | #存放结果文件的路径&名称 69 | good_grasp_file_name = os.environ['HOME']+"/dataset/simulate_grasp_dataset/{}/grasp_sampled/{}_{}".format(gripper_name, str(object_name), str(len(good_grasp))) 70 | 71 | #创建一个pickle文件，将good_grasp保存起来 72 | with open(good_grasp_file_name + '.pickle', 'wb') as f: 73 | pickle.dump(good_grasp, f) 74 | 75 | tmp = [] 76 | for grasp in good_grasp: 77 | grasp_config = grasp[0].configuration 78 | score = grasp[1] 79 | tmp.append(np.concatenate([grasp_config, score])) 80 | np.save(good_grasp_file_name + '.npy', np.array(tmp)) 81 | print("finished job ", object_name) 82 | 83 | 84 | def worker(i, sample_nums_per_round, max_iter_per_round, 85 | mini_grasp_amount_per_score,max_rounds, good_grasp): #主要是抓取采样器以及打分 100 20 86 | """ 87 | brief: 对指定模型，利用随机采样算法，进行抓取姿态的检测和打分 88 | param [in] 索引为i的CAD模型 89 | param [in] 采样器单次采样最少返回sample_nums_per_round个抓取 90 | param [in] 每轮采样最多迭代max_iter_per_round次 91 | param [in] 每个分数区间最少有mini_grasp_amount_per_score个抓取 92 | param [in] 结果存放在good_grasp中 93 | """ 94 | #截取目标对象名称 95 | object_name = file_list_all[i].split('/')[-1] 96 | print('a worker of task {} start, index = {}'.format(object_name, i)) 97 | 98 | #读取采样器初始化配置文件 99 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 100 | #加载夹爪配置参数，初始化夹爪对象 101 | gripper = RobotGripper.load(gripper_name, home_dir + "/code/dex-net/data/grippers") 102 | 103 | #设置抓取采样的方法 104 | grasp_sample_method = "antipodal" 105 | 106 | if grasp_sample_method == "uniform": 107 | grasp_sampler = UniformGraspSampler(gripper, yaml_config) 108 | elif grasp_sample_method == "gaussian": 109 | grasp_sampler = GaussianGraspSampler(gripper, yaml_config) 110 | elif grasp_sample_method == "antipodal": 111 | #读取夹爪对象与采样器配置，初始化指定的采样器 112 | grasp_sampler = AntipodalGraspSampler(gripper, yaml_config) 113 | elif grasp_sample_method == "gpg": 114 | grasp_sampler = GpgGraspSampler(gripper, yaml_config) 115 | elif grasp_sample_method == "point": 116 | grasp_sampler = PointGraspSampler(gripper, yaml_config) 117 | else: 118 | raise NameError("Can't support this sampler") 119 | #print("Log: do job", i) 120 | #设置obj模型文件与sdf文件路径 121 | if os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.obj") and os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.sdf"): 122 | of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj") 123 | sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf") 124 | else: 125 | print("can't find any cad_model or sdf file!") 126 | raise NameError("can't find any cad_model or sdf file!") 127 | 128 | #根据路径读取模型与sdf文件 129 | mesh = of.read() 130 | sdf = sf.read() 131 | #构建被抓取的CAD模型数据 132 | cad_model = GraspableObject3D(sdf, mesh) 133 | print("Log: opened object", i + 1, object_name) 134 | 135 | ######################################### 136 | #生成一个起点是2.0终点是0.75 步长为-0.4 （递减）的等距数列score_list_sub1 (2.0, 0.75, -0.4) 137 | score_list_sub1 = np.arange(0.0, 0.6, 0.2) 138 | #生成一个起点是0.5终点是0.36 步长为-0.05的等距数列score_list_sub2 (0.5, 0.36, -0.05) 139 | score_list_sub2 = np.arange(0.6,1, 0.1) 140 | end = np.array([1.0]) 141 | 142 | #将上面两个向量接起来，变成一个长条向量，使用不同的步长，目的是为了在更小摩擦力的时候，有更多的分辨率 143 | score_list = np.concatenate([score_list_sub1, score_list_sub2,end]) 144 | #####################准备开始采样############################ 145 | #填充一个与摩擦数量相同的数组，每个对应的元素都是0 146 | good_count_perfect = np.zeros(len(score_list)-1) 147 | count = 0 148 | #如果每个摩擦系数下，有效的抓取(满足力闭合或者其他判断标准)小于要求值，就一直循环查找，直到所有摩擦系数条件下至少都存在20个有效抓取 149 | while np.sum(good_count_perfect < mini_grasp_amount_per_score) != 0: 150 | #开始使用antipodes sample获得对映随机抓取，此时并不判断是否满足力闭合，只是先采集满足夹爪尺寸的抓取 151 | #如果一轮多次随机采样之后，发现无法获得指定数量的随机抓取，就会重复迭代计算3次，之后放弃，并把已经找到的抓取返回来 152 | grasps = grasp_sampler.generate_grasps_score(cad_model, target_num_grasps=sample_nums_per_round, grasp_gen_mult=10,max_iter=max_iter_per_round, 153 | vis=False, random_approach_angle=True) 154 | for grasp in grasps: 155 | for k in range(len(good_count_perfect)): 156 | #如果第k个区间内部的抓取数量还不够 157 | if good_count_perfect[k] < mini_grasp_amount_per_score: 158 | #判断当前抓取是否属于这个区间 159 | if grasp[1]>=score_list[k] and grasp[1]max_rounds: #如果检测轮数大于30轮了 167 | break 168 | 169 | print('Gripper:{} Object:{} After {} rounds found {} good grasps.'. 170 | format(gripper_name, object_name, count,len(good_grasp))) 171 | 172 | 173 | if __name__ == '__main__': 174 | 175 | #获取夹爪名称 176 | gripper_name=args.gripper 177 | home_dir = os.environ['HOME'] 178 | #存放CAD模型的文件夹 179 | file_dir = home_dir + "/dataset/simulate_grasp_dataset/ycb/google_512k/" #获取模型的路径 180 | file_list_all = get_file_name(file_dir) #返回所有cad模型所处的文件夹的路径列表 181 | object_numbers = file_list_all.__len__() #获取cad模型的数量 182 | 183 | job_list = np.arange(object_numbers) #返回一个长度为object_numbers的元组 0 1 2 3 ... 184 | job_list = list(job_list) #转换为列表 185 | #设置同时对几个模型进行采样 186 | pool_size = 50 187 | if pool_size>object_numbers: 188 | pool_size = object_numbers 189 | # Initialize pool 190 | pool = [] #创建列表 191 | count =0 192 | for i in range(pool_size): #想多线程处理多个模型，但是实际上本代码每次只处理一个 193 | count += 1 194 | pool.append(multiprocessing.Process(target=do_job, args=(i,))) #在pool末尾添加元素 195 | [p.start() for p in pool] #启动多线程 196 | # refill 197 | 198 | while count < object_numbers: #如果有些没处理完 199 | for ind, p in enumerate(pool): 200 | if not p.is_alive(): 201 | pool.pop(ind) 202 | p = multiprocessing.Process(target=do_job, args=(count,)) 203 | count += 1 204 | p.start() 205 | pool.append(p) 206 | break 207 | print('All job done.') 208 | -------------------------------------------------------------------------------- /debug_tools/group_mask_generate_REGNet.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from logging import raiseExceptions 3 | from math import pi 4 | import math 5 | import os 6 | import sys 7 | import argparse 8 | import time 9 | from matplotlib.pyplot import axis 10 | import mayavi 11 | import numpy as np 12 | import pickle 13 | import glob 14 | import random 15 | from mayavi import mlab 16 | from torch._C import set_autocast_enabled 17 | from tqdm import tqdm 18 | from numpy.core.fromnumeric import swapaxes, transpose 19 | import torch 20 | from tqdm import tqdm 21 | import multiprocessing 22 | 23 | from dexnet.grasping import GpgGraspSampler 24 | from autolab_core import RigidTransform 25 | from autolab_core import YamlConfig 26 | from dexnet.grasping import RobotGripper 27 | 28 | 29 | 30 | #解析命令行参数 31 | parser = argparse.ArgumentParser(description='Group and mask generation') 32 | parser.add_argument('--gripper', type=str, default='baxter') # 33 | parser.add_argument('--load_npy',action='store_true') #设置同时处理几个场景 34 | parser.add_argument('--process_num', type=int, default=50) #设置同时处理几个场景 35 | parser.add_argument('--dis_min', type=float, default=0.01) #距离聚类 36 | parser.add_argument('--theta_min', type=float, default=0.707) #角度聚类 37 | 38 | args = parser.parse_args() 39 | home_dir = os.environ['HOME'] 40 | #场景文件夹 41 | scenes_dir = home_dir+"/dataset/REGnet/temp_training_data/" 42 | data_path = glob.glob(scenes_dir+'*.p')#一共19000+ 帧场景 43 | data_path.sort() 44 | mask_files_dir = home_dir+"/dataset/REGnet/mask_files/" 45 | 46 | 47 | 48 | 49 | 50 | 51 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 52 | gripper = RobotGripper.load(args.gripper, home_dir + "/code/dex-net/data/grippers") 53 | ags = GpgGraspSampler(gripper, yaml_config) 54 | 55 | 56 | device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 57 | print(device) 58 | 59 | 60 | def get_rot_mat(poses_vector): 61 | center_point = poses_vector[:,0:3] #夹爪中心(指尖中心) 62 | major_pc = poses_vector[:,3:6] # (-1,3) 63 | angle = poses_vector[:,[7]]# (-1,1) 64 | 65 | 66 | # cal approach 67 | cos_t = np.cos(angle) #(-1,1) 68 | sin_t = np.sin(angle) 69 | zeros= np.zeros(cos_t.shape) #(-1,1) 70 | ones = np.ones(cos_t.shape) 71 | 72 | #绕抓取binormal轴的旋转矩阵 73 | R1 = np.c_[cos_t, zeros, -sin_t,zeros, ones, zeros,sin_t, zeros, cos_t].reshape(-1,3,3) #[len(grasps),3,3] 74 | #print(R1) 75 | axis_y = major_pc #(-1,3) 76 | 77 | #设定一个与抓取y轴垂直且与C:x-o-y平面平行的单位向量作为初始x轴 78 | axis_x = np.c_[axis_y[:,[1]], -axis_y[:,[0]], zeros] 79 | #查找模为0的行，替换为[1,0,0] 80 | axis_x[np.linalg.norm(axis_x,axis=1)==0]=np.array([1,0,0]) 81 | #单位化 82 | axis_x = axis_x / np.linalg.norm(axis_x,axis=1,keepdims=True) 83 | axis_y = axis_y / np.linalg.norm(axis_y,axis=1,keepdims=True) 84 | #右手定则，从x->y 85 | axis_z = np.cross(axis_x, axis_y) 86 | 87 | #这个R2就是一个临时的夹爪坐标系，但是它的姿态还不代表真正的夹爪姿态 88 | R2 = np.c_[axis_x, np.c_[axis_y, axis_z]].reshape(-1,3,3).swapaxes(1,2) 89 | #将现有的坐标系利用angle进行旋转，就得到了真正的夹爪坐标系， 90 | # 抽出x轴作为approach轴(原生dex-net夹爪坐标系) 91 | #由于是相对于运动坐标系的旋转，因此需要右乘 92 | R3=np.matmul(R2,R1) #(-1,3,3) 93 | ''' 94 | approach_normal =R3[:, :,0] 95 | #print(np.linalg.norm(approach_normal,axis=1,keepdims=True)) 96 | approach_normal = approach_normal / np.linalg.norm(approach_normal,axis=1,keepdims=True) 97 | #minor_pc=R3[:, :,2] 是一样的 98 | minor_pc = np.cross( approach_normal,major_pc) 99 | ''' 100 | #然后把平移向量放在每个旋转矩阵的最右侧，当成一列 101 | return R3 102 | 103 | def quaternion_from_matrix(matrix): 104 | """Return quaternion from rotation matrix. 105 | 106 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 107 | >>> q = quaternion_from_matrix(R) 108 | >>> np.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) 109 | True 110 | 111 | """ 112 | q = np.empty((4, ), dtype=np.float64) 113 | M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4] 114 | t = np.trace(M) 115 | if t > M[3, 3]: 116 | q[3] = t 117 | q[2] = M[1, 0] - M[0, 1] 118 | q[1] = M[0, 2] - M[2, 0] 119 | q[0] = M[2, 1] - M[1, 2] 120 | else: 121 | i, j, k = 0, 1, 2 122 | if M[1, 1] > M[0, 0]: 123 | i, j, k = 1, 2, 0 124 | if M[2, 2] > M[i, i]: 125 | i, j, k = 2, 0, 1 126 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 127 | q[i] = t 128 | q[j] = M[i, j] + M[j, i] 129 | q[k] = M[k, i] + M[i, k] 130 | q[3] = M[k, j] - M[j, k] 131 | q *= 0.5 / math.sqrt(t * M[3, 3]) 132 | return q 133 | 134 | def axis_angle(quaternion): 135 | """:obj:`np.ndarray` of float: The axis-angle representation for the rotation. 136 | """ 137 | qw, qx, qy, qz = quaternion 138 | theta = 2 * np.arccos(qw) 139 | return abs(theta) 140 | 141 | def rotation_from_matrix(matrix): 142 | """Return rotation angle and axis from rotation matrix. 143 | 144 | >>> angle = (random.random() - 0.5) * (2*math.pi) 145 | >>> direc = np.random.random(3) - 0.5 146 | >>> point = np.random.random(3) - 0.5 147 | >>> R0 = rotation_matrix(angle, direc, point) 148 | >>> angle, direc, point = rotation_from_matrix(R0) 149 | >>> R1 = rotation_matrix(angle, direc, point) 150 | >>> is_same_transform(R0, R1) 151 | True 152 | 153 | """ 154 | R = np.array(matrix, dtype=np.float64, copy=False) 155 | R33 = R[:3, :3] 156 | # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 157 | l, W = np.linalg.eig(R33.T) 158 | i = np.where(abs(np.real(l) - 1.0) < 1e-8)[0] 159 | if not len(i): 160 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 161 | direction = np.real(W[:, i[-1]]).squeeze() 162 | 163 | # rotation angle depending on direction 164 | cosa = (np.trace(R33) - 1.0) / 2.0 165 | if abs(direction[2]) > 1e-8: 166 | sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] 167 | elif abs(direction[1]) > 1e-8: 168 | sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] 169 | else: 170 | sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] 171 | angle = math.atan2(sina, cosa) 172 | return abs(angle) 173 | 174 | 175 | def posesTransWithinErrorBounds(trans1,trans2,rot1,rot2,trans_diff_threshold=0.05,rot_diff_threshold=0.5): 176 | '''计算两个抓取中心、pose之间的偏差是否超过阈值 177 | ''' 178 | trans_diff = np.linalg.norm(trans1 - trans2)#抓取中心偏差距离 179 | rot1_inverse = rot1.T 180 | rot_angle_diff = rotation_from_matrix(rot1_inverse.dot(rot2)) 181 | if trans_diffindex_j: 193 | centers_diff_ = centers_diff[index_j,index_i] 194 | theta_diff_=theta_diff[index_j,index_i] 195 | elif index_i30: 280 | # grasp_clusters = grasp_clusters[:30] 281 | 282 | #保存group with score 283 | pickel_name = scene_dir+'graspClusterWithScores.pickle' 284 | with open(cluster_pickel_name, 'wb') as f: 285 | pickle.dump(grasp_clusters, f) 286 | 287 | #当前场景聚类完毕 288 | print('Scene {}'.format(scene_index)+ ' legal grasp:{}'.format(len(legal_grasps))+' done') 289 | else: 290 | print(cluster_pickel_name+' Exist') 291 | 292 | 293 | 294 | def collision_check_pc_cuda(centers,poses,pc): 295 | """对CTG抓取姿态和Cpc进行碰撞检测(使用显卡加速计算) 296 | 返回符合要求的，场景点云点的索引，每个抓取都有自己的内部点索引，是一个二维list 297 | """ 298 | #对旋转后的抓取，逐个进行碰撞检测，并把没有碰撞的抓取保存下来 299 | bottom_centers = centers -ags.gripper.hand_depth * poses[:,:,0] 300 | #获得扩大夹爪尺寸 301 | hand_points = ags.get_hand_points_extend(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0]))# 302 | 303 | #mask =np.zeros(centers.shape[0]) 304 | poses_cuda=torch.from_numpy(poses).cuda() 305 | hand_points= torch.from_numpy(hand_points).cuda() 306 | bottom_centers = torch.from_numpy(bottom_centers).cuda() 307 | pc = torch.from_numpy(pc).cuda() 308 | 309 | 310 | #gripper_points_p = torch.tensor([hand_points[10][0],hand_points[13][1],hand_points[9][2]]).reshape(1,-1).cuda() 311 | #gripper_points_n = torch.tensor([hand_points[20][0],hand_points[9][1],hand_points[10][2]]).reshape(1,-1).cuda() 312 | gripper_points_p = torch.tensor([hand_points[4][0],hand_points[2][1],hand_points[1][2]]).reshape(1,-1).cuda() 313 | gripper_points_n = torch.tensor([hand_points[8][0],hand_points[1][1],hand_points[4][2]]).reshape(1,-1).cuda() 314 | grasps_neighbor_points_index =[] 315 | 316 | #对每个抓取进行碰撞检测 317 | for i in tqdm(range(len(bottom_centers)),desc='collision_check_pc'): 318 | #得到标准的旋转矩阵 319 | matrix = poses_cuda[i] 320 | #转置=求逆（酉矩阵） 321 | grasp_matrix = matrix.T # same as cal the inverse 322 | #获取所有的点相对于夹爪底部中心点的向量 323 | points = pc - bottom_centers[i].reshape(1, 3) 324 | points_g = torch.mm(grasp_matrix, points.T).T #[len(pc),3] 325 | # 326 | points_p = points_g-gripper_points_p #[len(pc),3] 327 | points_n = points_g-gripper_points_n #[len(pc),3] 328 | points_index =torch.where(torch.sum((torch.mul(points_p,points_n)<0),dim=1)==3)[0] 329 | 330 | 331 | 332 | #points_in_close_area=points_g[check_op] #(-1,3) 333 | #if points_in_gripper_index.shape[0] == 0:#不存在夹爪点云碰撞 334 | if len(points_index)==0: 335 | print(centers[i],i) 336 | #print(centers[i-1],poses[i-1],i-1) 337 | #print(centers[i+1],poses[i+1],i+1) 338 | raise ValueError("Grasp has no neighbor points!") 339 | #print("Grasp has no neighbor points!") 340 | else: 341 | grasps_neighbor_points_index.append(points_index.cpu().numpy()) 342 | 343 | return grasps_neighbor_points_index 344 | 345 | def display_grasps(center, pose, color): 346 | # 347 | center_point = center 348 | approach_normal = pose[:,0] 349 | major_pc = pose[:,1] 350 | 351 | #计算夹爪bottom_center 352 | grasp_bottom_center = -ags.gripper.hand_depth * approach_normal + center_point 353 | # 354 | hand_points = ags.get_hand_points(grasp_bottom_center, approach_normal, major_pc) 355 | #固定夹爪作为参考系时 356 | #local_hand_points = ags.get_hand_points(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0])) 357 | ags.show_grasp_3d(hand_points, color=color) 358 | #ags.show_grasp_norm_oneside(center_point,approach_normal,axis_y,minor_pc) 359 | 360 | return True 361 | 362 | def show_points(pc, clusters_sets=[],centers=[],poses=[],name='Scene Mask', scale_factor=.004): 363 | '''显示生成结果 364 | cluster_0 = pc[list(clusters_sets[0])] 365 | cluster_1 = pc[list(clusters_sets[1])] 366 | cluster_2 = pc[list(clusters_sets[2])] 367 | cluster_3 = pc[list(clusters_sets[3])] 368 | cluster_4 = pc[list(clusters_sets[4])] 369 | ''' 370 | m=len(clusters_sets) 371 | color_ = [(0, 0, 1),(1, 0, 0),(0, 1, 0),(0.22, 1, 1),(0.8, 0.8, 0.2), 372 | (0.5, 0, 1),(1, 0.62, 0),(0.53, 1, 0),(0.22, 0.63, 1),(0.8, 0.22, 0.2)] 373 | 374 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.25, 0.88, 0.81),size=(1800,1800)) 375 | mlab.points3d(pc[:, 0], pc[:, 1], 376 | pc[:, 2], color=(0.5,0.5,0.5), scale_factor=scale_factor) 377 | 378 | if m==0: 379 | mlab.show() 380 | else: 381 | if m>10: 382 | m=10 383 | 384 | m-=1 385 | for i in range(m): 386 | j=m-i 387 | mlab.points3d(pc[list(clusters_sets[j])][:, 0], pc[list(clusters_sets[j])][:, 1], 388 | pc[list(clusters_sets[j])][:, 2], color=color_[j], scale_factor=scale_factor) 389 | 390 | if len(centers)==0 or len(poses)==0: 391 | mlab.show() 392 | else: 393 | for index,center in enumerate(centers): 394 | display_grasps(center,poses[index],color="d") 395 | mlab.show() 396 | 397 | 398 | 399 | if __name__ == '__main__': 400 | 401 | #先用显卡进行差异矩阵的计算 402 | for i in range(len(data_path)):#第i个场景 403 | scene_name = data_path[i].split('/')[-1].split('.')[0] 404 | scene_dir = mask_files_dir+scene_name+'/' 405 | pickel_name = scene_dir+'/clusterMatrix_{}_{}.pickle'.format(args.dis_min,args.theta_min) 406 | if not os.path.exists(scene_dir): 407 | os.makedirs(scene_dir) 408 | 409 | if not os.path.exists(pickel_name): 410 | pickel_name_ = glob.glob(mask_files_dir+scene_name+'/clusterMatrix*.pickle') 411 | if len(pickel_name_):#如果存在生成好的文件 412 | #删除之前的文件 413 | for file in pickel_name_: 414 | os.remove(file) 415 | print('Delete' + file) 416 | 417 | 418 | #读取数据集数据 419 | data = np.load(data_path[i], allow_pickle=True)#读取样本数据 420 | #pc = data['view_cloud'].astype(np.float32)#点云三通道坐标数据 421 | 422 | #grasp_score = data['select_score'].astype(np.float32)#点云三通道坐标数据 423 | 424 | legal_grasps = data['select_frame'].astype(np.float32)#点云三通道坐标数据 425 | 426 | print('Scene {}'.format(i)+ ' legal grasp:{}'.format(len(legal_grasps))) 427 | grasp_centers = legal_grasps[:,:3,3]#抓取中心 [len(grasp),3] 428 | grasp_poses = legal_grasps[:,:3,:3]#抓取旋转矩阵 [len(grasp),3,3] 429 | 430 | #计算距离差矩阵 431 | grasp_centers_cuda = torch.from_numpy(grasp_centers).cuda().unsqueeze(0)#[1,len(grasps),3] 432 | grasp_centers_cuda_T = grasp_centers_cuda.clone().permute(1,0,2)#[len(grasps),1,3] 433 | 434 | grasp_centers_cuda = grasp_centers_cuda.repeat(grasp_centers.shape[0],1,1) 435 | grasp_centers_cuda_T = grasp_centers_cuda_T.repeat(1,grasp_centers.shape[0],1) 436 | 437 | grasp_centers_diff =grasp_centers_cuda-grasp_centers_cuda_T 438 | grasp_centers_diff_dis = torch.norm(grasp_centers_diff,dim=2)cores: 485 | pool_size = cores 486 | print('Cut pool size down to cores num') 487 | if pool_size>len(data_path): 488 | pool_size = len(data_path) 489 | print('We got {} raw pc, and pool size is {}'.format(len(data_path),pool_size)) 490 | scene_index = 0 491 | pool = [] 492 | for i in range(pool_size): 493 | pool.append(multiprocessing.Process(target=do_job,args=(scene_index,))) 494 | scene_index+=1 495 | [p.start() for p in pool] #启动多线程 496 | 497 | while scene_index0.8 and iou_r>0.8: 603 | delete_set_index.append(target_set_index)#交集占两者比例都大于80% 删除目标集 604 | clusters_sets[ref_index] = clusters_sets[ref_index] | clusters_sets[target_set_index] 605 | ref_index=next_ref_index# 606 | elif iou_t<0.8 and iou_r>0.8: 607 | if current_ref_index in delete_set_index: 608 | pass 609 | else: 610 | delete_set_index.append(current_ref_index)#交集占参考集比例大于80% 删除参考集 611 | elif iou_t>0.8 and iou_r<0.8: 612 | delete_set_index.append(target_set_index)#交集占目标集比例大于80% 删除目标集 613 | ref_index=next_ref_index 614 | else: 615 | ref_index=next_ref_index#交集占目标集与参考集比例都不超过80%，不删除集合 616 | 617 | #在所有的簇比对完成之后，才进行最终的删除 618 | clusters_sets = np.delete(clusters_sets,delete_set_index) 619 | clusters_index = np.delete(clusters_index,delete_set_index) 620 | 621 | #print('scene: {},raw cluster num: {}, current cluster num {},Ref index: {} '. 622 | # format(scene_index,raw_cluster_num,len(clusters_sets),ref_index)) 623 | 624 | grasp_index = [] 625 | for index in clusters_index: 626 | grasp_index=grasp_index+clusterWithScore[index][0] 627 | 628 | 629 | 630 | grasp_centers = grasp_centers[grasp_index] 631 | grasp_poses=grasp_poses[grasp_index] 632 | grasp_centers = grasp_centers[np.random.choice(len(grasp_centers), size=100, replace=True)] 633 | grasp_poses = grasp_poses[np.random.choice(len(grasp_poses), size=100, replace=True)] 634 | #显示结果 635 | #show_points(pc,clusters_sets,grasp_centers,grasp_poses) 636 | #show_points(pc,clusters_sets) 637 | show_points(pc) 638 | #mlab.close() 639 | 640 | print('Generate scene {} mask done, from {} to {}'.format(scene_index, raw_cluster_num, len(clusters_sets))) 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | print('All job done') 649 | 650 | 651 | 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | -------------------------------------------------------------------------------- /debug_tools/read_grasps_from_file.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # Author : Hongzhuo Liang 4 | # E-mail : liang@informatik.uni-hamburg.de 5 | # Description: 6 | # Date : 30/05/2018 9:57 AM 7 | # File Name : read_grasps_from_file.py 8 | # 使用python3 9 | import os 10 | import sys 11 | import re 12 | from meshpy.obj_file import ObjFile 13 | from meshpy.sdf_file import SdfFile 14 | from dexnet.grasping import GraspableObject3D 15 | import numpy as np 16 | from dexnet.visualization.visualizer3d import DexNetVisualizer3D as Vis 17 | from dexnet.grasping import RobotGripper 18 | from autolab_core import YamlConfig 19 | from mayavi import mlab 20 | from dexnet.grasping import GpgGraspSampler # temporary way for show 3D gripper using mayavi 21 | import glob 22 | import pickle 23 | import open3d as o3d 24 | 25 | import argparse 26 | 27 | #解析命令行参数 28 | parser = argparse.ArgumentParser(description='Show grasp from files') 29 | parser.add_argument('--gripper', type=str, default='baxter') 30 | args = parser.parse_args() 31 | 32 | 33 | # global configurations: 34 | home_dir = os.environ["HOME"] 35 | 36 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 37 | gripper = RobotGripper.load(args.gripper, home_dir + "/code/dex-net/data/grippers") 38 | ags = GpgGraspSampler(gripper, yaml_config) 39 | 40 | save_fig = False # save fig as png file 41 | show_fig = True # show the mayavi figure 42 | generate_new_file = False # whether generate new file for collision free grasps 43 | check_pcd_grasp_points = False 44 | 45 | 46 | def open_npy_and_obj(name_to_open_): 47 | 48 | #grasps_with_score 49 | npy_m_ = np.load(name_to_open_) 50 | #存放mesh模型的文件夹路径 51 | file_dir = home_dir + "/dataset/simulate_grasp_dataset/ycb/google_512k/" 52 | #获取当前模型名称 53 | object_name_ = name_to_open_.split("/")[-1].split('.npy')[0] 54 | #object_name_ = name_to_open_.split("/")[-1].split('_pgpd')[0] 55 | 56 | print(object_name_) 57 | 58 | ply_path_ = file_dir+object_name_+'_google_512k/'+object_name_+ "/google_512k/nontextured.ply" 59 | obj_path_= home_dir + "/dataset/simulate_grasp_dataset/ycb/google_16k/"+object_name_+"/google_16k/textured.obj" 60 | sdf_path_= file_dir+object_name_+'_google_512k/'+object_name_+"/google_512k/nontextured.sdf" 61 | graspable= file_dir+object_name_+'_google_512k/'+object_name_+"/google_512k/dex_net_graspable.pickle" 62 | 63 | 64 | if not check_pcd_grasp_points: 65 | #读取相关的obj以及sdf模型文件 66 | of = ObjFile(obj_path_) 67 | sf = SdfFile(sdf_path_) 68 | mesh = of.read() 69 | sdf = sf.read() 70 | #dex-net格式的可抓取对象 71 | with open(graspable, 'rb') as f: 72 | obj_= pickle.load(f) 73 | else: 74 | cloud_path = home_dir + "/dataset/ycb_rgbd/" + object_name_ + "/clouds/" 75 | pcd_files = glob.glob(cloud_path + "*.pcd") 76 | obj_ = pcd_files 77 | obj_.sort() 78 | return npy_m_, obj_, ply_path_, object_name_ 79 | 80 | 81 | def display_object(obj_): 82 | """display object only using mayavi""" 83 | Vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000)) 84 | Vis.mesh(obj_.mesh.trimesh, color=(0.5, 0.5, 0.5), style="surface") 85 | Vis.show() 86 | 87 | 88 | def display_gripper_on_object(obj_, grasp_): 89 | """display both object and gripper using mayavi""" 90 | # transfer wrong was fixed by the previews comment of meshpy modification. 91 | # gripper_name = "robotiq_85" 92 | # gripper = RobotGripper.load(gripper_name, home_dir + "/dex-net/data/grippers") 93 | # stable_pose = self.dataset.stable_pose(object.key, "pose_1") 94 | # T_obj_world = RigidTransform(from_frame="obj", to_frame="world") 95 | t_obj_gripper = grasp_.gripper_pose(gripper) 96 | 97 | stable_pose = t_obj_gripper 98 | grasp_ = grasp_.perpendicular_table(stable_pose) 99 | 100 | Vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000)) 101 | Vis.gripper_on_object(gripper, grasp_, obj_, 102 | gripper_color=(0.25, 0.25, 0.25), 103 | # stable_pose=stable_pose, # .T_obj_world, 104 | plot_table=False) 105 | Vis.show() 106 | 107 | def show_points(point, name='pc',color='lb', scale_factor=.0001): 108 | if color == 'b': 109 | color_f = (0, 0, 1) 110 | elif color == 'r': 111 | color_f = (1, 0, 0) 112 | elif color == 'g': 113 | color_f = (0, 1, 0) 114 | elif color == 'lb': # light blue 115 | color_f = (0.22, 1, 1) 116 | else: 117 | color_f = (1, 1, 1) 118 | if point.size == 3: # vis for only one point, shape must be (3,), for shape (1, 3) is not work 119 | point = point.reshape(3, ) 120 | mlab.points3d(point[0], point[1], point[2], color=color_f, scale_factor=scale_factor) 121 | else: # vis for multiple points 122 | mlab.points3d(point[:, 0], point[:, 1], point[:, 2], color=color_f, scale_factor=scale_factor) 123 | 124 | return point.shape[0] 125 | 126 | 127 | def display_grasps(grasp, graspable, color): 128 | """ 129 | 显示给定的抓取，做一下碰撞检测，和指定颜色 130 | grasp：单个抓取 131 | graspable：目标物体（仅做碰撞） 132 | color：夹爪的颜色 133 | """ 134 | center_point = grasp[0:3] #夹爪中心(指尖中心) 135 | major_pc = grasp[3:6] # binormal 136 | width = grasp[6] 137 | angle = grasp[7] 138 | level_score, refine_score = grasp[-2:] 139 | # cal approach 140 | cos_t = np.cos(angle) 141 | sin_t = np.sin(angle) 142 | #绕抓取y轴的旋转矩阵 143 | R1 = np.c_[[cos_t, 0, sin_t],[0, 1, 0],[-sin_t, 0, cos_t]] 144 | #print(R1) 145 | 146 | axis_y = major_pc 147 | #设定一个与y轴垂直且与世界坐标系x-o-y平面平行的单位向量作为初始x轴 148 | axis_x = np.array([axis_y[1], -axis_y[0], 0]) 149 | if np.linalg.norm(axis_x) == 0: 150 | axis_x = np.array([1, 0, 0]) 151 | #单位化 152 | axis_x = axis_x / np.linalg.norm(axis_x) 153 | axis_y = axis_y / np.linalg.norm(axis_y) 154 | #右手定则，从x->y 155 | axis_z = np.cross(axis_x, axis_y) 156 | 157 | #这个R2就是一个临时的夹爪坐标系，但是它的姿态还不代表真正的夹爪姿态 158 | R2 = np.c_[axis_x, np.c_[axis_y, axis_z]] 159 | 160 | #将现有的坐标系利用angle进行旋转，就得到了真正的夹爪坐标系， 161 | # 抽出x轴作为approach轴(原生dex-net夹爪坐标系) 162 | #由于是相对于运动坐标系的旋转，因此需要右乘 163 | approach_normal = R2.dot(R1)[:, 0] 164 | approach_normal = approach_normal / np.linalg.norm(approach_normal) 165 | 166 | 167 | minor_pc = np.cross( approach_normal,major_pc) 168 | 169 | #计算夹爪bottom_center 170 | grasp_bottom_center = -ags.gripper.hand_depth * approach_normal + center_point 171 | #以世界坐标系为参考系时 172 | hand_points = ags.get_hand_points(grasp_bottom_center, approach_normal, major_pc) 173 | #固定夹爪作为参考系时 174 | local_hand_points = ags.get_hand_points(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0])) 175 | #检测一下夹爪与目标物体的碰撞（这里使用原始函数，不检测与桌面的碰撞） 176 | if_collide = ags.check_collide(grasp_bottom_center, approach_normal, 177 | major_pc, minor_pc, graspable, local_hand_points) 178 | if_collide = False 179 | #只显示夹爪，不显示点云 180 | if not if_collide and (show_fig or save_fig): 181 | ags.show_grasp_3d(hand_points, color=color) 182 | #ags.show_grasp_norm_oneside(center_point,approach_normal,axis_y,minor_pc) 183 | 184 | return True 185 | elif not if_collide: 186 | return True 187 | else: 188 | return False 189 | 190 | 191 | def show_selected_grasps_with_color(m, ply_name_, title, obj_,pc): 192 | """显示选择出的抓取，将抓取序列中good的抓取和bad的抓取区分开来，并分别显示在两个窗口中 193 | m： 针对被抓物体的带有打分的grasp序列 194 | ply_name_ ： 被抓物体的mesh文件路径 195 | title：被抓物体的名称 196 | obj_：被抓物体的mesh文件 197 | """ 198 | print('Got {} grasp totally'.format(len(m))) 199 | #筛选出优质抓取 200 | m_good = m[m[:, -2] >= 2] 201 | if len(m_good)==0: 202 | print('No good grasps! Show next mesh') 203 | return 0 204 | #从优质的抓取中随机抽取出25个（如果都要的话，就太多了，不易于显示） 205 | max_n =50 206 | if len(m_good)>max_n: 207 | print('Got {} good grasps, show random {} grasps'.format(len(m_good),max_n)) 208 | info_good = 'Total:{}'.format(len(m)) + ' Good:{}'.format(len(m_good))+' Show:{}'.format(max_n) 209 | m_good = m_good[np.random.choice(len(m_good), size=max_n, replace=True)] 210 | else: 211 | print('Show {} good grasps'.format(len(m_good))) 212 | info_good = 'Total:{}'.format(len(m)) + ' Good:{}'.format(len(m_good))+' Show:{}'.format(len(m_good)) 213 | 214 | #筛选出不合格抓取 215 | m_bad = m[m[:, -2] <= 0.55] 216 | #抽选显示 217 | if len(m_bad)>max_n: 218 | print('Got {} bad grasps, show random {} grasps'.format(len(m_bad),max_n)) 219 | info_bad = 'Total:{}'.format(len(m)) + ' bad:{}'.format(len(m_bad))+' Show:{}'.format(max_n) 220 | m_bad = m_bad[np.random.choice(len(m_bad), size=max_n, replace=True)] 221 | else: 222 | print('Show {} bad grasps'.format(len(m_bad))) 223 | info_bad = 'Total:{}'.format(len(m)) + ' bad:{}'.format(len(m_bad))+' Show:{}'.format(len(m_bad)) 224 | 225 | collision_grasp_num = 0 226 | if save_fig or show_fig: 227 | # fig 1: good grasps 228 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000)) 229 | #导入目标物体的mesh模型 230 | mlab.pipeline.surface(mlab.pipeline.open(ply_name_)) 231 | #show_points(pc) 232 | 233 | for a in m_good: 234 | # display_gripper_on_object(obj, a) # real gripper 235 | collision_free = display_grasps(a, obj_, color="d") # simulated gripper 236 | if not collision_free: 237 | collision_grasp_num += 1 238 | 239 | if save_fig: 240 | mlab.title(info_good, size=0.5) 241 | mlab.savefig("good_"+title+".png") 242 | 243 | mlab.close() 244 | elif show_fig: 245 | mlab.title(info_good, size=0.5) 246 | 247 | 248 | if len(m_bad)!=0:# 249 | # fig 2: bad grasps 250 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000)) 251 | mlab.pipeline.surface(mlab.pipeline.open(ply_name_)) 252 | for a in m_bad: 253 | # display_gripper_on_object(obj, a) # real gripper 254 | collision_free = display_grasps(a, obj_, color=(1, 0, 0)) 255 | if not collision_free: 256 | collision_grasp_num += 1 257 | if save_fig: 258 | mlab.title(info_bad, size=0.5) 259 | mlab.savefig("bad_"+title+".png") 260 | mlab.close() 261 | elif show_fig: 262 | mlab.title(info_bad, size=0.5) 263 | mlab.show() 264 | elif generate_new_file: 265 | # only to calculate collision: 266 | collision_grasp_num = 0 267 | ind_good_grasp_ = [] 268 | for i_ in range(len(m)): 269 | collision_free = display_grasps(m[i_][0], obj_, color=(1, 0, 0)) 270 | if not collision_free: 271 | collision_grasp_num += 1 272 | else: 273 | ind_good_grasp_.append(i_) 274 | collision_grasp_num = str(collision_grasp_num) 275 | collision_grasp_num = (4-len(collision_grasp_num))*" " + collision_grasp_num 276 | print("collision_grasp_num =", collision_grasp_num, "| object name:", title) 277 | return ind_good_grasp_ 278 | 279 | 280 | def show_random_grasps(m, ply_name_, title, obj_): 281 | """显示选择出的抓取，将抓取序列中good的抓取和bad的抓取区分开来，并分别显示在两个窗口中 282 | m： 针对被抓物体的带有打分的grasp序列 283 | ply_name_ ： 被抓物体的mesh文件路径 284 | title：被抓物体的名称 285 | obj_：被抓物体的mesh文件 286 | """ 287 | #筛选出优质抓取 288 | #if len(m)>25: 289 | #从优质的抓取中随机抽取出25个（如果都要的话，就太多了，不易于显示） 290 | #m = m[np.random.choice(len(m), size=25, replace=True)] 291 | m_good = m[m[:, -1] > 0.1] 292 | m_good = m[np.random.choice(len(m), size=25, replace=True)] 293 | if len(m_good)==0: 294 | m_good = m 295 | 296 | collision_grasp_num = 0 297 | if save_fig or show_fig: 298 | # fig 1: good grasps 299 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000)) 300 | #导入目标物体的mesh模型 301 | mlab.pipeline.surface(mlab.pipeline.open(ply_name_)) 302 | 303 | for a in m_good: 304 | # display_gripper_on_object(obj, a) # real gripper 305 | collision_free = display_grasps(a, obj_, color="d") # simulated gripper 306 | if not collision_free: 307 | collision_grasp_num += 1 308 | 309 | if save_fig: 310 | mlab.savefig("good_"+title+".png") 311 | mlab.close() 312 | elif show_fig: 313 | mlab.title(title, size=0.5) 314 | mlab.show() 315 | elif generate_new_file: 316 | # only to calculate collision: 317 | collision_grasp_num = 0 318 | ind_good_grasp_ = [] 319 | for i_ in range(len(m)): 320 | collision_free = display_grasps(m[i_][0], obj_, color=(1, 0, 0)) 321 | if not collision_free: 322 | collision_grasp_num += 1 323 | else: 324 | ind_good_grasp_.append(i_) 325 | collision_grasp_num = str(collision_grasp_num) 326 | collision_grasp_num = (4-len(collision_grasp_num))*" " + collision_grasp_num 327 | print("collision_grasp_num =", collision_grasp_num, "| object name:", title) 328 | return ind_good_grasp_ 329 | 330 | 331 | def get_grasp_points_num(m, obj_): 332 | """获取夹爪内部点云的数量 333 | """ 334 | has_points_ = [] 335 | ind_points_ = [] 336 | for i_ in range(len(m)): 337 | grasps = m[i_][0] 338 | approach_normal = grasps.rotated_full_axis[:, 0] 339 | approach_normal = approach_normal / np.linalg.norm(approach_normal) 340 | major_pc = grasps.configuration[3:6] 341 | major_pc = major_pc / np.linalg.norm(major_pc) 342 | minor_pc = np.cross(approach_normal, major_pc) 343 | center_point = grasps.center 344 | 345 | #计算bottom_center 346 | grasp_bottom_center = -ags.gripper.hand_depth * approach_normal + center_point 347 | 348 | # hand_points = ags.get_hand_points(grasp_bottom_center, approach_normal, major_pc) 349 | local_hand_points = ags.get_hand_points(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0])) 350 | 351 | has_points_tmp, ind_points_tmp = ags.check_collision_square(grasp_bottom_center, approach_normal, 352 | major_pc, minor_pc, obj_, local_hand_points, 353 | "p_open") 354 | ind_points_tmp = len(ind_points_tmp) # here we only want to know the number of in grasp points. 355 | has_points_.append(has_points_tmp) 356 | ind_points_.append(ind_points_tmp) 357 | return has_points_, ind_points_ 358 | 359 | 360 | if __name__ == "__main__": 361 | #获取路径下，所有的.npy文件路径 362 | npy_names = glob.glob(home_dir + "/dataset/simulate_grasp_dataset/{}/antipodal_grasps/*.npy".format(args.gripper)) 363 | npy_names.sort() 364 | for i in range(len(npy_names)): 365 | #把夹爪姿态和打分，先从npy中读取出来 366 | grasps_with_score, obj, ply_path, obj_name = open_npy_and_obj(npy_names[i]) 367 | 368 | #获取原始点云 369 | mesh = o3d.io.read_triangle_mesh(ply_path) 370 | #得到点云对象 371 | raw_pc = o3d.geometry.TriangleMesh.sample_points_uniformly(mesh, np.asarray(mesh.vertices).shape[0] * 10) 372 | #均匀降采样 373 | voxel_pc = o3d.geometry.PointCloud.voxel_down_sample(raw_pc, 0.0005) 374 | #将点云转换为np对象 375 | pc = np.asarray(voxel_pc.points) 376 | 377 | 378 | 379 | print("load file {}".format(npy_names[i])) 380 | #显示抓取 381 | if 'banana' not in obj_name: 382 | continue 383 | ind_good_grasp = show_selected_grasps_with_color(grasps_with_score, ply_path, obj_name, obj,pc) 384 | print('All done') 385 | -------------------------------------------------------------------------------- /debug_tools/show_mesh_pc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 使用python3 3 | import os 4 | import sys 5 | import re 6 | from meshpy.obj_file import ObjFile 7 | from meshpy.sdf_file import SdfFile 8 | from dexnet.grasping import GraspableObject3D 9 | import numpy as np 10 | from dexnet.visualization.visualizer3d import DexNetVisualizer3D as Vis 11 | from dexnet.grasping import RobotGripper 12 | from autolab_core import YamlConfig 13 | from mayavi import mlab 14 | from dexnet.grasping import GpgGraspSampler # temporary way for show 3D gripper using mayavi 15 | import glob 16 | 17 | 18 | def show_points(point, name='raw_pc',color='lb', scale_factor=.004): 19 | mlab.figure(figure=name,bgcolor=(1, 1, 1), fgcolor=(0.25, 0.88, 0.81),size=(1800,1800)) 20 | if color == 'b': 21 | color_f = (0, 0, 1) 22 | elif color == 'r': 23 | color_f = (1, 0, 0) 24 | elif color == 'g': 25 | color_f = (0, 1, 0) 26 | elif color == 'lb': # light blue 27 | color_f = (0.22, 1, 1) 28 | else: 29 | color_f = (1, 1, 1) 30 | if point.size == 3: # vis for only one point, shape must be (3,), for shape (1, 3) is not work 31 | point = point.reshape(3, ) 32 | mlab.points3d(point[0], point[1], point[2], color=color_f, scale_factor=scale_factor) 33 | else: # vis for multiple points 34 | mlab.points3d(point[:, 0], point[:, 1], point[:, 2], color=color_f, scale_factor=scale_factor) 35 | 36 | 37 | -------------------------------------------------------------------------------- /debug_tools/show_raw_pc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | #读取模拟出的虚拟点云 3 | import os 4 | #复制文件 5 | import shutil 6 | import sys 7 | import glob 8 | import numpy as np 9 | import threading 10 | from mayavi import mlab 11 | import tty 12 | import termios 13 | import inspect 14 | import ctypes 15 | import argparse 16 | import time 17 | 18 | #解析命令行参数 19 | parser = argparse.ArgumentParser(description='Show raw point clouds') 20 | parser.add_argument('--gripper', type=str, default='baxter') 21 | parser.add_argument('--raw_pc', action='store_true')#出现raw 就选择显示原生点云，否则显示降采样后的点云 22 | 23 | parser.add_argument('--show', type=int, default=0)#可以选择直接显示某帧点云 24 | 25 | args = parser.parse_args() 26 | 27 | 28 | #用于结束键盘检测子线程 29 | def _async_raise(tid, exctype): 30 | """raises the exception, performs cleanup if needed""" 31 | tid = ctypes.c_long(tid) 32 | if not inspect.isclass(exctype): 33 | exctype = type(exctype) 34 | res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype)) 35 | if res == 0: 36 | raise ValueError("invalid thread id") 37 | elif res != 1: 38 | # """if it returns a number greater than one, you're in trouble, 39 | # and you should call it again with exc=NULL to revert the effect""" 40 | ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None) 41 | raise SystemError("PyThreadState_SetAsyncExc failed") 42 | 43 | #用于结束键盘检测子线程 44 | def stop_thread(thread): 45 | _async_raise(thread.ident, SystemExit) 46 | 47 | 48 | #按键检测 49 | def readchar(): 50 | fd = sys.stdin.fileno() 51 | old_settings = termios.tcgetattr(fd) 52 | try: 53 | tty.setraw(sys.stdin.fileno()) 54 | ch = sys.stdin.read(1) 55 | finally: 56 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 57 | return ch 58 | #按键检测 59 | def readkey(getchar_fn=None): 60 | getchar = getchar_fn or readchar 61 | c1 = getchar() 62 | if ord(c1) != 0x1b: 63 | return c1 64 | c2 = getchar() 65 | if ord(c2) != 0x5b: 66 | return c1 67 | c3 = getchar() 68 | return chr(0x10 + ord(c3) - 65) 69 | 70 | 71 | def show_points(point, name='raw_pc',color='lb', scale_factor=.004): 72 | mlab.figure(figure=name,bgcolor=(1, 1, 1), fgcolor=(0.25, 0.88, 0.81),size=(1800,1800)) 73 | if color == 'b': 74 | color_f = (0, 0, 1) 75 | elif color == 'r': 76 | color_f = (1, 0, 0) 77 | elif color == 'g': 78 | color_f = (0, 1, 0) 79 | elif color == 'lb': # light blue 80 | color_f = (0.22, 1, 1) 81 | else: 82 | color_f = (1, 1, 1) 83 | if point.size == 3: # vis for only one point, shape must be (3,), for shape (1, 3) is not work 84 | point = point.reshape(3, ) 85 | mlab.points3d(point[0], point[1], point[2], color=color_f, scale_factor=scale_factor) 86 | else: # vis for multiple points 87 | mlab.points3d(point[:, 0], point[:, 1], point[:, 2], color=color_f, scale_factor=scale_factor) 88 | 89 | def show_axis(scale_factor=.004): 90 | # un1 = grasp_bottom_center + 0.5 * grasp_axis * self.gripper.max_width 91 | un2 = [0,0,0] 92 | # un3 = grasp_bottom_center + 0.5 * minor_pc * self.gripper.max_width 93 | # un4 = grasp_bottom_center 94 | # un5 = grasp_bottom_center + 0.5 * grasp_normal * self.gripper.max_width 95 | # un6 = grasp_bottom_center 96 | #把当前选择的点画成一个球 97 | #show_points(np.array([0,0,0]), color='g', scale_factor=scale_factor * 4) 98 | # self.show_points(un1, scale_factor=scale_factor * 4) 99 | # self.show_points(un3, scale_factor=scale_factor * 4) 100 | # self.show_points(un5, scale_factor=scale_factor * 4) 101 | # self.show_line(un1, un2, color='g', scale_factor=scale_factor) # binormal/ major pc 102 | # self.show_line(un3, un4, color='b', scale_factor=scale_factor) # minor pc 103 | # self.show_line(un5, un6, color='r', scale_factor=scale_factor) # approach normal 104 | #画箭头，起始点坐标xyz为un2[0], un2[1], un2[2] 105 | #终止点坐标xyz 为grasp_axis[0], grasp_axis[1], grasp_axis[2] 106 | mlab.quiver3d(un2[0], un2[1], un2[2], 0,1,0, 107 | scale_factor=0.5, line_width=1, color=(0, 1, 0), mode='arrow') 108 | mlab.quiver3d(un2[0], un2[1], un2[2], 0,0,1, 109 | scale_factor=0.5, line_width=1, color=(0, 0, 1), mode='arrow') 110 | mlab.quiver3d(un2[0], un2[1], un2[2], 1,0,0, 111 | scale_factor=0.5, line_width=1, color=(1, 0, 0), mode='arrow') 112 | 113 | 114 | 115 | #按键检测子线程 116 | def do_job(): 117 | global all_done,quit 118 | while not all_done: 119 | if all_done: 120 | break 121 | #读取外部按键 122 | key=readkey() 123 | if key=='q': 124 | quit = True #退出程序 125 | print('已经按下q 键，关掉点云窗口即可完全退出') 126 | break 127 | 128 | def get_raw_pc_list(file_dir_,type='pc.npy'): 129 | file_list = [] 130 | for root, dirs, files in os.walk(file_dir_): 131 | for file in files: 132 | if file==type: 133 | file_list.append(os.path.join(root,file)) 134 | #排序 135 | file_list.sort() 136 | return file_list 137 | 138 | 139 | 140 | if __name__ == '__main__': 141 | 142 | home_dir = os.environ['HOME'] 143 | pcs_dir = home_dir+"/dataset/simulate_grasp_dataset/{}/scenes/".format(args.gripper) 144 | #pcs_path_list = glob.glob(pcs_dir+"*/raw_pc.npy") 145 | if args.raw_pc: 146 | pcs_path_list = get_raw_pc_list(pcs_dir,'raw_pc.npy') #该方法可以根据场景编号排序 147 | max_digits = len(pcs_path_list[0].split('/')[-2]) 148 | selected_path = os.path.join(pcs_dir,str(args.show).zfill(max_digits),'raw_pc.npy') 149 | else: 150 | pcs_path_list = get_raw_pc_list(pcs_dir,'pc.npy') #该方法可以根据场景编号排序 151 | max_digits = len(pcs_path_list[0].split('/')[-2]) 152 | selected_path = os.path.join(pcs_dir,str(args.show).zfill(max_digits),'pc.npy') 153 | 154 | 155 | 156 | 157 | all_done = False 158 | quit = False 159 | print("按下q退出查看") 160 | lock = threading.Lock() 161 | 162 | if args.show ==0: 163 | #开辟一个子线程做按键检测 164 | keyboardcheck = threading.Thread(target=do_job,) 165 | keyboardcheck.start() 166 | 167 | for pc_path in pcs_path_list: 168 | #读取 169 | pc_raw = np.load(pc_path) 170 | #剔除NAN值 171 | pc = pc_raw[~np.isnan(pc_raw).any(axis=1)] 172 | 173 | show_points(pc,name='scene index '+pc_path.split('/')[-2]) 174 | show_axis() 175 | print("Show:",pc_path) 176 | print("关闭点云窗口播放下一张") 177 | 178 | mlab.show() 179 | 180 | if quit: 181 | print("用户中断点云查看") 182 | break 183 | while not quit: 184 | time.sleep(0.8) 185 | print('所有点云播放完毕，按下q退出子线程') 186 | keyboardcheck.join() 187 | 188 | 189 | else:#直接查看某帧 190 | if args.show>=len(pcs_path_list): 191 | raise NameError("Can not find point cloud ",args.show) 192 | 193 | pc_raw = np.load(selected_path) 194 | pc = pc_raw[~np.isnan(pc_raw).any(axis=1)] 195 | show_points(pc,name='scene index '+selected_path.split('/')[-2]) 196 | mlab.show() 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | -------------------------------------------------------------------------------- /debug_tools/test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | import multiprocessing 4 | from autolab_core import YamlConfig 5 | import argparse 6 | import os 7 | from mayavi import mlab 8 | from tqdm import tqdm 9 | import time 10 | #解析命令行参数 11 | parser = argparse.ArgumentParser(description='Test') 12 | parser.add_argument('--gripper', type=str, default='baxter') 13 | args = parser.parse_args() 14 | 15 | a =200 16 | max_digits = 5 17 | print(str(a).zfill(5)) 18 | 19 | a = np.arange(90).reshape(-1,3,3) 20 | b = np.arange(90).reshape(-1,3,3) 21 | print(a[0]) 22 | print(b[0]) 23 | print(a[0].dot(b[0])) 24 | print("============================") 25 | 26 | c =np.matmul(a,b) 27 | d =np.c_[c,np.array([0,0,1]).reshape(-1,3,1).repeat(c.shape[0],axis=0)] 28 | e = np.array([0,0,0,1]).reshape(1,1,4).repeat(c.shape[0],axis=0) 29 | d=np.concatenate((d,e),axis = 1) 30 | print(c[0]) 31 | print("============================") 32 | 33 | 34 | a= np.arange(36).reshape(-1,3,3) #(-1,3,3) 35 | print(a) 36 | print("#") 37 | b = a[:,:,0] #(-1,3) 38 | b[:,2]=0#(-1,3) 39 | print(b) 40 | print("#") 41 | print(a) 42 | print("#") 43 | #minus_mask = (a[:,:,0][:,2]==0) 44 | #print(minus_mask) 45 | 46 | print("============================") 47 | """使用索引和fancy indexing对numpy进行切片索引，是否会返回一个新对象？ 48 | 参考 https://scipy-cookbook.readthedocs.io/items/ViewsVsCopies.html 49 | """ 50 | arr = np.arange(10) 51 | slice = arr[2:5] #对arr进行切片，并不会返回新对象，而是返回一个reference/view 指向该部分的内容 52 | slice[:] = 12 #使用该reference直接修改内容，会直接导致原arr对象被修改 53 | print(arr) 54 | slice[:]= slice[:]/2 #使用符号运算 55 | print(slice) 56 | print(arr) 57 | slice = slice / 2 58 | print(slice) 59 | print(arr) 60 | print("#") 61 | 62 | 63 | 64 | index_b =[1,2,3] 65 | #在等号右侧使用fancy indexing， 66 | slice_b = arr[index_b] #返回的不是一个view，而是copy对应部分数据的新对象 67 | print(slice_b) 68 | slice_b[0]=100 69 | print(slice_b) 70 | print(arr) 71 | print('#') 72 | #在等号左侧直接使用fancy indexing, 并不会创建view也不会创建copy，就相当于直接操作 73 | #因为没有必要做这些 74 | arr[index_b] = 100 75 | print(arr) 76 | print('#') 77 | # 78 | arr[index_b][1] = 100 79 | print(arr) 80 | print('#') 81 | 82 | #在右侧的简单索引 83 | arr = np.arange(9).reshape(3,3) 84 | slice_c = arr[0] #退化(-1,) 返回view 85 | slice_c[0]=50 86 | print(arr) 87 | print('#') 88 | 89 | arr = np.arange(9) 90 | slice_c = arr[0] #退化(-1,) 返回的还是一个view，但是因为此时 arr[0]退化为一个np.int64的数字 91 | #slice_c[0]=50 #是无法使用这种访问的形式来直接修改arr的值的 92 | slice_c=50 #这样子实际上是把slice_c 分配到了一个新的python int 类型对象上，并不会修改arr值 93 | print(arr) 94 | """总结： 95 | 1.在右侧的简单索引或者切片返回的是view 96 | 2.在右侧的fancy indexing 返回的是copy 97 | 3.在左侧的简单索引切片或者fancy indexing，都相当于直接操作原数组，不创建view或者copy 98 | """ 99 | 100 | 101 | 102 | print("============================") 103 | 104 | a = np.arange(2,7,2) 105 | print(a) 106 | b=np.arange(20) 107 | 108 | b=list(set(b).difference(set(a))) 109 | print(b) 110 | 111 | print("============================") 112 | 113 | 114 | 115 | 116 | def do_job(job_id): #处理函数 处理index=i的模型 117 | grasps_with_score_ = range(5) 118 | print("Worker {} got {} grasps.".format(job_id,len(grasps_with_score_))) 119 | #虽然grasps_with_score是全局变量，但是已经在子线程中拷贝了一份， 120 | # 这里修改的是子进程中的数据，并不会影响主线程中的grasps_with_score 121 | global grasps_with_score 122 | #添加到外部的采集库中 123 | grasps_with_score+=grasps_with_score_ 124 | print("Now we have {} grasps in total".format(len(grasps_with_score))) 125 | 126 | 127 | def do_job1(job_id): #测试多线程进度条 128 | #trange(i)是tqdm(range(i))的一种简单写法 129 | for i in tqdm(range(100), desc='Processing'+str(job_id)): 130 | time.sleep(0.5) 131 | 132 | def do_job2(id): 133 | '''测试多线程下的显卡调用 134 | ''' 135 | 136 | 137 | from dexnet.grasping import RobotGripper 138 | from dexnet.grasping import GpgGraspSampler 139 | 140 | if __name__ == '__main__': 141 | home_dir = os.environ['HOME'] 142 | manager = multiprocessing.Manager() 143 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 144 | gripper = RobotGripper.load(args.gripper, home_dir + "/code/dex-net/data/grippers") 145 | ags = GpgGraspSampler(gripper, yaml_config) 146 | 147 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000)) 148 | grasp_bottom_center = np.array([0, 0, 0]) 149 | approach_normal = np.array([1, 0, 0]) 150 | binormal = np.array([0, 1, 0]) 151 | minor_pc = np.array([0,0,1]) 152 | center_point=grasp_bottom_center+ags.gripper.hand_depth * approach_normal 153 | ps = ags.get_hand_points(grasp_bottom_center, approach_normal, binormal) 154 | 155 | ags.show_grasp_3d(ps) 156 | ags.show_points(ps,scale_factor=0.005) 157 | for i,p in enumerate(ps): 158 | mlab.text3d(p[0],p[1],p[2],str(i),scale = (0.005),color=(0,0,1)) 159 | 160 | 161 | #grasp_bottom_center = -ags.gripper.hand_depth * np.array([1,0,0]) + np.array([0,0,0]) 162 | ''' 163 | mlab.text3d(0,-0.008,-0.01,'bottom_center',scale = (0.002),color=(0,0,1)) 164 | mlab.text3d(0.03,0,0,'approach_normal',scale = (0.004),color=(1,0,0)) 165 | mlab.text3d(0,0.03,0,'binormal',scale = (0.004),color=(0,1,0)) 166 | mlab.text3d(0,0,0.03,'minor_pc',scale = (0.004),color=(0,0,1)) 167 | ''' 168 | 169 | ags.show_grasp_norm_oneside(center_point,approach_normal,binormal,minor_pc,scale_factor=0.001) 170 | 171 | 172 | mlab.show() 173 | 174 | grasps_with_score=manager.list() 175 | pool =[] 176 | 177 | for i in range(10): 178 | pool.append(multiprocessing.Process(target=do_job1, args=(i,))) 179 | #启动多线程 180 | [p.start() for p in pool] 181 | #等待所有进程结束，返回主进程 182 | [p.join() for p in pool] 183 | #grasps_with_score = [x for x in grasps_with_score] 184 | #print(len(grasps_with_score)) 185 | 186 | #print(type(grasps_with_score)) 187 | 188 | -------------------------------------------------------------------------------- /debug_tools/test1.py: -------------------------------------------------------------------------------- 1 | from multiprocessing import Pool, cpu_count 2 | import os, time, random 3 | from tqdm import tqdm 4 | import glob 5 | 6 | 7 | class MyMultiprocess(object): 8 | def __init__(self, process_num): 9 | self.pool = Pool(processes=process_num) 10 | 11 | def work(self, func, args): 12 | for arg in args: 13 | self.pool.apply_async(func, (arg,)) 14 | self.pool.close() 15 | self.pool.join() 16 | 17 | 18 | def func(num): 19 | name = num 20 | for i in tqdm(range(5), ncols=80, desc='执行任务' + str(name) + ' pid:' + str(os.getpid())): 21 | # time.sleep(random.random() * 3) 22 | time.sleep(1) 23 | 24 | def takeSceneIndex(elem): 25 | index = elem.split('/')[-2] 26 | return int(index) 27 | 28 | if __name__ == "__main__": 29 | 30 | home_dir = os.environ['HOME'] 31 | #场景文件夹 32 | scenes_dir = home_dir+"/dataset/simulate_grasp_dataset/baxter/scenes/" 33 | scenes_grasp_file_paths = glob.glob(scenes_dir+'*/legal_grasps_with_score.npy') 34 | print(type(scenes_grasp_file_paths)) 35 | scenes_grasp_file_paths.sort() 36 | 37 | raw_pc_paths = glob.glob(scenes_dir+'*/raw_pc.npy') 38 | 39 | 40 | 41 | print('父进程 %s.' % os.getpid()) 42 | mymultiprocess = MyMultiprocess(cpu_count()) 43 | start = time.time() 44 | mymultiprocess.work(func=func, args=range(10)) 45 | end = time.time() 46 | print("\n应用多进程耗时: %0.2f seconds" % (end - start)) 47 | 48 | start = time.time() 49 | for i in range(10): 50 | func(i) 51 | end = time.time() 52 | print("\n不用多进程耗时: %0.2f seconds" % (end - start)) -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210828201622210-1630152983587.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210828201622210-1630152983587.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210828201730737.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210828201730737.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210828213543866.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210828213543866.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210828213735005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210828213735005.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829092109553.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829092109553.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829100519836.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829100519836.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829103319772.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829103319772.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829112931824.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829112931824.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829120857478.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829120857478.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829155239115.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829155239115.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829191441837.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829191441837.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829192726091.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829192726091.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829193625661.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829193625661.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829193716537.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829193716537.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829195830930.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829195830930.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829200430654.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829200430654.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829202625667.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829202625667.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829205518316.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829205518316.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210829211317675.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210829211317675.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210902110348277.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210902110348277.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210904191110685.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210904191110685.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210904192108478.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210904192108478.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210906085839583.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210906085839583.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.assets/image-20210907165347485.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/Dex-net典范抓取坐标系与求解.assets/image-20210907165347485.png -------------------------------------------------------------------------------- /doc/Dex-net典范抓取坐标系与求解.md: -------------------------------------------------------------------------------- 1 | # Dex-net 抓取表示与旋转 2 | 3 | Dex-Net设定，坐标系主要有如下三种： 4 | 5 | 1. $W$ （世界）参考坐标系 6 | 2. $M$ 夹爪mesh模型坐标系 7 | 3. $G'$ 夹爪坐标系 gripper frame 夹爪坐标系固定在实际的机器人夹爪上，不同于抓取坐标系，抓取坐标系是夹爪坐标系的目标位置 8 | 4. $GP$ 抓取碰撞检测坐标系 collision check frame 9 | 5. $G$ 抓取典范坐标系，简称抓取坐标系， grasp canonical frame 。在dex-net中，抓取典范坐标系$G$与虚拟夹爪的固连方式不一定 和 夹爪坐标系$G'$ 与真实夹爪相同。 10 | 11 | ## （世界）参考坐标系 12 | 13 | 世界参考坐标系会随着不同的场景而发生变化，根据具体的应用而定，比如: 14 | 15 | - 在对mesh模型抓取采样过程中，世界参考坐标系一般选取的是mesh模型自带参考系； 16 | - 在抓取执行的时候，世界参考系一般选取的是机械臂基座坐标系； 17 | 18 | 在计算各种位置姿态时候，需要时时提醒自己，该位姿的参考系是哪个。 19 | 20 | 21 | 22 | ## 夹爪mesh模型坐标系 23 | 24 | 该坐标系表示了夹爪的mesh模型本身的参考坐标系，这个坐标系不参与抓取计算，在den-net中仅仅用于演示结果渲染 25 | 26 | 27 | 28 | 29 | 30 | ## 夹爪坐标系 Gripper frame 31 | 32 | 其中，不同型号的夹爪坐标系$G'$与实际夹爪固连的方式可能是不同的，比如panda夹爪与baxter的夹爪坐标系设定是不同的(panda x轴朝向前方，baxter x轴朝向后方)： 33 | 34 | image-20210829112931824 35 | 36 | 每个型号的gripper frame都与其具体的物理夹爪相对应。 37 | 38 | 39 | 40 | 41 | 42 | ## 抓取碰撞检测坐标系 collision check frame 43 | 44 | 与抓取典范坐标系有些相似，只是坐标系原点移动到了grasp bottom center位置处，这个坐标系一眼在代码里用于某个抓取姿态下，夹爪与环境的碰撞检测，因为图上的各个标号点的位置计算，都是根据bottom center坐标以及三个坐标轴的向量确定的。 45 | 46 | 但是注意，采样出来的抓取，都是以抓取典范坐标系表示的，而非碰撞坐标系。 47 | 48 | image-20210904192108478 49 | 50 | 51 | 52 | 53 | 54 | ## 抓取典范坐标系 Grasp canonical frame 55 | 56 | 思考，思考实际执行抓取时候，机械臂将会驱动夹爪坐标系$G'$到达指定的（相对于基座坐标系）位置姿态来到达指定的位姿来执行抓取，然而，不同型号夹爪惯用的夹爪坐标系$G'$固连方式不同，如何保证算法计算出的结果可以正确地应用于不同型号的夹爪上呢？ 57 | 58 | Dex-net 通过设定一个虚拟夹爪，并在其上固定一个虚拟的典范（标准）坐标系作为中介，并且在计算过程中，算法统一使用该典范抓取坐标系来计算和表示抓取的位置姿态（相对于mesh坐标系或者相对于相机坐标系）； 59 | 60 | 最终，使用不同的夹爪执行抓取时，只需要根据具体的机器人夹爪，给定夹爪坐标系到典范抓取坐标系的变换矩阵$^{G'}T_G$，就可以将算法计算的抓取姿态转换为夹爪姿态； 61 | 62 | image-20210829120857478 63 | 64 | 这种典范坐标系，就称为**抓取典范坐标系grasp canonical frame**, 其实也可以理解为**典范夹爪坐标系**（选定了一个标准规范的夹爪，试用其夹爪坐标系作为中介）；后文简记为grasp frame。 65 | 66 | ### 抓取典范坐标系的基本设定与抓取的表示 67 | 68 | Dex-net设定的抓取典范坐标系定义满足右手定则，保证了典范坐标系的基本形式是确定的。 69 | 70 | image-20210829103319772 71 | 72 | 之后，Dex-net将典范坐标系固连在虚拟夹爪上，为每个轴分配具体的含义。具体固连方式如下图，认为原点处于两个夹爪指尖的连线中心点位置，夹爪坐标系x轴指向夹爪外部(接近轴)，y轴就是两个夹爪合并的方向，z轴与这两个方向正交；设定z轴的指向为虚拟夹爪的前方，虚拟夹爪也是有正反的，这样才能唯一确定夹爪与典范抓取坐标系的固连方式。 73 | 74 | image-20210829092109553 75 | 76 | 其中，典范抓取坐标系各个轴的名称为 77 | 78 | - x轴 称为approach axis 接近轴 79 | - y轴 称为 binormal axis 或者 grasp axis 或者major_pc 80 | - z轴 称为 minor_pc 81 | 82 | 83 | 84 | **注意**：这里只是确定了典范抓取坐标系的形式，以及与虚拟夹爪的固连方式；但是典范坐标系在世界参考坐标系中的具体位置姿态还是没有确定的。 85 | 86 | 87 | 88 | ### 抓取姿态的表示(7维度抓取向量) 89 | 90 | 在抓取采样时，一个具体的抓取姿态，可以使用典范抓取坐标系G与mesh坐标系M的相对变换关系$^{M}T_G$ 来确定，Dex-net在使用Antipodal等采样法对mesh模型C采样候选抓取时，使用一个7维度的向量$g=(p,axis,\theta)\in \mathbb{R}^7$来表示变换关系$^{M}T_G$ ，其中$p=(x,y,z)\in \mathbb{R}^3$为坐标系原点， $axis=(axis_x,axis_y,axis_z)\in \mathbb{R}^3$代表典范坐标系y轴的单位向量， $\theta\in [-\pi,\pi]$ 为典范坐标系绕G-Y轴的旋转量（具体旋转起点）。 91 | 92 | 包括抓取采样算法获得的候选抓取姿态，都是使用的$g=(p,axis,\theta)\in \mathbb{R}^7$来进行的表示，以下是抓取采样器输出的抓取向量，Dex-net除了输出位置姿态外，还输出了一些夹爪张开宽度等参数，拓展成为10维度向量，代表一个完整的抓取配置`Grasp configuration`： 93 | 94 | ```python 95 | def configuration_from_params(center, axis, width, angle=0, jaw_width=0, min_width=0): 96 | """ Converts grasp parameters to a configuration vector. 97 | """ 98 | if np.abs(np.linalg.norm(axis) - 1.0) > 1e-5: 99 | raise ValueError('Illegal grasp axis. Must be norm one') 100 | #首先创建一个1行10列的行向量 101 | configuration = np.zeros(10) 102 | #前3个数字是，夹爪指尖连线中心点坐标，表示夹爪的位置 103 | configuration[0:3] = center 104 | #3~5是抓取的闭合轴G-Y 105 | configuration[3:6] = axis 106 | #猜测是计算出的夹爪张开宽度，没搞明白 107 | configuration[6] = width 108 | #绕G-Y轴旋转的角度 109 | configuration[7] = angle 110 | #没搞明白 111 | configuration[8] = jaw_width 112 | #该型号夹爪最小的闭合宽度 113 | configuration[9] = min_width 114 | return configuration 115 | 116 | ``` 117 | 118 | 119 | 120 | ### 使用抓取向量确定抓取典范坐标系位置与姿态 121 | 122 | 思考，如何从这个7维度的向量$g=(p,axis,\theta)\in \mathbb{R}^7$来唯一确定的典范坐标系姿态$^{M}T_G$ ？即到底是如何确定抓取坐标系三个坐标轴G-X,G-Y,G-Z在模型参考系中的单位向量呢？ 123 | 124 | 考虑如下的情况（即仅考虑旋转）,假设抓取坐标系G-Y轴向量与模型参考系M-Y重合时，即$\vec{axis}=(0,1,0)$ 时 125 | 126 | 1. 选取向量$\vec{approach'}=(axis_y,-axis_x,0)=(1,0,0)$ 作为临时approach轴，此时该向量与G-Y轴垂直，且满足G-Z坐标值为0，即与M:X-O-Y平面平行 127 | 2. 将$\vec{approach'}$绕G-Y轴旋转$\alpha$角，得到抓取坐标系的approach向量（G-X轴） 128 | 3. 使用符合右手螺旋定律的叉乘，找到G-Z轴 129 | 130 | 131 | 132 | image-20210829193716537 133 | 134 | 一般情况： 135 | 136 | 1. 在模型参考系M:X-O-Y平面中找到一条与G-Y轴垂直的向量，作为临时$\vec{approach'}$轴 137 | 2. 将临时$\vec{approach'}$轴绕G-Y轴正方向旋转$\alpha$角，得到真实G-X轴即$\vec{approach}$轴。 138 | 3. 使用符合右手螺旋定律进行叉乘，得到G-Z轴 139 | 140 | 141 | 142 | 143 | 144 | 具体的，参考系为世界坐标系记为W，抓取坐标系记为G 145 | 146 | - G-Y轴方向确定的方式是，在采样过程中，先在物体上随机抽点，作为接触点`c1` ，之后在指定的摩擦锥范围内做射线，穿透待抓取物体，在另一侧找到射线与物体的接触点`c2`，则以向量$\vec{c_1c_2}$ 作为抓取射线，其单位向量就作为G-Y轴 147 | 148 | ```python 149 | #dex-net/grasping/grasp.py 150 | @staticmethod 151 | def axis_from_endpoints(g1, g2): 152 | """ Normalized axis of grasp from endpoints as np 3-arrays """ 153 | grasp_axis = g2 - g1 154 | if np.linalg.norm(grasp_axis) == 0: 155 | return grasp_axis 156 | return grasp_axis / np.linalg.norm(grasp_axis) 157 | ``` 158 | 159 | - G-X轴具体的确定方法是，先设定一个与G-Y轴垂直，且与W: X-O-Y平面（即世界坐标系X-O-Y平面平行）的单位向量作为G-X轴的初始方向，之后，绕着G-Y轴（满足右手螺旋定律）正方向旋转$\alpha$角，得到正确的G-X轴。 160 | 161 | ```python 162 | #设定一个与y轴垂直且与世界坐标系x-o-y平面平行的单位向量作为初始x轴 163 | axis_x = np.array([axis_y[1], -axis_y[0], 0]) #该向量z轴值为0，说明该向量与参考系的x-o-y平面平行 164 | if np.linalg.norm(axis_x) == 0: 165 | axis_x = np.array([1, 0, 0]) 166 | #单位化 167 | axis_x = axis_x / np.linalg.norm(axis_x) 168 | #这个R2就是一个临时的抓取坐标系，但是它的姿态还不代表真正的抓取姿态 169 | R2 = np.c_[axis_x, np.c_[axis_y, axis_z]] 170 | #将现有的坐标系利用angle进行旋转，就得到了真正的夹爪坐标系， 171 | # 抽出x轴作为抓取坐标系approach轴，由于是相对于运动坐标系的旋转，因此需要右乘 172 | approach_normal = R2.dot(R1)[:, 0] 173 | approach_normal = approach_normal / np.linalg.norm(approach_normal) 174 | 175 | ``` 176 | 177 | 思考：`与G-Y轴垂直，且与世界坐标系X-O-Y平面平行的单位向量`有两个，它们方向相反，分别是$(-GY_y,GY_x,0)$与$(GY_y,-GY_x,0)$ ，dex-net人为选定了第二种作为G-X轴初始方向，并以此为基准，以右手螺旋定则，求解出旋转角度$\alpha $ 178 | 179 | - G-Z轴的确定是，以右手法则为标准，从G-X旋向G-Y轴，以此获得G-Z方向 180 | 181 | ```python 182 | #右手定则，从x->y 183 | minor_pc = np.cross( approach_normal,major_pc) 184 | ``` 185 | 186 | 187 | 188 | image-20210829100519836 189 | 190 | 191 | 192 | ## 夹爪坐标系与抓取典范坐标系之间的变换设定 193 | 194 | 不同型号夹爪可能具有不同的惯用夹爪坐标系，可以通过创建修改不同夹爪文件夹路径下的`T_grasp_gripper.tf`文件来设定抓取典范坐标系与夹爪坐标系之间的变换关系。 195 | 196 | ![image-20210829195830930](Dex-net典范抓取坐标系与求解.assets/image-20210829195830930.png) 197 | 198 | 其中： 199 | 200 | - `gripper.obj` 真实夹爪的物体mesh模型 201 | - `params.json` 量取真实的夹爪尺寸，用来对虚拟典范夹爪的尺寸进行设定，用来碰撞检测等操作 202 | - `T_grasp_gripper.tf` 设定 gripper坐标系 to grasp坐标系的变换（gripper坐标系在抓取规范坐标系中的表示） 203 | - `T_mesh_gripper.tf` 设定gripper坐标系 to mesh模型坐标系的变换（gripper坐标系在mesh坐标系中的表示）该变换主要用于图像渲染，不参与计算 204 | 205 | 打开baxter夹爪的`T_grasp_gripper.tf`内容格式如下 206 | 207 | ![image-20210829202625667](Dex-net典范抓取坐标系与求解.assets/image-20210829202625667.png) 208 | 209 | 该文件可以使用`autolab_core`库标准， 210 | 211 | ```python 212 | from autolab_core import RigidTransform 213 | #使用RigidTransform.load()可以直接读取 214 | T_grasp_gripper = RigidTransform.load(os.path.join(gripper_dir, gripper_name, T_GRASP_GRIPPER_FILENAME)) 215 | ########################### 216 | #关于该T_GRASP_GRIPPER_FILE的内容定义 217 | def load(filename): 218 | """Load a RigidTransform from a file. 219 | The file format is: 220 | from_frame 第1行指定父坐标系 221 | to_frame 第2行指定子坐标系 222 | translation (space separated) 第3行是平移变换 223 | rotation_row_0 (space separated) 第4~6行构成了标准旋转矩阵（不是转置，就是标准旋转矩阵） 224 | rotation_row_1 (space separated) 225 | rotation_row_2 (space separated) 226 | 227 | ``` 228 | 229 | 旋转矩阵的每一列分别代表子坐标系$x,y,z$轴单位向量在父坐标系中的表示，那么 230 | 231 | ![image-20210829205518316](Dex-net典范抓取坐标系与求解.assets/image-20210829205518316.png) 232 | 233 | 旋转矩阵第一列表示(子)抓取典范坐标系x轴 在gripper坐标系(父)中的向量为$(0,0,1)$ 即与gripper坐标系z轴相同 234 | 235 | 选抓矩阵第二列表示(子)抓取典范坐标系y轴 在gripper坐标系(父)中的向量为$(0,1,0)$ 即与gripper坐标系y轴相同 236 | 237 | 选抓矩阵第三列表示(子)抓取典范坐标系z轴 在gripper坐标系(父)中的向量为$(-1,0,0)$ 即与gripper坐标系x轴相反 238 | 239 | 对比查看两个夹爪的示意图，验证了上述分析 240 | 241 | image-20210829211317675 242 | 243 | 244 | 245 | ## Dex-net直接输出针对某特定夹爪的位置姿态（相对于mesh参考坐标系） 246 | 247 | dex-net采样得到的初步抓取姿态是典范抓取坐标系姿态，在实现上，是将一个抓取包裹成一个`dexnet.grasping.ParallelJawPtGrasp3D`类对象实例返回， 248 | 249 | 通过`dexnet.grasping.ParallelJawPtGrasp3D.configure`函数获取10维度的抓取配置向量； 250 | 251 | 另外可以使用`dexnet.grasping.ParallelJawPtGrasp3D.gripper_pose( gripper=None)`函数来直接返回指定型号夹爪的夹爪坐标系与物体mesh坐标系之间的变换 252 | 253 | ![image-20210906085839583](Dex-net典范抓取坐标系与求解.assets/image-20210906085839583.png) 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | ## 抓取姿态的表示 262 | 263 | 一个抓取的位置姿态，最少只需要6个自由度表示，但是标准的$4\times4$变换矩阵一共存在12个未知数， 264 | $$ 265 | ^{C}T_G= \begin{bmatrix}n_x&o_x&a_x&t_x\\n_y&o_y&a_y&t_y\\n_z&o_z&a_z&t_z\\0&0&0&1\\\end{bmatrix}=\begin{bmatrix}R&\vec{t}\\0&1 \end{bmatrix} 266 | $$ 267 | 显然，该表达式中必定存在一定的约束条件将上述的信息约束为6，约束条件为： 268 | 269 | - 三个向量$n,o,a$相互垂直； 270 | - 每个由方向余弦表示的单位向量长度为1 271 | 272 | 因此，除了位置向量$\vec{t}=(o_x,o_y,o_z)$是必须给定的之外，旋转矩阵中的9个未知量，最少只需要3个即可推导出其他的三个值。 273 | 274 | **例子1：**对于夹爪坐标系$G$在相机参考系$C$下的变换为 275 | $$ 276 | ^{C}T_G=\begin{bmatrix}?&0&?&3\\0.5&?&?&9\\0&?&?&7\\0&0&0&1\end{bmatrix} 277 | $$ 278 | 此时，旋转矩阵中只给定$n_y=0.5,n_z=0,o_a=0$这三个量，既可完全解出 279 | $$ 280 | ^{C}T_G=\begin{bmatrix}0.866&0&0.5&3\\0.5&0&-0.866&9\\0&1&0&7\\0&0&0&1\end{bmatrix} 281 | $$ 282 | 283 | 284 | **例子2** 285 | 286 | 并不是随意给定都能得到唯一的旋转矩阵，对于夹爪坐标系$G$在相机参考系$C$下的变换为 287 | $$ 288 | ^{C}T_G=\begin{bmatrix}?&0&?&5\\0.707&?&?&3\\?&?&0&2\\0&0&0&1\end{bmatrix} 289 | $$ 290 | 通过上述约束可以推算出，该位置姿态为： 291 | $$ 292 | ^{C}T_G=\begin{bmatrix}0.707&0&0.707&5\\0.707&0&-0.707&3\\0&1&0&2\\0&0&0&1\end{bmatrix} 或^{C}T_G=\begin{bmatrix}-0.707&0&-0.707&5\\0.707&0&-0.707&3\\0&1&0&2\\0&0&0&1\end{bmatrix} 293 | $$ 294 | 295 | 296 | 297 | 298 | 一个抓取的姿态，可以使用$g=(p,r)=(p_x,p_y,p_z,r_x,r_y,r_z)\in \mathbb{R^6}$来表示，那为什么不直接回归$(p_x,p_y,p_z,r_x,r_y,r_z)$呢？ 299 | 300 | 301 | 302 | 论文一直强调，抓取姿态空间是不连续的是什么意思？ 303 | 304 | 305 | 306 | # Dex-net 虚拟夹爪参数设定 307 | 308 | 309 | 310 | dex-net中，使用到夹爪的步骤主要有两个： 311 | 312 | - 抓取采样阶段 313 | - 显示夹爪以及碰撞检测阶段 314 | 315 | 这两阶段的参数，都在`dex-net/data/grippers//params.json`中进行定义。 316 | 317 | ## 用于抓取采样 318 | 319 | 这些参数仅用于采样，所以，夹爪只需要知道最大宽度和最大深度等等一些基本的参数就行，此时不考虑夹爪的具体“厚度”，也不考虑由于夹爪指头的粗细导致的与物体的碰撞检测，比如插入马克杯子握把；具体的碰撞检测放在后续的阶段处理。 320 | ```bash 321 | "min_width": #夹爪的最小闭合宽度 322 | "force_limit": #抓取力度限制 323 | "max_width": #夹爪最大张开距离 dist<1,2> 324 | "finger_radius": #这个参数按照默认的0.01就行，不懂，也好像没用到 325 | "max_depth": #夹爪的最大深度，竖向的距离 dist<1,5> 326 | ``` 327 | 328 | 329 | 330 | 331 | 332 | ## 用于显示和碰撞检测 333 | 334 | 如果还需要进行与点云等碰撞检测，那么需要设定如下参数： 335 | 336 | ```bash 337 | "hand_depth": #和max_depth设置相同数字 dist<1,5> 338 | "hand_height": #夹爪的minor_pc方向的厚度 dist<1,4> 339 | "finger_width": #夹爪手指头在binormal方向的厚度 dist<1,9> 340 | "hand_outer_diameter": #两个手指外侧在binormal方向的宽度 dist<9,13> 341 | "real_finger_width": #也是两个夹爪的厚度，和finger_width写一样就行 dist<1,9> 342 | "real_hand_depth": #和hand_depth保持一致，代码中两者是相同的 dist<1,5> 343 | ``` 344 | 345 | 346 | 347 | image-20210907165347485 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | -------------------------------------------------------------------------------- /doc/Markdown.md: -------------------------------------------------------------------------------- 1 | Signed Distance Field （SDF） -------------------------------------------------------------------------------- /doc/sdf原理.md: -------------------------------------------------------------------------------- 1 | Obj文件 2 | 3 | https://www.cnblogs.com/daofaziran/p/11540517.html 4 | 5 | 描述一个3D模型，对模型上采样很多点，再 将这些点按照一定顺序连接成为三角面，然后把三角面连接在一起； 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 获取模型点云[N,3] 16 | 17 | 18 | 19 | 首先是估计每个点的表面法向量[N,3] 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 点云进行体素降采样，减少点数得到[n,3] 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 使用FPS降采样[m,3] 作为 c1点 36 | 37 | 38 | 39 | 计算每一个点云点的Darboux frame[m,3,3] 40 | 41 | 42 | 43 | 根据设定的摩擦参数，计算每个c1点的摩擦锥角度？[m,1] 44 | 45 | 46 | 47 | 随机从摩擦锥内部选取角度，构造射线[m,3] 48 | 49 | 50 | 51 | 在点云上，寻找射线穿过的点 [m x j,3] 找到它们的索引 52 | 53 | 54 | 55 | 搜索它们的摩擦锥， 判断，如果射线也处在它们的 摩擦锥范围内 就记录下它们的点 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | Signed Distance Field （SDF） -------------------------------------------------------------------------------- /doc/抓取典范坐标系.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/抓取典范坐标系.png -------------------------------------------------------------------------------- /doc/点云与夹爪坐标变换.assets/image-20210831190104710.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/点云与夹爪坐标变换.assets/image-20210831190104710.png -------------------------------------------------------------------------------- /doc/点云与夹爪坐标变换.assets/image-20210831190838330.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/点云与夹爪坐标变换.assets/image-20210831190838330.png -------------------------------------------------------------------------------- /doc/点云与夹爪坐标变换.assets/image-20210831191151570.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/点云与夹爪坐标变换.assets/image-20210831191151570.png -------------------------------------------------------------------------------- /doc/点云与夹爪坐标变换.assets/image-20210831210923247.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/点云与夹爪坐标变换.assets/image-20210831210923247.png -------------------------------------------------------------------------------- /doc/点云与夹爪坐标变换.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # 刚体、变换、位姿、坐标系 4 | 5 | ## 变换与位姿 6 | 7 | `变换`和`位姿`都是坐标系之间的关系，因此说变换说位姿一定要记得带上坐标系；变换和位姿在本质上是说的一样东西，但是在说法上又有所不同，比如： 8 | 9 | image-20210831210923247 10 | 11 | **相机坐标系相对于机器人基座坐标系的位姿，就等同于，从机器人基座坐标系到相机坐标系的变换；** 12 | 13 | > 注意上面句子的句式，规定一个良好的句式有利于以后对坐标变换系统的理解。 14 | 15 | 换句话说，相机坐标系相对于机器人基座坐标系的位姿矩阵，含义和数值上都等同于，(from `parent/souce frame` to `child/target frame` )从机器人基座坐标系(父/源坐标系)到相机坐标系(子/目标坐标系)的变换矩阵$^{B}T_C$； 16 | 17 | 这里说的变换主要是名词含义，指代的是坐标系之间的转换（名），数学上指的是标准齐次$4\times4$变换矩阵： 18 | $$ 19 | ^{B}T_C= \begin{bmatrix}n_x&o_x&a_x&t_x\\n_y&o_y&a_y&t_y\\n_z&o_z&a_z&t_z\\0&0&0&1\\\end{bmatrix}=\begin{bmatrix}R&\vec{t}\\0&1 \end{bmatrix} 20 | $$ 21 | 其中，旋转子矩阵$R$的三个列向量$\vec{n}$、$\vec{o}$、$\vec{a}$就是相机坐标系$n,o,a$轴在机器人基座坐标系中的单位向量，平移向量$\vec{t}=[t_x,t_y,t_z]^T$ 是相机坐标系原点在机器人坐标系中的位置坐标。 22 | $$ 23 | R=\begin{bmatrix} n_x&o_x&a_x \\ n_y&o_y&a_y\\n_z&o_z&a_z\end{bmatrix} 24 | $$ 25 | 当变换只有旋转没有平移时，相当于变换矩阵$T$退化为$3\times3$旋转矩阵$R$ 。 26 | 27 | 后文也有地方提到"把坐标系A变换到坐标系B"，这里的`变换`更多是动词含义，指代实际动作，结合具体情况理解。 28 | 29 | **说变换，就都要带上坐标系，反之，坐标系之间的关系，都是说的变换** 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | ## 求变换还是求变换后的点坐标？ 38 | 39 | 有时候，我们只是想知道父子坐标系之间的变换就行，比如： 40 | 41 | - 执行抓取时，我们只需要知道检测出的抓取(坐标系)$G$相对于机器人基座坐标系$B$的位姿$^{B}T_G$，即基座坐标系到抓取坐标系之间的变换就可以将夹爪伸到抓取位姿执行抓取；（此时假设典范抓取坐标系$G$和夹爪坐标系$G'$的配置方式是一致的，即无需进行旋转，见dex-net具体文档） 42 | 43 | **图** 44 | 45 | - 机器人手眼标定，我们需要的相机坐标系$C$ 相对于机器人基座坐标系$B$的位姿$^{B}T_C$。 46 | 47 | **图** 48 | 49 | 但是有时候，我们需要的不仅是变换，还需要知道子坐标系所固连的刚体上每个点相对于父坐标系下的位置坐标，比如： 50 | 51 | - 抓取过程中，我们假设想把虚拟夹爪显示出来，就需要知道虚拟夹爪上的每个点$p$在参考系（此时参考系是机器人基座坐标系）中的坐标$^{B}p$；此时有 52 | $$ 53 | ^{B}p = ^{B}T_G\times ^{G}p 54 | $$ 55 | 其中，$^{G}p$是虚拟夹爪上每个点相对于夹爪参考系的坐标，由于是固连的，因此它是已知的。 56 | 57 | **图** 58 | 59 | - 机器人手眼标定后，希望将深度相机采集到的点云以机器人坐标系为参考系显示出来，就需要知道固连在相机坐标系下的点云中的每个点$p$在机器人基座坐标系中的坐标$^{B}p$，此时有 60 | $$ 61 | ^{B}p = ^{B}T_C\times ^{C}p 62 | $$ 63 | 其中$^{C}p$是点云各点相对于相机坐标系的位置坐标，深度相机通过内参标定之后，该点云值就是准确可知的。 64 | 65 | **图** 66 | 67 | 68 | 69 | 因此，面向任务，要先想清楚，**我们的任务到底是什么？要的是坐标系之间的变换，还是想要刚体上每个点相对于新参考系的位置？** 70 | 71 | **求变换，和求刚体点位置是两码事** 72 | 73 | 在具体编程时，我们只需要把想要的目标数据放在等号左侧，把计算所需要的数据放在等号右侧，然后去找全等号右侧的数据就可以进行计算了。 74 | 75 | 76 | 77 | ## 任务举例 78 | 79 | ### 任务说明 80 | 81 | 希望将使用dex-net中的Antipodal采样器采集到的抓取（相对于mesh坐标系）与点云相互配准，并使用mayavi显示出来点云和夹爪， 82 | 83 | **图** 84 | 85 | ### 描述符号 86 | 87 | 首先应该列举出任务中存在和需要的刚体以及它们固连的坐标系系统，并用符号进行描述。 88 | 89 | 任务中存在如下刚体： 90 | 91 | - 点云 ，上面的点记为$p$ 92 | - mesh模型 上面的点记为$m$ 93 | - 虚拟夹爪 上面的点记为$g$ 94 | 95 | 存在如下坐标系系统 96 | 97 | - 相机坐标系 $C$ 固连在点云上 98 | 99 | - 仿真世界坐标系 $W$ 100 | 101 | - mesh坐标系 $M$ 固连在mesh模型上 102 | 103 | - 夹爪典范坐标系 $G $ 固连在虚拟夹爪上 104 | 105 | 106 | 107 | ### 问题定义 108 | 109 | 目标是将抓取典范坐标系$G$以正确的位置姿态旋转到点云上，与点云相互锚定；因此需要求出抓取坐标系$G$相对于点云坐标系（相机坐标系）$C$的位姿，即相机坐标系$C$到抓取坐标系$G$的变换$^{C}T_G$，并求出虚拟夹爪上各点$g$相对于相机坐标系的位置坐标$^{C}g$ 110 | 111 | **图** 112 | 113 | 114 | 115 | ### 具体计算步骤 116 | 117 | 1. 写出需要的目标与计算公式 118 | 119 | $$ 120 | ^{C}g= ^{C}T_G\times^{G}g 121 | $$ 122 | 123 | 124 | 125 | ​ 所以，应该做的事情是，**求变换，就单独求变换，求完之后，再求各个点的位置**。 126 | 127 | 2. 思考周全现有的坐标系系统 128 | 129 | 观察整个问题，可以得知问题中存在如下坐标系系统 130 | 131 | - 相机坐标系 $C$ 固连在点云上 132 | 133 | - 仿真世界坐标系 $W$ 134 | 135 | - mesh坐标系 $M$ 固连在mesh模型上 136 | 137 | - 夹爪典范坐标系 $G $ 固连在虚拟夹爪上 138 | 139 | 3. 摆出现有已知的坐标系变换 140 | 141 | 由于实际上求$ ^{C}T_G$涉及到多个坐标系进行链式变换，因此需要对该连是变换序列进行推导，首先可以先排列出当前已知的变换： 142 | 143 | - 相机坐标系$C$相对于仿真世界坐标系$W$中的位置姿态，即世界坐标系到相机坐标系的变换$^{W}T_C$ 144 | 145 | - 模型坐标系$M$相对于仿真世界坐标系$W$中的位置姿态，即世界坐标系到模型坐标系的变换$^{W}T_M$ 146 | 147 | - 夹爪坐标系$G$相对于mesh坐标系$M$的位置姿态，即mesh坐标系到夹爪坐标系的变换$^{M}T_G$ 148 | 149 | 150 | 151 | 4. 推导链式变换序列 152 | 153 | 需要的变换$^{C}T_G$的变换序列是: 154 | 155 | $$ 156 | ^{C}T_G =^{C}T_W\times^{W}T_M\times^{M}T_G={^{W}T_C}^{-1}\times^{W}T_M\times^{M}T_G 157 | $$ 158 | 159 | 160 | 161 | 4. 进行点坐标计算 162 | $$ 163 | ^{C}g= ^{C}T_G\times^{G}g={^{W}T_C}^{-1}\times^{W}T_M\times^{M}T_G\times^{G}g 164 | $$ 165 | 166 | 167 | 168 | #### 注意点 169 | 170 | 1. 点坐标补位 171 | $$ 172 | ^{C}g= ^{C}T_G\times^{G}g 173 | $$ 174 | 上述公式中的变换矩阵$^{C}T_G$默认包含了旋转与平移两部分，是标准齐次$4\times4$变换矩阵，点坐标$^{G}g$默认也是4维的齐次坐标向量（三维坐标后补1）。 175 | 176 | 当变换只有旋转没有平移时，相当于变换矩阵$^{C}T_G$退化为$3\times3$旋转矩阵$R$ ，此时$^{G}g$直接相乘就好，不需要齐次。 177 | 178 | 2. 变换矩阵求逆 179 | 180 | 对于标准齐次$4\times4$变换矩阵$T$而言，它的求逆运算比较麻烦，通常将变换矩阵拆分为标准$3\times3$旋转矩阵$R$以及平移向量 $\vec{t}$ （列向量）两部分来计算： 181 | $$ 182 | T = \begin{bmatrix}n_x&o_x&a_x&t_x\\n_y&o_y&a_y&t_y\\n_z&o_z&a_z&t_z\\0&0&0&1\\\end{bmatrix}=\begin{bmatrix}R&\vec{t}\\0&1 \end{bmatrix} 183 | $$ 184 | 185 | 186 | 标准$3\times3$旋转矩阵$R$是酉矩阵，它的**逆运算等于它的简单转置**，即，$R^{-1}=R^T$；平移部分则是向量$\vec{t}$分别与向量$\vec{n},\vec{o},\vec{a}$点积的负值 187 | $$ 188 | T^{-1} = \begin{bmatrix}n_x&n_y&n_z&-\vec{n}\cdot\vec{t}\\o_x&o_y&o_z&-\vec{o}\cdot\vec{t}\\a_z&a_z&a_z&-\vec{a}\cdot\vec{t}\\0&0&0&1\\\end{bmatrix}=\begin{bmatrix}R^T&-R\cdot\vec{t}\\0&1 \end{bmatrix} 189 | $$ 190 | 191 | 192 | 这样做对于计算变换矩阵的逆是很有帮助的，而直接对矩阵$T$整体进行逆运算是一个很冗长的过程。 193 | 194 | 在实际的计算过程中，往往就是用这种方式，比如`autolab_core`中的`rigid_transformations.RigidTransform.inverse`函数 195 | 196 | ```python 197 | def inverse(self): 198 | """Take the inverse of the rigid transform. 199 | 200 | Returns 201 | ------- 202 | :obj:`RigidTransform` 203 | The inverse of this RigidTransform. 204 | """ 205 | inv_rotation = self.rotation.T #旋转矩阵的逆 206 | inv_translation = np.dot(-self.rotation.T, self.translation) #平移向量的逆 207 | return RigidTransform(inv_rotation, inv_translation, 208 | from_frame=self._to_frame, 209 | to_frame=self._from_frame) 210 | ``` 211 | 212 | 213 | 214 | ### 编程实现 215 | 216 | 对于公式 217 | $$ 218 | ^{C}g= ^{C}T_G\times^{G}g={^{W}T_C}^{-1}\times^{W}T_M\times^{M}T_G\times^{G}g 219 | $$ 220 | 221 | 222 | 1. 获取变换${^{W}T_C}^{-1}$ 223 | 224 | ```python 225 | from autolab_core import RigidTransform 226 | #WTC 227 | world_to_scaner = np.load(world_to_scaner_path_list[scene_index]) 228 | world_to_scaner_quaternion = world_to_scaner[3:7]#四元数 229 | world_to_scaner_rot = RigidTransform.rotation_from_quaternion(world_to_scaner_quaternion)#转换到旋转矩阵 230 | world_to_scaner_trans =world_to_scaner[0:3]#平移向量 231 | world_to_scaner_T = RigidTransform(world_to_scaner_rot,world_to_scaner_trans) #构造WTC刚体变换对象 232 | #CTW 233 | scaner_to_world_T = world_to_scaner_T.inverse().matrix #得到逆矩阵 234 | ``` 235 | 236 | 2. 获取变换$^{W}T_M$ 237 | 238 | ```python 239 | #获取WTM 240 | world_to_mesh_7d = table_mesh_poses_array[mesh_index] 241 | world_to_mesh_rot = RigidTransform.rotation_from_quaternion(world_to_mesh_7d[3:]) 242 | world_to_mesh_trans = world_to_mesh_7d[0:3] 243 | world_to_mesh_T = RigidTransform(world_to_mesh_rot,world_to_mesh_trans).matrix 244 | ``` 245 | 246 | 3. 获取变换$^{M}T_G$ 247 | 248 | ```python 249 | #MTG 列表，由于autolab_core.RigidTransform不支持多维度批量的变换，所以这里自己写 250 | mesh_to_grasps_rot=get_rot_mat(grasps_with_score) #mesh_to_grasps_rot.shape = (30,3,3) 251 | mesh_to_grasps_trans = grasps_with_score[:,0:3] #mesh_to_grasps_trans.shape=(30,3) 252 | mesh_to_grasps_rot_trans = np.concatenate((mesh_to_grasps_rot,mesh_to_grasps_trans.reshape(-1,3,1)),axis=2) #shape=(30,3,4) 253 | temp = np.array([0,0,0,1]).reshape(1,1,4).repeat(mesh_to_grasps_rot.shape[0],axis=0) #补第四行 254 | mesh_to_grasps_T =np.concatenate((mesh_to_grasps_rot_trans,temp),axis=1) #再拼成标准4X4 , shape=(30,4,4) 255 | ``` 256 | 257 | 4. 计算总变换$^{C}T_G $ 就是抓取坐标系相对于相机坐标系下的位置姿态 258 | 259 | ```python 260 | #计算CTG 261 | scaner_to_grasps_T =np.matmul(np.matmul(scaner_to_world_T,world_to_mesh_T),mesh_to_grasps_T) #计算4x4变换矩阵 262 | ``` 263 | 264 | 5. 计算点坐标$^{C}g$（在本任务中就是计算虚拟夹爪上的点，用于显示和碰撞检测等操作） 265 | 266 | 计算点坐标$^{C}g$有两种方式： 267 | 268 | - 计算变换时，以整体形式计算，得到最终的$4\times4$齐次变换矩阵之后，再将其分解，抽出$3\times3$变换矩阵$R$和平移向量$\vec{t}$，与点$g$ 向量**先旋转再平移(相加)，顺序不能相反**，此时$^{C}g$为三维坐标 269 | 270 | $$ 271 | ^{C}g=R\cdot ^{G}g+\vec{t} 272 | $$ 273 | 274 | - 将三维度点坐标$^{G}g$拓展为四维齐次坐标向量，然后直接使用$4\times4$齐次变换矩阵进行变换，得到的也是齐次的4维坐标向量 275 | $$ 276 | ^{C}g= ^{C}T_G\times^{G}g 277 | $$ 278 | 279 | ```python 280 | scaner_to_grasp_T=scaner_to_grasps_T[0] 281 | scaner_to_grasp_rot = scaner_to_grasp_T[0:3,0:3] 282 | scaner_to_grasp_trans = scaner_to_grasp_T[0:3,3].reshape(-1,3) 283 | 284 | Gp = np.array([1,2,3]) #随便设置一个点Gp 285 | Gp_q = np.array([1,2,3,1])#将Gp拓展为齐次坐标向量Gp_q 286 | 287 | #分解为R t 两个变换步骤，使用非齐次点Gp 288 | Cp_r_t=scaner_to_grasp_rot.dot(Gp) + scaner_to_grasp_trans#先旋转再平移 289 | Cp_t_r=scaner_to_grasp_rot.dot(Gp+ scaner_to_grasp_trans)#先平移再旋转 290 | 291 | #直接使用4x4变换矩阵计算,使用齐次点Gp_q，得到齐次坐标点Cp_q 292 | Cp_q = scaner_to_grasp_T.dot(Gp_q) 293 | 294 | print(Cp_r_t) 295 | >>>[-1.58108308 3.49795968 -0.32400429] #正确，先旋转再平移 296 | print(Cp_t_r) 297 | >>>[-0.62973867 2.98622214 0.68276591] #说明顺序是错的 298 | print(Cp_q[0:3]) 299 | >>>[-1.58108308 3.49795968 -0.32400429] #正确 300 | ``` 301 | 302 | 303 | 304 | 305 | 306 | # 额外举例 307 | 308 | 关于直接使用$4\times4$标准变换矩阵形式求解目标变换或者点坐标的形式，注意点主要是，在求解点坐标时，需要对点坐标进行齐次拓展； 309 | 310 | 而使用旋转矩阵与平移向量分立的方式求解目标变换，稍微有些复杂，以下主要举的是分立方式的求解。 311 | 312 | ## 分立求解点坐标 313 | 314 | **例子1：**已知相机坐标系$C$下各点$p$相对于相机的位置$^{C}p$，以及相机坐标系相对与机器人基座坐标系的位姿$^{B}T_C$，求解各点相对于机器人基座坐标系的点坐标$^{B}p$ 315 | 316 | image-20210831210923247 317 | 318 | 答案： 319 | $$ 320 | \begin{bmatrix} ^{B}p\\1\end{bmatrix}= ^{B}T_C\cdot\begin{bmatrix} ^{C}p\\1\end{bmatrix}= \begin{bmatrix} R&t\\0&1\end{bmatrix}\cdot\begin{bmatrix} ^{C}p\\1\end{bmatrix}=\begin{bmatrix} R\cdot^{G}p+t\\1\end{bmatrix} 321 | $$ 322 | 其中，$R,t$ 分别是从坐标系$B$ 到$C$ 的旋转矩阵以及 平移向量。 323 | 324 | 325 | 326 | **例2（已知逆矩阵）：**与上个例子同样的图，已知各个点$p$在基座坐标系$B$下的点坐标$^{B}p$ ，以及相机坐标系相对与机器人基座坐标系的位姿$^{B}T_C$，求解各个点相对于相机坐标系的点坐标$^{C}p$ 327 | 328 | 分析，这个例子和例1不太一样，因为，已知的变换矩阵不是正常需要的$^{C}T_B$，而是已知它的逆变换$^{B}T_C$，若 329 | $$ 330 | ^{B}T_C= \begin{bmatrix} R&t\\0&1\end{bmatrix} 331 | $$ 332 | 答案： 333 | $$ 334 | ^{C}p= ^{C}T_B\cdot^{B}p\\ 335 | ={^{B}T_C}^{-1}\cdot^{B}p\\ 336 | = \begin{bmatrix} R^T&-R^Tt\\0&1\end{bmatrix}\cdot^{B}p\\ 337 | =R^T\cdot^{B}p+(-R^T\cdot t)\\ 338 | =R^T\cdot^{B}p-R^T\cdot t\\ 339 | =R^T(^{B}p-t) 340 | $$ 341 | 需要注意的是，虽然看似最终结果是$^{C}p=R^T(^{C}p-t)$，类似于先平移再旋转，但是注意这里的减去的平移向量$t$，并不是旋转矩阵中的标准平移向量$-R^T\cdot t$，这个公式只是相当于带有逆矩阵的变形公式罢了。 342 | 343 | 注意：自己写的时候，最好算一下，记得分立求解点坐标时候：**要先旋转得到旋转后的坐标，再平移（相加）**，如果看别人代码的时候，发现有“先平移”的，记得可能只是带有逆矩阵运算的变形公式罢了；拿不准的话，就动手算算。 344 | 345 | 346 | 347 | ## 分立求解变换 348 | 349 | 350 | 351 | 对于变换，也可以通过分立法求解，但是它们也有一些特点： 352 | 353 | **例3：**对于变换$^{C}T_M=^{C}T_M\cdot ^{W}T_M$，若已知变换$^{C}T_M$与$^{W}T_M$，利用分立法求$^{C}T_M$ 354 | 355 | 答案： 356 | $$ 357 | ^{C}T_M=^{C}T_W\cdot^{W}T_M\\ 358 | =\begin{bmatrix} R_1&t_1\\0&1\end{bmatrix}\cdot \begin{bmatrix} R_2&t_2\\0&1\end{bmatrix}\\ 359 | =\begin{bmatrix} R_1\cdot R_2&R_1t_2+t_1\\0&1\end{bmatrix} 360 | =\begin{bmatrix} R_3&t_3\\0&1\end{bmatrix} 361 | $$ 362 | 即，变换$^{C}T_M$的旋转矩阵$rot=R_3=R_1\cdot R_2$，平移矩阵$trans=t_3=R_1\cdot t_2+t_1$ 363 | 364 | 365 | 366 | **例4：**对于$^{C}T_M={^{W}T_C}^{-1}\cdot ^{W}T_M$，若已知变换$^{W}T_C$与$^{W}T_M$，利用分立法求解$^{C}T_M$ 367 | 368 | 答案：若 369 | $$ 370 | ^{C}T_M =\begin{bmatrix} R_1&t_1\\0&1\end{bmatrix}\\ 371 | ^{W}T_M =\begin{bmatrix} R_2&t_2\\0&1\end{bmatrix} 372 | $$ 373 | 则有 374 | $$ 375 | ^{C}T_M={^{W}T_C}^{-1}\cdot ^{W}T_M=\begin{bmatrix} R_1^T&-R_1^T\cdot t_1\\0&1\end{bmatrix}\cdot \begin{bmatrix} R_2&t_2\\0&1\end{bmatrix}\\ 376 | =\begin{bmatrix} R_1^T\cdot R_2&(R_1^T\cdot t_2-R_1^T\cdot t_1)\\0&1\end{bmatrix}\\ 377 | = \begin{bmatrix} R_3&t_3\\0&1\end{bmatrix} 378 | $$ 379 | 即，变换$^{C}T_M$的旋转矩阵$rot=R_3=R_1^T\cdot R_2$，平移矩阵$trans=t_3=R_1^T\cdot t_2-R_1^T\cdot t_1$ 380 | 381 | **注意：**这些公式实际上真的很难记，**因此一般先确定链式变换的推导公式，再去演算即可**，这种分立求变换，比较少用于多重链式变换，而多用于求解点坐标的最后一步（与旧点坐标点乘的环节） 382 | 383 | 384 | 385 | # 总结 386 | 387 | 总之就是： 388 | 389 | 1. 先确定任务是求变换还是求点坐标； 390 | 2. 无论是哪个，都找出现有的已知的变换，写出来链式变换公式； 391 | 3. 再确定是想用标准$4\times4$变换矩阵求解，还是分立法求解： 392 | - 如果想要用标准$4\times4$变换矩阵求解，就构造出标准变换矩阵，注意如果是求点坐标，需要对点坐标进行齐次拓展 393 | - 如果是想要分立求解（一般用于变换矩阵与点坐标相乘的那一步），就一定列出分块矩阵，推导下，多步变换不好记比较复杂 394 | 395 | 396 | 397 | 另外：时刻注意参考坐标系是谁，时刻问问自己，正在计算的变换(位姿)或者坐标，它的相对参考系是谁。 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | -------------------------------------------------------------------------------- /doc/碰撞坐标系.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/simulate_dataset_generation/deec3d73493d62601be88c6081a4e05ac611010e/doc/碰撞坐标系.png -------------------------------------------------------------------------------- /generate_mujoco_xml.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | #从一个指定的文件中，读取所列的mesh的路径；生成mujoco需要的xml场景文件 3 | #将生成的xml文件，分别保存在各自的场景文件夹中 4 | # 5 | import os 6 | from string import digits 7 | import sys 8 | import random 9 | import numpy as np 10 | import pickle 11 | import argparse 12 | 13 | #解析命令行参数 14 | parser = argparse.ArgumentParser(description='Generate Mujoco xml files') 15 | parser.add_argument('--gripper', type=str, default='baxter') 16 | parser.add_argument('--mesh_num', type=int, default=10) 17 | parser.add_argument('--scene_num', type=int, default=500) 18 | args = parser.parse_args() 19 | 20 | 21 | def get_file_name(file_dir_): 22 | file_list = [] 23 | for root, dirs, files in os.walk(file_dir_): 24 | for file in files: 25 | if file.find('bg_')==-1: 26 | file_list.append(file.split('.')[0]) 27 | #排序 28 | #file_list.sort() 29 | return file_list 30 | 31 | if __name__ == '__main__': 32 | 33 | #夹爪名称 34 | gripper_name = args.gripper 35 | #场景中模型数量 36 | mesh_num = args.mesh_num 37 | #场景数量 38 | scene_num = args.scene_num 39 | 40 | 41 | print("为{}生成{}帧场景，每帧{}个物体".format(gripper_name,scene_num,mesh_num)) 42 | #获取最大的位数，用来补0 43 | max_digits = len(str(scene_num)) 44 | 45 | home_dir = os.environ['HOME'] 46 | 47 | #设定legal_meshes的路径记录文件地址 48 | legal_meshes_pickle = home_dir+'/dataset/simulate_grasp_dataset/'+gripper_name+'/good_meshes.pickle' 49 | #读取legal_meshes的路径记录文件 50 | with open(legal_meshes_pickle,'rb') as mesh: 51 | mesh_list=pickle.load(mesh) 52 | 53 | #获取16k mesh库目录 54 | meshes_dir ,_=os.path.split(mesh_list[0]) 55 | #提取出mesh文件名称，目的是去写到xml文件中 56 | mesh_list = [ mesh.split('/')[-1].split('.')[0] for mesh in mesh_list] 57 | 58 | 59 | #场景文件夹主目录 60 | scenes_dir = home_dir+'/dataset/simulate_grasp_dataset/baxter/scenes/' 61 | 62 | 63 | xml_template_string = """ 64 | 65 | 66 | 67 | 68 | 125 | """ 126 | 127 | #生成n种场景，每种场景中的物体种类都重新挑选 128 | for scene_n in range(scene_num): 129 | #打乱mesh_list顺序 130 | random.shuffle(mesh_list) 131 | #截取指定的k个文件,抽选子集 132 | mesh_list_ = mesh_list[:mesh_num] 133 | 134 | xml_string = xml_template_string 135 | 136 | #===修改meshes文件夹地址 137 | temp_string="meshdir=\""+meshes_dir+"\"" 138 | #查找mesh地址标签并分割 139 | temp_list = xml_string.split('meshdir=') 140 | xml_string = temp_string.join(temp_list) 141 | #===修改asset标签，导入stl模型 142 | temp_string="" 143 | for i in range(mesh_num): 144 | temp_string +=" \n" 145 | #查找mesh标签并分割 146 | temp_list = xml_string.split(" \n") 147 | xml_string = temp_string.join(temp_list) 148 | 149 | #===修改body标签 150 | temp_string="" 151 | #先采样各个位置，防止初始位置相互触碰，导致物理模拟出现失败 152 | pos_group = np.zeros([1,3]) 153 | maximum_rounds = 10 154 | #最大可以检测10轮 155 | for _ in range(maximum_rounds): 156 | #对每个物体进行采样 157 | for i in range(mesh_num): 158 | #每个物体最大可以采样20次 159 | for j in range(20): 160 | #采样的约束范围 161 | pos_x = round((random.random()-0.5)*0.3,2) 162 | pos_y =round((random.random()-0.5)*0.3,2) 163 | pos_z = round(random.random()*1.5+1.2,2) 164 | pos = np.array([[pos_x,pos_y,pos_z]]) 165 | #如果是第一个 166 | if not pos_group[0][2]: 167 | pos_group = pos 168 | break 169 | else: 170 | #计算与已有的每个位置之间的差值 171 | pos_delta = np.linalg.norm(pos_group - pos,axis=1) 172 | if np.sum(pos_delta<0.2):#限制每两个物体坐标系原点之间的最小距离 173 | continue 174 | else:#和已有的位置都挺远的 175 | #把采样出来的位置添加到group中 176 | pos_group = np.concatenate((pos_group,pos),axis=0) 177 | break 178 | #如果采样出的合法位置少于要求的数量 179 | if pos_group.shape[0]\n \ 202 | \n \n \n" 203 | #查找body标签并分割 204 | temp_list = xml_string.split(" \n") 205 | xml_string = temp_string.join(temp_list) 206 | 207 | 208 | #场景的具体名称 209 | new_xml_name = "" #清零 210 | new_xml_name = "scene_mujoco"+".xml" 211 | 212 | #找到对应的场景文件夹目录，没有的话就创建 213 | scene_dir = os.path.join(scenes_dir+str(scene_n).zfill(max_digits)) 214 | if not os.path.exists(scene_dir): 215 | os.makedirs(scene_dir) 216 | 217 | #创建新的xml文件并写入修改后的内容 218 | xml = open(os.path.join(scene_dir,new_xml_name),'w+') 219 | xml.write(xml_string) 220 | xml.close() 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | -------------------------------------------------------------------------------- /get_legal_grasps_with_score.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from math import pi 3 | import os 4 | import sys 5 | import argparse 6 | import time 7 | from matplotlib.pyplot import axis 8 | import mayavi 9 | import numpy as np 10 | import pickle 11 | import glob 12 | import random 13 | from autolab_core import RigidTransform 14 | from autolab_core import YamlConfig 15 | from dexnet.grasping import GpgGraspSampler 16 | from mayavi import mlab 17 | from tqdm import tqdm 18 | from dexnet.grasping import RobotGripper 19 | from numpy.core.fromnumeric import swapaxes, transpose 20 | import torch 21 | import open3d as o3d 22 | 23 | #解析命令行参数 24 | parser = argparse.ArgumentParser(description='Get legal grasps with score') 25 | parser.add_argument('--gripper', type=str, default='baxter') # 26 | parser.add_argument('--load_npy',action='store_true') #设置同时处理几个场景 27 | 28 | args = parser.parse_args() 29 | 30 | device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 31 | print(device) 32 | 33 | home_dir = os.environ['HOME'] 34 | 35 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 36 | gripper = RobotGripper.load(args.gripper, home_dir + "/code/dex-net/data/grippers") 37 | ags = GpgGraspSampler(gripper, yaml_config) 38 | 39 | 40 | class GetLegalGrasps: 41 | def __init__(self,use_dense=False) -> None: 42 | self.minimum_points_num=10 #限制夹爪内部最少点数 43 | self.minimum_insert_dist=0.01 #限制夹爪内部点数最小嵌入距离 44 | self.table_hight = 0.75 #桌面高度，用于检测夹爪桌面碰撞 45 | self.safe_dis = 0.005 #与桌面的最小安全距离 46 | self.max_angle=50 #夹爪接近角度限制 47 | 48 | scenes_dir = home_dir+"/dataset/simulate_grasp_dataset/{}/scenes/".format(args.gripper) 49 | #获取采样grasp所在文件夹地址 50 | candidate_grasp_score_dir = home_dir+"/dataset/simulate_grasp_dataset/{}/antipodal_grasps/".format(args.gripper) 51 | #获取点云路径列表 52 | pc_path_list = glob.glob(scenes_dir+'*/pc.npy') 53 | pc_path_list.sort() 54 | 55 | table_pc_path_list = glob.glob(scenes_dir+'*/table_pc.npy') 56 | table_pc_path_list.sort() 57 | #获取对应的mesh&pose 列表 58 | meshes_pose_path_list = glob.glob(scenes_dir+'*/table_meshes_with_pose.pickle') 59 | meshes_pose_path_list.sort() 60 | #获取相机在世界坐标系下的位置姿态 61 | world_to_scaner_path_list = glob.glob(scenes_dir+'*/world_to_scaner.npy') 62 | world_to_scaner_path_list.sort() 63 | #获取采样grasp&score 64 | candidate_grasp_score_list = glob.glob(candidate_grasp_score_dir+'*.npy') 65 | 66 | #对每一帧场景处理 67 | for scene_index,pc_path in enumerate(pc_path_list): 68 | #读取当前场景raw点云 69 | self.pc= np.load(pc_path) 70 | self.dense_pc = np.load(table_pc_path_list[scene_index]) 71 | 72 | if use_dense: 73 | self.used_pc = self.dense_pc 74 | else: 75 | self.used_pc = self.pc 76 | 77 | #获取场景抓取信息 78 | self.grasp_centers,self.bottom_centers,self.grasp_poses,self.grasp_scores,\ 79 | self.local_hand_points,self.local_hand_points_extend,self.hand_points=\ 80 | self.get_scene_grasps(world_to_scaner_path_list,meshes_pose_path_list,\ 81 | candidate_grasp_score_list,scene_index) 82 | 83 | print('Start job ', pc_path) 84 | 85 | #self.show_grasps_pc(title='raw') 86 | self.collision_check_table_cuda() 87 | #self.show_grasps_pc(title='table_check') 88 | self.restrict_approach_angle() 89 | #self.show_grasps_pc(title='angle_check') 90 | self.collision_check_pc_cuda() 91 | #self.show_grasps_pc(title='pc_check') 92 | #保存不与桌子碰撞的抓取 93 | #限制抓取approach轴与桌面垂直方向的角度 94 | #验证结果 95 | #self.collision_check_validate() 96 | 97 | #保存为'legal_grasp_with_score.npy' 98 | save_path =os.path.join(os.path.split(pc_path)[0],'legal_grasps_with_score.npy') 99 | 100 | #先批量取出虚拟夹爪各点相对于相机坐标系C的坐标 101 | 102 | grasp_dict ={} 103 | grasp_dict['bottom_centers'] = self.bottom_centers 104 | grasp_dict['grasp_centers'] = self.grasp_centers 105 | grasp_dict['grasp_poses'] = self.grasp_poses 106 | grasp_dict['grasp_score'] = self.grasp_scores 107 | grasp_dict['local_hand_points'] =self.local_hand_points 108 | grasp_dict['local_hand_points_extend'] =self.local_hand_points_extend 109 | grasp_dict['hand_points'] = self.hand_points#所有抓取姿态下各个夹爪的坐标 110 | grasp_dict['pc'] = self.pc 111 | grasp_dict['dense_pc'] = self.dense_pc 112 | 113 | np.save(save_path,grasp_dict) 114 | print('Job done ',save_path,' good grasps num',len(self.grasp_poses)) 115 | 116 | def get_scene_grasps(self,world_to_scaner_path_list,meshes_pose_path_list,grasp_files_path,scene_index): 117 | '''获取每一帧场景中的抓取信息 118 | ''' 119 | #WTC 120 | world_to_scaner = np.load(world_to_scaner_path_list[scene_index]) 121 | world_to_scaner_quaternion = world_to_scaner[3:7]#四元数 122 | self.world_to_scaner_rot = RigidTransform.rotation_from_quaternion( 123 | world_to_scaner_quaternion)#转换到旋转矩阵 124 | self.world_to_scaner_trans =world_to_scaner[0:3]#平移向量 125 | 126 | 127 | #构造WTC刚体变换对象 128 | world_to_scaner_T = RigidTransform(self.world_to_scaner_rot,self.world_to_scaner_trans) 129 | 130 | #CTW 世界在相机坐标系中的位置姿态 131 | self.scaner_to_world_T = world_to_scaner_T.inverse().matrix #得到逆矩阵 132 | 133 | #GTB 碰撞坐标系在抓取典范坐标系下的位置姿态 134 | translation=np.array([-ags.gripper.hand_depth,0,0]) 135 | grasp_to_bottom_T = RigidTransform(np.eye(3),translation).matrix 136 | 137 | 138 | #打开当前场景的'table_meshes_with_pose.pickle' 139 | with open(meshes_pose_path_list[scene_index],'rb') as f: 140 | table_meshes_with_pose = pickle.load(f) 141 | 142 | #读取当前帧包含有那些mesh，把列表读取出来 143 | self.mesh_list = table_meshes_with_pose[0] 144 | self.mesh_poses_array = table_meshes_with_pose[1] 145 | 146 | grasp_centers = np.empty(shape=(0,3)) 147 | grasp_poses =np.empty(shape=(0,3,3)) 148 | grasp_scores = np.empty(shape=(0,1)) 149 | #计算所有夹爪的点 150 | hand_points = np.empty([0,21,3]) 151 | 152 | #本地夹爪各点坐标 153 | local_hand_points = ags.get_hand_points(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0]))#[21,3] 154 | 155 | 156 | #对场景中每一个模型 157 | for mesh_index,mesh in enumerate(self.mesh_list): 158 | #从grasp库中查找并读取当前mesh的抓取采样结果 159 | mesh_name = mesh.split('/')[-1].split('.')[0] 160 | if mesh_name =='bg_table': #跳过桌子 161 | continue 162 | #if 'banana' not in mesh_name: 163 | # continue 164 | for path in grasp_files_path: 165 | if mesh_name in path: 166 | #读取模型之上的抓取 167 | grasps_with_score = np.load(path) # 168 | 169 | 170 | #WTM 模型在场景中的位置姿态 171 | world_to_mesh_7d = self.mesh_poses_array[mesh_index] 172 | world_to_mesh_rot = RigidTransform.rotation_from_quaternion(world_to_mesh_7d[3:]) 173 | world_to_mesh_trans = world_to_mesh_7d[0:3] 174 | world_to_mesh_T = RigidTransform(world_to_mesh_rot,world_to_mesh_trans).matrix 175 | 176 | #MTG 模型上的抓取相对于模型坐标系的位置姿态 177 | mesh_to_grasps_rot=self.get_rot_mat(grasps_with_score) 178 | mesh_to_grasps_trans = grasps_with_score[:,0:3] 179 | mesh_to_grasps_rot_trans = np.concatenate((mesh_to_grasps_rot,mesh_to_grasps_trans.reshape(-1,3,1)),axis=2) 180 | temp = np.array([0,0,0,1]).reshape(1,1,4).repeat(mesh_to_grasps_rot.shape[0],axis=0) #补第四行 181 | mesh_to_grasps_T =np.concatenate((mesh_to_grasps_rot_trans,temp),axis=1) #再拼成标准4X4 182 | 183 | #计算CTG 抓取坐标系在相机坐标系下的位姿 184 | scaner_to_grasps_T =np.matmul(np.matmul(self.scaner_to_world_T,world_to_mesh_T),mesh_to_grasps_T) 185 | scaner_to_grasps_rot = scaner_to_grasps_T[:,0:3,0:3] 186 | scaner_to_grasps_trans = scaner_to_grasps_T[:,0:3,3].reshape(-1,3) 187 | 188 | #计算CTB 夹爪碰撞坐标系在相机坐标系下的位姿 189 | scaner_to_bottoms_T = np.matmul(scaner_to_grasps_T,grasp_to_bottom_T) #[-1,4,4] 190 | scaner_to_bottoms_rot = scaner_to_bottoms_T[:,0:3,0:3] #[-1,3,3] 191 | scaner_to_bottoms_trans = scaner_to_bottoms_T[:,0:3,3].reshape(-1,3) #[-1,3] 192 | 193 | #计算p 194 | Bp =np.swapaxes(np.expand_dims(local_hand_points,0),1,2)#.repeat(scaner_to_bottoms_rot.shape[0],axis=0) 195 | RCp = np.swapaxes(np.matmul(scaner_to_bottoms_rot,Bp),1,2) #[-1,21,3] 196 | 197 | Cp = RCp + np.expand_dims(scaner_to_bottoms_trans,1) #[-1,21,3] 198 | 199 | 200 | 201 | #将所有的模型的grasp 统一添加到场景的grasp 列表中 202 | grasp_centers = np.concatenate((grasp_centers,scaner_to_grasps_trans),axis=0)#() 203 | grasp_poses=np.concatenate((grasp_poses,scaner_to_grasps_rot),axis=0)#(-1,3,3) 204 | grasp_scores = np.concatenate((grasp_scores,grasps_with_score[:,[10]]),axis=0) #(-1,1) 205 | hand_points = np.concatenate((hand_points,Cp),axis=0)#(-1,21,3) 206 | 207 | local_hand_points_extend = ags.get_hand_points_extend(np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0])) 208 | bottom_centers = grasp_centers -ags.gripper.hand_depth * grasp_poses[:,:,0] 209 | 210 | 211 | #计算所有夹爪的点 212 | ''' 213 | hand_points = np.empty([0,21,3]) 214 | for i in range(grasp_centers.shape[0]): 215 | points = ags.get_hand_points(bottom_centers[i], 216 | grasp_poses[i,:,0], 217 | grasp_poses[i,:,1]).reshape(1,21,3)#(1,21,3) 218 | hand_points = np.concatenate((hand_points,points),axis=0)#(-1,21,3) 219 | ''' 220 | 221 | return grasp_centers,bottom_centers,grasp_poses,grasp_scores,local_hand_points,local_hand_points_extend,hand_points 222 | 223 | 224 | 225 | def collision_check_pc_cuda(self): 226 | """对CTG抓取姿态和Cpc进行碰撞检测(使用显卡加速计算) 227 | """ 228 | #对旋转后的抓取，逐个进行碰撞检测，并把没有碰撞的抓取保存下来 229 | #mask =np.zeros(centers.shape[0]) 230 | if self.grasp_centers.shape[0]==0: 231 | return 0 232 | 233 | before = len(self.grasp_centers) 234 | poses_cuda=torch.from_numpy(self.grasp_poses).cuda() 235 | mask_cuda = torch.zeros(self.grasp_centers.shape[0]).cuda() 236 | local_hand_points= torch.from_numpy(self.local_hand_points).cuda() 237 | bottom_centers = torch.from_numpy(self.bottom_centers).cuda() 238 | pc = torch.from_numpy(self.used_pc).cuda() 239 | 240 | gripper_points_p = torch.tensor([local_hand_points[4][0],local_hand_points[2][1],local_hand_points[1][2], 241 | local_hand_points[12][0],local_hand_points[9][1],local_hand_points[10][2], 242 | local_hand_points[3][0],local_hand_points[13][1],local_hand_points[2][2], 243 | local_hand_points[12][0],local_hand_points[15][1],local_hand_points[11][2]]).reshape(4,1,-1).cuda() 244 | 245 | gripper_points_n = torch.tensor([local_hand_points[8][0],local_hand_points[1][1],local_hand_points[4][2], 246 | local_hand_points[10][0],local_hand_points[1][1],local_hand_points[9][2], 247 | local_hand_points[7][0],local_hand_points[2][1],local_hand_points[3][2], 248 | local_hand_points[20][0],local_hand_points[11][1],local_hand_points[12][2]]).reshape(4,1,-1).cuda() 249 | 250 | #对每个抓取进行碰撞检测 251 | for i in tqdm(range(len(bottom_centers)),desc='collision_check_pc'): 252 | 253 | #得到标准的旋转矩阵 254 | matrix = poses_cuda[i] 255 | #转置=求逆（酉矩阵） 256 | grasp_matrix = matrix.T # same as cal the inverse 257 | #获取所有的点相对于夹爪底部中心点的向量 258 | points = pc - bottom_centers[i].reshape(1, 3) 259 | points_g = torch.mm(grasp_matrix, points.T).T 260 | #查找左侧夹爪碰撞检查 261 | points_p = points_g.repeat(4,1,1) 262 | points_n = points_g.repeat(4,1,1) 263 | 264 | points_p = points_p-gripper_points_p 265 | points_n = points_n-gripper_points_n 266 | check_op =torch.where(torch.sum((torch.mul(points_p,points_n)<0)[0],dim=1)==3)[0] 267 | 268 | #check_c = (torch.mul(points_p,points_n)<0)[1:] 269 | check_ = torch.where(torch.sum((torch.mul(points_p,points_n)<0)[1:],dim=2)==3)[0] 270 | 271 | points_in_close_area=points_g[check_op] #(-1,3) 272 | #if points_in_gripper_index.shape[0] == 0:#不存在夹爪点云碰撞 273 | if len(check_)==0: 274 | collision = False 275 | #检查夹爪内部点数是否够 276 | if len(points_in_close_area):#夹爪内部有点 277 | deepist_point_x = torch.min(points_in_close_area[:,0]) 278 | insert_dist = ags.gripper.hand_depth-deepist_point_x.cpu() 279 | #设置夹爪内部点的最少点数,以及插入夹爪的最小深度 280 | if len(points_in_close_area)(self.table_hight+self.safe_dis) 333 | mask = mask.flatten()#(-1,) 334 | 335 | self.bad_grasp_centers = self.grasp_centers[~mask] #(-1,3) 336 | self.bad_grasp_poses = self.grasp_poses[~mask] #(-1,3,3) 337 | self.bad_bottom_centers=self.bottom_centers[~mask] 338 | self.bad_hand_points = self.hand_points[~mask] 339 | 340 | self.grasp_centers = self.grasp_centers[mask] #(-1,3) 341 | self.grasp_poses = self.grasp_poses[mask] #(-1,3,3) 342 | self.grasp_scores = self.grasp_scores[mask]#(-1,) 343 | self.bottom_centers=self.bottom_centers[mask] 344 | self.hand_points = self.hand_points[mask] 345 | 346 | after =len(self.grasp_centers) 347 | print('Collision_check_table done: ',before,' to ',after) 348 | 349 | def restrict_approach_angle(self): 350 | """仅仅保留抓取approach轴(世界坐标系W)与世界坐标系-z轴夹角 小于max_angle(度)的抓取 351 | 防止抓取失败率过高 352 | """ 353 | if self.grasp_centers.shape[0]==0: 354 | return 0 355 | 356 | before = len(self.grasp_centers) 357 | 358 | #抽取出各个抓取approach轴在世界坐标系W下的单位向量 359 | grasps_approach = self.grasp_poses[:,:,0] #(-1,3) 360 | #单位化 361 | grasps_approach = grasps_approach/np.linalg.norm(grasps_approach,axis=1,keepdims=True) #(-1,3) 362 | 363 | cos_angles = grasps_approach.dot(np.array([0,0,-1]).T) #(-1,) 364 | #限制抓取approach轴与世界-z轴的角度范围 365 | mask = cos_angles>np.cos(self.max_angle/180*pi) 366 | #print(cos_angles[mask],np.cos(45/180*pi)) 367 | self.bad_grasp_centers = self.grasp_centers[~mask] 368 | self.bad_grasp_poses = self.grasp_poses[~mask] 369 | self.bad_hand_points = self.hand_points[~mask] 370 | self.bad_bottom_centers=self.bottom_centers[~mask] 371 | 372 | self.grasp_centers = self.grasp_centers[mask] 373 | self.grasp_poses = self.grasp_poses[mask] 374 | self.grasp_scores = self.grasp_scores[mask] 375 | self.bottom_centers=self.bottom_centers[mask] 376 | self.hand_points = self.hand_points[mask] 377 | 378 | after =len(self.grasp_centers) 379 | print('Restrict_approach_angl done: ',before,' to ',after) 380 | 381 | def collision_check_validate(self): 382 | """验证夹爪闭合区域是否有点 383 | """ 384 | #mask =np.zeros(centers.shape[0]) 385 | poses_cuda=torch.from_numpy(self.grasp_poses).cuda() 386 | local_hand_points= torch.from_numpy(self.local_hand_points).cuda() 387 | bottom_centers = torch.from_numpy(self.bottom_centers).cuda() 388 | pc = torch.from_numpy(self.pc).cuda() 389 | 390 | 391 | gripper_points_p = torch.tensor([local_hand_points[4][0],local_hand_points[2][1],local_hand_points[1][2]]).reshape(1,-1).cuda() 392 | gripper_points_n = torch.tensor([local_hand_points[8][0],local_hand_points[1][1],local_hand_points[4][2]]).reshape(1,-1).cuda() 393 | grasps_neighbor_points_index =[] 394 | 395 | #对每个抓取进行碰撞检测 396 | for i in tqdm(range(len(bottom_centers)),desc='collision_check_validate'): 397 | #得到标准的旋转矩阵 398 | matrix = poses_cuda[i] 399 | #转置=求逆（酉矩阵） 400 | grasp_matrix = matrix.T # same as cal the inverse 401 | #获取所有的点相对于夹爪底部中心点的向量 402 | points = pc - bottom_centers[i].reshape(1, 3) 403 | points_g = torch.mm(grasp_matrix, points.T).T #[len(pc),3] 404 | # 405 | points_p = points_g-gripper_points_p #[len(pc),3] 406 | points_n = points_g-gripper_points_n #[len(pc),3] 407 | points_index =torch.where(torch.sum((torch.mul(points_p,points_n)<0),dim=1)==3)[0] 408 | 409 | if len(points_index)==0: 410 | raise ValueError("Grasp has no neighbor points!") 411 | #print("Grasp has no neighbor points!") 412 | else: 413 | grasps_neighbor_points_index.append(points_index.cpu().numpy()) 414 | 415 | def get_rot_mat(self,poses_vector): 416 | '''从抓取向量中计算出夹爪相对于mesh模型的姿态 417 | ''' 418 | major_pc = poses_vector[:,3:6] # (-1,3) 419 | angle = poses_vector[:,[7]]# (-1,1) 420 | 421 | 422 | # cal approach 423 | cos_t = np.cos(angle) #(-1,1) 424 | sin_t = np.sin(angle) 425 | zeros= np.zeros(cos_t.shape) #(-1,1) 426 | ones = np.ones(cos_t.shape) 427 | 428 | #绕抓取binormal轴的旋转矩阵 429 | R1 = np.c_[cos_t, zeros, -sin_t,zeros, ones, zeros,sin_t, zeros, cos_t].reshape(-1,3,3) #[len(grasps),3,3] 430 | #print(R1) 431 | axis_y = major_pc #(-1,3) 432 | 433 | #设定一个与抓取y轴垂直且与C:x-o-y平面平行的单位向量作为初始x轴 434 | axis_x = np.c_[axis_y[:,[1]], -axis_y[:,[0]], zeros] 435 | #查找模为0的行，替换为[1,0,0] 436 | axis_x[np.linalg.norm(axis_x,axis=1)==0]=np.array([1,0,0]) 437 | #单位化 438 | axis_x = axis_x / np.linalg.norm(axis_x,axis=1,keepdims=True) 439 | axis_y = axis_y / np.linalg.norm(axis_y,axis=1,keepdims=True) 440 | #右手定则，从x->y 441 | axis_z = np.cross(axis_x, axis_y) 442 | 443 | #这个R2就是一个临时的夹爪坐标系，但是它的姿态还不代表真正的夹爪姿态 444 | R2 = np.c_[axis_x, np.c_[axis_y, axis_z]].reshape(-1,3,3).swapaxes(1,2) 445 | #将现有的坐标系利用angle进行旋转，就得到了真正的夹爪坐标系， 446 | # 抽出x轴作为approach轴(原生dex-net夹爪坐标系) 447 | #由于是相对于运动坐标系的旋转，因此需要右乘 448 | R3=np.matmul(R2,R1) 449 | ''' 450 | approach_normal =R3[:, :,0] 451 | #print(np.linalg.norm(approach_normal,axis=1,keepdims=True)) 452 | approach_normal = approach_normal / np.linalg.norm(approach_normal,axis=1,keepdims=True) 453 | #minor_pc=R3[:, :,2] 是一样的 454 | minor_pc = np.cross( approach_normal,major_pc) 455 | ''' 456 | #然后把平移向量放在每个旋转矩阵的最右侧，当成一列 457 | return R3 458 | 459 | 460 | def get_grasp_angle(self): 461 | #将抓取转变回8d（7+1）向量保存（相对于相机坐标系） 462 | binormals=self.grasp_poses[:,:,1] #抽出抓取的binormal轴 退化为(-1,3) 463 | approachs=self.grasp_poses[:,:,0] #抽出approach轴 (-1,3) 464 | #计算angles，找到与binormals垂直且平行于C:x-o-y平面的向量 465 | temp =np.concatenate((binormals[:,[1]],-binormals[:,[0]]),axis =1) #(-1,2) 466 | project = np.zeros((temp.shape[0],temp.shape[1]+1)) 467 | project[:,:-1]=temp #(-1,3) 468 | #计算投影approach与binormal之间的角度 469 | cos_angles = np.sum(approachs*project,axis=1)#退化(-1,) 470 | #检测负号 471 | minus_mask =approachs[:,2]>0 472 | #求出angles 473 | angles = np.arccos(cos_angles) 474 | angles[minus_mask] = -angles[minus_mask] 475 | 476 | def show_grasps_pc(self,max_n=200,show_bad=False,show_mesh=False,title='scene'): 477 | """显示点云和抓取，仅限于单进程debug 478 | """ 479 | mlab.figure(figure=title+'_good grasp',bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000))#创建一个窗口 480 | _ = self.show_points(self.used_pc) 481 | print(title+'_good grasp:{}'.format(self.grasp_centers.shape[0])) 482 | 483 | #是否把原始的模型也显示出来 484 | if show_mesh: 485 | for mesh_index,path in enumerate(self.mesh_list): 486 | #mesh = mlab.pipeline.open(path) 487 | #获取原始点云 488 | path = path.split('.obj')[0]+'.stl' 489 | if 'bg' in path: 490 | continue 491 | 492 | mesh = o3d.io.read_triangle_mesh(path) 493 | #得到点云对象 494 | raw_pc = o3d.geometry.TriangleMesh.sample_points_uniformly(mesh, np.asarray(mesh.vertices).shape[0] * 10) 495 | #均匀降采样 496 | voxel_pc = o3d.geometry.PointCloud.voxel_down_sample(raw_pc, 0.001) 497 | #将点云转换为np对象 498 | pc = np.asarray(voxel_pc.points).T 499 | #WTM 模型在场景中的位置姿态 500 | world_to_mesh_7d = self.mesh_poses_array[mesh_index] 501 | world_to_mesh_rot = RigidTransform.rotation_from_quaternion(world_to_mesh_7d[3:]) 502 | world_to_mesh_trans = world_to_mesh_7d[0:3] 503 | world_to_mesh_T = RigidTransform(world_to_mesh_rot,world_to_mesh_trans).matrix 504 | 505 | CTM = np.matmul(self.scaner_to_world_T,world_to_mesh_T) 506 | ctm_rot = CTM[0:3,0:3] 507 | ctm_trans = CTM[0:3,3].reshape(-1,3) 508 | 509 | pc = ctm_rot.dot(pc).T+ctm_trans 510 | 511 | 512 | 513 | _ = self.show_points(pc,color='b') 514 | 515 | 516 | 517 | #随机抽选出n个抓取进行显示 518 | if self.grasp_centers.shape[0]>max_n: 519 | print(title+'_good grasp:{} , show random {} grasps here'.format(self.grasp_centers.shape[0],max_n)) 520 | mask = random.sample(range(self.grasp_centers.shape[0]),max_n) 521 | good_hand_points = self.hand_points[mask] 522 | else: 523 | print(title+'_good grasp:{}'.format(self.grasp_centers.shape[0])) 524 | good_hand_points = self.hand_points 525 | 526 | #显示good抓取出来 527 | for index,hand_points in enumerate(good_hand_points): 528 | ags.show_grasp_3d(hand_points) 529 | 530 | #grasp_bottom_center = -ags.gripper.hand_depth * good_poses[index][:,0] + center 531 | #显示抓取的编号 532 | #mlab.text3d(grasp_bottom_center[0],grasp_bottom_center[1],grasp_bottom_center[2], 533 | # str(index),scale = (0.01),color=(1,0,0)) 534 | 535 | #显示bad抓取 536 | if not show_bad: 537 | pass 538 | else: 539 | mlab.figure(figure=title+'_bad grasp',bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000))#创建一个窗口 540 | _ = self.show_points(self.used_pc) 541 | #如果错误抓取太多，就随机抽选一些,方便显示 542 | max_n =200 543 | if self.bad_centers.shape[0]>max_n: 544 | print(title+'_bad grasp:{} , show random {} grasps here'.\ 545 | format(self.bad_grasp_centers.shape[0],max_n)) 546 | mask = random.sample(range(self.bad_grasp_centers.shape[0]),max_n) 547 | bad_hand_points = self.bad_hand_points[mask] 548 | else: 549 | print(title+'_bad grasp:{}'.format(self.bad_grasp_centers.shape[0])) 550 | bad_hand_points = self.bad_hand_points 551 | 552 | 553 | for index,hand_points in enumerate(bad_hand_points): 554 | ags.show_grasp_3d(hand_points) 555 | #grasp_bottom_center = -ags.gripper.hand_depth * bad_poses[index][:,0] + center 556 | #显示抓取的编号 557 | #mlab.text3d(grasp_bottom_center[0],grasp_bottom_center[1],grasp_bottom_center[2], 558 | # str(index),scale = (0.01),color=(1,0,0)) 559 | mlab.show() 560 | 561 | 562 | 563 | 564 | 565 | def show_points(self,point, name='pc',color='lb', scale_factor=.004): 566 | if color == 'b': 567 | color_f = (0, 0, 1) 568 | elif color == 'r': 569 | color_f = (1, 0, 0) 570 | elif color == 'g': 571 | color_f = (0, 1, 0) 572 | elif color == 'lb': # light blue 573 | color_f = (0.22, 1, 1) 574 | else: 575 | color_f = (1, 1, 1) 576 | if point.size == 3: # vis for only one point, shape must be (3,), for shape (1, 3) is not work 577 | point = point.reshape(3, ) 578 | mlab.points3d(point[0], point[1], point[2], color=color_f, scale_factor=scale_factor) 579 | else: # vis for multiple points 580 | mlab.points3d(point[:, 0], point[:, 1], point[:, 2], color=color_f, scale_factor=scale_factor) 581 | 582 | return point.shape[0] 583 | 584 | 585 | 586 | 587 | if __name__ == '__main__': 588 | 589 | do_job = GetLegalGrasps(use_dense=False) 590 | 591 | print('All done') 592 | 593 | -------------------------------------------------------------------------------- /group_mask_generate.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from logging import raiseExceptions 3 | from math import pi 4 | import math 5 | import os 6 | import sys 7 | import argparse 8 | import time 9 | from matplotlib.pyplot import axis 10 | import mayavi 11 | import numpy as np 12 | import pickle 13 | import glob 14 | import random 15 | from mayavi import mlab 16 | from torch._C import set_autocast_enabled 17 | from tqdm import tqdm 18 | from numpy.core.fromnumeric import swapaxes, transpose 19 | import torch 20 | from tqdm import tqdm 21 | import multiprocessing 22 | import copy 23 | from dexnet.grasping import GpgGraspSampler 24 | from autolab_core import RigidTransform 25 | from autolab_core import YamlConfig 26 | from dexnet.grasping import RobotGripper 27 | 28 | 29 | 30 | #解析命令行参数 31 | parser = argparse.ArgumentParser(description='Group and mask generation') 32 | parser.add_argument('--gripper', type=str, default='baxter') # 33 | parser.add_argument('--load_npy',action='store_true') #设置同时处理几个场景 34 | parser.add_argument('--process_num', type=int, default=70) #设置同时处理几个场景 35 | parser.add_argument('--dis_min', type=float, default=0.01) #距离聚类 36 | parser.add_argument('--theta_min', type=float, default=0.706) #角度聚类 37 | parser.add_argument('--redo', type=bool, default=False) #是否重新计算聚类矩阵 38 | 39 | 40 | parameters = parser.parse_args() 41 | home_dir = os.environ['HOME'] 42 | #场景文件夹 43 | scenes_dir = home_dir+"/dataset/simulate_grasp_dataset/{}/scenes/".format(parameters.gripper) 44 | legal_grasp_file_paths = glob.glob(scenes_dir+'*/legal_grasps_with_score.npy') 45 | legal_grasp_file_paths.sort() 46 | 47 | final_dir = home_dir+"/dataset/simulate_grasp_dataset/{}/final/".format(parameters.gripper) 48 | 49 | yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") 50 | gripper = RobotGripper.load(parameters.gripper, home_dir + "/code/dex-net/data/grippers") 51 | graspSampler = GpgGraspSampler(gripper, yaml_config) 52 | 53 | 54 | device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 55 | print(device) 56 | 57 | 58 | 59 | 60 | 61 | 62 | class Mask_generate: 63 | '''根据已有的抓取生成点云mask 64 | ''' 65 | def __init__(self,grasp_file_paths,final_dir,parameters,sampler,show_result=False,dense_pc=False) -> None: 66 | '''grasp_file_paths 抓取文件的路径 67 | parameters 命令行参数 68 | sampler dex抓取采样器 69 | ''' 70 | self.grasp_file_paths=grasp_file_paths 71 | #self.process_num=process_num 72 | self.scene_num = len(grasp_file_paths) 73 | self.parameters = parameters 74 | self.sampler = sampler 75 | self.dense_pc = dense_pc 76 | self.final_dir = final_dir #存放最终文件的文件夹 77 | 78 | if show_result: 79 | #显示结果，测试用 80 | self.show_result() 81 | 82 | else: 83 | #首先计算每个抓取之间的差别 84 | #self.compute_grasp_diff() 85 | #多进程进行场景抓取聚类，排序 86 | #self.grasp_grouping() 87 | #对每个场景的每个合法抓取进行夹爪周边点云索引计算，并保存 88 | #self.compute_neighbor_points() 89 | #计算mask 90 | #self.mask_generate() 91 | 92 | self.generate_final_files() 93 | 94 | def generate_final_files(self): 95 | if not os.path.exists(self.final_dir): 96 | os.makedirs(self.final_dir) 97 | 98 | max_digits = len(str(len(self.grasp_file_paths))) 99 | #scene_dir = os.path.join(scenes_dir+str(scene_n).zfill(max_digits)) 100 | 101 | for scene_index,grasp_file in enumerate(self.grasp_file_paths): 102 | #读取场景grasps 103 | data = np.load(grasp_file, allow_pickle=True).item() 104 | #删除不必要的数据 105 | keys = list(data.keys()) 106 | for key in keys: 107 | if '_temp' in key: 108 | del data[key] 109 | 110 | #保存 111 | final_path = os.path.join(self.final_dir,str(scene_index).zfill(max_digits)+'.npy') 112 | np.save(final_path,data) 113 | print('Final file {} done') 114 | 115 | ''' 116 | dir = os.path.dirname(grasp_file) 117 | ob = glob.glob(dir+'/*.pickle') 118 | for file in ob: 119 | if 'table_meshes_with_pose' in file: 120 | pass 121 | else: 122 | os.remove(file) 123 | ''' 124 | 125 | 126 | 127 | 128 | 129 | 130 | def show_result(self): 131 | final_files = glob.glob(self.final_dir+'*.npy') 132 | final_files.sort() 133 | for scene_index,final_file in enumerate(final_files): 134 | #读取场景grasps 135 | data = np.load(final_file, allow_pickle=True).item() 136 | grasp_num = len(data['grasp_centers']) 137 | 138 | print('Scene {}'.format(scene_index)+ ' legal grasp:{}'.format(grasp_num)) 139 | 140 | if self.dense_pc: 141 | pc = data['dense_pc'] 142 | else: 143 | pc = data['pc'] #场景点云 144 | mask = data['grasp_mask'] 145 | self.show_mask(pc,mask) 146 | time.sleep(0.1) 147 | #mlab.close() 148 | 149 | 150 | def temp(self,table_pc_list): 151 | for scene_index,grasp_file in enumerate(self.grasp_file_paths): 152 | #读取场景grasps 153 | data = np.load(grasp_file, allow_pickle=True).item() 154 | #ta = np.load(table_pc_list[scene_index]) 155 | #data['dense_pc'] = ta 156 | #del data['table_pc'] 157 | 158 | #先批量取出虚拟夹爪各点相对于相机坐标系C的坐标 159 | hand_points = np.empty([0,21,3]) 160 | grasps_pose = data['grasp_poses'] 161 | bottom_centers = data['bottom_centers'] 162 | for i in range(grasps_pose.shape[0]): 163 | points = self.sampler.get_hand_points(bottom_centers[i], 164 | grasps_pose[i,:,0], 165 | grasps_pose[i,:,1]).reshape(1,-1,3)#(1,21,3) 166 | hand_points = np.concatenate((hand_points,points),axis=0)#(-1,21,3) 167 | data['hand_points'] = hand_points 168 | 169 | np.save(grasp_file,data) 170 | print('Scene {}'.format(scene_index) + 'done') 171 | 172 | 173 | 174 | def compute_grasp_diff(self): 175 | ''' 176 | ''' 177 | for scene_index,grasp_file in enumerate(self.grasp_file_paths): 178 | #读取场景grasps 179 | #读取场景grasps 180 | data = np.load(grasp_file, allow_pickle=True).item() 181 | 182 | print('Scene {}'.format(scene_index)+ ' legal grasp:{}'.format(len(data['grasp_centers']))) 183 | grasp_centers = data['grasp_centers'].astype(np.float32) #抓取中心 184 | grasp_poses =data['grasp_poses'].astype(np.float32) #抓取旋转矩阵 185 | 186 | #计算距离差矩阵 187 | grasp_centers_cuda = torch.from_numpy(grasp_centers).cuda().unsqueeze(0)#[1,len(grasps),3] 188 | grasp_centers_cuda_T = grasp_centers_cuda.clone().permute(1,0,2)#[len(grasps),1,3] 189 | 190 | grasp_centers_cuda = grasp_centers_cuda.repeat(grasp_centers.shape[0],1,1) 191 | grasp_centers_cuda_T = grasp_centers_cuda_T.repeat(1,grasp_centers.shape[0],1) 192 | 193 | grasp_centers_diff =grasp_centers_cuda-grasp_centers_cuda_T 194 | grasp_centers_diff_dis = torch.norm(grasp_centers_diff,dim=2)< self.parameters.dis_min#[len(grasps),len(grasps)] 195 | #grasp_centers_diff_dis 196 | 197 | 198 | #计算 199 | grasp_poses_cuda = torch.from_numpy(grasp_poses).cuda()#[len(grasps),3,3] 200 | #姿态逆矩阵 201 | grasp_poses_inverse_cuda=grasp_poses_cuda.clone().permute(0,2,1)#.repeat(1,grasp_poses_inverse_cuda.shape[0],1,1) #转置求逆,增加维度[len(grasps),1,3,3] 202 | 203 | grasp_poses_inverse_cuda=grasp_poses_inverse_cuda.unsqueeze(1).\ 204 | repeat(1,grasp_poses_inverse_cuda.shape[0],1,1) 205 | grasp_poses_cuda = grasp_poses_cuda.unsqueeze(0).\ 206 | repeat(grasp_poses_inverse_cuda.shape[0],1,1,1)#[len(grasps),len(grasps),3,3] 207 | 208 | grasp_poses_diff = grasp_poses_inverse_cuda.matmul(grasp_poses_cuda)#[len(grasps),len(grasps),3,3] 209 | 210 | #l = torch.trace(grasp_poses_diff[1,1,:3,:3]) 211 | mask = torch.zeros((grasp_poses_inverse_cuda.shape[0], grasp_poses_inverse_cuda.shape[0],3, 3)).cuda() 212 | mask[:, :,torch.arange(0,3), torch.arange(0,3) ] = 1.0 213 | 214 | grasp_poses_diff = grasp_poses_diff.matmul(mask.float()) 215 | grasp_poses_diff_trace = torch.sum(grasp_poses_diff,dim=(2,3))#[len(grasp),len(grasp)] 216 | 217 | cosa = (grasp_poses_diff_trace - 1.0) / 2.0 #[len(grasp),len(grasp)] 218 | theta = torch.abs(torch.acos(cosa))< self.parameters.theta_min#[len(grasp),len(grasp)] 219 | 220 | #保存距离矩阵 221 | data['grasp_dis_diff_temp'] = grasp_centers_diff_dis.cpu().numpy() 222 | data['grasp_pose_diff_temp'] = theta.cpu().numpy() 223 | 224 | #更新数据 225 | np.save(grasp_file,data) 226 | print('Scene {}'.format(scene_index) + 'done') 227 | 228 | 229 | def grasp_grouping(self): 230 | '''多线程对每个抓取进行聚类 231 | ''' 232 | pool_size=self.parameters.process_num #选择同时使用多少个进程处理 233 | cores = multiprocessing.cpu_count() 234 | if pool_size>cores: 235 | pool_size = cores 236 | print('Cut pool size down to cores num') 237 | if pool_size>self.scene_num: 238 | pool_size = self.scene_num 239 | print('We got {} pc, and pool size is {}'.format(self.scene_num,pool_size)) 240 | scene_index = 0 241 | pool = [] 242 | for i in range(pool_size): 243 | pool.append(multiprocessing.Process(target=self.do_job,args=(scene_index,))) 244 | scene_index+=1 245 | [p.start() for p in pool] #启动多线程 246 | 247 | while scene_indexcores: 280 | pool_size = cores 281 | print('Cut pool size down to cores num') 282 | if pool_size>self.scene_num: 283 | pool_size = self.scene_num 284 | print('We got {} pc, and pool size is {}'.format(self.scene_num,pool_size)) 285 | scene_index = 0 286 | pool = [] 287 | for i in range(pool_size): 288 | pool.append(multiprocessing.Process(target=self.do_job_mask,args=(scene_index,))) 289 | scene_index+=1 290 | [p.start() for p in pool] #启动多线程 291 | 292 | while scene_index0.8 or iou_r>0.8:#交集占两者比例都大于80% 删除目标集 399 | delete_set_index.append(target_set_index) 400 | #求参考簇和目标簇的并集 401 | ref_set = ref_set | target_set 402 | 403 | #最后还要跟之前保存的所有的集合的并集比较一下 404 | inter_set = new_clusters_all&ref_set 405 | iou_r = len(inter_set)/len(ref_set) 406 | if iou_r >0.8: 407 | pass #说明以前的集合里面基本上是有了，不需要了 408 | else: 409 | new_clusters_sets.append(ref_set) 410 | #更新 411 | new_clusters_all = new_clusters_all | ref_set 412 | 413 | #clusters_sets[ref_index] = u_set 414 | ref_index+=1#交集占目标集与参考集比例都不超过80%，不删除集合 415 | 416 | #在参考簇对比完所有的簇之后，删除list中的目标簇 417 | #clusters_sets = np.delete(clusters_sets,delete_set_index) 418 | #self.show_clusters(pc,[clusters_sets[ref_index-1]]) 419 | #clusters_index = np.delete(clusters_index,delete_set_index) 420 | 421 | return new_clusters_sets 422 | 423 | 424 | def iou_check_(self,clusters_sets): 425 | ''' 根据不同聚类簇之间的重叠率，来对原始的cluster进行融合、删除等操作 426 | ''' 427 | raw_cluster_num = len(clusters_sets) 428 | clusters_index = np.array(list(range(raw_cluster_num))) 429 | clusters_sets = np.array(clusters_sets) 430 | 431 | #融合聚类簇，进一步调整簇的范围，将大规模重叠的地方整合起来，主要是球体的地方会大规模重叠 432 | ref_index = 0 #参考簇index 433 | while ref_index < len(clusters_sets)-1: 434 | delete_set_index = [] #删除集合index 435 | ref_set = clusters_sets[ref_index] #取出当前参考簇 436 | 437 | next_ref_index = ref_index+1 #下一个参考簇index 438 | current_ref_index = ref_index 439 | #从参考簇下一个开始与参考簇对比 440 | for target_set_index in range(next_ref_index,len(clusters_sets)): 441 | target_set = clusters_sets[target_set_index]#取出目标集 442 | inter_set = ref_set.intersection(target_set)#求 "参考集-目标集"之间的交集 443 | 444 | iou_t = len(inter_set)/len(target_set)#交集 占 目标集的比例 445 | iou_r = len(inter_set)/len(ref_set)#交集 占 参考集的比例 446 | 447 | if iou_t>0.8 or iou_r>0.8:#交集占两者比例都大于80% 删除目标集 448 | if target_set_index in delete_set_index: 449 | pass 450 | else: 451 | delete_set_index.append(target_set_index) 452 | #求参考簇和目标簇的并集 453 | clusters_sets[ref_index] = clusters_sets[ref_index] | clusters_sets[target_set_index] 454 | 455 | ref_index=next_ref_index#移动参考index到next 456 | ''' 457 | elif iou_t<0.8 and iou_r>0.8:#占参考簇大于0.8占目标簇小于0.8 458 | if current_ref_index in delete_set_index: 459 | pass 460 | else: 461 | delete_set_index.append(current_ref_index)#交集占参考集比例大于80% 删除参考集 462 | elif iou_t>0.8 and iou_r<0.8: 463 | delete_set_index.append(target_set_index)#交集占目标集比例大于80% 删除目标集 464 | ref_index=next_ref_index 465 | ''' 466 | else: 467 | ref_index=next_ref_index#交集占目标集与参考集比例都不超过80%，不删除集合 468 | 469 | #在参考簇对比完所有的簇之后，删除list中的目标簇 470 | clusters_sets = np.delete(clusters_sets,delete_set_index) 471 | clusters_index = np.delete(clusters_index,delete_set_index) 472 | return clusters_sets 473 | 474 | 475 | def posesTransWithinErrorBoundsCuda(self,index_i,index_j,centers_diff,theta_diff): 476 | '''计算两个抓取中心、pose之间的偏差是否超过阈值 477 | ''' 478 | 479 | if index_i>index_j: 480 | centers_diff_ = centers_diff[index_j,index_i] 481 | theta_diff_=theta_diff[index_j,index_i] 482 | elif index_i30: 539 | # grasp_clusters = grasp_clusters[:30] 540 | 541 | #保存group with score 542 | data['grasp_clusters_temp']=grasp_clusters 543 | np.save(grasp_file,data) 544 | 545 | #当前场景聚类完毕 546 | print('Scene {}'.format(scene_index)+ ' legal grasp:{}'.format(len(data['grasp_centers']))+' done') 547 | 548 | 549 | # 获取列表的第二个元素 550 | def takeSecond(self,elem): 551 | return elem[1] 552 | 553 | 554 | def get_gripper_neighor_points_cuda(self,data): 555 | """对CTG抓取姿态和Cpc进行碰撞检测(使用显卡加速计算) 556 | 返回符合要求的，场景点云点的索引，每个抓取都有自己的内部点索引，是一个二维list 557 | """ 558 | 559 | grasp_poses = data['grasp_poses']#抓取旋转矩阵 560 | bottom_centers = data['bottom_centers'] 561 | local_hand_points_extend = data['local_hand_points_extend'] 562 | if self.dense_pc: 563 | pc = data['dense_pc'] 564 | else: 565 | pc = data['pc'] 566 | 567 | 568 | #mask =np.zeros(centers.shape[0]) 569 | poses_cuda=torch.from_numpy(grasp_poses).cuda() 570 | hand_points= torch.from_numpy(local_hand_points_extend).cuda() 571 | bottom_centers = torch.from_numpy(bottom_centers).cuda() 572 | pc = torch.from_numpy(pc).cuda() 573 | 574 | 575 | #gripper_points_p = torch.tensor([hand_points[10][0],hand_points[13][1],hand_points[9][2]]).reshape(1,-1).cuda() 576 | #gripper_points_n = torch.tensor([hand_points[20][0],hand_points[9][1],hand_points[10][2]]).reshape(1,-1).cuda() 577 | gripper_points_p = torch.tensor([hand_points[4][0],hand_points[2][1],hand_points[1][2]]).reshape(1,-1).cuda() 578 | gripper_points_n = torch.tensor([hand_points[8][0],hand_points[1][1],hand_points[4][2]]).reshape(1,-1).cuda() 579 | grasps_neighbor_points_index =[] 580 | 581 | #对每个抓取进行碰撞检测 582 | for i in tqdm(range(len(bottom_centers)),desc='collision_check_pc'): 583 | #得到标准的旋转矩阵 584 | matrix = poses_cuda[i] 585 | #转置=求逆（酉矩阵） 586 | grasp_matrix = matrix.T # same as cal the inverse 587 | #获取所有的点相对于夹爪底部中心点的向量 588 | points = pc - bottom_centers[i].reshape(1, 3) 589 | points_g = torch.mm(grasp_matrix, points.T).T #[len(pc),3] 590 | # 591 | points_p = points_g-gripper_points_p #[len(pc),3] 592 | points_n = points_g-gripper_points_n #[len(pc),3] 593 | points_index =torch.where(torch.sum((torch.mul(points_p,points_n)<0),dim=1)==3)[0] 594 | 595 | #points_in_close_area=points_g[check_op] #(-1,3) 596 | #if points_in_gripper_index.shape[0] == 0:#不存在夹爪点云碰撞 597 | if len(points_index)==0: 598 | raise ValueError("Grasp has no neighbor points!") 599 | #print("Grasp has no neighbor points!") 600 | else: 601 | grasps_neighbor_points_index.append(points_index.cpu().numpy()) 602 | 603 | return grasps_neighbor_points_index 604 | 605 | def show_mask(self,pc,mask=[],hand_points=[],name='Scene Mask', scale_factor=.004): 606 | '''显示生成结果 607 | ''' 608 | m=mask.shape[1] #cluster数量 609 | color_ = [(0, 0, 1),(1, 0, 0),(0, 1, 0),(0.22, 1, 1),(0.8, 0.8, 0.2), 610 | (0.5, 0, 1),(1, 0.62, 0),(0.53, 1, 0),(0.22, 0.63, 1),(0.8, 0.22, 0.2)] 611 | 612 | #画出点云 613 | mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.25, 0.88, 0.81),size=(1800,1800)) 614 | mlab.points3d(pc[:, 0], pc[:, 1], 615 | pc[:, 2], color=(0.5,0.5,0.5), scale_factor=scale_factor) 616 | 617 | # 618 | if m==0: 619 | mlab.show() 620 | 621 | for i in range(m): 622 | index = np.where(mask[:,i]==1)[0] 623 | mlab.points3d(pc[index][:, 0], pc[index][:, 1],pc[index][:, 2], \ 624 | color=color_[i], scale_factor=scale_factor) 625 | 626 | if len(hand_points)==0: 627 | mlab.show() 628 | else: 629 | index = list(range(hand_points.shape[0])) 630 | random.shuffle(index) 631 | max_n=500 632 | if len(index)10:#仅仅显示排名前10的聚类结果 658 | m=10 659 | 660 | #m-=1 661 | #n=m-1 662 | for i in range(m): 663 | j=i #反序显示，将最大的簇最后显示 664 | mlab.points3d(pc[list(clusters_sets[j])][:, 0], pc[list(clusters_sets[j])][:, 1],pc[list(clusters_sets[j])][:, 2], color=color_[j], scale_factor=scale_factor) 665 | 666 | if len(hand_points)==0: 667 | mlab.show() 668 | else: 669 | index = list(range(hand_points.shape[0])) 670 | random.shuffle(index) 671 | max_n=500 672 | if len(index)=0.75) #z坐标高于0.75m的桌面以上点云 68 | pc = pc[index_]#检索出原始点云桌面以上的点云table 69 | scene_in = raw_pc_paths[scene_index].split('/')[-2] 70 | path = os.path.join(scenes_dir,scene_in,'dense_pc.npy') 71 | np.save(path,pc) 72 | 73 | 74 | #随机采样，采样出n个点; 保持每一帧点云的数量都是一致的 75 | pc = pc[np.random.choice(len(pc), size=args.points_num, replace=True)] 76 | 77 | #为三个轴方向添加随机高斯噪声 78 | noise = np.random.random(size=(len(pc),3))*args.noise #[len(pc),3] 79 | pc = pc+noise # 80 | 81 | 82 | #将处理后的点云保存下来 83 | 84 | path = os.path.join(scenes_dir,scene_in,'pc.npy') 85 | np.save(path,pc) 86 | 87 | -------------------------------------------------------------------------------- /poses_simulation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import os 3 | import sys 4 | import random 5 | import numpy as np 6 | import mujoco_py 7 | import pickle 8 | import argparse 9 | import multiprocessing 10 | import shutil 11 | import glob 12 | 13 | #解析命令行参数 14 | parser = argparse.ArgumentParser(description='Simulate poses for meshes') 15 | parser.add_argument('--gripper', type=str, default='baxter') 16 | args = parser.parse_args() 17 | 18 | def get_file_name(file_dir_): 19 | file_list = [] 20 | for root, dirs, files in os.walk(file_dir_): 21 | for file in files: 22 | #查找场景xml文件 23 | if file.find('scene_') != -1 and file.find('.xml') != -1: 24 | #保存下来该文件的路径 25 | file_list.append(os.path.join(root,file)) 26 | #排序 27 | file_list.sort() 28 | return file_list 29 | 30 | def get_mesh_list(xml_string): 31 | mesh_list=[] 32 | #按照回车分割 33 | xml_string_list = xml_string.split('\n') 34 | for string in xml_string_list: 35 | if string.find('0.001): 126 | steady = True 127 | else: 128 | pos_old = pos_now 129 | 130 | pos_now=pos_now.reshape(-1,7) 131 | print("警告：这里需要优化，一些物体可能在空中",__file__,sys._getframe().f_lineno) 132 | mesh_on_table = pos_now[:,2]>0.5 #找到稳定姿态的高度高于0.5m的mesh 133 | #mesh_on_table = mesh_on_table[:,0] 134 | 135 | #抽取出满足某个条件的行（保留合法的姿态） 136 | pos_legal = pos_now[mesh_on_table,:] 137 | table_pose=np.reshape(table_pose,(-1,7)) 138 | #拼接起来啊 139 | pos_legal = np.concatenate((table_pose,pos_legal),axis = 0) 140 | 141 | 142 | 143 | #保留合法的mesh名称（带有路径） 144 | mesh_legal = [] 145 | #首先把桌子的路径先加上去 146 | mesh_legal.append(os.path.join(scene_i_meshes_dir,'bg_table.obj')) 147 | #检索桌面上的物体 148 | for index,item in enumerate(mesh_on_table): 149 | if item: 150 | mesh_legal.append(os.path.join(scene_i_meshes_dir,scene_i_meshes_list[index]+'.obj')) 151 | 152 | 153 | #使用npy保存位置合法姿态列表 154 | #np.save(os.path.join(scene_i_xml_root_path,"legal_poses.npy"),pos_legal) 155 | 156 | #直接用pickel保存 157 | table_meshes_with_pose=(mesh_legal,pos_legal) 158 | with open(os.path.join(scene_i_xml_root_path,"table_meshes_with_pose.pickle"),'wb') as f: 159 | pickle.dump(table_meshes_with_pose, f) 160 | 161 | 162 | def mycopyfile(srcfile,dstpath): # 复制函数 163 | if not os.path.isfile(srcfile): 164 | print ("%s not exist!"%(srcfile)) 165 | else: 166 | fpath,fname=os.path.split(srcfile) # 分离文件名和路径 167 | if not os.path.exists(dstpath): 168 | os.makedirs(dstpath) # 创建路径 169 | shutil.copy(srcfile, dstpath + fname) # 复制文件 170 | print ("copy %s -> %s"%(srcfile, dstpath + fname)) 171 | 172 | 173 | 174 | if __name__ == '__main__': 175 | 176 | gripper_name = args.gripper 177 | 178 | home_dir = os.environ['HOME'] 179 | 180 | scenes_dir = home_dir+"/dataset/simulate_grasp_dataset/"+gripper_name+"/scenes" 181 | #获取所有场景的xml文件的路径列表 182 | scenes_xml_path_list = get_file_name(scenes_dir) 183 | print("为夹爪{}找到{}帧场景".format(gripper_name,len(scenes_xml_path_list))) 184 | #用于debug 185 | #simulate_file = scene_xml_dir+"test.xml" 186 | mangaer = multiprocessing.Manager() 187 | failed_list =mangaer.list() 188 | 189 | #设置同时仿真几个场景 190 | pool_size= multiprocessing.cpu_count() # 191 | if pool_size>len(scenes_xml_path_list): 192 | pool_size = len(scenes_xml_path_list) 193 | #pool_size = 1 194 | scnen_index = 0 195 | pool = [] 196 | for i in range(pool_size): 197 | pool.append(multiprocessing.Process(target=do_job,args=(scnen_index,))) 198 | scnen_index+=1 199 | [p.start() for p in pool] #启动多进程 200 | #refull 201 | while scnen_indexlen(table_obj_poses_path): 163 | pool_size = len(table_obj_poses_path) 164 | scene_index = 0 165 | pool = [] 166 | for i in range(pool_size): 167 | pool.append(multiprocessing.Process(target=do_job,args=(scene_index,))) 168 | scene_index+=1 169 | [p.start() for p in pool] #启动多线程 170 | 171 | while scene_indexlen(objects_to_download): 138 | pool_size = len(objects_to_download) 139 | count = 0 140 | pool = [] 141 | for i in range(pool_size): 142 | count = count+1 143 | pool.append(multiprocessing.Process(target=do_job,args=(objects[i],files_to_download,))) 144 | [p.start() for p in pool] #启动多线程 145 | 146 | while count