├── .vscode ├── launch.json └── settings.json ├── README.md ├── build └── lib │ └── easydexnet │ ├── __init__.py │ ├── camare.py │ ├── colcor.py │ ├── contact.py │ ├── data_saver.py │ ├── dex_object.py │ ├── grasp.py │ ├── grasp_2d.py │ ├── grasp_sampler.py │ ├── mesh.py │ ├── quality.py │ ├── quality_function.py │ ├── render.py │ ├── stable_poses.py │ └── vision.py ├── config ├── add_obj.yaml ├── generate.yaml └── test.yaml ├── data ├── Waterglass_800_tex.obj ├── bar_clamp.obj ├── table.obj └── test.obj ├── dist └── easy_dexnet-0.1.0-py3.6.egg ├── setup.py ├── src ├── easy_dexnet.egg-info │ ├── PKG-INFO │ ├── SOURCES.txt │ ├── dependency_links.txt │ ├── requires.txt │ └── top_level.txt └── easydexnet │ ├── __init__.py │ ├── __pycache__ │ ├── __init__.cpython-36.pyc │ ├── camare.cpython-36.pyc │ ├── colcor.cpython-36.pyc │ ├── contact.cpython-36.pyc │ ├── data_saver.cpython-36.pyc │ ├── dex_object.cpython-36.pyc │ ├── grasp.cpython-36.pyc │ ├── grasp_2d.cpython-36.pyc │ ├── grasp_sampler.cpython-36.pyc │ ├── mesh.cpython-36.pyc │ ├── quality.cpython-36.pyc │ ├── quality_function.cpython-36.pyc │ ├── render.cpython-36.pyc │ ├── stable_poses.cpython-36.pyc │ └── vision.cpython-36.pyc │ ├── camare.py │ ├── colcor.py │ ├── contact.py │ ├── data_saver.py │ ├── dex_object.py │ ├── grasp.py │ ├── grasp_2d.py │ ├── grasp_sampler.py │ ├── mesh.py │ ├── quality.py │ ├── quality_function.py │ ├── render.py │ ├── stable_poses.py │ └── vision.py ├── test ├── test.log └── test.py └── tools ├── add_obj_to_hdf5.py └── generate.py /.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: Current File (Integrated Terminal)", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "console": "integratedTerminal" 13 | }, 14 | { 15 | "name": "Python: Remote Attach", 16 | "type": "python", 17 | "request": "attach", 18 | "port": 5678, 19 | "host": "localhost", 20 | "pathMappings": [ 21 | { 22 | "localRoot": "${workspaceFolder}", 23 | "remoteRoot": "." 24 | } 25 | ] 26 | }, 27 | { 28 | "name": "Python: Module", 29 | "type": "python", 30 | "request": "launch", 31 | "module": "enter-your-module-name-here", 32 | "console": "integratedTerminal" 33 | }, 34 | { 35 | "name": "Python: Django", 36 | "type": "python", 37 | "request": "launch", 38 | "program": "${workspaceFolder}/manage.py", 39 | "console": "integratedTerminal", 40 | "args": [ 41 | "runserver", 42 | "--noreload", 43 | "--nothreading" 44 | ], 45 | "django": true 46 | }, 47 | { 48 | "name": "Python: Flask", 49 | "type": "python", 50 | "request": "launch", 51 | "module": "flask", 52 | "env": { 53 | "FLASK_APP": "app.py" 54 | }, 55 | "args": [ 56 | "run", 57 | "--no-debugger", 58 | "--no-reload" 59 | ], 60 | "jinja": true 61 | }, 62 | { 63 | "name": "Python: Current File (External Terminal)", 64 | "type": "python", 65 | "request": "launch", 66 | "program": "${file}", 67 | "console": "externalTerminal" 68 | } 69 | ] 70 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "D:\\ProgramData\\Miniconda3\\python.exe" 3 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # easy-dexnet 2 | 基于dex-net2.0的论文，较为简单的Dex-Net2.0的实现 3 | 对于伯克利的dex-net实现，这里主要进行了如下改动 4 | 1. 抓取生成过程中改用tvtk采样表面对映点，而非使用SDF 5 | 2. 对于mesh文件的处理全部由trimesh完成 6 | 3. 生成图片样本由pyrender库完成 7 | 4. 所有程序全部基于python3.5 8 | 除了以上的主要改动，由于这里是完全重写的程序，所有大部分的实现细节也都有改动 9 | 10 | ### 安装部署 11 | easydexnet是基于python3.5编写，需要首先安装python3.5，另外tvtk需要另外安装 12 | > git clone https://github.com/LaiQE/easy-dexnet.git 13 | cd easy-dexnet 14 | python setup.py develop 15 | 16 | ### 使用 17 | - 从obj文件生成所有夹爪与抓取品质并保存到HDF5 18 | 参考tools/add_obj_to_hdf5.py 19 | 需要改动config/add_obj.yaml配置文件 20 | - 从HDF5的数据库中生成gqcnn训练数据 21 | 参考tools/generate.py 22 | 需要改动config/generate.yaml配置文件 23 | - 其他用法 24 | 在easy-dexnet中主要用DexObject类管理所有的数据与计算，其他用法参考DexObject类 25 | 26 | ### 参考 27 | http://berkeleyautomation.github.io/dex-net/ 28 | 29 | 30 | [![996.icu](https://img.shields.io/badge/link-996.icu-red.svg)](https://996.icu) 31 | -------------------------------------------------------------------------------- /build/lib/easydexnet/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding:utf-8 -*- 3 | 4 | from .grasp_sampler import GraspSampler_2f 5 | from .mesh import BaseMesh 6 | from .contact import Contact 7 | from .grasp import Grasp_2f 8 | from .stable_poses import StablePoses 9 | from .vision import DexScene 10 | from .quality import grasp_quality 11 | from .dex_object import DexObject 12 | from .render import ImageRender,DataGenerator,DepthRender 13 | from .data_saver import DexSaver 14 | -------------------------------------------------------------------------------- /build/lib/easydexnet/camare.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Camera(object): 5 | def __init__(self, pose, model): 6 | self._pose = pose 7 | self._model = model 8 | 9 | @property 10 | def pose(self): 11 | return self._pose 12 | 13 | @property 14 | def model(self): 15 | return self._model 16 | 17 | 18 | class RandomCamera(Camera): 19 | """ 随机生成相机参数的类 20 | """ 21 | 22 | def __init__(self, config): 23 | if 'camera' in config.keys(): 24 | config = config['camera'] 25 | self._config = config 26 | self._pose = self.random_pose() 27 | self._model = self.random_model() 28 | 29 | @staticmethod 30 | def sph2cart(r, az, elev): 31 | """ 转换球坐标系到笛卡尔坐标系 32 | r : 半径, 即远点到目标点的距离 33 | az : 方位角, 绕z轴旋转的角度 34 | elev : 俯仰角, 与z轴的夹角 35 | """ 36 | x = r * np.cos(az) * np.sin(elev) 37 | y = r * np.sin(az) * np.sin(elev) 38 | z = r * np.cos(elev) 39 | return x, y, z 40 | 41 | def random_pose(self, num=1): 42 | poses = [] 43 | cfg = self._config 44 | for _ in range(num): 45 | radius = np.random.uniform(cfg['min_radius'], cfg['max_radius']) 46 | elev = np.deg2rad(np.random.uniform( 47 | cfg['min_elev'], cfg['max_elev'])) 48 | az = np.deg2rad(np.random.uniform(cfg['min_az'], cfg['max_az'])) 49 | roll = np.deg2rad(np.random.uniform( 50 | cfg['min_roll'], cfg['max_roll'])) 51 | x = np.random.uniform(cfg['min_x'], cfg['max_x']) 52 | y = np.random.uniform(cfg['min_y'], cfg['max_y']) 53 | poses.append(self._camera_pose(radius, elev, az, roll, x, y)) 54 | return np.squeeze(poses) 55 | 56 | def random_model(self, num=1): 57 | models = [] 58 | cfg = self._config 59 | for _ in range(num): 60 | yfov = np.deg2rad(np.random.uniform( 61 | cfg['min_yfov'], cfg['max_yfov'])) 62 | znear = np.random.uniform(cfg['min_znear'], cfg['max_znear']) 63 | aspectRatio = cfg['aspectRatio'] 64 | models.append(np.array([yfov, znear, aspectRatio])) 65 | return np.squeeze(models) 66 | 67 | def _camera_pose(self, radius, elev, az, roll, x, y): 68 | """ 从给定的参数计算相机的位置矩阵 69 | radius : 相机到原点的距离 70 | elev : 俯仰角, 与z轴的夹角 71 | az : 方位角, 绕z轴旋转的角度 72 | roll : 横滚角，绕相机z轴旋转的夹角 73 | x,y : 物体的x,y轴偏移, 这里折算到相机的位姿上 """ 74 | # 生成相机的中点位置 75 | delta_t = np.array([x, y, 0]) 76 | camera_center_obj = np.array( 77 | [self.sph2cart(radius, az, elev)]).squeeze() + delta_t 78 | camera_z_obj = np.array([self.sph2cart(radius, az, elev)]).squeeze() 79 | camera_z_obj = camera_z_obj / np.linalg.norm(camera_z_obj) 80 | 81 | # 计算x轴和y轴方向, x轴在水平面上 82 | camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0]) 83 | if np.linalg.norm(camera_x_par_obj) == 0: 84 | camera_x_par_obj = np.array([1, 0, 0]) 85 | camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj) 86 | camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) 87 | camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) 88 | # 保证y轴朝下 89 | if camera_y_par_obj[2] > 0: 90 | camera_y_par_obj = -camera_y_par_obj 91 | camera_x_par_obj = -camera_x_par_obj.copy() 92 | # camera_y_par_obj = -camera_y_par_obj 93 | 94 | # 旋转相机 95 | R_obj_camera_par = np.c_[camera_x_par_obj, 96 | camera_y_par_obj, camera_z_obj] 97 | R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0], 98 | [np.sin(roll), np.cos(roll), 0], 99 | [0, 0, 1]]) 100 | R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera) 101 | 102 | matrix = np.eye(4) 103 | matrix[:3, :3] = R_obj_camera 104 | matrix[:3, 3] = camera_center_obj 105 | 106 | return matrix 107 | -------------------------------------------------------------------------------- /build/lib/easydexnet/colcor.py: -------------------------------------------------------------------------------- 1 | # 颜色值参考https://blog.csdn.net/guoxinian/article/details/80242353 2 | cnames = { 3 | 'aliceblue': '#F0F8FF', 4 | 'antiquewhite': '#FAEBD7', 5 | 'aqua': '#00FFFF', 6 | 'aquamarine': '#7FFFD4', 7 | 'azure': '#F0FFFF', 8 | 'beige': '#F5F5DC', 9 | 'bisque': '#FFE4C4', 10 | 'black': '#000000', 11 | 'blanchedalmond': '#FFEBCD', 12 | 'blue': '#0000FF', 13 | 'blueviolet': '#8A2BE2', 14 | 'brown': '#A52A2A', 15 | 'burlywood': '#DEB887', 16 | 'cadetblue': '#5F9EA0', 17 | 'chartreuse': '#7FFF00', 18 | 'chocolate': '#D2691E', 19 | 'coral': '#FF7F50', 20 | 'cornflowerblue': '#6495ED', 21 | 'cornsilk': '#FFF8DC', 22 | 'crimson': '#DC143C', 23 | 'cyan': '#00FFFF', 24 | 'darkblue': '#00008B', 25 | 'darkcyan': '#008B8B', 26 | 'darkgoldenrod': '#B8860B', 27 | 'darkgray': '#A9A9A9', 28 | 'darkgreen': '#006400', 29 | 'darkkhaki': '#BDB76B', 30 | 'darkmagenta': '#8B008B', 31 | 'darkolivegreen': '#556B2F', 32 | 'darkorange': '#FF8C00', 33 | 'darkorchid': '#9932CC', 34 | 'darkred': '#8B0000', 35 | 'darksalmon': '#E9967A', 36 | 'darkseagreen': '#8FBC8F', 37 | 'darkslateblue': '#483D8B', 38 | 'darkslategray': '#2F4F4F', 39 | 'darkturquoise': '#00CED1', 40 | 'darkviolet': '#9400D3', 41 | 'deeppink': '#FF1493', 42 | 'deepskyblue': '#00BFFF', 43 | 'dimgray': '#696969', 44 | 'dodgerblue': '#1E90FF', 45 | 'firebrick': '#B22222', 46 | 'floralwhite': '#FFFAF0', 47 | 'forestgreen': '#228B22', 48 | 'fuchsia': '#FF00FF', 49 | 'gainsboro': '#DCDCDC', 50 | 'ghostwhite': '#F8F8FF', 51 | 'gold': '#FFD700', 52 | 'goldenrod': '#DAA520', 53 | 'gray': '#808080', 54 | 'green': '#008000', 55 | 'greenyellow': '#ADFF2F', 56 | 'honeydew': '#F0FFF0', 57 | 'hotpink': '#FF69B4', 58 | 'indianred': '#CD5C5C', 59 | 'indigo': '#4B0082', 60 | 'ivory': '#FFFFF0', 61 | 'khaki': '#F0E68C', 62 | 'lavender': '#E6E6FA', 63 | 'lavenderblush': '#FFF0F5', 64 | 'lawngreen': '#7CFC00', 65 | 'lemonchiffon': '#FFFACD', 66 | 'lightblue': '#ADD8E6', 67 | 'lightcoral': '#F08080', 68 | 'lightcyan': '#E0FFFF', 69 | 'lightgoldenrodyellow': '#FAFAD2', 70 | 'lightgreen': '#90EE90', 71 | 'lightgray': '#D3D3D3', 72 | 'lightpink': '#FFB6C1', 73 | 'lightsalmon': '#FFA07A', 74 | 'lightseagreen': '#20B2AA', 75 | 'lightskyblue': '#87CEFA', 76 | 'lightslategray': '#778899', 77 | 'lightsteelblue': '#B0C4DE', 78 | 'lightyellow': '#FFFFE0', 79 | 'lime': '#00FF00', 80 | 'limegreen': '#32CD32', 81 | 'linen': '#FAF0E6', 82 | 'magenta': '#FF00FF', 83 | 'maroon': '#800000', 84 | 'mediumaquamarine': '#66CDAA', 85 | 'mediumblue': '#0000CD', 86 | 'mediumorchid': '#BA55D3', 87 | 'mediumpurple': '#9370DB', 88 | 'mediumseagreen': '#3CB371', 89 | 'mediumslateblue': '#7B68EE', 90 | 'mediumspringgreen': '#00FA9A', 91 | 'mediumturquoise': '#48D1CC', 92 | 'mediumvioletred': '#C71585', 93 | 'midnightblue': '#191970', 94 | 'mintcream': '#F5FFFA', 95 | 'mistyrose': '#FFE4E1', 96 | 'moccasin': '#FFE4B5', 97 | 'navajowhite': '#FFDEAD', 98 | 'navy': '#000080', 99 | 'oldlace': '#FDF5E6', 100 | 'olive': '#808000', 101 | 'olivedrab': '#6B8E23', 102 | 'orange': '#FFA500', 103 | 'orangered': '#FF4500', 104 | 'orchid': '#DA70D6', 105 | 'palegoldenrod': '#EEE8AA', 106 | 'palegreen': '#98FB98', 107 | 'paleturquoise': '#AFEEEE', 108 | 'palevioletred': '#DB7093', 109 | 'papayawhip': '#FFEFD5', 110 | 'peachpuff': '#FFDAB9', 111 | 'peru': '#CD853F', 112 | 'pink': '#FFC0CB', 113 | 'plum': '#DDA0DD', 114 | 'powderblue': '#B0E0E6', 115 | 'purple': '#800080', 116 | 'red': '#FF0000', 117 | 'rosybrown': '#BC8F8F', 118 | 'royalblue': '#4169E1', 119 | 'saddlebrown': '#8B4513', 120 | 'salmon': '#FA8072', 121 | 'sandybrown': '#FAA460', 122 | 'seagreen': '#2E8B57', 123 | 'seashell': '#FFF5EE', 124 | 'sienna': '#A0522D', 125 | 'silver': '#C0C0C0', 126 | 'skyblue': '#87CEEB', 127 | 'slateblue': '#6A5ACD', 128 | 'slategray': '#708090', 129 | 'snow': '#FFFAFA', 130 | 'springgreen': '#00FF7F', 131 | 'steelblue': '#4682B4', 132 | 'tan': '#D2B48C', 133 | 'teal': '#008080', 134 | 'thistle': '#D8BFD8', 135 | 'tomato': '#FF6347', 136 | 'turquoise': '#40E0D0', 137 | 'violet': '#EE82EE', 138 | 'wheat': '#F5DEB3', 139 | 'white': '#FFFFFF', 140 | 'whitesmoke': '#F5F5F5', 141 | 'yellow': '#FFFF00', 142 | 'yellowgreen': '#9ACD32'} -------------------------------------------------------------------------------- /build/lib/easydexnet/contact.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import logging 3 | 4 | 5 | class Contact(object): 6 | """ 接触点类型，表示一个接触点 7 | 主要用来计算接触点的摩擦锥 8 | """ 9 | 10 | def __init__(self, point, normal, grasp_direction, moment_arm=None, config=None): 11 | """ 12 | point: 接触点在物体坐标系的坐标点 13 | normal: 接触点所在面片的法线, 方向向外 14 | grasp_direction: 这个接触点的力作用方向, 方向向内 15 | moment_arm: 力臂,一个向量(接触点坐标 - 重心坐标) 16 | """ 17 | self._point = point 18 | self._normal = normal / np.linalg.norm(normal) 19 | self._grasp_direction = grasp_direction / \ 20 | np.linalg.norm(grasp_direction) 21 | self._moment_arm = moment_arm 22 | self._friction_cone = 0.5 23 | if config is not None: 24 | self._friction_cone = config['default_friction_coef'] 25 | self._num_cone_faces = 8 26 | if config is not None: 27 | self._num_cone_faces = config['num_cone_faces'] 28 | 29 | @property 30 | def point(self): 31 | return self._point 32 | 33 | @property 34 | def normal(self): 35 | return self._normal 36 | 37 | @property 38 | def grasp_direction(self): 39 | return self._grasp_direction 40 | 41 | def tangents(self, direction=None): 42 | """ 计算接触点的切线向量, 方向向量和切线向量在右手坐标系下定 43 | 优化了原Dex-Net中的算法 44 | Parameters 45 | ---------- 46 | direction : 3个元素的矩阵,用以计算与这个方向正交的平面 47 | 48 | Returns 49 | ------- 50 | direction : 方向向量,如果未指定则为法向量的反方向 51 | t1 : 第一个切线向量 52 | t2 : 第二个切线向量 53 | """ 54 | if direction is None: 55 | direction = -self._normal 56 | 57 | if np.dot(self._normal, direction) > 0: 58 | direction = -1 * direction 59 | 60 | x = np.array([1, 0, 0]) 61 | # 计算x轴在切平面上的投影,作为第一个切向量 62 | v = x - direction*(x.dot(direction)/direction.dot(direction)) 63 | w = np.cross(direction, v) 64 | 65 | v = v / np.linalg.norm(v) 66 | w = w / np.linalg.norm(w) 67 | 68 | return direction, v, w 69 | 70 | def friction_cone(self, num_cone_faces=None, friction_coef=None): 71 | """ 计算接触点处的摩擦锥. 72 | 73 | Parameters 74 | ---------- 75 | num_cone_faces : int,摩擦锥近似的面数 76 | friction_coef : float,摩擦系数 77 | 78 | Returns 79 | ------- 80 | success : bool,摩擦锥计算是否成功 81 | cone_support : 摩擦锥的边界向量 82 | normal : 向外的法向量 83 | """ 84 | if num_cone_faces is None: 85 | num_cone_faces = self._num_cone_faces 86 | if friction_coef is None: 87 | friction_coef = self._friction_cone 88 | 89 | # 这里不能加, 每次计算的摩擦锥可能都不一样, 摩擦系数可能变 90 | # if self._friction_cone is not None and self._normal is not None: 91 | # return True, self._friction_cone, self._normal 92 | 93 | # 获取切向量 94 | in_normal, t1, t2 = self.tangents() 95 | 96 | # 检查是否有相对滑动, 即切线方向的力始终大于摩擦力 97 | grasp_direction = self._grasp_direction 98 | normal_force_mag = np.dot(grasp_direction, in_normal) # 法线方向分力 99 | tan_force_x = np.dot(grasp_direction, t1) # t1方向分力 100 | tan_force_y = np.dot(grasp_direction, t2) # t2方向分力 101 | tan_force_mag = np.sqrt(tan_force_x**2 + tan_force_y**2) # 切平面上的分力 102 | friction_force_mag = friction_coef * normal_force_mag # 最大静摩擦 103 | 104 | # 如果切面方向力大于最大静摩擦, 则生成失败 105 | if friction_force_mag < tan_force_mag: 106 | return False, None, self._normal 107 | 108 | # 计算摩擦锥 109 | force = in_normal 110 | cone_support = np.zeros((3, num_cone_faces)) 111 | for j in range(num_cone_faces): 112 | tan_vec = t1 * np.cos(2 * np.pi * (float(j) / num_cone_faces)) + \ 113 | t2 * np.sin(2 * np.pi * (float(j) / num_cone_faces)) 114 | cone_support[:, j] = force + friction_coef * tan_vec 115 | 116 | # self._friction_cone = cone_support 117 | return True, cone_support, self._normal 118 | 119 | def torques(self, forces): 120 | """求出接触点上一组力矢量所能施加的力矩 121 | forces : 3xN 力矢量 122 | Returns: 3xN 一组力矩 123 | """ 124 | num_forces = forces.shape[1] 125 | torques = np.zeros([3, num_forces]) 126 | moment_arm = self._moment_arm 127 | for i in range(num_forces): 128 | torques[:, i] = np.cross(moment_arm, forces[:, i]) 129 | return True, torques 130 | 131 | def normal_force_magnitude(self): 132 | """ 计算法线方向上的力的大小 133 | """ 134 | normal_force_mag = 1.0 135 | if self._grasp_direction is not None and self._normal is not None: 136 | in_normal = -self._normal 137 | in_direction_norm = self._grasp_direction 138 | normal_force_mag = np.dot(in_direction_norm, in_normal) 139 | return max(normal_force_mag, 0.0) 140 | -------------------------------------------------------------------------------- /build/lib/easydexnet/data_saver.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import numpy as np 4 | 5 | 6 | class DataSaver(object): 7 | def __init__(self, path, max_data, name): 8 | self._path = path 9 | self._max = max_data 10 | self._counter = 0 11 | self._buffer = None 12 | self._save_counter = 0 13 | self._name = name 14 | 15 | def update_counter(self): 16 | self._counter = self._counter + 1 17 | if self._counter == self._max: 18 | self._counter = 0 19 | self.save() 20 | 21 | def add(self, data): 22 | if self._buffer is None: 23 | self._buffer = np.zeros((self._max,) + data.shape) 24 | if not(data.shape == self._buffer.shape[1:]): 25 | raise ValueError() 26 | self._buffer[self._counter] = data 27 | self.update_counter() 28 | 29 | def save(self): 30 | save_name = self._name + '%06d' % (self._save_counter) + '.npy' 31 | self._save_counter = self._save_counter + 1 32 | save_file = os.path.join(self._path, save_name) 33 | np.save(save_file, self._buffer) 34 | 35 | def close(self): 36 | if self._counter > 0: 37 | self._buffer = self._buffer[:self._counter] 38 | self.save() 39 | total_num = self._counter + (self._save_counter - 1) * self._max 40 | logging.info(self._name+' saver: 共存储了%d个数据'%(total_num)) 41 | 42 | 43 | class DexSaver(object): 44 | def __init__(self, path, config): 45 | self._max = config['datapoints_per_file'] 46 | self._path = path 47 | self._depth_saver = self.create_saver('depth') 48 | self._hand_pose_saver = self.create_saver('hand_pose') 49 | self._quality_saver = self.create_saver('quality') 50 | 51 | def create_saver(self, name): 52 | path = os.path.join(self._path, name) 53 | if not os.path.exists(path): 54 | os.mkdir(path) 55 | return DataSaver(path, self._max, name) 56 | 57 | def add(self, depth, grasp, q, coll): 58 | self._depth_saver.add(depth) 59 | self._hand_pose_saver.add(grasp.to_saver()) 60 | coll_free_metric = np.array([(1 * coll) * q]) 61 | self._quality_saver.add(coll_free_metric) 62 | 63 | def close(self): 64 | self._depth_saver.close() 65 | self._hand_pose_saver.close() 66 | self._quality_saver.close() 67 | -------------------------------------------------------------------------------- /build/lib/easydexnet/dex_object.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | import logging 4 | import os.path 5 | import numpy as np 6 | from .mesh import BaseMesh 7 | from .stable_poses import StablePoses 8 | from .grasp_sampler import GraspSampler_2f 9 | from .quality import grasp_quality 10 | from .grasp import Grasp_2f 11 | 12 | 13 | class DexObject(object): 14 | """ 用于管理所有数据的类，其中包含了计算dex-net数据集中一个物体对象 15 | 所需的所有数据，包括，网格数据、所有稳定姿态、所有候选抓取点 16 | """ 17 | 18 | def __init__(self, mesh, config, poses=None, grasps=None, qualitis=None, name=None): 19 | self._mesh = mesh 20 | self._config = config 21 | self._poses = poses 22 | self._grasps = grasps 23 | self._qualitis = qualitis 24 | self._name = name 25 | if not poses: 26 | self._poses = self.get_poses( 27 | self._mesh, config['stable_pose_min_p']) 28 | if not grasps: 29 | self._grasps = self.get_grasps(self._mesh, self._config) 30 | if not qualitis: 31 | self._qualitis, self._grasps = self.get_quality( 32 | self._grasps, self._mesh, self._config) 33 | for q, g in zip(self._qualitis, self._grasps): 34 | g.quality = q 35 | 36 | @property 37 | def name(self): 38 | return self._name 39 | 40 | @property 41 | def mesh(self): 42 | return self._mesh 43 | 44 | @property 45 | def poses(self): 46 | return self._poses 47 | 48 | @property 49 | def grasps(self): 50 | return self._grasps 51 | 52 | @property 53 | def qualitis(self): 54 | return self._qualitis 55 | 56 | @staticmethod 57 | def from_trimesh(tri_mesh, config, name=None): 58 | mesh = BaseMesh(tri_mesh, name) 59 | return DexObject(mesh, config) 60 | 61 | @staticmethod 62 | def from_file(file_path, config, name=None): 63 | if not name: 64 | name = os.path.splitext(os.path.basename(file_path))[0] 65 | mesh = BaseMesh.from_file(file_path, name) 66 | return DexObject(mesh, config, name=name) 67 | 68 | @staticmethod 69 | def get_poses(mesh, threshold): 70 | """ 从网格对象中获取所有稳定位姿 """ 71 | if not isinstance(mesh, (BaseMesh)): 72 | raise TypeError('mesh must be the class BaseMesh') 73 | tri_mesh = mesh.tri_mesh 74 | _center_mass = mesh.center_mass 75 | matrixs, probabilitys = tri_mesh.convex_hull.compute_stable_poses( 76 | center_mass=_center_mass, threshold=threshold) 77 | return StablePoses.from_raw_poses(matrixs, probabilitys) 78 | 79 | @staticmethod 80 | def get_grasps(mesh, config): 81 | if not isinstance(mesh, (BaseMesh)): 82 | raise TypeError('mesh must be the class BaseMesh') 83 | sampler = GraspSampler_2f(config=config) 84 | num_sample = config['num_sample'] 85 | grasps = sampler.generate_grasps(mesh, num_sample) 86 | return grasps 87 | 88 | @staticmethod 89 | def get_quality(grasps, mesh, config): 90 | metrics = config['metrics'] 91 | valid_grasps = [] 92 | qualitis = [] 93 | for grasp in grasps: 94 | try: 95 | quality = grasp_quality(grasp, mesh, metrics) 96 | except Exception as e: 97 | logging.warning('抓取品质计算无效') 98 | print('抓取品质计算无效', e) 99 | else: 100 | qualitis.append(quality) 101 | valid_grasps.append(grasp) 102 | return qualitis, valid_grasps 103 | 104 | @staticmethod 105 | def from_hdf5_group(obj_group, config, name=None): 106 | grasps, metrics = DexObject.grasps_from_hdf5(obj_group, config) 107 | mesh = DexObject.mesh_from_hdf5(obj_group, name) 108 | poses = DexObject.poses_from_hdf5(obj_group, config) 109 | return DexObject(mesh, config, poses, grasps, metrics, name) 110 | 111 | @staticmethod 112 | def grasps_from_hdf5(obj_group, config): 113 | group = obj_group[config['hdf5_config']['grasps_group']] 114 | metrics_name = config['hdf5_config']['metrics_name'] 115 | grasps = [] 116 | metrics = [] 117 | for grasp_name, grasp_group in group.items(): 118 | configuration = grasp_group.attrs['configuration'] 119 | g = Grasp_2f.from_configuration(configuration, config) 120 | grasps.append(g) 121 | m = grasp_group['metrics'].attrs[metrics_name] 122 | metrics.append(m) 123 | # print(m) 124 | return grasps, metrics 125 | 126 | @staticmethod 127 | def mesh_from_hdf5(obj_group, name): 128 | triangles = np.array(obj_group['mesh/triangles']) 129 | vertices = np.array(obj_group['mesh/vertices']) 130 | mesh = BaseMesh.from_data(vertices, triangles, name=name) 131 | return mesh 132 | 133 | @staticmethod 134 | def poses_from_hdf5(obj_group, config): 135 | group = obj_group[config['hdf5_config']['stable_poses_group']] 136 | vertices = np.array(obj_group['mesh/vertices']) 137 | poses = [] 138 | for pose_name, pose in group.items(): 139 | p = pose.attrs['p'] 140 | r = pose.attrs['r'] 141 | if r.shape[0] == 3: 142 | v = r.dot(vertices.T) 143 | min_z = np.min(v[2, :]) 144 | matrix = np.eye(4) 145 | matrix[:3, :3] = r 146 | matrix[2, 3] = -min_z 147 | else: 148 | matrix = r 149 | poses.append(StablePoses(matrix, p)) 150 | return poses 151 | 152 | def to_hdf5_group(self, obj_group, config): 153 | if self._name in list(obj_group.keys()): 154 | del obj_group[self._name] 155 | group = obj_group.require_group(self._name) 156 | self.grasps_to_hdf5(group, config) 157 | self.mesh_to_hdf5(group) 158 | self.poses_to_hdf5(group, config) 159 | 160 | def grasps_to_hdf5(self, obj_group, config): 161 | group_name = config['hdf5_config']['grasps_group'] 162 | metrics_name = config['hdf5_config']['metrics_name'] 163 | group = obj_group.require_group(group_name) 164 | for i in range(len(self._grasps)): 165 | configuration = self._grasps[i].to_configuration() 166 | metrics = self._qualitis[i] 167 | grasp_name = 'grasp_%d' % (i) 168 | grasp_group = group.require_group(grasp_name) 169 | grasp_group.attrs['configuration'] = configuration 170 | metrics_group = grasp_group.require_group('metrics') 171 | metrics_group.attrs[metrics_name] = metrics 172 | 173 | def mesh_to_hdf5(self, obj_group): 174 | triangles = self._mesh.tri_mesh.faces 175 | vertices = self._mesh.tri_mesh.vertices 176 | group = obj_group.require_group('mesh') 177 | group.create_dataset('triangles', data=triangles) 178 | group.create_dataset('vertices', data=vertices) 179 | 180 | def poses_to_hdf5(self, obj_group, config): 181 | group_name = config['hdf5_config']['stable_poses_group'] 182 | group = obj_group.require_group(group_name) 183 | for i, pose in enumerate(self._poses): 184 | pose_name = 'pose_%d' % (i) 185 | pose_group = group.require_group(pose_name) 186 | pose_group.attrs['r'] = pose.matrix 187 | pose_group.attrs['p'] = pose.probability 188 | -------------------------------------------------------------------------------- /build/lib/easydexnet/grasp.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import numpy as np 3 | from .contact import Contact 4 | 5 | 6 | class Grasp_2f(object): 7 | """ 点接触二指夹爪抓取点模型类 8 | """ 9 | 10 | def __init__(self, center, axis, width=None, min_width=None, config=None): 11 | """ 12 | center: 夹爪的中心点 13 | axis: 夹爪的二指连线方向向量 14 | width: 最大张开距离 15 | min_width: 夹爪闭合时的宽度 16 | """ 17 | self._center = center 18 | self._axis = axis / np.linalg.norm(axis) 19 | self._max_grasp_width = width 20 | self._config = config 21 | if width is None and config is not None: 22 | self._max_grasp_width = config['grispper']['max_width'] 23 | self._min_grasp_width = min_width 24 | if min_width is None and config is not None: 25 | self._min_grasp_width = config['grispper']['min_width'] 26 | self._alpha = 0.05 27 | if config is not None: 28 | self._alpha = config['grisp_distance_alpha'] 29 | self._quality = None 30 | 31 | @property 32 | def quality(self): 33 | return self._quality 34 | 35 | @quality.setter 36 | def quality(self, q): 37 | self._quality = q 38 | 39 | @property 40 | def center(self): 41 | return self._center 42 | 43 | @property 44 | def axis(self): 45 | return self._axis 46 | 47 | @property 48 | def endpoints(self): 49 | """ 返回夹爪的两个端点 """ 50 | half_axis = (self._max_grasp_width / 2.0) * self._axis 51 | point0 = self._center - half_axis 52 | point1 = self._center + half_axis 53 | return point0, point1 54 | 55 | @property 56 | def width(self): 57 | return self._max_grasp_width 58 | 59 | @staticmethod 60 | def distance(g1, g2, alpha=0.05): 61 | """ 计算两个夹爪对象之间的距离 62 | """ 63 | center_dist = np.linalg.norm(g1.center - g2.center) 64 | axis_dist = (2.0 / np.pi) * np.arccos(np.abs(g1.axis.dot(g2.axis))) 65 | return center_dist + alpha * axis_dist 66 | 67 | def _find_contact(self, mesh, out_point, center_point): 68 | """ 找到一个接触点, 从外部点到中心点的第一个接触点 69 | mesh: 一个BaseMesh对象 70 | out_point: 夹爪的一个端点 71 | center_point: 夹爪的中心点 72 | """ 73 | points, cell_ids = mesh.intersect_line(out_point, center_point) 74 | direction = center_point - out_point 75 | 76 | if points.shape[0] < 1: 77 | return False, None 78 | 79 | # 求取法向量, 这里的法向量要保证朝向外侧 80 | normal = mesh.tri_mesh.face_normals[cell_ids[0]] 81 | if np.dot(direction, normal) > 0: 82 | normal = -normal 83 | # 求取接触点的力臂,由质心指向接触点的向量 84 | moment_arm = points[0] - mesh.center_mass 85 | c = Contact(points[0], normal, direction, moment_arm) 86 | return True, c 87 | 88 | def close_fingers(self, mesh, check_approach=True): 89 | """ 闭合夹爪,返回接触点 90 | mesh: 一个BaseMesh对象 91 | check_approach: 是否进行碰撞检测 92 | """ 93 | # TODO 检查在接近路径上的碰撞点,暂时先不写 94 | if check_approach: 95 | pass 96 | 97 | point0, point1 = self.endpoints 98 | # 先判断中间的交点是否为偶数并且是否有足够的点,奇数交点个数则表示出错 99 | points, _ = mesh.intersect_line(point0, point1) 100 | if ((points.shape[0] % 2) != 0) or points.shape[0] < 2: 101 | logging.debug('close_fingers 交点生成出错') 102 | return False, None 103 | 104 | is_c0, c0 = self._find_contact(mesh, point0, self._center) 105 | is_c1, c1 = self._find_contact(mesh, point1, self._center) 106 | if not (is_c0 and is_c1): 107 | logging.debug('close_fingers 接触点寻找失败'+str(is_c0)+str(is_c0)) 108 | return False, None 109 | contacts = [c0, c1] 110 | 111 | return True, contacts 112 | 113 | def _check_approch(self, mesh, point, axis, dis): 114 | """ 检查路径上是否有碰撞 115 | mesh : 网格 116 | point : 检查点 117 | axis : 检查的方向,检查(point,point+axis*dis) 118 | dis : 检查的距离 """ 119 | points, _ = mesh.intersect_line(point, point+axis*dis) 120 | if points.shape[0] > 0: 121 | return False 122 | return True 123 | 124 | def get_approch(self, poses): 125 | """ 计算在抓取路径, 即与z轴最近的方向 """ 126 | poses_r = poses.matrix[:3, :3] 127 | # 稳定姿态的z轴在物体坐标下的表示 128 | poses_z = poses_r[2, :] 129 | axis = self._axis 130 | # 计算z轴在抓取轴为法线的平面上的投影作为抓取路径的反方向 131 | approach = poses_z - axis*(poses_z.dot(axis)/axis.dot(axis)) 132 | approach_L = np.linalg.norm(approach) 133 | poses_z_L = np.linalg.norm(poses_z) 134 | angle = np.arccos(poses_z.dot(approach)/(approach_L*poses_z_L)) 135 | angle = np.rad2deg(angle) 136 | approach = -approach / np.linalg.norm(approach) 137 | return approach, angle 138 | 139 | def check_approach(self, mesh, poses, config): 140 | """ 检查夹取路径上是否有碰撞 """ 141 | # 获取抓取路径 142 | approch, _ = self.get_approch(poses) 143 | check_cfg = config['collision_checker'] 144 | # 夹爪宽度方向 145 | width_axis = np.cross(approch, self._axis) 146 | w_axis = width_axis / np.linalg.norm(width_axis) 147 | check_num = max(check_cfg['checker_point_num'], 3) 148 | axis_offiset = check_cfg['axis_offiset'] 149 | width_offset = check_cfg['width_offset'] 150 | # 轴向补偿列表 151 | aixs_list = np.linspace(-axis_offiset/2, axis_offiset/2, check_num) 152 | # 宽度补偿列表 153 | width_list = np.linspace(-width_offset/2, width_offset/2, check_num) 154 | 155 | for p in self.endpoints: 156 | axis = self._axis 157 | # 最终检查列表 158 | check_list = [p + a*axis + w * w_axis 159 | for a in aixs_list for w in width_list] 160 | for check_point in check_list: 161 | result = self._check_approch(mesh, check_point, 162 | -approch, check_cfg['test_dist']) 163 | if not result: 164 | return False 165 | return True 166 | 167 | def apply_transform(self, matrix): 168 | center = np.r_[self.center, 1] 169 | center = matrix.dot(center)[:3] 170 | axis = matrix[:3, :3].dot(self._axis) 171 | return Grasp_2f(center, axis, config=self._config) 172 | 173 | @staticmethod 174 | def from_configuration(configuration, config): 175 | if not isinstance(configuration, np.ndarray) or (configuration.shape[0] != 9 and configuration.shape[0] != 10): 176 | raise ValueError('Configuration must be numpy ndarray of size 9 or 10') 177 | if configuration.shape[0] == 9: 178 | min_grasp_width = 0 179 | else: 180 | min_grasp_width = configuration[9] 181 | if np.abs(np.linalg.norm(configuration[3:6]) - 1.0) > 1e-5: 182 | raise ValueError('Illegal grasp axis. Must be norm one') 183 | center = configuration[0:3] 184 | axis = configuration[3:6] 185 | width = configuration[6] 186 | return Grasp_2f(center, axis, width, min_grasp_width, config) 187 | 188 | def to_configuration(self): 189 | configuration = np.zeros(10) 190 | configuration[0:3] = self._center 191 | configuration[3:6] = self._axis 192 | configuration[6] = self._max_grasp_width 193 | configuration[7] = 0 194 | configuration[8] = 0 195 | configuration[9] = self._min_grasp_width 196 | return configuration 197 | -------------------------------------------------------------------------------- /build/lib/easydexnet/grasp_2d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Grasp2D(object): 4 | """ 5 | 2D夹爪类型，夹爪投影到深度图像上的坐标. 6 | center : 夹爪中心坐标，像素坐标表示 7 | angle : 抓取方向和相机x坐标的夹角 8 | depth : 夹爪中心点的深度 9 | width : 夹爪的宽度像素坐标 10 | """ 11 | def __init__(self, center, angle, depth, width=0.0): 12 | self._center = center 13 | self._angle = angle 14 | self._depth = depth 15 | self._width_px = width 16 | 17 | @property 18 | def center(self): 19 | return self._center.astype(np.int) 20 | 21 | @property 22 | def center_float(self): 23 | return self._center 24 | 25 | @property 26 | def angle(self): 27 | return self._angle 28 | 29 | @property 30 | def depth(self): 31 | return self._depth 32 | 33 | @property 34 | def axis(self): 35 | """ Returns the grasp axis. """ 36 | return np.array([np.cos(self._angle), np.sin(self._angle)]) 37 | 38 | @property 39 | def endpoints(self): 40 | """ Returns the grasp endpoints """ 41 | p0 = self._center - (float(self._width_px) / 2) * self.axis 42 | p1 = self._center + (float(self._width_px) / 2) * self.axis 43 | p0 = p0.astype(np.int) 44 | p1 = p1.astype(np.int) 45 | return p0, p1 46 | 47 | @property 48 | def feature_vec(self): 49 | """ 生成抓取的特征向量，由两个端点和中心距离组成 50 | """ 51 | p1, p2 = self.endpoints 52 | return np.r_[p1, p2, self._depth] 53 | 54 | @staticmethod 55 | def from_feature_vec(v, width=None): 56 | """ 通过特征向量创建一个抓取- 57 | v : 抓取的特征向量 58 | width : 夹爪的宽度 59 | """ 60 | # read feature vec 61 | p1 = v[:2] 62 | p2 = v[2:4] 63 | depth = v[4] 64 | if width is None: 65 | width = np.linalg.norm(p1 - p2) 66 | 67 | # compute center and angle 68 | center_px = (p1 + p2) / 2 69 | axis = p2 - p1 70 | if np.linalg.norm(axis) > 0: 71 | axis = axis / np.linalg.norm(axis) 72 | if axis[1] > 0: 73 | angle = np.arccos(axis[0]) 74 | else: 75 | angle = -np.arccos(axis[0]) 76 | return Grasp2D(center_px, angle, depth, width=width) 77 | 78 | @staticmethod 79 | def image_dist(g1, g2, alpha=1.0): 80 | """ 计算两个抓取在像素坐标下的距离 81 | """ 82 | # point to point distances 83 | point_dist = np.linalg.norm(g1.center - g2.center) 84 | 85 | # axis distances 86 | axis_dist = np.arccos(np.abs(g1.axis.dot(g2.axis))) 87 | 88 | return point_dist + alpha * axis_dist 89 | 90 | def to_saver(self): 91 | s = np.zeros((5,)) 92 | s[:2] = self._center 93 | s[2] = self._angle 94 | s[3] = self._depth 95 | s[4] = self._width_px 96 | return s -------------------------------------------------------------------------------- /build/lib/easydexnet/grasp_sampler.py: -------------------------------------------------------------------------------- 1 | import random 2 | import logging 3 | import numpy as np 4 | from .grasp import Grasp_2f 5 | from .contact import Contact 6 | from .quality import force_closure_2f 7 | 8 | 9 | class GraspSampler_2f(object): 10 | """ 二指夹爪抓取点采样器，采用对映采样器 11 | 1. 随机均匀采样物体表面的点 12 | 2. 在该点的摩擦锥内随机采样一个抓取方向 13 | 3. 沿着这个方向采样另一个抓取点 14 | 4. 检查抓取点 15 | """ 16 | def __init__(self, width=None, min_contact_dist=None, config=None): 17 | """ 18 | width : 夹爪的最大宽度 19 | min_contact_dist : 接触点之间容忍的最小距离 20 | friction_coef : 摩擦系数 21 | dist_thresh : 夹爪之间容忍的最小间距 22 | """ 23 | self._max_grasp_width = width 24 | if width is None and config is not None: 25 | self._max_grasp_width = config['grispper']['max_width'] 26 | self._min_contact_dist = min_contact_dist 27 | if min_contact_dist is None and config is not None: 28 | self._min_contact_dist = config['min_contact_dist'] 29 | self._friction_coef = 0.5 30 | if config is not None: 31 | self._friction_coef = config['default_friction_coef'] 32 | self._grasp_dist_thresh = 0.0005 33 | if config is not None: 34 | self._grasp_dist_thresh = config['grasp_dist_thresh'] 35 | if config is not None: 36 | self._config = config 37 | 38 | def _sample_vector(self, contact, num_samples=1): 39 | """ 在给定点接触点的摩擦锥内采样随机的方向向量 40 | 采样的方向随机向内或向外,这里朝向无所谓 41 | contract : 一个接触点 42 | friction_coef : 摩擦系数 43 | num_samples : 采样个数 44 | return : 方向向量列表 45 | """ 46 | friction_coef = self._friction_coef 47 | n, tx, ty = contact.tangents() 48 | v_samples = [] 49 | # TODO 这里的循环可以改成纯numpy的形式 50 | for _ in range(num_samples): 51 | theta = 2 * np.pi * np.random.rand() 52 | r = friction_coef * np.random.rand() 53 | v = n + r * np.cos(theta) * tx + r * np.sin(theta) * ty 54 | v = -v / np.linalg.norm(v) 55 | v_samples.append(v) 56 | return v_samples 57 | 58 | def _find_grasp(self, mesh, point, vector, test_dist=0.5): 59 | """ 利用给定的点和方向，找到另一个接触点,并生成抓取模型 60 | mesh : 待抓取的物体网格 61 | point : 已知的一个接触点 62 | vector : 给定的抓取方向 63 | test_dist : 用于测试的距离, 物体最大距离的一半 64 | --- 65 | is_grasp : 生成抓取模型是否成功 66 | grasp : 生成的抓取模型 67 | """ 68 | given_point = point 69 | given_point_index = -1 70 | # 先在大范围上计算出所有的由给定点和方向指定的直线 71 | p0 = given_point - vector*test_dist 72 | p1 = given_point + vector*test_dist 73 | points, _ = mesh.intersect_line(p0, p1) 74 | 75 | # 如果交点个数为奇数则出错 76 | points_len = points.shape[0] 77 | if points_len % 2 != 0 or points_len == 0: 78 | logging.debug('_find_grasp 交点数检查出错，物体:%s ' % (mesh.name)) 79 | return False, None 80 | 81 | # 找到当前点的位置, 由于计算误差用一个极小数测试是否一致 82 | for i, p in enumerate(points): 83 | if np.linalg.norm(p - given_point) < 1.e-6: 84 | given_point_index = i 85 | break 86 | # 如果给定点没有在计算出的交点内，则出错 87 | if given_point_index == -1: 88 | logging.debug('_find_grasp 给定点未在物体表面，物体:%s ' % (mesh.name)) 89 | return False, None 90 | # 生成候选的配对点 91 | if given_point_index % 2 == 0: 92 | candidate_point_index = list( 93 | range(given_point_index+1, points_len, 2)) 94 | left_point_index = given_point_index - 1 95 | right_dir = 1 96 | else: 97 | candidate_point_index = list(range(given_point_index-1, -1, -2)) 98 | left_point_index = given_point_index + 1 99 | right_dir = -1 100 | # 最后找到抓取列表 101 | grasps = [] 102 | # 对于每一个候选配对点检查距离是否小于最小距离或是否会被下一个点挡住 103 | # TODO 这里可以写的更清晰一点 104 | for inedx in candidate_point_index: 105 | p = points[inedx] 106 | # 判断两个点之间的距离是否大于要求的最小距离 107 | distance = np.linalg.norm(given_point - p) 108 | distance_is = distance > self._min_contact_dist and distance < self._max_grasp_width 109 | center = (given_point + p) / 2 110 | # 判断夹爪的右边是否不会碰到 111 | right_point_index = inedx + right_dir 112 | if right_point_index >= points_len or right_point_index < 0: 113 | right_is = True 114 | else: 115 | right_distance = np.linalg.norm( 116 | points[right_point_index] - center) 117 | right_is = right_distance > self._max_grasp_width / 2 118 | # 判断夹爪左边是否不会碰到 119 | if left_point_index >= points_len or left_point_index < 0: 120 | left_is = True 121 | else: 122 | left_distance = np.linalg.norm( 123 | points[left_point_index] - center) 124 | left_is = left_distance > self._max_grasp_width / 2 125 | # 如果通过检测 126 | if distance_is and left_is and right_is: 127 | grasp = Grasp_2f(center, p-given_point, 128 | self._max_grasp_width, config=self._config) 129 | # logging.debug('_find_grasp 找到一个合适的抓取，物体:%s ' % (mesh.name)) 130 | grasps.append(grasp) 131 | if len(grasps) > 0: 132 | logging.debug('_find_grasp 成功生成%d个抓取，物体:%s ' % 133 | (len(grasps), mesh.name)) 134 | return True, grasps 135 | logging.debug('_find_grasp 抓取生成失败，物体:%s ' % (mesh.name)) 136 | return False, None 137 | 138 | def sample_grasps(self, mesh, num_grasps, num_samples=2): 139 | """ 采样抓一组取候选点. 140 | mesh : 待抓取的物体网格 141 | num_grasps : int采样个数 142 | num_samples : 每个点采样的方向数 143 | return: 候选抓取点列表 144 | """ 145 | grasps = [] 146 | # 获取表面点 147 | points, face_index = mesh.tri_mesh.sample( 148 | num_grasps, return_index=True) 149 | 150 | # 对于每个表面点 151 | for point, face in zip(points, face_index): 152 | # 计算摩擦锥 153 | normal = mesh.tri_mesh.face_normals[face] 154 | c1 = Contact(point, normal, -normal) 155 | # 在摩擦锥内采样抓取轴 156 | v_samples = self._sample_vector(c1, num_samples=num_samples) 157 | 158 | for v in v_samples: 159 | # 找到另一个接触点 160 | grasp_is, grasp = self._find_grasp(mesh, point, v) 161 | 162 | if not grasp_is: 163 | continue 164 | 165 | # 获取真实的接触点 (之前的接触点容易会变化) 166 | for g in grasp: 167 | success, c = g.close_fingers(mesh) 168 | if not success: 169 | logging.debug('sample_grasps 夹爪闭合失败') 170 | continue 171 | 172 | # 检查摩擦圆锥是否力闭合 173 | if force_closure_2f(c[0], c[1], self._friction_coef): 174 | grasps.append(g) 175 | logging.debug('sample_grasps 成功生成一个抓取') 176 | 177 | # 打乱样本 178 | random.shuffle(grasps) 179 | return grasps 180 | 181 | def generate_grasps(self, mesh, target_num_grasps, grasp_gen_mult=2, max_iter=3): 182 | """ 生成候选抓取点 183 | mesh : 待抓取的物体 184 | target_num_grasps : 目标生成抓取点数目 185 | grasp_gen_mult : 过采样倍数 186 | max_iter : 最大迭代次数 187 | 188 | Return : 候选抓取列表 189 | """ 190 | num_grasps_remaining = target_num_grasps 191 | 192 | grasps = [] 193 | k = 1 194 | while num_grasps_remaining > 0 and k <= max_iter: 195 | # 过采样抓取点 196 | num_grasps_generate = grasp_gen_mult * num_grasps_remaining 197 | new_grasps = self.sample_grasps(mesh, num_grasps_generate) 198 | 199 | # 新加入的夹爪必须和之前的所有夹爪距离大于限定值 200 | for grasp in new_grasps: 201 | min_dist = np.inf 202 | for cur_grasp in grasps: 203 | dist = Grasp_2f.distance(cur_grasp, grasp) 204 | if dist < min_dist: 205 | min_dist = dist 206 | if min_dist >= self._grasp_dist_thresh: 207 | grasps.append(grasp) 208 | 209 | grasp_gen_mult = int(grasp_gen_mult * 2) 210 | num_grasps_remaining = target_num_grasps - len(grasps) 211 | k += 1 212 | if len(grasps) < target_num_grasps: 213 | logging.warning('generate_grasps 生成数目未达到, 生成数目%d,物体 %s' % 214 | (len(grasps), mesh.name)) 215 | 216 | random.shuffle(grasps) 217 | if len(grasps) > target_num_grasps: 218 | grasps = grasps[:target_num_grasps] 219 | return grasps 220 | -------------------------------------------------------------------------------- /build/lib/easydexnet/mesh.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | import os 5 | import trimesh 6 | import numpy as np 7 | from tvtk.api import tvtk 8 | 9 | 10 | class BaseMesh(object): 11 | ''' 基础的网格文件类，用以保存原始数据 12 | ''' 13 | 14 | def __init__(self, trimesh_obj, name=None): 15 | ''' 16 | trimesh_obj: 一个trimesh对象 17 | name: 文件的名字 18 | ''' 19 | # 一个trimesh对象,用来保存网格数据 20 | self._trimesh_obj = self._process_mesh(trimesh_obj) 21 | # 一个tvtk中的obb对象,用来计算相交线 22 | self._obb_tree = self._generate_obbtree(self._trimesh_obj) 23 | self._name = name 24 | if self._trimesh_obj.is_watertight: 25 | self._center_mass = self._trimesh_obj.center_mass 26 | else: 27 | self._center_mass = self._trimesh_obj.centroid 28 | 29 | @property 30 | def tri_mesh(self): 31 | return self._trimesh_obj 32 | 33 | @property 34 | def center_mass(self): 35 | return self._center_mass 36 | 37 | @property 38 | def name(self): 39 | return self._name 40 | 41 | def intersect_line(self, lineP0, lineP1): 42 | ''' 计算与线段相交的交点，这里调用了tvtk的方法 43 | lineP0: 线段的第一个点，长度3的数组 44 | lineP1: 线段的第二个点 45 | return 46 | points: 所有的交点坐标 47 | cell_ids: 每个交点所属的面片ID ''' 48 | intersectPoints = tvtk.to_vtk(tvtk.Points()) 49 | intersectCells = tvtk.to_vtk(tvtk.IdList()) 50 | self._obb_tree.intersect_with_line( 51 | lineP0, lineP1, intersectPoints, intersectCells) 52 | intersectPoints = tvtk.to_tvtk(intersectPoints) 53 | intersectCells = tvtk.to_tvtk(intersectCells) 54 | points = intersectPoints.to_array() 55 | cell_ids = [intersectCells.get_id( 56 | i) for i in range(intersectCells.number_of_ids)] 57 | return points, cell_ids 58 | 59 | def _generate_obbtree(self, trimesh_obj): 60 | ''' 用来生成一个可用的obbtree对象，加速后面相交的判断 ''' 61 | poly = tvtk.PolyData(points=trimesh_obj.vertices, 62 | polys=trimesh_obj.faces) 63 | tree = tvtk.OBBTree(data_set=poly, tolerance=1.e-8) 64 | tree.build_locator() 65 | return tree 66 | 67 | def _process_mesh(self, trimesh_obj): 68 | ''' 用来预处理mesh数据,这里只是简单的调用了trimesh的预处理程序 ''' 69 | # TODO 可以补上对物体水密性的处理 70 | if not trimesh_obj._validate: 71 | trimesh_obj._validate = True 72 | trimesh_obj.process() 73 | return trimesh_obj 74 | 75 | def bounding_box(self): 76 | max_coords = np.max(self._trimesh_obj.vertices, axis=0) 77 | min_coords = np.min(self._trimesh_obj.vertices, axis=0) 78 | return min_coords, max_coords 79 | 80 | def apply_transform(self, matrix): 81 | tri = self._trimesh_obj.copy() 82 | tri = tri.apply_transform(matrix) 83 | return BaseMesh(tri, self._name) 84 | 85 | @staticmethod 86 | def from_file(file_path, name=None): 87 | if name is None: 88 | name = os.path.splitext(os.path.basename(file_path))[0] 89 | tri_mesh = trimesh.load_mesh(file_path, validate=True) 90 | return BaseMesh(tri_mesh, name) 91 | 92 | @staticmethod 93 | def from_data(vertices, triangles, normals=None, name=None): 94 | trimesh_obj = trimesh.Trimesh(vertices=vertices, 95 | faces=triangles, 96 | face_normals=normals, 97 | validate=True) 98 | return BaseMesh(trimesh_obj, name) 99 | -------------------------------------------------------------------------------- /build/lib/easydexnet/quality.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import numpy as np 3 | from .contact import Contact 4 | from .quality_function import PointGraspMetrics3D 5 | 6 | def force_closure_2f(c1, c2, friction_coef, use_abs_value=False): 7 | """" 检查两个接触点是否力闭合 8 | c1 : 第一个接触点 9 | c2 : 第二个接触点 10 | friction_coef : 摩擦系数 11 | use_abs_value : 当抓取点的朝向未指定时，这个参数有用 12 | Returns 0，1表示是否力闭合 13 | """ 14 | if c1.point is None or c2.point is None or c1.normal is None or c2.normal is None: 15 | return 0 16 | p1, p2 = c1.point, c2.point 17 | n1, n2 = -c1.normal, -c2.normal # inward facing normals 18 | 19 | if (p1 == p2).all(): # same point 20 | return 0 21 | 22 | for normal, contact, other_contact in [(n1, p1, p2), (n2, p2, p1)]: 23 | diff = other_contact - contact 24 | normal_proj = normal.dot(diff) / np.linalg.norm(normal) 25 | if use_abs_value: 26 | normal_proj = abs(normal_proj) 27 | 28 | if normal_proj < 0: 29 | return 0 # wrong side 30 | alpha = np.arccos(normal_proj / np.linalg.norm(diff)) 31 | if alpha > np.arctan(friction_coef): 32 | return 0 # outside of friction cone 33 | return 1 34 | 35 | def grasp_quality(grasp, obj, params): 36 | """ 37 | 计算抓取品质, 在Dex-Net中用到了机器学习方法产生的鲁棒抓取品质 38 | 这里为了简单起见, 只是简单的使用epsilon品质 39 | grasp : 要计算质量的抓取 40 | obj : 计算抓取的物体 41 | params : 抓取参数, 参数列表里面的metrics 42 | target_wrench, 这个参数与稳定姿态有关, 参数表里并未设置 43 | """ 44 | method = params['quality_method'] 45 | friction_coef = params['friction_coef'] 46 | num_cone_faces = params['num_cone_faces'] 47 | soft_fingers = params['soft_fingers'] 48 | if not hasattr(PointGraspMetrics3D, method): 49 | raise ValueError('Illegal point grasp metric %s specified' %(method)) 50 | 51 | contacts_found, contacts = grasp.close_fingers(obj) 52 | 53 | if not contacts_found: 54 | logging.error('抓取品质计算, 接触点无法找到') 55 | return 0 56 | 57 | if method == 'force_closure': 58 | if len(contacts) == 2: 59 | c1, c2 = contacts 60 | return PointGraspMetrics3D.force_closure(c1, c2, friction_coef) 61 | method = 'force_closure_qp' 62 | 63 | num_contacts = len(contacts) 64 | forces = np.zeros([3,0]) 65 | torques = np.zeros([3,0]) 66 | normals = np.zeros([3,0]) 67 | for i in range(num_contacts): 68 | contact = contacts[i] 69 | # 计算摩擦锥 70 | force_success, contact_forces, contact_outward_normal = contact.friction_cone(num_cone_faces, friction_coef) 71 | 72 | if not force_success: 73 | logging.error('抓取品质计算, 摩擦锥计算失败') 74 | return 0 75 | 76 | # 计算摩擦力矩 77 | torque_success, contact_torques = contact.torques(contact_forces) 78 | if not torque_success: 79 | logging.error('抓取品质计算, 摩擦力矩计算失败') 80 | return 0 81 | 82 | # 计算法向力大小 83 | n = contact.normal_force_magnitude() 84 | 85 | forces = np.c_[forces, n * contact_forces] 86 | torques = np.c_[torques, n * contact_torques] 87 | normals = np.c_[normals, n * -contact_outward_normal] # store inward pointing normals 88 | 89 | 90 | if normals.shape[1] == 0: 91 | logging.error('抓取品质计算, 法线计算失败') 92 | return 0 93 | 94 | # normalize torques 95 | if 'torque_scaling' not in params.keys(): 96 | torque_scaling = 1.0 97 | if method == 'ferrari_canny_L1': 98 | _, mx = obj.bounding_box() 99 | torque_scaling = 1.0 / np.median(mx) 100 | params['torque_scaling'] = torque_scaling 101 | 102 | Q_func = getattr(PointGraspMetrics3D, method) 103 | quality = Q_func(forces, torques, normals, 104 | soft_fingers=soft_fingers, 105 | params=params) 106 | 107 | return quality -------------------------------------------------------------------------------- /build/lib/easydexnet/quality_function.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 这个文件是dex-net中的源码修改的 3 | """ 4 | import logging 5 | import numpy as np 6 | try: 7 | import pyhull.convex_hull as cvh 8 | except: 9 | logging.warning('Failed to import pyhull') 10 | try: 11 | import cvxopt as cvx 12 | except: 13 | logging.warning('Failed to import cvx') 14 | import os 15 | import scipy.spatial as ss 16 | import sys 17 | import time 18 | 19 | import matplotlib.pyplot as plt 20 | from mpl_toolkits.mplot3d import Axes3D 21 | 22 | # turn off output logging 23 | cvx.solvers.options['show_progress'] = False 24 | 25 | class PointGraspMetrics3D: 26 | """ Class to wrap functions for quasistatic point grasp quality metrics. 27 | """ 28 | 29 | @staticmethod 30 | def grasp_matrix(forces, torques, normals, soft_fingers=False, 31 | finger_radius=0.005, params=None): 32 | """ Computes the grasp map between contact forces and wrenchs on the object in its reference frame. 33 | 34 | Parameters 35 | ---------- 36 | forces : 3xN :obj:`numpy.ndarray` 37 | set of forces on object in object basis 38 | torques : 3xN :obj:`numpy.ndarray` 39 | set of torques on object in object basis 40 | normals : 3xN :obj:`numpy.ndarray` 41 | surface normals at the contact points 42 | soft_fingers : bool 43 | whether or not to use the soft finger contact model 44 | finger_radius : float 45 | the radius of the fingers to use 46 | params : :obj:`GraspQualityConfig` 47 | set of parameters for grasp matrix and contact model 48 | 49 | Returns 50 | ------- 51 | G : 6xM :obj:`numpy.ndarray` 52 | grasp map 53 | """ 54 | if params is not None and 'finger_radius' in params.keys(): 55 | finger_radius = params['finger_radius'] 56 | num_forces = forces.shape[1] 57 | num_torques = torques.shape[1] 58 | if num_forces != num_torques: 59 | raise ValueError('Need same number of forces and torques') 60 | 61 | num_cols = num_forces 62 | if soft_fingers: 63 | num_normals = 2 64 | if normals.ndim > 1: 65 | num_normals = 2*normals.shape[1] 66 | # print(num_normals, normals.ndim) 67 | num_cols = num_cols + num_normals 68 | 69 | G = np.zeros([6, num_cols]) 70 | for i in range(num_forces): 71 | G[:3,i] = forces[:,i] 72 | G[3:,i] = params['torque_scaling'] * torques[:,i] 73 | 74 | if soft_fingers: 75 | torsion = np.pi * finger_radius**2 * params['friction_coef'] * normals * params['torque_scaling'] 76 | pos_normal_i = int(-num_normals) 77 | neg_normal_i = int(-num_normals + num_normals / 2) 78 | # print(pos_normal_i, neg_normal_i) 79 | G[3:,pos_normal_i:neg_normal_i] = torsion 80 | G[3:,neg_normal_i:] = -torsion 81 | 82 | return G 83 | 84 | @staticmethod 85 | def force_closure(c1, c2, friction_coef, use_abs_value=True): 86 | """" Checks force closure using the antipodality trick. 87 | 88 | Parameters 89 | ---------- 90 | c1 : :obj:`Contact3D` 91 | first contact point 92 | c2 : :obj:`Contact3D` 93 | second contact point 94 | friction_coef : float 95 | coefficient of friction at the contact point 96 | use_abs_value : bool 97 | whether or not to use directoinality of the surface normal (useful when mesh is not oriented) 98 | 99 | Returns 100 | ------- 101 | int : 1 if in force closure, 0 otherwise 102 | """ 103 | if c1.point is None or c2.point is None or c1.normal is None or c2.normal is None: 104 | return 0 105 | p1, p2 = c1.point, c2.point 106 | n1, n2 = -c1.normal, -c2.normal # inward facing normals 107 | 108 | if (p1 == p2).all(): # same point 109 | return 0 110 | 111 | for normal, contact, other_contact in [(n1, p1, p2), (n2, p2, p1)]: 112 | diff = other_contact - contact 113 | normal_proj = normal.dot(diff) / np.linalg.norm(normal) 114 | if use_abs_value: 115 | normal_proj = abs(normal.dot(diff)) / np.linalg.norm(normal) 116 | 117 | if normal_proj < 0: 118 | return 0 # wrong side 119 | alpha = np.arccos(normal_proj / np.linalg.norm(diff)) 120 | if alpha > np.arctan(friction_coef): 121 | return 0 # outside of friction cone 122 | return 1 123 | 124 | @staticmethod 125 | def force_closure_qp(forces, torques, normals, soft_fingers=False, 126 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 127 | params=None): 128 | """ Checks force closure by solving a quadratic program (whether or not zero is in the convex hull) 129 | 130 | Parameters 131 | ---------- 132 | forces : 3xN :obj:`numpy.ndarray` 133 | set of forces on object in object basis 134 | torques : 3xN :obj:`numpy.ndarray` 135 | set of torques on object in object basis 136 | normals : 3xN :obj:`numpy.ndarray` 137 | surface normals at the contact points 138 | soft_fingers : bool 139 | whether or not to use the soft finger contact model 140 | wrench_norm_thresh : float 141 | threshold to use to determine equivalence of target wrenches 142 | wrench_regularizer : float 143 | small float to make quadratic program positive semidefinite 144 | params : :obj:`GraspQualityConfig` 145 | set of parameters for grasp matrix and contact model 146 | 147 | Returns 148 | ------- 149 | int : 1 if in force closure, 0 otherwise 150 | """ 151 | if params is not None: 152 | if 'wrench_norm_thresh' in params.keys(): 153 | wrench_norm_thresh = params['wrench_norm_thresh'] 154 | if 'wrench_regularizer' in params.keys(): 155 | wrench_regularizer = params['wrench_regularizer'] 156 | 157 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers, params=params) 158 | min_norm, _ = PointGraspMetrics3D.min_norm_vector_in_facet(G, wrench_regularizer=wrench_regularizer) 159 | return 1 * (min_norm < wrench_norm_thresh) # if greater than wrench_norm_thresh, 0 is outside of hull 160 | 161 | @staticmethod 162 | def partial_closure(forces, torques, normals, soft_fingers=False, 163 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 164 | params=None): 165 | """ Evalutes partial closure: whether or not the forces and torques can resist a specific wrench. 166 | Estimates resistance by sollving a quadratic program (whether or not the target wrench is in the convex hull). 167 | 168 | Parameters 169 | ---------- 170 | forces : 3xN :obj:`numpy.ndarray` 171 | set of forces on object in object basis 172 | torques : 3xN :obj:`numpy.ndarray` 173 | set of torques on object in object basis 174 | normals : 3xN :obj:`numpy.ndarray` 175 | surface normals at the contact points 176 | soft_fingers : bool 177 | whether or not to use the soft finger contact model 178 | wrench_norm_thresh : float 179 | threshold to use to determine equivalence of target wrenches 180 | wrench_regularizer : float 181 | small float to make quadratic program positive semidefinite 182 | params : :obj:`GraspQualityConfig` 183 | set of parameters for grasp matrix and contact model 184 | 185 | Returns 186 | ------- 187 | int : 1 if in partial closure, 0 otherwise 188 | """ 189 | force_limit = None 190 | if params is None: 191 | return 0 192 | force_limit = params['force_limits'] 193 | target_wrench = params['target_wrench'] 194 | if 'wrench_norm_thresh' in params.keys(): 195 | wrench_norm_thresh = params['wrench_norm_thresh'] 196 | if 'wrench_regularizer' in params.keys(): 197 | wrench_regularizer = params['wrench_regularizer'] 198 | 199 | # reorganize the grasp matrix for easier constraint enforcement in optimization 200 | num_fingers = normals.shape[1] 201 | num_wrenches_per_finger = forces.shape[1] / num_fingers 202 | G = np.zeros([6,0]) 203 | for i in range(num_fingers): 204 | start_i = num_wrenches_per_finger * i 205 | end_i = num_wrenches_per_finger * (i + 1) 206 | G_i = PointGraspMetrics3D.grasp_matrix(forces[:,start_i:end_i], torques[:,start_i:end_i], normals[:,i:i+1], 207 | soft_fingers, params=params) 208 | G = np.c_[G, G_i] 209 | 210 | wrench_resisted, _ = PointGraspMetrics3D.wrench_in_positive_span(G, target_wrench, force_limit, num_fingers, 211 | wrench_norm_thresh=wrench_norm_thresh, 212 | wrench_regularizer=wrench_regularizer) 213 | return 1 * wrench_resisted 214 | 215 | @staticmethod 216 | def wrench_resistance(forces, torques, normals, soft_fingers=False, 217 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 218 | finger_force_eps=1e-9, params=None): 219 | """ Evalutes wrench resistance: the inverse norm of the contact forces required to resist a target wrench 220 | Estimates resistance by sollving a quadratic program (min normal contact forces to produce a wrench). 221 | 222 | Parameters 223 | ---------- 224 | forces : 3xN :obj:`numpy.ndarray` 225 | set of forces on object in object basis 226 | torques : 3xN :obj:`numpy.ndarray` 227 | set of torques on object in object basis 228 | normals : 3xN :obj:`numpy.ndarray` 229 | surface normals at the contact points 230 | soft_fingers : bool 231 | whether or not to use the soft finger contact model 232 | wrench_norm_thresh : float 233 | threshold to use to determine equivalence of target wrenches 234 | wrench_regularizer : float 235 | small float to make quadratic program positive semidefinite 236 | finger_force_eps : float 237 | small float to prevent numeric issues in wrench resistance metric 238 | params : :obj:`GraspQualityConfig` 239 | set of parameters for grasp matrix and contact model 240 | 241 | Returns 242 | ------- 243 | float : value of wrench resistance metric 244 | """ 245 | force_limit = None 246 | if params is None: 247 | return 0 248 | force_limit = params['force_limits'] 249 | target_wrench = params['target_wrench'] 250 | if 'wrench_norm_thresh' in params.keys(): 251 | wrench_norm_thresh = params['wrench_norm_thresh'] 252 | if 'wrench_regularizer' in params.keys(): 253 | wrench_regularizer = params['wrench_regularizer'] 254 | if 'finger_force_eps' in params.keys(): 255 | finger_force_eps = params['finger_force_eps'] 256 | 257 | # reorganize the grasp matrix for easier constraint enforcement in optimization 258 | num_fingers = normals.shape[1] 259 | num_wrenches_per_finger = forces.shape[1] / num_fingers 260 | G = np.zeros([6,0]) 261 | for i in range(num_fingers): 262 | start_i = num_wrenches_per_finger * i 263 | end_i = num_wrenches_per_finger * (i + 1) 264 | G_i = PointGraspMetrics3D.grasp_matrix(forces[:,start_i:end_i], torques[:,start_i:end_i], normals[:,i:i+1], 265 | soft_fingers, params=params) 266 | G = np.c_[G, G_i] 267 | 268 | # compute metric from finger force norm 269 | Q = 0 270 | wrench_resisted, finger_force_norm = PointGraspMetrics3D.wrench_in_positive_span(G, target_wrench, force_limit, num_fingers, 271 | wrench_norm_thresh=wrench_norm_thresh, 272 | wrench_regularizer=wrench_regularizer) 273 | if wrench_resisted: 274 | Q = 1.0 / (finger_force_norm + finger_force_eps) - 1.0 / (2 * force_limit) 275 | return Q 276 | 277 | @staticmethod 278 | def min_singular(forces, torques, normals, soft_fingers=False, params=None): 279 | """ Min singular value of grasp matrix - measure of wrench that grasp is "weakest" at resisting. 280 | 281 | Parameters 282 | ---------- 283 | forces : 3xN :obj:`numpy.ndarray` 284 | set of forces on object in object basis 285 | torques : 3xN :obj:`numpy.ndarray` 286 | set of torques on object in object basis 287 | normals : 3xN :obj:`numpy.ndarray` 288 | surface normals at the contact points 289 | soft_fingers : bool 290 | whether or not to use the soft finger contact model 291 | params : :obj:`GraspQualityConfig` 292 | set of parameters for grasp matrix and contact model 293 | 294 | Returns 295 | ------- 296 | float : value of smallest singular value 297 | """ 298 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 299 | _, S, _ = np.linalg.svd(G) 300 | min_sig = S[5] 301 | return min_sig 302 | 303 | @staticmethod 304 | def wrench_volume(forces, torques, normals, soft_fingers=False, params=None): 305 | """ Volume of grasp matrix singular values - score of all wrenches that the grasp can resist. 306 | 307 | Parameters 308 | ---------- 309 | forces : 3xN :obj:`numpy.ndarray` 310 | set of forces on object in object basis 311 | torques : 3xN :obj:`numpy.ndarray` 312 | set of torques on object in object basis 313 | normals : 3xN :obj:`numpy.ndarray` 314 | surface normals at the contact points 315 | soft_fingers : bool 316 | whether or not to use the soft finger contact model 317 | params : :obj:`GraspQualityConfig` 318 | set of parameters for grasp matrix and contact model 319 | 320 | Returns 321 | ------- 322 | float : value of wrench volume 323 | """ 324 | k = 1 325 | if params is not None and 'k' in params.keys(): 326 | k = params['k'] 327 | 328 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 329 | _, S, _ = np.linalg.svd(G) 330 | sig = S 331 | return k * np.sqrt(np.prod(sig)) 332 | 333 | @staticmethod 334 | def grasp_isotropy(forces, torques, normals, soft_fingers=False, params=None): 335 | """ Condition number of grasp matrix - ratio of "weakest" wrench that the grasp can exert to the "strongest" one. 336 | 337 | Parameters 338 | ---------- 339 | forces : 3xN :obj:`numpy.ndarray` 340 | set of forces on object in object basis 341 | torques : 3xN :obj:`numpy.ndarray` 342 | set of torques on object in object basis 343 | normals : 3xN :obj:`numpy.ndarray` 344 | surface normals at the contact points 345 | soft_fingers : bool 346 | whether or not to use the soft finger contact model 347 | params : :obj:`GraspQualityConfig` 348 | set of parameters for grasp matrix and contact model 349 | 350 | Returns 351 | ------- 352 | float : value of grasp isotropy metric 353 | """ 354 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 355 | _, S, _ = np.linalg.svd(G) 356 | max_sig = S[0] 357 | min_sig = S[5] 358 | isotropy = min_sig / max_sig 359 | if np.isnan(isotropy) or np.isinf(isotropy): 360 | return 0 361 | return isotropy 362 | 363 | @staticmethod 364 | def ferrari_canny_L1(forces, torques, normals, soft_fingers=False, params=None, 365 | wrench_norm_thresh=1e-3, 366 | wrench_regularizer=1e-10): 367 | """ Ferrari & Canny's L1 metric. Also known as the epsilon metric. 368 | 369 | Parameters 370 | ---------- 371 | forces : 3xN :obj:`numpy.ndarray` 372 | set of forces on object in object basis 373 | torques : 3xN :obj:`numpy.ndarray` 374 | set of torques on object in object basis 375 | normals : 3xN :obj:`numpy.ndarray` 376 | surface normals at the contact points 377 | soft_fingers : bool 378 | whether or not to use the soft finger contact model 379 | params : :obj:`GraspQualityConfig` 380 | set of parameters for grasp matrix and contact model 381 | wrench_norm_thresh : float 382 | threshold to use to determine equivalence of target wrenches 383 | wrench_regularizer : float 384 | small float to make quadratic program positive semidefinite 385 | 386 | Returns 387 | ------- 388 | float : value of metric 389 | """ 390 | if params is not None and 'wrench_norm_thresh' in params.keys(): 391 | wrench_norm_thresh = params['wrench_norm_thresh'] 392 | if params is not None and 'wrench_regularizer' in params.keys(): 393 | wrench_regularizer = params['wrench_regularizer'] 394 | 395 | # create grasp matrix 396 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, 397 | soft_fingers, params=params) 398 | s = time.time() 399 | # center grasp matrix for better convex hull comp 400 | hull = cvh.ConvexHull(G.T) 401 | # TODO: suppress ridiculous amount of output for perfectly valid input to qhull 402 | e = time.time() 403 | logging.debug('CVH took %.3f sec' %(e - s)) 404 | 405 | debug = False 406 | if debug: 407 | fig = plt.figure() 408 | torques = G[3:,:].T 409 | ax = Axes3D(fig) 410 | ax.scatter(torques[:,0], torques[:,1], torques[:,2], c='b', s=50) 411 | ax.scatter(0, 0, 0, c='k', s=80) 412 | ax.set_xlim3d(-1.5, 1.5) 413 | ax.set_ylim3d(-1.5, 1.5) 414 | ax.set_zlim3d(-1.5, 1.5) 415 | ax.set_xlabel('tx') 416 | ax.set_ylabel('ty') 417 | ax.set_zlabel('tz') 418 | plt.show() 419 | 420 | if len(hull.vertices) == 0: 421 | logging.warning('Convex hull could not be computed') 422 | return 0.0 423 | 424 | # determine whether or not zero is in the convex hull 425 | s = time.time() 426 | min_norm_in_hull, v = PointGraspMetrics3D.min_norm_vector_in_facet(G, wrench_regularizer=wrench_regularizer) 427 | e = time.time() 428 | logging.debug('Min norm took %.3f sec' %(e - s)) 429 | 430 | # if norm is greater than 0 then forces are outside of hull 431 | if min_norm_in_hull > wrench_norm_thresh: 432 | logging.debug('Zero not in convex hull') 433 | return 0.0 434 | 435 | # if there are fewer nonzeros than D-1 (dim of space minus one) 436 | # then zero is on the boundary and therefore we do not have 437 | # force closure 438 | if np.sum(v > 1e-4) <= G.shape[0]-1: 439 | logging.debug('Zero not in interior of convex hull') 440 | return 0.0 441 | 442 | # find minimum norm vector across all facets of convex hull 443 | s = time.time() 444 | min_dist = sys.float_info.max 445 | closest_facet = None 446 | for v in hull.vertices: 447 | if np.max(np.array(v)) < G.shape[1]: # because of some occasional odd behavior from pyhull 448 | facet = G[:, v] 449 | dist, _ = PointGraspMetrics3D.min_norm_vector_in_facet(facet, wrench_regularizer=wrench_regularizer) 450 | if dist < min_dist: 451 | min_dist = dist 452 | closest_facet = v 453 | e = time.time() 454 | logging.debug('Min dist took %.3f sec for %d vertices' %(e - s, len(hull.vertices))) 455 | 456 | return min_dist 457 | 458 | @staticmethod 459 | def wrench_in_positive_span(wrench_basis, target_wrench, force_limit, num_fingers=1, 460 | wrench_norm_thresh = 1e-4, wrench_regularizer = 1e-10): 461 | """ Check whether a target can be exerted by positive combinations of wrenches in a given basis with L1 norm fonger force limit limit. 462 | 463 | Parameters 464 | ---------- 465 | wrench_basis : 6xN :obj:`numpy.ndarray` 466 | basis for the wrench space 467 | target_wrench : 6x1 :obj:`numpy.ndarray` 468 | target wrench to resist 469 | force_limit : float 470 | L1 upper bound on the forces per finger (aka contact point) 471 | num_fingers : int 472 | number of contacts, used to enforce L1 finger constraint 473 | wrench_norm_thresh : float 474 | threshold to use to determine equivalence of target wrenches 475 | wrench_regularizer : float 476 | small float to make quadratic program positive semidefinite 477 | 478 | Returns 479 | ------- 480 | int 481 | whether or not wrench can be resisted 482 | float 483 | minimum norm of the finger forces required to resist the wrench 484 | """ 485 | num_wrenches = wrench_basis.shape[1] 486 | 487 | # quadratic and linear costs 488 | P = wrench_basis.T.dot(wrench_basis) + wrench_regularizer*np.eye(num_wrenches) 489 | q = -wrench_basis.T.dot(target_wrench) 490 | 491 | # inequalities 492 | lam_geq_zero = -1 * np.eye(num_wrenches) 493 | 494 | num_wrenches_per_finger = num_wrenches / num_fingers 495 | force_constraint = np.zeros([num_fingers, num_wrenches]) 496 | for i in range(num_fingers): 497 | start_i = num_wrenches_per_finger * i 498 | end_i = num_wrenches_per_finger * (i + 1) 499 | force_constraint[i, start_i:end_i] = np.ones(num_wrenches_per_finger) 500 | 501 | G = np.r_[lam_geq_zero, force_constraint] 502 | h = np.zeros(num_wrenches+num_fingers) 503 | for i in range(num_fingers): 504 | h[num_wrenches+i] = force_limit 505 | 506 | # convert to cvx and solve 507 | P = cvx.matrix(P) 508 | q = cvx.matrix(q) 509 | G = cvx.matrix(G) 510 | h = cvx.matrix(h) 511 | sol = cvx.solvers.qp(P, q, G, h) 512 | v = np.array(sol['x']) 513 | 514 | min_dist = np.linalg.norm(wrench_basis.dot(v).ravel() - target_wrench)**2 515 | 516 | # add back in the target wrench 517 | return min_dist < wrench_norm_thresh, np.linalg.norm(v) 518 | 519 | @staticmethod 520 | def min_norm_vector_in_facet(facet, wrench_regularizer=1e-10): 521 | """ Finds the minimum norm point in the convex hull of a given facet (aka simplex) by solving a QP. 522 | 523 | Parameters 524 | ---------- 525 | facet : 6xN :obj:`numpy.ndarray` 526 | vectors forming the facet 527 | wrench_regularizer : float 528 | small float to make quadratic program positive semidefinite 529 | 530 | Returns 531 | ------- 532 | float 533 | minimum norm of any point in the convex hull of the facet 534 | Nx1 :obj:`numpy.ndarray` 535 | vector of coefficients that achieves the minimum 536 | """ 537 | dim = facet.shape[1] # num vertices in facet 538 | 539 | # create alpha weights for vertices of facet 540 | G = facet.T.dot(facet) 541 | grasp_matrix = G + wrench_regularizer * np.eye(G.shape[0]) 542 | 543 | # Solve QP to minimize .5 x'Px + q'x subject to Gx <= h, Ax = b 544 | P = cvx.matrix(2 * grasp_matrix) # quadratic cost for Euclidean dist 545 | q = cvx.matrix(np.zeros((dim, 1))) 546 | G = cvx.matrix(-np.eye(dim)) # greater than zero constraint 547 | h = cvx.matrix(np.zeros((dim, 1))) 548 | A = cvx.matrix(np.ones((1, dim))) # sum constraint to enforce convex 549 | b = cvx.matrix(np.ones(1)) # combinations of vertices 550 | 551 | sol = cvx.solvers.qp(P, q, G, h, A, b) 552 | v = np.array(sol['x']) 553 | min_norm = np.sqrt(sol['primal objective']) 554 | 555 | return abs(min_norm), v 556 | 557 | -------------------------------------------------------------------------------- /build/lib/easydexnet/render.py: -------------------------------------------------------------------------------- 1 | """ 渲染深度图的模块 2 | 这里使用pyrender包进行渲染 3 | 渲染时最重要的就是注意相机的位置和物体的位置 4 | """ 5 | import logging 6 | import numpy as np 7 | import scipy.stats as ss 8 | import pyrender 9 | import cv2 10 | from trimesh.visual.color import hex_to_rgba 11 | from .camare import RandomCamera 12 | from .grasp_2d import Grasp2D 13 | from .colcor import cnames 14 | from .vision import DexScene 15 | 16 | 17 | class RenderScene(DexScene): 18 | """ 渲染器的场景, 从pyrender中继承 19 | """ 20 | 21 | def add_obj(self, mesh, matrix=np.eye(4), offset=False, color=None): 22 | """ 添加一个物体到渲染的环境中 23 | mesh : BashMesh类型的对象 24 | matrix : (4,4)的变换矩阵 25 | offset : 补偿物体位置，在xOy平面上平移物体，使中心与原点对齐 26 | """ 27 | matrix_ = matrix.copy() 28 | if offset: 29 | center = mesh.center_mass 30 | off = matrix_.dot(np.r_[center, 1.0])[:2] 31 | matrix_[:2, 3] = matrix_[:2, 3] - off 32 | tri = mesh.tri_mesh 33 | if color is not None: 34 | if isinstance(color, (str)): 35 | color = hex_to_rgba(cnames[color]) 36 | color[-1] = 200 37 | tri.visual.face_colors = color 38 | tri.visual.vertex_colors = color 39 | render_mesh = pyrender.Mesh.from_trimesh(tri) 40 | n = pyrender.Node(mesh=render_mesh, matrix=matrix_) 41 | self.add_node(n) 42 | return matrix_ 43 | 44 | def add_camera(self, camrea): 45 | """ 向场景中添加一个相机 46 | camrea_matrix : 相机相对于世界坐标系的变换矩阵 47 | model : 相机模型参数数组 48 | """ 49 | model = camrea.model 50 | yfov_ = model[0] 51 | aspectRatio_ = model[2] 52 | znear_ = model[1] 53 | camera_ = pyrender.PerspectiveCamera( 54 | yfov=yfov_, aspectRatio=aspectRatio_, znear=znear_) 55 | self.add(camera_, pose=camrea.pose) 56 | return camera_.get_projection_matrix() 57 | 58 | def add_light(self, pose): 59 | light = pyrender.DirectionalLight(color=[1.0, 1.0, 1.0], intensity=2.0) 60 | # light = pyrender.SpotLight(color=np.ones( 61 | # 3), intensity=3.0, innerConeAngle=np.pi/16.0, outerConeAngle=np.pi/6.0) 62 | self.add(light, pose=pose) 63 | 64 | 65 | class ImageRender(object): 66 | """ 保存了一副完整的深度图和创建深度图时环境的类 67 | 如果没有则渲染在一组给定的相机参数和物体位姿下的深度图 68 | """ 69 | 70 | def __init__(self, mesh, pose, table, config, camera=None, data=None): 71 | if 'camera' in config.keys(): 72 | config = config['camera'] 73 | self._size = np.array( 74 | [config['im_width'], config['im_height']], np.int) 75 | self._obj_color = config['obj_color'] 76 | self._table_color = config['table_color'] 77 | self._mesh = mesh 78 | self._pose = pose 79 | self._camera = camera 80 | self._table = table 81 | self._data = data 82 | self._camera_projection_matrix = None 83 | self._obj_matrix = None 84 | if camera is None: 85 | self._camera = RandomCamera(config) 86 | if data is None: 87 | self._data = self.render_image() 88 | # 视口变换矩阵 89 | self._viewport = self.get_viewport(self._size) 90 | 91 | @property 92 | def camera(self): 93 | return self._camera 94 | 95 | @property 96 | def data(self): 97 | return self._data 98 | 99 | @property 100 | def depth(self): 101 | return self._data[1] 102 | 103 | def get_viewport(self, size): 104 | """ 计算视口变换矩阵 """ 105 | scale = np.array([[size[0]/2, 0, 0], 106 | [0, size[1]/2, 0], 107 | [0, 0, 1]]) 108 | map_m = np.array([[1, 0, 1], 109 | [0, -1, 1], 110 | [0, 0, 1]]) 111 | return scale.dot(map_m) 112 | 113 | def render_image(self): 114 | scene = RenderScene() 115 | scene.add_obj(self._table, color=self._table_color) 116 | self._obj_matrix = scene.add_obj( 117 | self._mesh, self._pose.matrix, True, color=self._obj_color) 118 | self._camera_projection_matrix = scene.add_camera(self._camera) 119 | scene.add_light(self._camera.pose) 120 | self.scene = scene 121 | r = pyrender.OffscreenRenderer(self._size[0], self._size[1]) 122 | rgb, depth = r.render(scene) 123 | return [rgb, depth] 124 | 125 | def render_obj_point(self, point): 126 | """ 计算物体坐标系下的点在该图片下的特征向量 127 | [x, y, z, depth] """ 128 | pose = self._camera.pose 129 | world_to_camera = np.linalg.inv(pose) 130 | point_in_world = self._obj_matrix.dot(np.r_[point, 1.0]) 131 | point_in_camera = world_to_camera.dot(point_in_world) 132 | point_in_image = self._camera_projection_matrix.dot( 133 | point_in_camera) 134 | return point_in_image 135 | 136 | def render_grasp(self, grasp_3d): 137 | """ openGl在进行渲染时的过程参考下面的博客 138 | https://blog.csdn.net/linuxheik/article/details/81747087 139 | https://blog.csdn.net/wangdingqiaoit/article/details/51589825 140 | 这里再上面得到的NDC坐标之后还要进行视口变换 141 | 由于最终的图像是Y轴朝下的，而这里的NDC坐标是Y轴朝上 142 | 为了保证最终的图像不变，这里的y坐标全部要取反 143 | """ 144 | p0_3d, p1_3d = grasp_3d.endpoints 145 | p0_2d = self.render_obj_point(p0_3d) 146 | p1_2d = self.render_obj_point(p1_3d) 147 | p0_2d = p0_2d / p0_2d[-1] 148 | p1_2d = p1_2d / p1_2d[-1] 149 | p0_in_image = self._viewport.dot(np.r_[p0_2d[:2], 1.])[:2] 150 | p1_in_image = self._viewport.dot(np.r_[p1_2d[:2], 1.])[:2] 151 | center_3d = grasp_3d.center 152 | center_2d = self.render_obj_point(center_3d) 153 | v = np.r_[p0_in_image, p1_in_image, center_2d[-1]] 154 | return Grasp2D.from_feature_vec(v) 155 | 156 | 157 | class DataGenerator(object): 158 | """ 数据生成器，从图像中生成最终的训练样本 159 | 1. 平移夹爪中心到图像中心 160 | 2. 旋转夹爪抓取轴与x轴对齐 161 | 3. 裁剪到初步大小 162 | 4. 缩放到最终大小 163 | 注意: Dex-Net源码中这里夹爪宽度并没有与图像对齐,有待改进 164 | """ 165 | 166 | def __init__(self, image, grasp_2d, config): 167 | self._image = image 168 | self._grasp = grasp_2d 169 | if "gqcnn" in config.keys(): 170 | self._config = config["gqcnn"] 171 | 172 | @property 173 | def output(self): 174 | grasp = self._grasp 175 | crop_size = [self._config['crop_width'], self._config['crop_height']] 176 | out_size = [self._config['final_width'], self._config['final_height']] 177 | image = self.transform(self._image, grasp.center_float, grasp.angle) 178 | return self.crop_resize(image, crop_size, out_size) 179 | 180 | @staticmethod 181 | def transform(image, center, angle): 182 | """ 先把图片平移到给定点，再旋转给定角度 183 | 注意:图片保存时维度0是行(即y轴)，维度1是列(即x轴) 184 | """ 185 | angle_ = np.rad2deg(angle) 186 | image_size = np.array(image.shape[:2][::-1]).astype(np.int) 187 | translation = image_size / 2 - center 188 | trans_map = np.c_[np.eye(2), translation] 189 | rot_map = cv2.getRotationMatrix2D( 190 | tuple(image_size / 2), angle_, 1) 191 | trans_map_aff = np.r_[trans_map, [[0, 0, 1]]] 192 | rot_map_aff = np.r_[rot_map, [[0, 0, 1]]] 193 | full_map = rot_map_aff.dot(trans_map_aff) 194 | full_map = full_map[:2, :] 195 | im_data_tf = cv2.warpAffine(image, full_map, tuple( 196 | image_size), flags=cv2.INTER_NEAREST) 197 | return im_data_tf 198 | 199 | @staticmethod 200 | def crop_resize(image, crop_size, out_size, center=None): 201 | if center is None: 202 | center = (np.array(image.shape[:2][::-1]) - 1) / 2 203 | diag = np.array(crop_size) / 2 204 | start = center - diag 205 | end = center + diag 206 | image_crop = image[int(start[1]):int( 207 | end[1]), int(start[0]):int(end[0])].copy() 208 | image_out = cv2.resize( 209 | image_crop, (int(out_size[0]), int(out_size[0]))) 210 | return image_out 211 | 212 | 213 | class DepthRender(object): 214 | def __init__(self, dex_obj, table, saver, config): 215 | self._dex_obj = dex_obj 216 | self._config = config 217 | self._table = table 218 | self._saver = saver 219 | 220 | @property 221 | def dex_obj(self): 222 | return self._dex_obj 223 | 224 | def render(self): 225 | mesh = self._dex_obj.mesh 226 | table = self._table 227 | config = self._config 228 | saver = self._saver 229 | render_num = config['render']['images_per_stable_pose'] 230 | # 对每个姿势迭代 231 | for pose in self._dex_obj.poses: 232 | vaild_grasps, collision = self.vaild_grasps(pose) 233 | for _ in range(render_num): 234 | render = ImageRender(mesh, pose, table, config) 235 | depth = render.depth 236 | # 对每个抓取 237 | for g, col in zip(vaild_grasps, collision): 238 | quality = g.quality 239 | if quality is None: 240 | logging.error('抓取品质为None') 241 | g_2d = render.render_grasp(g) 242 | out = DataGenerator(depth, g_2d, config).output 243 | saver.add(out, g_2d, quality, col) 244 | 245 | def vaild_grasps(self, pose): 246 | """ 获取该位姿下所有有效的夹爪，和是否碰撞 247 | """ 248 | max_angle = self._config['render']['max_grasp_approch'] 249 | mesh = self._dex_obj.mesh 250 | grasps = self._dex_obj.grasps 251 | vaild_g = [] 252 | collision = [] 253 | for g in grasps: 254 | if g.get_approch(pose)[1] < max_angle: 255 | vaild_g.append(g) 256 | collision.append(g.check_approach(mesh, pose, self._config)) 257 | return vaild_g, collision 258 | -------------------------------------------------------------------------------- /build/lib/easydexnet/stable_poses.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | import numpy as np 5 | 6 | 7 | class StablePoses(object): 8 | def __init__(self, matrix, probability): 9 | self._matrix = matrix 10 | self._probability = probability 11 | 12 | @property 13 | def matrix(self): 14 | return self._matrix 15 | 16 | @property 17 | def probability(self): 18 | return self._probability 19 | 20 | @staticmethod 21 | def from_raw_poses(matrixs, probabilitys): 22 | poses = [] 23 | for matrix, probability in zip(matrixs, probabilitys): 24 | poses.append(StablePoses(matrix, probability)) 25 | return poses 26 | -------------------------------------------------------------------------------- /build/lib/easydexnet/vision.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import trimesh 3 | import pyrender 4 | from .colcor import cnames 5 | 6 | 7 | class DexScene(pyrender.Scene): 8 | def add_obj(self, mesh, matrix=np.eye(4), color='lightblue'): 9 | tri = mesh.tri_mesh 10 | if isinstance(color, (str)): 11 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 12 | color[-1] = 200 13 | tri.visual.face_colors = color 14 | tri.visual.vertex_colors = color 15 | render_mesh = pyrender.Mesh.from_trimesh(tri) 16 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 17 | self.add_node(n) 18 | 19 | def add_grasp(self, grasp, matrix=np.eye(4), radius=0.0025, color='red'): 20 | def vector_to_rotation(vector): 21 | z = np.array(vector) 22 | z = z / np.linalg.norm(z) 23 | x = np.array([1, 0, 0]) 24 | x = x - z*(x.dot(z)/z.dot(z)) 25 | x = x / np.linalg.norm(x) 26 | y = np.cross(z, x) 27 | return np.c_[x, y, z] 28 | grasp_vision = trimesh.creation.capsule(grasp.width, radius) 29 | rotation = vector_to_rotation(grasp.axis) 30 | trasform = np.eye(4) 31 | trasform[:3, :3] = rotation 32 | center = grasp.center - (grasp.width / 2) * grasp.axis 33 | trasform[:3, 3] = center 34 | grasp_vision.apply_transform(trasform) 35 | if isinstance(color, (str)): 36 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 37 | grasp_vision.visual.face_colors = color 38 | grasp_vision.visual.vertex_colors = color 39 | render_mesh = pyrender.Mesh.from_trimesh(grasp_vision) 40 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 41 | self.add_node(n) 42 | 43 | def add_grasp_center(self, grasp, matrix=np.eye(4), radius=0.003, color='black'): 44 | point_vision = trimesh.creation.uv_sphere(radius) 45 | trasform = np.eye(4) 46 | trasform[:3, 3] = grasp.center 47 | point_vision.apply_transform(trasform) 48 | if isinstance(color, (str)): 49 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 50 | point_vision.visual.face_colors = color 51 | point_vision.visual.vertex_colors = color 52 | render_mesh = pyrender.Mesh.from_trimesh(point_vision) 53 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 54 | self.add_node(n) 55 | -------------------------------------------------------------------------------- /config/add_obj.yaml: -------------------------------------------------------------------------------- 1 | path: 2 | logging_path: H:/Robot/easy-dexnet/test/test.log 3 | hdf5_path: H:/Robot/template/test.hdf5 4 | obj_path: H:/Robot/template/obj 5 | 6 | obj_list: all 7 | recover: 0 8 | # 夹爪参数,最大开度和最小开度 9 | grispper: 10 | max_width: 0.047 11 | min_width: 0.0 12 | 13 | # 计算夹爪距离时的默认alpha值 14 | grisp_distance_alpha: 0.05 15 | 16 | # 默认的摩擦系数 17 | default_friction_coef: 0.5 18 | 19 | # 计算摩擦锥时的默认多面体近似面数 20 | num_cone_faces: 8 21 | 22 | # 稳定姿态阈值 23 | stable_pose_min_p: 0.05 24 | 25 | # 接触点之间容许的最小宽度 26 | min_contact_dist: 0.0025 27 | 28 | # 不同抓取点之间的容许的最小距离 29 | grasp_dist_thresh: 0.0005 30 | 31 | # 每个物体采样的数目 32 | num_sample: 150 33 | 34 | datapoints_per_file: 1000 35 | 36 | metrics: 37 | quality_method: ferrari_canny_L1 38 | friction_coef: 0.5 39 | num_cone_faces: 8 40 | soft_fingers: 1 41 | all_contacts_required: 1 42 | 43 | # optional deterministic parameters 44 | torque_scaling: 1000 45 | wrench_norm_thresh: 0.001 46 | wrench_regularizer: 0.0000000001 47 | 48 | # 夹爪参数 49 | finger_radius: 0.01 50 | force_limits: 20.0 51 | 52 | # 碰撞检测器配置 53 | collision_checker: 54 | # 宽度补偿, 假设夹爪存在一定的宽度, 即夹爪的横向宽度 55 | width_offset: 0.005 56 | # 深度补偿, 在夹爪抓取路径方向上进行补偿 57 | depth_offiset: 0.005 58 | # 轴向补偿: 在抓取轴方向上进行补偿 59 | axis_offiset: 0.005 60 | # 检测点数目 61 | checker_point_num: 5 62 | # 检查距离 63 | test_dist: 0.5 64 | 65 | 66 | # 相机的参数 67 | camera: 68 | # 相机可视角度 69 | min_yfov: 60 70 | max_yfov: 70 71 | # 相机纵横比 72 | aspectRatio: 1.0 73 | # 相机模型投影近平面距离,即焦距 74 | min_znear: 0.05 75 | max_znear: 0.07 76 | # 相机离原点的距离 77 | min_radius: 0.65 78 | max_radius: 0.75 79 | # 相机位置与z轴的夹角 80 | min_elev: 0.1 81 | max_elev: 5.0 # in degrees 82 | # 相机位置的方位角, 与x轴的夹角 83 | min_az: 0.0 84 | max_az: 360.0 # degrees 85 | # 相机坐标绕z轴旋转的角度 86 | min_roll: -0.2 87 | max_roll: 0.2 # degrees 88 | # 物体偏离中心的距离, 这里直接转换成相机位置的变换，物体还是放在原点 89 | min_x: -0.1 90 | max_x: 0.1 91 | min_y: -0.1 92 | max_y: 0.1 93 | # 渲染的图片的大小 94 | im_width: 600 95 | im_height: 600 96 | 97 | obj_color: orange 98 | table_color: white 99 | 100 | gqcnn: 101 | crop_width: 96 102 | crop_height: 96 103 | final_width: 32 104 | final_height: 32 105 | 106 | hdf5_config: 107 | grasps_group: grasps/yumi_metal_spline 108 | stable_poses_group: stable_poses 109 | metrics_name: robust_ferrari_canny 110 | 111 | render: 112 | images_per_stable_pose: 5 113 | max_grasp_approch: 40 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /config/generate.yaml: -------------------------------------------------------------------------------- 1 | path: 2 | logging_path: H:/Robot/easy-dexnet/test/test.log 3 | data_path: H:/Robot/template/test.hdf5 4 | out_path: H:/Robot/template/output 5 | table_path: H:/Robot/easy-dexnet/data/table.obj 6 | 7 | obj_list: all 8 | # 夹爪参数,最大开度和最小开度 9 | grispper: 10 | max_width: 0.05 11 | min_width: 0.0 12 | 13 | # 计算夹爪距离时的默认alpha值 14 | grisp_distance_alpha: 0.05 15 | 16 | # 默认的摩擦系数 17 | default_friction_coef: 0.5 18 | 19 | # 计算摩擦锥时的默认多面体近似面数 20 | num_cone_faces: 8 21 | 22 | # 稳定姿态阈值 23 | stable_pose_min_p: 0.0 24 | 25 | # 接触点之间容许的最小宽度 26 | min_contact_dist: 0.0025 27 | 28 | # 不同抓取点之间的容许的最小距离 29 | grasp_dist_thresh: 0.0005 30 | 31 | # 每个物体采样的数目 32 | num_sample: 25 33 | 34 | datapoints_per_file: 1000 35 | 36 | metrics: 37 | quality_method: ferrari_canny_L1 38 | friction_coef: 0.5 39 | num_cone_faces: 8 40 | soft_fingers: 1 41 | all_contacts_required: 1 42 | 43 | # optional deterministic parameters 44 | torque_scaling: 1000 45 | wrench_norm_thresh: 0.001 46 | wrench_regularizer: 0.0000000001 47 | 48 | # 夹爪参数 49 | finger_radius: 0.01 50 | force_limits: 20.0 51 | 52 | # 碰撞检测器配置 53 | collision_checker: 54 | # 宽度补偿, 假设夹爪存在一定的宽度, 即夹爪的横向宽度 55 | width_offset: 0.005 56 | # 深度补偿, 在夹爪抓取路径方向上进行补偿 57 | depth_offiset: 0.005 58 | # 轴向补偿: 在抓取轴方向上进行补偿 59 | axis_offiset: 0.005 60 | # 检测点数目 61 | checker_point_num: 5 62 | # 检查距离 63 | test_dist: 0.5 64 | 65 | 66 | # 相机的参数 67 | camera: 68 | # 相机可视角度 69 | min_yfov: 60 70 | max_yfov: 70 71 | # 相机纵横比 72 | aspectRatio: 1.0 73 | # 相机模型投影近平面距离,即焦距 74 | min_znear: 0.05 75 | max_znear: 0.07 76 | # 相机离原点的距离 77 | min_radius: 0.65 78 | max_radius: 0.75 79 | # 相机位置与z轴的夹角 80 | min_elev: 0.1 81 | max_elev: 5.0 # in degrees 82 | # 相机位置的方位角, 与x轴的夹角 83 | min_az: 0.0 84 | max_az: 360.0 # degrees 85 | # 相机坐标绕z轴旋转的角度 86 | min_roll: -0.2 87 | max_roll: 0.2 # degrees 88 | # 物体偏离中心的距离, 这里直接转换成相机位置的变换，物体还是放在原点 89 | min_x: -0.1 90 | max_x: 0.1 91 | min_y: -0.1 92 | max_y: 0.1 93 | # 渲染的图片的大小 94 | im_width: 600 95 | im_height: 600 96 | 97 | obj_color: orange 98 | table_color: white 99 | 100 | gqcnn: 101 | crop_width: 96 102 | crop_height: 96 103 | final_width: 32 104 | final_height: 32 105 | 106 | hdf5_config: 107 | grasps_group: grasps/yumi_metal_spline 108 | stable_poses_group: stable_poses 109 | metrics_name: robust_ferrari_canny 110 | 111 | render: 112 | images_per_stable_pose: 5 113 | max_grasp_approch: 40 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /config/test.yaml: -------------------------------------------------------------------------------- 1 | # 夹爪参数,最大开度和最小开度 2 | grispper: 3 | max_width: 0.05 4 | min_width: 0.0 5 | 6 | # 计算夹爪距离时的默认alpha值 7 | grisp_distance_alpha: 0.05 8 | 9 | # 默认的摩擦系数 10 | default_friction_coef: 0.5 11 | 12 | # 计算摩擦锥时的默认多面体近似面数 13 | num_cone_faces: 8 14 | 15 | # 稳定姿态阈值 16 | stable_pose_min_p: 0.0 17 | 18 | # 接触点之间容许的最小宽度 19 | min_contact_dist: 0.0025 20 | 21 | # 不同抓取点之间的容许的最小距离 22 | grasp_dist_thresh: 0.0005 23 | 24 | # 每个物体采样的数目 25 | num_sample: 25 26 | 27 | datapoints_per_file: 1000 28 | 29 | metrics: 30 | quality_method: ferrari_canny_L1 31 | friction_coef: 0.5 32 | num_cone_faces: 8 33 | soft_fingers: 1 34 | all_contacts_required: 1 35 | 36 | # optional deterministic parameters 37 | torque_scaling: 1000 38 | wrench_norm_thresh: 0.001 39 | wrench_regularizer: 0.0000000001 40 | 41 | # 夹爪参数 42 | finger_radius: 0.01 43 | force_limits: 20.0 44 | 45 | # 碰撞检测器配置 46 | collision_checker: 47 | # 宽度补偿, 假设夹爪存在一定的宽度, 即夹爪的横向宽度 48 | width_offset: 0.005 49 | # 深度补偿, 在夹爪抓取路径方向上进行补偿 50 | depth_offiset: 0.005 51 | # 轴向补偿: 在抓取轴方向上进行补偿 52 | axis_offiset: 0.005 53 | # 检测点数目 54 | checker_point_num: 5 55 | # 检查距离 56 | test_dist: 0.5 57 | 58 | 59 | # 相机的参数 60 | camera: 61 | # 相机可视角度 62 | min_yfov: 60 63 | max_yfov: 70 64 | # 相机纵横比 65 | aspectRatio: 1.0 66 | # 相机模型投影近平面距离,即焦距 67 | min_znear: 0.05 68 | max_znear: 0.07 69 | # 相机离原点的距离 70 | min_radius: 0.65 71 | max_radius: 0.75 72 | # 相机位置与z轴的夹角 73 | min_elev: 0.1 74 | max_elev: 5.0 # in degrees 75 | # 相机位置的方位角, 与x轴的夹角 76 | min_az: 0.0 77 | max_az: 360.0 # degrees 78 | # 相机坐标绕z轴旋转的角度 79 | min_roll: -0.2 80 | max_roll: 0.2 # degrees 81 | # 物体偏离中心的距离, 这里直接转换成相机位置的变换，物体还是放在原点 82 | min_x: -0.1 83 | max_x: 0.1 84 | min_y: -0.1 85 | max_y: 0.1 86 | # 渲染的图片的大小 87 | im_width: 600 88 | im_height: 600 89 | 90 | obj_color: orange 91 | table_color: white 92 | 93 | gqcnn: 94 | crop_width: 96 95 | crop_height: 96 96 | final_width: 32 97 | final_height: 32 98 | 99 | hdf5_config: 100 | grasps_group: grasps/yumi_metal_spline 101 | stable_poses_group: stable_poses 102 | metrics_name: robust_ferrari_canny 103 | 104 | render: 105 | images_per_stable_pose: 5 106 | max_grasp_approch: 40 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /dist/easy_dexnet-0.1.0-py3.6.egg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/dist/easy_dexnet-0.1.0-py3.6.egg -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Setup of easy-dexnet python codebase 4 | Author: Lai QE 5 | """ 6 | from setuptools import setup,find_packages 7 | 8 | requirements = [ 9 | 'trimesh', 10 | 'pyrender', 11 | 'opencv-python' 12 | ] 13 | 14 | setup(name='easy-dexnet', 15 | version='0.1.0', 16 | description='easy-dexnet project', 17 | author='LaiQE', 18 | author_email='laiqe@qq.com', 19 | package_dir = {'': 'src'}, 20 | packages=find_packages('src'), 21 | install_requires=requirements, 22 | # test_suite='test' 23 | ) 24 | 25 | -------------------------------------------------------------------------------- /src/easy_dexnet.egg-info/PKG-INFO: -------------------------------------------------------------------------------- 1 | Metadata-Version: 1.0 2 | Name: easy-dexnet 3 | Version: 0.1.0 4 | Summary: easy-dexnet project 5 | Home-page: UNKNOWN 6 | Author: LaiQE 7 | Author-email: 1311127080@qq.com 8 | License: UNKNOWN 9 | Description: UNKNOWN 10 | Platform: UNKNOWN 11 | -------------------------------------------------------------------------------- /src/easy_dexnet.egg-info/SOURCES.txt: -------------------------------------------------------------------------------- 1 | README.md 2 | setup.py 3 | src/easy_dexnet.egg-info/PKG-INFO 4 | src/easy_dexnet.egg-info/SOURCES.txt 5 | src/easy_dexnet.egg-info/dependency_links.txt 6 | src/easy_dexnet.egg-info/requires.txt 7 | src/easy_dexnet.egg-info/top_level.txt 8 | src/easydexnet/__init__.py 9 | src/easydexnet/camare.py 10 | src/easydexnet/colcor.py 11 | src/easydexnet/contact.py 12 | src/easydexnet/data_saver.py 13 | src/easydexnet/dex_object.py 14 | src/easydexnet/grasp.py 15 | src/easydexnet/grasp_2d.py 16 | src/easydexnet/grasp_sampler.py 17 | src/easydexnet/mesh.py 18 | src/easydexnet/quality.py 19 | src/easydexnet/quality_function.py 20 | src/easydexnet/render.py 21 | src/easydexnet/stable_poses.py 22 | src/easydexnet/vision.py 23 | test/test.py -------------------------------------------------------------------------------- /src/easy_dexnet.egg-info/dependency_links.txt: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/easy_dexnet.egg-info/requires.txt: -------------------------------------------------------------------------------- 1 | trimesh 2 | pyrender 3 | opencv-python 4 | -------------------------------------------------------------------------------- /src/easy_dexnet.egg-info/top_level.txt: -------------------------------------------------------------------------------- 1 | easydexnet 2 | -------------------------------------------------------------------------------- /src/easydexnet/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding:utf-8 -*- 3 | 4 | from .grasp_sampler import GraspSampler_2f 5 | from .mesh import BaseMesh 6 | from .contact import Contact 7 | from .grasp import Grasp_2f 8 | from .stable_poses import StablePoses 9 | from .vision import DexScene 10 | from .quality import grasp_quality 11 | from .dex_object import DexObject 12 | from .render import ImageRender,DataGenerator,DepthRender 13 | from .data_saver import DexSaver 14 | -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/camare.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/camare.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/colcor.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/colcor.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/contact.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/contact.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/data_saver.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/data_saver.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/dex_object.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/dex_object.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/grasp.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/grasp.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/grasp_2d.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/grasp_2d.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/grasp_sampler.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/grasp_sampler.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/mesh.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/mesh.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/quality.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/quality.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/quality_function.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/quality_function.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/render.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/render.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/stable_poses.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/stable_poses.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/__pycache__/vision.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LaiQE/easy-dexnet/0fc77f7e8ea01b3f752579a7a3bc2c1a9cde32c7/src/easydexnet/__pycache__/vision.cpython-36.pyc -------------------------------------------------------------------------------- /src/easydexnet/camare.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Camera(object): 5 | def __init__(self, pose, model): 6 | self._pose = pose 7 | self._model = model 8 | 9 | @property 10 | def pose(self): 11 | return self._pose 12 | 13 | @property 14 | def model(self): 15 | return self._model 16 | 17 | 18 | class RandomCamera(Camera): 19 | """ 随机生成相机参数的类 20 | """ 21 | 22 | def __init__(self, config): 23 | if 'camera' in config.keys(): 24 | config = config['camera'] 25 | self._config = config 26 | self._pose = self.random_pose() 27 | self._model = self.random_model() 28 | 29 | @staticmethod 30 | def sph2cart(r, az, elev): 31 | """ 转换球坐标系到笛卡尔坐标系 32 | r : 半径, 即远点到目标点的距离 33 | az : 方位角, 绕z轴旋转的角度 34 | elev : 俯仰角, 与z轴的夹角 35 | """ 36 | x = r * np.cos(az) * np.sin(elev) 37 | y = r * np.sin(az) * np.sin(elev) 38 | z = r * np.cos(elev) 39 | return x, y, z 40 | 41 | def random_pose(self, num=1): 42 | poses = [] 43 | cfg = self._config 44 | for _ in range(num): 45 | radius = np.random.uniform(cfg['min_radius'], cfg['max_radius']) 46 | elev = np.deg2rad(np.random.uniform( 47 | cfg['min_elev'], cfg['max_elev'])) 48 | az = np.deg2rad(np.random.uniform(cfg['min_az'], cfg['max_az'])) 49 | roll = np.deg2rad(np.random.uniform( 50 | cfg['min_roll'], cfg['max_roll'])) 51 | x = np.random.uniform(cfg['min_x'], cfg['max_x']) 52 | y = np.random.uniform(cfg['min_y'], cfg['max_y']) 53 | poses.append(self._camera_pose(radius, elev, az, roll, x, y)) 54 | return np.squeeze(poses) 55 | 56 | def random_model(self, num=1): 57 | models = [] 58 | cfg = self._config 59 | for _ in range(num): 60 | yfov = np.deg2rad(np.random.uniform( 61 | cfg['min_yfov'], cfg['max_yfov'])) 62 | znear = np.random.uniform(cfg['min_znear'], cfg['max_znear']) 63 | aspectRatio = cfg['aspectRatio'] 64 | models.append(np.array([yfov, znear, aspectRatio])) 65 | return np.squeeze(models) 66 | 67 | def _camera_pose(self, radius, elev, az, roll, x, y): 68 | """ 从给定的参数计算相机的位置矩阵 69 | radius : 相机到原点的距离 70 | elev : 俯仰角, 与z轴的夹角 71 | az : 方位角, 绕z轴旋转的角度 72 | roll : 横滚角，绕相机z轴旋转的夹角 73 | x,y : 物体的x,y轴偏移, 这里折算到相机的位姿上 """ 74 | # 生成相机的中点位置 75 | delta_t = np.array([x, y, 0]) 76 | camera_center_obj = np.array( 77 | [self.sph2cart(radius, az, elev)]).squeeze() + delta_t 78 | camera_z_obj = np.array([self.sph2cart(radius, az, elev)]).squeeze() 79 | camera_z_obj = camera_z_obj / np.linalg.norm(camera_z_obj) 80 | 81 | # 计算x轴和y轴方向, x轴在水平面上 82 | camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0]) 83 | if np.linalg.norm(camera_x_par_obj) == 0: 84 | camera_x_par_obj = np.array([1, 0, 0]) 85 | camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj) 86 | camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) 87 | camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) 88 | # 保证y轴朝下 89 | if camera_y_par_obj[2] > 0: 90 | camera_y_par_obj = -camera_y_par_obj 91 | camera_x_par_obj = -camera_x_par_obj.copy() 92 | # camera_y_par_obj = -camera_y_par_obj 93 | 94 | # 旋转相机 95 | R_obj_camera_par = np.c_[camera_x_par_obj, 96 | camera_y_par_obj, camera_z_obj] 97 | R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0], 98 | [np.sin(roll), np.cos(roll), 0], 99 | [0, 0, 1]]) 100 | R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera) 101 | 102 | matrix = np.eye(4) 103 | matrix[:3, :3] = R_obj_camera 104 | matrix[:3, 3] = camera_center_obj 105 | 106 | return matrix 107 | -------------------------------------------------------------------------------- /src/easydexnet/colcor.py: -------------------------------------------------------------------------------- 1 | # 颜色值参考https://blog.csdn.net/guoxinian/article/details/80242353 2 | cnames = { 3 | 'aliceblue': '#F0F8FF', 4 | 'antiquewhite': '#FAEBD7', 5 | 'aqua': '#00FFFF', 6 | 'aquamarine': '#7FFFD4', 7 | 'azure': '#F0FFFF', 8 | 'beige': '#F5F5DC', 9 | 'bisque': '#FFE4C4', 10 | 'black': '#000000', 11 | 'blanchedalmond': '#FFEBCD', 12 | 'blue': '#0000FF', 13 | 'blueviolet': '#8A2BE2', 14 | 'brown': '#A52A2A', 15 | 'burlywood': '#DEB887', 16 | 'cadetblue': '#5F9EA0', 17 | 'chartreuse': '#7FFF00', 18 | 'chocolate': '#D2691E', 19 | 'coral': '#FF7F50', 20 | 'cornflowerblue': '#6495ED', 21 | 'cornsilk': '#FFF8DC', 22 | 'crimson': '#DC143C', 23 | 'cyan': '#00FFFF', 24 | 'darkblue': '#00008B', 25 | 'darkcyan': '#008B8B', 26 | 'darkgoldenrod': '#B8860B', 27 | 'darkgray': '#A9A9A9', 28 | 'darkgreen': '#006400', 29 | 'darkkhaki': '#BDB76B', 30 | 'darkmagenta': '#8B008B', 31 | 'darkolivegreen': '#556B2F', 32 | 'darkorange': '#FF8C00', 33 | 'darkorchid': '#9932CC', 34 | 'darkred': '#8B0000', 35 | 'darksalmon': '#E9967A', 36 | 'darkseagreen': '#8FBC8F', 37 | 'darkslateblue': '#483D8B', 38 | 'darkslategray': '#2F4F4F', 39 | 'darkturquoise': '#00CED1', 40 | 'darkviolet': '#9400D3', 41 | 'deeppink': '#FF1493', 42 | 'deepskyblue': '#00BFFF', 43 | 'dimgray': '#696969', 44 | 'dodgerblue': '#1E90FF', 45 | 'firebrick': '#B22222', 46 | 'floralwhite': '#FFFAF0', 47 | 'forestgreen': '#228B22', 48 | 'fuchsia': '#FF00FF', 49 | 'gainsboro': '#DCDCDC', 50 | 'ghostwhite': '#F8F8FF', 51 | 'gold': '#FFD700', 52 | 'goldenrod': '#DAA520', 53 | 'gray': '#808080', 54 | 'green': '#008000', 55 | 'greenyellow': '#ADFF2F', 56 | 'honeydew': '#F0FFF0', 57 | 'hotpink': '#FF69B4', 58 | 'indianred': '#CD5C5C', 59 | 'indigo': '#4B0082', 60 | 'ivory': '#FFFFF0', 61 | 'khaki': '#F0E68C', 62 | 'lavender': '#E6E6FA', 63 | 'lavenderblush': '#FFF0F5', 64 | 'lawngreen': '#7CFC00', 65 | 'lemonchiffon': '#FFFACD', 66 | 'lightblue': '#ADD8E6', 67 | 'lightcoral': '#F08080', 68 | 'lightcyan': '#E0FFFF', 69 | 'lightgoldenrodyellow': '#FAFAD2', 70 | 'lightgreen': '#90EE90', 71 | 'lightgray': '#D3D3D3', 72 | 'lightpink': '#FFB6C1', 73 | 'lightsalmon': '#FFA07A', 74 | 'lightseagreen': '#20B2AA', 75 | 'lightskyblue': '#87CEFA', 76 | 'lightslategray': '#778899', 77 | 'lightsteelblue': '#B0C4DE', 78 | 'lightyellow': '#FFFFE0', 79 | 'lime': '#00FF00', 80 | 'limegreen': '#32CD32', 81 | 'linen': '#FAF0E6', 82 | 'magenta': '#FF00FF', 83 | 'maroon': '#800000', 84 | 'mediumaquamarine': '#66CDAA', 85 | 'mediumblue': '#0000CD', 86 | 'mediumorchid': '#BA55D3', 87 | 'mediumpurple': '#9370DB', 88 | 'mediumseagreen': '#3CB371', 89 | 'mediumslateblue': '#7B68EE', 90 | 'mediumspringgreen': '#00FA9A', 91 | 'mediumturquoise': '#48D1CC', 92 | 'mediumvioletred': '#C71585', 93 | 'midnightblue': '#191970', 94 | 'mintcream': '#F5FFFA', 95 | 'mistyrose': '#FFE4E1', 96 | 'moccasin': '#FFE4B5', 97 | 'navajowhite': '#FFDEAD', 98 | 'navy': '#000080', 99 | 'oldlace': '#FDF5E6', 100 | 'olive': '#808000', 101 | 'olivedrab': '#6B8E23', 102 | 'orange': '#FFA500', 103 | 'orangered': '#FF4500', 104 | 'orchid': '#DA70D6', 105 | 'palegoldenrod': '#EEE8AA', 106 | 'palegreen': '#98FB98', 107 | 'paleturquoise': '#AFEEEE', 108 | 'palevioletred': '#DB7093', 109 | 'papayawhip': '#FFEFD5', 110 | 'peachpuff': '#FFDAB9', 111 | 'peru': '#CD853F', 112 | 'pink': '#FFC0CB', 113 | 'plum': '#DDA0DD', 114 | 'powderblue': '#B0E0E6', 115 | 'purple': '#800080', 116 | 'red': '#FF0000', 117 | 'rosybrown': '#BC8F8F', 118 | 'royalblue': '#4169E1', 119 | 'saddlebrown': '#8B4513', 120 | 'salmon': '#FA8072', 121 | 'sandybrown': '#FAA460', 122 | 'seagreen': '#2E8B57', 123 | 'seashell': '#FFF5EE', 124 | 'sienna': '#A0522D', 125 | 'silver': '#C0C0C0', 126 | 'skyblue': '#87CEEB', 127 | 'slateblue': '#6A5ACD', 128 | 'slategray': '#708090', 129 | 'snow': '#FFFAFA', 130 | 'springgreen': '#00FF7F', 131 | 'steelblue': '#4682B4', 132 | 'tan': '#D2B48C', 133 | 'teal': '#008080', 134 | 'thistle': '#D8BFD8', 135 | 'tomato': '#FF6347', 136 | 'turquoise': '#40E0D0', 137 | 'violet': '#EE82EE', 138 | 'wheat': '#F5DEB3', 139 | 'white': '#FFFFFF', 140 | 'whitesmoke': '#F5F5F5', 141 | 'yellow': '#FFFF00', 142 | 'yellowgreen': '#9ACD32'} -------------------------------------------------------------------------------- /src/easydexnet/contact.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import logging 3 | 4 | 5 | class Contact(object): 6 | """ 接触点类型，表示一个接触点 7 | 主要用来计算接触点的摩擦锥 8 | """ 9 | 10 | def __init__(self, point, normal, grasp_direction, moment_arm=None, config=None): 11 | """ 12 | point: 接触点在物体坐标系的坐标点 13 | normal: 接触点所在面片的法线, 方向向外 14 | grasp_direction: 这个接触点的力作用方向, 方向向内 15 | moment_arm: 力臂,一个向量(接触点坐标 - 重心坐标) 16 | """ 17 | self._point = point 18 | self._normal = normal / np.linalg.norm(normal) 19 | self._grasp_direction = grasp_direction / \ 20 | np.linalg.norm(grasp_direction) 21 | self._moment_arm = moment_arm 22 | self._friction_cone = 0.5 23 | if config is not None: 24 | self._friction_cone = config['default_friction_coef'] 25 | self._num_cone_faces = 8 26 | if config is not None: 27 | self._num_cone_faces = config['num_cone_faces'] 28 | 29 | @property 30 | def point(self): 31 | return self._point 32 | 33 | @property 34 | def normal(self): 35 | return self._normal 36 | 37 | @property 38 | def grasp_direction(self): 39 | return self._grasp_direction 40 | 41 | def tangents(self, direction=None): 42 | """ 计算接触点的切线向量, 方向向量和切线向量在右手坐标系下定 43 | 优化了原Dex-Net中的算法 44 | Parameters 45 | ---------- 46 | direction : 3个元素的矩阵,用以计算与这个方向正交的平面 47 | 48 | Returns 49 | ------- 50 | direction : 方向向量,如果未指定则为法向量的反方向 51 | t1 : 第一个切线向量 52 | t2 : 第二个切线向量 53 | """ 54 | if direction is None: 55 | direction = -self._normal 56 | 57 | if np.dot(self._normal, direction) > 0: 58 | direction = -1 * direction 59 | 60 | x = np.array([1, 0, 0]) 61 | # 计算x轴在切平面上的投影,作为第一个切向量 62 | v = x - direction*(x.dot(direction)/direction.dot(direction)) 63 | w = np.cross(direction, v) 64 | 65 | v = v / np.linalg.norm(v) 66 | w = w / np.linalg.norm(w) 67 | 68 | return direction, v, w 69 | 70 | def friction_cone(self, num_cone_faces=None, friction_coef=None): 71 | """ 计算接触点处的摩擦锥. 72 | 73 | Parameters 74 | ---------- 75 | num_cone_faces : int,摩擦锥近似的面数 76 | friction_coef : float,摩擦系数 77 | 78 | Returns 79 | ------- 80 | success : bool,摩擦锥计算是否成功 81 | cone_support : 摩擦锥的边界向量 82 | normal : 向外的法向量 83 | """ 84 | if num_cone_faces is None: 85 | num_cone_faces = self._num_cone_faces 86 | if friction_coef is None: 87 | friction_coef = self._friction_cone 88 | 89 | # 这里不能加, 每次计算的摩擦锥可能都不一样, 摩擦系数可能变 90 | # if self._friction_cone is not None and self._normal is not None: 91 | # return True, self._friction_cone, self._normal 92 | 93 | # 获取切向量 94 | in_normal, t1, t2 = self.tangents() 95 | 96 | # 检查是否有相对滑动, 即切线方向的力始终大于摩擦力 97 | grasp_direction = self._grasp_direction 98 | normal_force_mag = np.dot(grasp_direction, in_normal) # 法线方向分力 99 | tan_force_x = np.dot(grasp_direction, t1) # t1方向分力 100 | tan_force_y = np.dot(grasp_direction, t2) # t2方向分力 101 | tan_force_mag = np.sqrt(tan_force_x**2 + tan_force_y**2) # 切平面上的分力 102 | friction_force_mag = friction_coef * normal_force_mag # 最大静摩擦 103 | 104 | # 如果切面方向力大于最大静摩擦, 则生成失败 105 | if friction_force_mag < tan_force_mag: 106 | return False, None, self._normal 107 | 108 | # 计算摩擦锥 109 | force = in_normal 110 | cone_support = np.zeros((3, num_cone_faces)) 111 | for j in range(num_cone_faces): 112 | tan_vec = t1 * np.cos(2 * np.pi * (float(j) / num_cone_faces)) + \ 113 | t2 * np.sin(2 * np.pi * (float(j) / num_cone_faces)) 114 | cone_support[:, j] = force + friction_coef * tan_vec 115 | 116 | # self._friction_cone = cone_support 117 | return True, cone_support, self._normal 118 | 119 | def torques(self, forces): 120 | """求出接触点上一组力矢量所能施加的力矩 121 | forces : 3xN 力矢量 122 | Returns: 3xN 一组力矩 123 | """ 124 | num_forces = forces.shape[1] 125 | torques = np.zeros([3, num_forces]) 126 | moment_arm = self._moment_arm 127 | for i in range(num_forces): 128 | torques[:, i] = np.cross(moment_arm, forces[:, i]) 129 | return True, torques 130 | 131 | def normal_force_magnitude(self): 132 | """ 计算法线方向上的力的大小 133 | """ 134 | normal_force_mag = 1.0 135 | if self._grasp_direction is not None and self._normal is not None: 136 | in_normal = -self._normal 137 | in_direction_norm = self._grasp_direction 138 | normal_force_mag = np.dot(in_direction_norm, in_normal) 139 | return max(normal_force_mag, 0.0) 140 | -------------------------------------------------------------------------------- /src/easydexnet/data_saver.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import numpy as np 4 | 5 | 6 | class DataSaver(object): 7 | def __init__(self, path, max_data, name): 8 | self._path = path 9 | self._max = max_data 10 | self._counter = 0 11 | self._buffer = None 12 | self._save_counter = 0 13 | self._name = name 14 | 15 | def update_counter(self): 16 | self._counter = self._counter + 1 17 | if self._counter == self._max: 18 | self._counter = 0 19 | self.save() 20 | 21 | def add(self, data): 22 | if self._buffer is None: 23 | self._buffer = np.zeros((self._max,) + data.shape) 24 | if not(data.shape == self._buffer.shape[1:]): 25 | raise ValueError() 26 | self._buffer[self._counter] = data 27 | self.update_counter() 28 | 29 | def save(self): 30 | save_name = self._name + '%06d' % (self._save_counter) + '.npy' 31 | self._save_counter = self._save_counter + 1 32 | save_file = os.path.join(self._path, save_name) 33 | np.save(save_file, self._buffer) 34 | 35 | def close(self): 36 | if self._counter > 0: 37 | self._buffer = self._buffer[:self._counter] 38 | self.save() 39 | total_num = self._counter + (self._save_counter - 1) * self._max 40 | logging.info(self._name+' saver: 共存储了%d个数据' % (total_num)) 41 | 42 | 43 | class DexSaver(object): 44 | def __init__(self, path, config): 45 | self._max = config['datapoints_per_file'] 46 | self._path = path 47 | self._depth_saver = self.create_saver('depth') 48 | self._hand_pose_saver = self.create_saver('hand_pose') 49 | self._quality_saver = self.create_saver('quality') 50 | 51 | def create_saver(self, name): 52 | path = os.path.join(self._path, name) 53 | if not os.path.exists(path): 54 | os.mkdir(path) 55 | return DataSaver(path, self._max, name) 56 | 57 | def add(self, depth, grasp, q, coll): 58 | self._depth_saver.add(depth) 59 | self._hand_pose_saver.add(grasp.to_saver()) 60 | coll_free_metric = np.array([(1 * coll) * q]) 61 | self._quality_saver.add(coll_free_metric) 62 | 63 | def close(self): 64 | self._depth_saver.close() 65 | self._hand_pose_saver.close() 66 | self._quality_saver.close() 67 | -------------------------------------------------------------------------------- /src/easydexnet/dex_object.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | import logging 4 | import os.path 5 | import numpy as np 6 | from .mesh import BaseMesh 7 | from .stable_poses import StablePoses 8 | from .grasp_sampler import GraspSampler_2f 9 | from .quality import grasp_quality 10 | from .grasp import Grasp_2f 11 | 12 | 13 | class DexObject(object): 14 | """ 用于管理所有数据的类，其中包含了计算dex-net数据集中一个物体对象 15 | 所需的所有数据，包括，网格数据、所有稳定姿态、所有候选抓取点 16 | """ 17 | 18 | def __init__(self, mesh, config, poses=None, grasps=None, qualitis=None, name=None): 19 | self._mesh = mesh 20 | self._config = config 21 | self._poses = poses 22 | self._grasps = grasps 23 | self._qualitis = qualitis 24 | self._name = name 25 | if not poses: 26 | self._poses = self.get_poses( 27 | self._mesh, config['stable_pose_min_p']) 28 | if not grasps: 29 | self._grasps = self.get_grasps(self._mesh, self._config) 30 | if not qualitis: 31 | self._qualitis, self._grasps = self.get_quality( 32 | self._grasps, self._mesh, self._config) 33 | for q, g in zip(self._qualitis, self._grasps): 34 | g.quality = q 35 | 36 | @property 37 | def name(self): 38 | return self._name 39 | 40 | @property 41 | def mesh(self): 42 | return self._mesh 43 | 44 | @property 45 | def poses(self): 46 | return self._poses 47 | 48 | @property 49 | def grasps(self): 50 | return self._grasps 51 | 52 | @property 53 | def qualitis(self): 54 | return self._qualitis 55 | 56 | @staticmethod 57 | def from_trimesh(tri_mesh, config, name=None): 58 | mesh = BaseMesh(tri_mesh, name) 59 | return DexObject(mesh, config) 60 | 61 | @staticmethod 62 | def from_file(file_path, config, name=None): 63 | if not name: 64 | name = os.path.splitext(os.path.basename(file_path))[0] 65 | mesh = BaseMesh.from_file(file_path, name) 66 | return DexObject(mesh, config, name=name) 67 | 68 | @staticmethod 69 | def get_poses(mesh, threshold): 70 | """ 从网格对象中获取所有稳定位姿 """ 71 | if not isinstance(mesh, (BaseMesh)): 72 | raise TypeError('mesh must be the class BaseMesh') 73 | tri_mesh = mesh.tri_mesh 74 | _center_mass = mesh.center_mass 75 | matrixs, probabilitys = tri_mesh.convex_hull.compute_stable_poses( 76 | center_mass=_center_mass, threshold=threshold) 77 | return StablePoses.from_raw_poses(matrixs, probabilitys) 78 | 79 | @staticmethod 80 | def get_grasps(mesh, config): 81 | if not isinstance(mesh, (BaseMesh)): 82 | raise TypeError('mesh must be the class BaseMesh') 83 | sampler = GraspSampler_2f(config=config) 84 | num_sample = config['num_sample'] 85 | grasps = sampler.generate_grasps(mesh, num_sample) 86 | return grasps 87 | 88 | @staticmethod 89 | def get_quality(grasps, mesh, config): 90 | metrics = config['metrics'] 91 | valid_grasps = [] 92 | qualitis = [] 93 | for grasp in grasps: 94 | try: 95 | quality = grasp_quality(grasp, mesh, metrics) 96 | except Exception as e: 97 | logging.warning('抓取品质计算无效') 98 | print('抓取品质计算无效', e) 99 | else: 100 | qualitis.append(quality) 101 | valid_grasps.append(grasp) 102 | return qualitis, valid_grasps 103 | 104 | @staticmethod 105 | def from_hdf5_group(obj_group, config, name=None): 106 | grasps, metrics = DexObject.grasps_from_hdf5(obj_group, config) 107 | mesh = DexObject.mesh_from_hdf5(obj_group, name) 108 | poses = DexObject.poses_from_hdf5(obj_group, config) 109 | return DexObject(mesh, config, poses, grasps, metrics, name) 110 | 111 | @staticmethod 112 | def grasps_from_hdf5(obj_group, config): 113 | group = obj_group[config['hdf5_config']['grasps_group']] 114 | metrics_name = config['hdf5_config']['metrics_name'] 115 | grasps = [] 116 | metrics = [] 117 | for grasp_name, grasp_group in group.items(): 118 | configuration = grasp_group.attrs['configuration'] 119 | g = Grasp_2f.from_configuration(configuration, config) 120 | grasps.append(g) 121 | m = grasp_group['metrics'].attrs[metrics_name] 122 | metrics.append(m) 123 | # print(m) 124 | return grasps, metrics 125 | 126 | @staticmethod 127 | def mesh_from_hdf5(obj_group, name): 128 | triangles = np.array(obj_group['mesh/triangles']) 129 | vertices = np.array(obj_group['mesh/vertices']) 130 | mesh = BaseMesh.from_data(vertices, triangles, name=name) 131 | return mesh 132 | 133 | @staticmethod 134 | def poses_from_hdf5(obj_group, config): 135 | group = obj_group[config['hdf5_config']['stable_poses_group']] 136 | vertices = np.array(obj_group['mesh/vertices']) 137 | poses = [] 138 | for pose_name, pose in group.items(): 139 | p = pose.attrs['p'] 140 | r = pose.attrs['r'] 141 | if r.shape[0] == 3: 142 | v = r.dot(vertices.T) 143 | min_z = np.min(v[2, :]) 144 | matrix = np.eye(4) 145 | matrix[:3, :3] = r 146 | matrix[2, 3] = -min_z 147 | else: 148 | matrix = r 149 | poses.append(StablePoses(matrix, p)) 150 | return poses 151 | 152 | def to_hdf5_group(self, obj_group, config): 153 | if self._name in list(obj_group.keys()): 154 | del obj_group[self._name] 155 | group = obj_group.require_group(self._name) 156 | self.grasps_to_hdf5(group, config) 157 | self.mesh_to_hdf5(group) 158 | self.poses_to_hdf5(group, config) 159 | 160 | def grasps_to_hdf5(self, obj_group, config): 161 | group_name = config['hdf5_config']['grasps_group'] 162 | metrics_name = config['hdf5_config']['metrics_name'] 163 | group = obj_group.require_group(group_name) 164 | for i in range(len(self._grasps)): 165 | configuration = self._grasps[i].to_configuration() 166 | metrics = self._qualitis[i] 167 | grasp_name = 'grasp_%d' % (i) 168 | grasp_group = group.require_group(grasp_name) 169 | grasp_group.attrs['configuration'] = configuration 170 | metrics_group = grasp_group.require_group('metrics') 171 | metrics_group.attrs[metrics_name] = metrics 172 | 173 | def mesh_to_hdf5(self, obj_group): 174 | triangles = self._mesh.tri_mesh.faces 175 | vertices = self._mesh.tri_mesh.vertices 176 | group = obj_group.require_group('mesh') 177 | group.create_dataset('triangles', data=triangles) 178 | group.create_dataset('vertices', data=vertices) 179 | 180 | def poses_to_hdf5(self, obj_group, config): 181 | group_name = config['hdf5_config']['stable_poses_group'] 182 | group = obj_group.require_group(group_name) 183 | for i, pose in enumerate(self._poses): 184 | pose_name = 'pose_%d' % (i) 185 | pose_group = group.require_group(pose_name) 186 | pose_group.attrs['r'] = pose.matrix 187 | pose_group.attrs['p'] = pose.probability 188 | -------------------------------------------------------------------------------- /src/easydexnet/grasp.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import numpy as np 3 | from .contact import Contact 4 | 5 | 6 | class Grasp_2f(object): 7 | """ 点接触二指夹爪抓取点模型类 8 | """ 9 | 10 | def __init__(self, center, axis, width=None, min_width=None, config=None): 11 | """ 12 | center: 夹爪的中心点 13 | axis: 夹爪的二指连线方向向量 14 | width: 最大张开距离 15 | min_width: 夹爪闭合时的宽度 16 | """ 17 | self._center = center 18 | self._axis = axis / np.linalg.norm(axis) 19 | self._max_grasp_width = width 20 | self._config = config 21 | if width is None and config is not None: 22 | self._max_grasp_width = config['grispper']['max_width'] 23 | self._min_grasp_width = min_width 24 | if min_width is None and config is not None: 25 | self._min_grasp_width = config['grispper']['min_width'] 26 | self._alpha = 0.05 27 | if config is not None: 28 | self._alpha = config['grisp_distance_alpha'] 29 | self._quality = None 30 | 31 | @property 32 | def quality(self): 33 | return self._quality 34 | 35 | @quality.setter 36 | def quality(self, q): 37 | self._quality = q 38 | 39 | @property 40 | def center(self): 41 | return self._center 42 | 43 | @property 44 | def axis(self): 45 | return self._axis 46 | 47 | @property 48 | def endpoints(self): 49 | """ 返回夹爪的两个端点 """ 50 | half_axis = (self._max_grasp_width / 2.0) * self._axis 51 | point0 = self._center - half_axis 52 | point1 = self._center + half_axis 53 | return point0, point1 54 | 55 | @property 56 | def width(self): 57 | return self._max_grasp_width 58 | 59 | @staticmethod 60 | def distance(g1, g2, alpha=0.05): 61 | """ 计算两个夹爪对象之间的距离 62 | """ 63 | center_dist = np.linalg.norm(g1.center - g2.center) 64 | axis_dist = (2.0 / np.pi) * np.arccos(np.abs(g1.axis.dot(g2.axis))) 65 | return center_dist + alpha * axis_dist 66 | 67 | def _find_contact(self, mesh, out_point, center_point): 68 | """ 找到一个接触点, 从外部点到中心点的第一个接触点 69 | mesh: 一个BaseMesh对象 70 | out_point: 夹爪的一个端点 71 | center_point: 夹爪的中心点 72 | """ 73 | points, cell_ids = mesh.intersect_line(out_point, center_point) 74 | direction = center_point - out_point 75 | 76 | if points.shape[0] < 1: 77 | return False, None 78 | 79 | # 求取法向量, 这里的法向量要保证朝向外侧 80 | normal = mesh.tri_mesh.face_normals[cell_ids[0]] 81 | if np.dot(direction, normal) > 0: 82 | normal = -normal 83 | # 求取接触点的力臂,由质心指向接触点的向量 84 | moment_arm = points[0] - mesh.center_mass 85 | c = Contact(points[0], normal, direction, moment_arm) 86 | return True, c 87 | 88 | def close_fingers(self, mesh, check_approach=True): 89 | """ 闭合夹爪,返回接触点 90 | mesh: 一个BaseMesh对象 91 | check_approach: 是否进行碰撞检测 92 | """ 93 | # TODO 检查在接近路径上的碰撞点,暂时先不写 94 | if check_approach: 95 | pass 96 | 97 | point0, point1 = self.endpoints 98 | # 先判断中间的交点是否为偶数并且是否有足够的点,奇数交点个数则表示出错 99 | points, _ = mesh.intersect_line(point0, point1) 100 | if ((points.shape[0] % 2) != 0) or points.shape[0] < 2: 101 | logging.debug('close_fingers 交点生成出错') 102 | return False, None 103 | 104 | is_c0, c0 = self._find_contact(mesh, point0, self._center) 105 | is_c1, c1 = self._find_contact(mesh, point1, self._center) 106 | if not (is_c0 and is_c1): 107 | logging.debug('close_fingers 接触点寻找失败'+str(is_c0)+str(is_c0)) 108 | return False, None 109 | contacts = [c0, c1] 110 | 111 | return True, contacts 112 | 113 | def _check_approch(self, mesh, point, axis, dis): 114 | """ 检查路径上是否有碰撞 115 | mesh : 网格 116 | point : 检查点 117 | axis : 检查的方向,检查(point,point+axis*dis) 118 | dis : 检查的距离 """ 119 | points, _ = mesh.intersect_line(point, point+axis*dis) 120 | if points.shape[0] > 0: 121 | return False 122 | return True 123 | 124 | def get_approch(self, poses): 125 | """ 计算在抓取路径, 即与z轴最近的方向 """ 126 | poses_r = poses.matrix[:3, :3] 127 | # 稳定姿态的z轴在物体坐标下的表示 128 | poses_z = poses_r[2, :] 129 | axis = self._axis 130 | # 计算z轴在抓取轴为法线的平面上的投影作为抓取路径的反方向 131 | approach = poses_z - axis*(poses_z.dot(axis)/axis.dot(axis)) 132 | approach_L = np.linalg.norm(approach) 133 | poses_z_L = np.linalg.norm(poses_z) 134 | angle = np.arccos(poses_z.dot(approach)/(approach_L*poses_z_L)) 135 | angle = np.rad2deg(angle) 136 | approach = -approach / np.linalg.norm(approach) 137 | return approach, angle 138 | 139 | def check_approach(self, mesh, poses, config): 140 | """ 检查夹取路径上是否有碰撞 """ 141 | # 获取抓取路径 142 | approch, _ = self.get_approch(poses) 143 | check_cfg = config['collision_checker'] 144 | # 夹爪宽度方向 145 | width_axis = np.cross(approch, self._axis) 146 | w_axis = width_axis / np.linalg.norm(width_axis) 147 | check_num = max(check_cfg['checker_point_num'], 3) 148 | axis_offiset = check_cfg['axis_offiset'] 149 | width_offset = check_cfg['width_offset'] 150 | # 轴向补偿列表 151 | aixs_list = np.linspace(-axis_offiset/2, axis_offiset/2, check_num) 152 | # 宽度补偿列表 153 | width_list = np.linspace(-width_offset/2, width_offset/2, check_num) 154 | 155 | for p in self.endpoints: 156 | axis = self._axis 157 | # 最终检查列表 158 | check_list = [p + a*axis + w * w_axis 159 | for a in aixs_list for w in width_list] 160 | for check_point in check_list: 161 | result = self._check_approch(mesh, check_point, 162 | -approch, check_cfg['test_dist']) 163 | if not result: 164 | return False 165 | return True 166 | 167 | def apply_transform(self, matrix): 168 | center = np.r_[self.center, 1] 169 | center = matrix.dot(center)[:3] 170 | axis = matrix[:3, :3].dot(self._axis) 171 | return Grasp_2f(center, axis, config=self._config) 172 | 173 | @staticmethod 174 | def from_configuration(configuration, config): 175 | if not isinstance(configuration, np.ndarray) or (configuration.shape[0] != 9 and configuration.shape[0] != 10): 176 | raise ValueError('Configuration must be numpy ndarray of size 9 or 10') 177 | if configuration.shape[0] == 9: 178 | min_grasp_width = 0 179 | else: 180 | min_grasp_width = configuration[9] 181 | if np.abs(np.linalg.norm(configuration[3:6]) - 1.0) > 1e-5: 182 | raise ValueError('Illegal grasp axis. Must be norm one') 183 | center = configuration[0:3] 184 | axis = configuration[3:6] 185 | width = configuration[6] 186 | return Grasp_2f(center, axis, width, min_grasp_width, config) 187 | 188 | def to_configuration(self): 189 | configuration = np.zeros(10) 190 | configuration[0:3] = self._center 191 | configuration[3:6] = self._axis 192 | configuration[6] = self._max_grasp_width 193 | configuration[7] = 0 194 | configuration[8] = 0 195 | configuration[9] = self._min_grasp_width 196 | return configuration 197 | -------------------------------------------------------------------------------- /src/easydexnet/grasp_2d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Grasp2D(object): 5 | """ 6 | 2D夹爪类型，夹爪投影到深度图像上的坐标. 7 | center : 夹爪中心坐标，像素坐标表示 8 | angle : 抓取方向和相机x坐标的夹角 9 | depth : 夹爪中心点的深度 10 | width : 夹爪的宽度像素坐标 11 | """ 12 | 13 | def __init__(self, center, angle, depth, width=0.0): 14 | self._center = center 15 | self._angle = angle 16 | self._depth = depth 17 | self._width_px = width 18 | 19 | @property 20 | def center(self): 21 | return self._center.astype(np.int) 22 | 23 | @property 24 | def center_float(self): 25 | return self._center 26 | 27 | @property 28 | def angle(self): 29 | return self._angle 30 | 31 | @property 32 | def depth(self): 33 | return self._depth 34 | 35 | @property 36 | def axis(self): 37 | """ Returns the grasp axis. """ 38 | return np.array([np.cos(self._angle), np.sin(self._angle)]) 39 | 40 | @property 41 | def endpoints(self): 42 | """ Returns the grasp endpoints """ 43 | p0 = self._center - (float(self._width_px) / 2) * self.axis 44 | p1 = self._center + (float(self._width_px) / 2) * self.axis 45 | p0 = p0.astype(np.int) 46 | p1 = p1.astype(np.int) 47 | return p0, p1 48 | 49 | @property 50 | def feature_vec(self): 51 | """ 生成抓取的特征向量，由两个端点和中心距离组成 52 | """ 53 | p1, p2 = self.endpoints 54 | return np.r_[p1, p2, self._depth] 55 | 56 | @staticmethod 57 | def from_feature_vec(v, width=None): 58 | """ 通过特征向量创建一个抓取- 59 | v : 抓取的特征向量 60 | width : 夹爪的宽度 61 | """ 62 | # read feature vec 63 | p1 = v[:2] 64 | p2 = v[2:4] 65 | depth = v[4] 66 | if width is None: 67 | width = np.linalg.norm(p1 - p2) 68 | 69 | # compute center and angle 70 | center_px = (p1 + p2) / 2 71 | axis = p2 - p1 72 | if np.linalg.norm(axis) > 0: 73 | axis = axis / np.linalg.norm(axis) 74 | if axis[1] > 0: 75 | angle = np.arccos(axis[0]) 76 | else: 77 | angle = -np.arccos(axis[0]) 78 | return Grasp2D(center_px, angle, depth, width=width) 79 | 80 | @staticmethod 81 | def image_dist(g1, g2, alpha=1.0): 82 | """ 计算两个抓取在像素坐标下的距离 83 | """ 84 | # point to point distances 85 | point_dist = np.linalg.norm(g1.center - g2.center) 86 | 87 | # axis distances 88 | axis_dist = np.arccos(np.abs(g1.axis.dot(g2.axis))) 89 | 90 | return point_dist + alpha * axis_dist 91 | 92 | def to_saver(self): 93 | s = np.zeros((5,)) 94 | s[:2] = self._center 95 | s[2] = self._depth 96 | s[3] = self._angle 97 | s[4] = self._width_px 98 | return s 99 | -------------------------------------------------------------------------------- /src/easydexnet/grasp_sampler.py: -------------------------------------------------------------------------------- 1 | import random 2 | import logging 3 | import numpy as np 4 | from .grasp import Grasp_2f 5 | from .contact import Contact 6 | from .quality import force_closure_2f 7 | 8 | 9 | class GraspSampler_2f(object): 10 | """ 二指夹爪抓取点采样器，采用对映采样器 11 | 1. 随机均匀采样物体表面的点 12 | 2. 在该点的摩擦锥内随机采样一个抓取方向 13 | 3. 沿着这个方向采样另一个抓取点 14 | 4. 检查抓取点 15 | """ 16 | def __init__(self, width=None, min_contact_dist=None, config=None): 17 | """ 18 | width : 夹爪的最大宽度 19 | min_contact_dist : 接触点之间容忍的最小距离 20 | friction_coef : 摩擦系数 21 | dist_thresh : 夹爪之间容忍的最小间距 22 | """ 23 | self._max_grasp_width = width 24 | if width is None and config is not None: 25 | self._max_grasp_width = config['grispper']['max_width'] 26 | self._min_contact_dist = min_contact_dist 27 | if min_contact_dist is None and config is not None: 28 | self._min_contact_dist = config['min_contact_dist'] 29 | self._friction_coef = 0.5 30 | if config is not None: 31 | self._friction_coef = config['default_friction_coef'] 32 | self._grasp_dist_thresh = 0.0005 33 | if config is not None: 34 | self._grasp_dist_thresh = config['grasp_dist_thresh'] 35 | if config is not None: 36 | self._config = config 37 | 38 | def _sample_vector(self, contact, num_samples=1): 39 | """ 在给定点接触点的摩擦锥内采样随机的方向向量 40 | 采样的方向随机向内或向外,这里朝向无所谓 41 | contract : 一个接触点 42 | friction_coef : 摩擦系数 43 | num_samples : 采样个数 44 | return : 方向向量列表 45 | """ 46 | friction_coef = self._friction_coef 47 | n, tx, ty = contact.tangents() 48 | v_samples = [] 49 | # TODO 这里的循环可以改成纯numpy的形式 50 | for _ in range(num_samples): 51 | theta = 2 * np.pi * np.random.rand() 52 | r = friction_coef * np.random.rand() 53 | v = n + r * np.cos(theta) * tx + r * np.sin(theta) * ty 54 | v = -v / np.linalg.norm(v) 55 | v_samples.append(v) 56 | return v_samples 57 | 58 | def _find_grasp(self, mesh, point, vector, test_dist=0.5): 59 | """ 利用给定的点和方向，找到另一个接触点,并生成抓取模型 60 | mesh : 待抓取的物体网格 61 | point : 已知的一个接触点 62 | vector : 给定的抓取方向 63 | test_dist : 用于测试的距离, 物体最大距离的一半 64 | --- 65 | is_grasp : 生成抓取模型是否成功 66 | grasp : 生成的抓取模型 67 | """ 68 | given_point = point 69 | given_point_index = -1 70 | # 先在大范围上计算出所有的由给定点和方向指定的直线 71 | p0 = given_point - vector*test_dist 72 | p1 = given_point + vector*test_dist 73 | points, _ = mesh.intersect_line(p0, p1) 74 | 75 | # 如果交点个数为奇数则出错 76 | points_len = points.shape[0] 77 | if points_len % 2 != 0 or points_len == 0: 78 | logging.debug('_find_grasp 交点数检查出错，物体:%s ' % (mesh.name)) 79 | return False, None 80 | 81 | # 找到当前点的位置, 由于计算误差用一个极小数测试是否一致 82 | for i, p in enumerate(points): 83 | if np.linalg.norm(p - given_point) < 1.e-6: 84 | given_point_index = i 85 | break 86 | # 如果给定点没有在计算出的交点内，则出错 87 | if given_point_index == -1: 88 | logging.debug('_find_grasp 给定点未在物体表面，物体:%s ' % (mesh.name)) 89 | return False, None 90 | # 生成候选的配对点 91 | if given_point_index % 2 == 0: 92 | candidate_point_index = list( 93 | range(given_point_index+1, points_len, 2)) 94 | left_point_index = given_point_index - 1 95 | right_dir = 1 96 | else: 97 | candidate_point_index = list(range(given_point_index-1, -1, -2)) 98 | left_point_index = given_point_index + 1 99 | right_dir = -1 100 | # 最后找到抓取列表 101 | grasps = [] 102 | # 对于每一个候选配对点检查距离是否小于最小距离或是否会被下一个点挡住 103 | # TODO 这里可以写的更清晰一点 104 | for inedx in candidate_point_index: 105 | p = points[inedx] 106 | # 判断两个点之间的距离是否大于要求的最小距离 107 | distance = np.linalg.norm(given_point - p) 108 | distance_is = distance > self._min_contact_dist and distance < self._max_grasp_width 109 | center = (given_point + p) / 2 110 | # 判断夹爪的右边是否不会碰到 111 | right_point_index = inedx + right_dir 112 | if right_point_index >= points_len or right_point_index < 0: 113 | right_is = True 114 | else: 115 | right_distance = np.linalg.norm( 116 | points[right_point_index] - center) 117 | right_is = right_distance > self._max_grasp_width / 2 118 | # 判断夹爪左边是否不会碰到 119 | if left_point_index >= points_len or left_point_index < 0: 120 | left_is = True 121 | else: 122 | left_distance = np.linalg.norm( 123 | points[left_point_index] - center) 124 | left_is = left_distance > self._max_grasp_width / 2 125 | # 如果通过检测 126 | if distance_is and left_is and right_is: 127 | grasp = Grasp_2f(center, p-given_point, 128 | self._max_grasp_width, config=self._config) 129 | # logging.debug('_find_grasp 找到一个合适的抓取，物体:%s ' % (mesh.name)) 130 | grasps.append(grasp) 131 | if len(grasps) > 0: 132 | logging.debug('_find_grasp 成功生成%d个抓取，物体:%s ' % 133 | (len(grasps), mesh.name)) 134 | return True, grasps 135 | logging.debug('_find_grasp 抓取生成失败，物体:%s ' % (mesh.name)) 136 | return False, None 137 | 138 | def sample_grasps(self, mesh, num_grasps, num_samples=2): 139 | """ 采样抓一组取候选点. 140 | mesh : 待抓取的物体网格 141 | num_grasps : int采样个数 142 | num_samples : 每个点采样的方向数 143 | return: 候选抓取点列表 144 | """ 145 | grasps = [] 146 | # 获取表面点 147 | points, face_index = mesh.tri_mesh.sample( 148 | num_grasps, return_index=True) 149 | 150 | # 对于每个表面点 151 | for point, face in zip(points, face_index): 152 | # 计算摩擦锥 153 | normal = mesh.tri_mesh.face_normals[face] 154 | c1 = Contact(point, normal, -normal) 155 | # 在摩擦锥内采样抓取轴 156 | v_samples = self._sample_vector(c1, num_samples=num_samples) 157 | 158 | for v in v_samples: 159 | # 找到另一个接触点 160 | grasp_is, grasp = self._find_grasp(mesh, point, v) 161 | 162 | if not grasp_is: 163 | continue 164 | 165 | # 获取真实的接触点 (之前的接触点容易会变化) 166 | for g in grasp: 167 | success, c = g.close_fingers(mesh) 168 | if not success: 169 | logging.debug('sample_grasps 夹爪闭合失败') 170 | continue 171 | 172 | # 检查摩擦圆锥是否力闭合 173 | if force_closure_2f(c[0], c[1], self._friction_coef): 174 | grasps.append(g) 175 | logging.debug('sample_grasps 成功生成一个抓取') 176 | 177 | # 打乱样本 178 | random.shuffle(grasps) 179 | return grasps 180 | 181 | def generate_grasps(self, mesh, target_num_grasps, grasp_gen_mult=2, max_iter=3): 182 | """ 生成候选抓取点 183 | mesh : 待抓取的物体 184 | target_num_grasps : 目标生成抓取点数目 185 | grasp_gen_mult : 过采样倍数 186 | max_iter : 最大迭代次数 187 | 188 | Return : 候选抓取列表 189 | """ 190 | num_grasps_remaining = target_num_grasps 191 | 192 | grasps = [] 193 | k = 1 194 | while num_grasps_remaining > 0 and k <= max_iter: 195 | # 过采样抓取点 196 | num_grasps_generate = grasp_gen_mult * num_grasps_remaining 197 | new_grasps = self.sample_grasps(mesh, num_grasps_generate) 198 | 199 | # 新加入的夹爪必须和之前的所有夹爪距离大于限定值 200 | for grasp in new_grasps: 201 | min_dist = np.inf 202 | for cur_grasp in grasps: 203 | dist = Grasp_2f.distance(cur_grasp, grasp) 204 | if dist < min_dist: 205 | min_dist = dist 206 | if min_dist >= self._grasp_dist_thresh: 207 | grasps.append(grasp) 208 | 209 | grasp_gen_mult = int(grasp_gen_mult * 2) 210 | num_grasps_remaining = target_num_grasps - len(grasps) 211 | k += 1 212 | if len(grasps) < target_num_grasps: 213 | logging.warning('generate_grasps 生成数目未达到, 生成数目%d,物体 %s' % 214 | (len(grasps), mesh.name)) 215 | 216 | random.shuffle(grasps) 217 | if len(grasps) > target_num_grasps: 218 | grasps = grasps[:target_num_grasps] 219 | return grasps 220 | -------------------------------------------------------------------------------- /src/easydexnet/mesh.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | import os 5 | import trimesh 6 | import numpy as np 7 | from tvtk.api import tvtk 8 | 9 | 10 | class BaseMesh(object): 11 | ''' 基础的网格文件类，用以保存原始数据 12 | ''' 13 | 14 | def __init__(self, trimesh_obj, name=None): 15 | ''' 16 | trimesh_obj: 一个trimesh对象 17 | name: 文件的名字 18 | ''' 19 | # 一个trimesh对象,用来保存网格数据 20 | self._trimesh_obj = self._process_mesh(trimesh_obj) 21 | # 一个tvtk中的obb对象,用来计算相交线 22 | self._obb_tree = self._generate_obbtree(self._trimesh_obj) 23 | self._name = name 24 | if self._trimesh_obj.is_watertight: 25 | self._center_mass = self._trimesh_obj.center_mass 26 | else: 27 | self._center_mass = self._trimesh_obj.centroid 28 | 29 | @property 30 | def tri_mesh(self): 31 | return self._trimesh_obj 32 | 33 | @property 34 | def center_mass(self): 35 | return self._center_mass 36 | 37 | @property 38 | def name(self): 39 | return self._name 40 | 41 | def intersect_line(self, lineP0, lineP1): 42 | ''' 计算与线段相交的交点，这里调用了tvtk的方法 43 | lineP0: 线段的第一个点，长度3的数组 44 | lineP1: 线段的第二个点 45 | return 46 | points: 所有的交点坐标 47 | cell_ids: 每个交点所属的面片ID ''' 48 | intersectPoints = tvtk.to_vtk(tvtk.Points()) 49 | intersectCells = tvtk.to_vtk(tvtk.IdList()) 50 | self._obb_tree.intersect_with_line( 51 | lineP0, lineP1, intersectPoints, intersectCells) 52 | intersectPoints = tvtk.to_tvtk(intersectPoints) 53 | intersectCells = tvtk.to_tvtk(intersectCells) 54 | points = intersectPoints.to_array() 55 | cell_ids = [intersectCells.get_id( 56 | i) for i in range(intersectCells.number_of_ids)] 57 | return points, cell_ids 58 | 59 | def _generate_obbtree(self, trimesh_obj): 60 | ''' 用来生成一个可用的obbtree对象，加速后面相交的判断 ''' 61 | poly = tvtk.PolyData(points=trimesh_obj.vertices, 62 | polys=trimesh_obj.faces) 63 | tree = tvtk.OBBTree(data_set=poly, tolerance=1.e-8) 64 | tree.build_locator() 65 | return tree 66 | 67 | def _process_mesh(self, trimesh_obj): 68 | ''' 用来预处理mesh数据,这里只是简单的调用了trimesh的预处理程序 ''' 69 | # TODO 可以补上对物体水密性的处理 70 | if not trimesh_obj._validate: 71 | trimesh_obj._validate = True 72 | trimesh_obj.process() 73 | return trimesh_obj 74 | 75 | def bounding_box(self): 76 | max_coords = np.max(self._trimesh_obj.vertices, axis=0) 77 | min_coords = np.min(self._trimesh_obj.vertices, axis=0) 78 | return min_coords, max_coords 79 | 80 | def apply_transform(self, matrix): 81 | tri = self._trimesh_obj.copy() 82 | tri = tri.apply_transform(matrix) 83 | return BaseMesh(tri, self._name) 84 | 85 | @staticmethod 86 | def from_file(file_path, name=None): 87 | if name is None: 88 | name = os.path.splitext(os.path.basename(file_path))[0] 89 | tri_mesh = trimesh.load_mesh(file_path, validate=True) 90 | return BaseMesh(tri_mesh, name) 91 | 92 | @staticmethod 93 | def from_data(vertices, triangles, normals=None, name=None): 94 | trimesh_obj = trimesh.Trimesh(vertices=vertices, 95 | faces=triangles, 96 | face_normals=normals, 97 | validate=True) 98 | return BaseMesh(trimesh_obj, name) 99 | -------------------------------------------------------------------------------- /src/easydexnet/quality.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import numpy as np 3 | from .contact import Contact 4 | from .quality_function import PointGraspMetrics3D 5 | 6 | def force_closure_2f(c1, c2, friction_coef, use_abs_value=False): 7 | """" 检查两个接触点是否力闭合 8 | c1 : 第一个接触点 9 | c2 : 第二个接触点 10 | friction_coef : 摩擦系数 11 | use_abs_value : 当抓取点的朝向未指定时，这个参数有用 12 | Returns 0，1表示是否力闭合 13 | """ 14 | if c1.point is None or c2.point is None or c1.normal is None or c2.normal is None: 15 | return 0 16 | p1, p2 = c1.point, c2.point 17 | n1, n2 = -c1.normal, -c2.normal # inward facing normals 18 | 19 | if (p1 == p2).all(): # same point 20 | return 0 21 | 22 | for normal, contact, other_contact in [(n1, p1, p2), (n2, p2, p1)]: 23 | diff = other_contact - contact 24 | normal_proj = normal.dot(diff) / np.linalg.norm(normal) 25 | if use_abs_value: 26 | normal_proj = abs(normal_proj) 27 | 28 | if normal_proj < 0: 29 | return 0 # wrong side 30 | alpha = np.arccos(normal_proj / np.linalg.norm(diff)) 31 | if alpha > np.arctan(friction_coef): 32 | return 0 # outside of friction cone 33 | return 1 34 | 35 | def grasp_quality(grasp, obj, params): 36 | """ 37 | 计算抓取品质, 在Dex-Net中用到了机器学习方法产生的鲁棒抓取品质 38 | 这里为了简单起见, 只是简单的使用epsilon品质 39 | grasp : 要计算质量的抓取 40 | obj : 计算抓取的物体 41 | params : 抓取参数, 参数列表里面的metrics 42 | target_wrench, 这个参数与稳定姿态有关, 参数表里并未设置 43 | """ 44 | method = params['quality_method'] 45 | friction_coef = params['friction_coef'] 46 | num_cone_faces = params['num_cone_faces'] 47 | soft_fingers = params['soft_fingers'] 48 | if not hasattr(PointGraspMetrics3D, method): 49 | raise ValueError('Illegal point grasp metric %s specified' %(method)) 50 | 51 | contacts_found, contacts = grasp.close_fingers(obj) 52 | 53 | if not contacts_found: 54 | logging.error('抓取品质计算, 接触点无法找到') 55 | return 0 56 | 57 | if method == 'force_closure': 58 | if len(contacts) == 2: 59 | c1, c2 = contacts 60 | return PointGraspMetrics3D.force_closure(c1, c2, friction_coef) 61 | method = 'force_closure_qp' 62 | 63 | num_contacts = len(contacts) 64 | forces = np.zeros([3,0]) 65 | torques = np.zeros([3,0]) 66 | normals = np.zeros([3,0]) 67 | for i in range(num_contacts): 68 | contact = contacts[i] 69 | # 计算摩擦锥 70 | force_success, contact_forces, contact_outward_normal = contact.friction_cone(num_cone_faces, friction_coef) 71 | 72 | if not force_success: 73 | logging.error('抓取品质计算, 摩擦锥计算失败') 74 | return 0 75 | 76 | # 计算摩擦力矩 77 | torque_success, contact_torques = contact.torques(contact_forces) 78 | if not torque_success: 79 | logging.error('抓取品质计算, 摩擦力矩计算失败') 80 | return 0 81 | 82 | # 计算法向力大小 83 | n = contact.normal_force_magnitude() 84 | 85 | forces = np.c_[forces, n * contact_forces] 86 | torques = np.c_[torques, n * contact_torques] 87 | normals = np.c_[normals, n * -contact_outward_normal] # store inward pointing normals 88 | 89 | 90 | if normals.shape[1] == 0: 91 | logging.error('抓取品质计算, 法线计算失败') 92 | return 0 93 | 94 | # normalize torques 95 | if 'torque_scaling' not in params.keys(): 96 | torque_scaling = 1.0 97 | if method == 'ferrari_canny_L1': 98 | _, mx = obj.bounding_box() 99 | torque_scaling = 1.0 / np.median(mx) 100 | params['torque_scaling'] = torque_scaling 101 | 102 | Q_func = getattr(PointGraspMetrics3D, method) 103 | quality = Q_func(forces, torques, normals, 104 | soft_fingers=soft_fingers, 105 | params=params) 106 | 107 | return quality -------------------------------------------------------------------------------- /src/easydexnet/quality_function.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 这个文件是dex-net中的源码修改的 3 | """ 4 | import logging 5 | import numpy as np 6 | try: 7 | import pyhull.convex_hull as cvh 8 | except: 9 | logging.warning('Failed to import pyhull') 10 | try: 11 | import cvxopt as cvx 12 | except: 13 | logging.warning('Failed to import cvx') 14 | import os 15 | import scipy.spatial as ss 16 | import sys 17 | import time 18 | 19 | import matplotlib.pyplot as plt 20 | from mpl_toolkits.mplot3d import Axes3D 21 | 22 | # turn off output logging 23 | cvx.solvers.options['show_progress'] = False 24 | 25 | class PointGraspMetrics3D: 26 | """ Class to wrap functions for quasistatic point grasp quality metrics. 27 | """ 28 | 29 | @staticmethod 30 | def grasp_matrix(forces, torques, normals, soft_fingers=False, 31 | finger_radius=0.005, params=None): 32 | """ Computes the grasp map between contact forces and wrenchs on the object in its reference frame. 33 | 34 | Parameters 35 | ---------- 36 | forces : 3xN :obj:`numpy.ndarray` 37 | set of forces on object in object basis 38 | torques : 3xN :obj:`numpy.ndarray` 39 | set of torques on object in object basis 40 | normals : 3xN :obj:`numpy.ndarray` 41 | surface normals at the contact points 42 | soft_fingers : bool 43 | whether or not to use the soft finger contact model 44 | finger_radius : float 45 | the radius of the fingers to use 46 | params : :obj:`GraspQualityConfig` 47 | set of parameters for grasp matrix and contact model 48 | 49 | Returns 50 | ------- 51 | G : 6xM :obj:`numpy.ndarray` 52 | grasp map 53 | """ 54 | if params is not None and 'finger_radius' in params.keys(): 55 | finger_radius = params['finger_radius'] 56 | num_forces = forces.shape[1] 57 | num_torques = torques.shape[1] 58 | if num_forces != num_torques: 59 | raise ValueError('Need same number of forces and torques') 60 | 61 | num_cols = num_forces 62 | if soft_fingers: 63 | num_normals = 2 64 | if normals.ndim > 1: 65 | num_normals = 2*normals.shape[1] 66 | # print(num_normals, normals.ndim) 67 | num_cols = num_cols + num_normals 68 | 69 | G = np.zeros([6, num_cols]) 70 | for i in range(num_forces): 71 | G[:3,i] = forces[:,i] 72 | G[3:,i] = params['torque_scaling'] * torques[:,i] 73 | 74 | if soft_fingers: 75 | torsion = np.pi * finger_radius**2 * params['friction_coef'] * normals * params['torque_scaling'] 76 | pos_normal_i = int(-num_normals) 77 | neg_normal_i = int(-num_normals + num_normals / 2) 78 | # print(pos_normal_i, neg_normal_i) 79 | G[3:,pos_normal_i:neg_normal_i] = torsion 80 | G[3:,neg_normal_i:] = -torsion 81 | 82 | return G 83 | 84 | @staticmethod 85 | def force_closure(c1, c2, friction_coef, use_abs_value=True): 86 | """" Checks force closure using the antipodality trick. 87 | 88 | Parameters 89 | ---------- 90 | c1 : :obj:`Contact3D` 91 | first contact point 92 | c2 : :obj:`Contact3D` 93 | second contact point 94 | friction_coef : float 95 | coefficient of friction at the contact point 96 | use_abs_value : bool 97 | whether or not to use directoinality of the surface normal (useful when mesh is not oriented) 98 | 99 | Returns 100 | ------- 101 | int : 1 if in force closure, 0 otherwise 102 | """ 103 | if c1.point is None or c2.point is None or c1.normal is None or c2.normal is None: 104 | return 0 105 | p1, p2 = c1.point, c2.point 106 | n1, n2 = -c1.normal, -c2.normal # inward facing normals 107 | 108 | if (p1 == p2).all(): # same point 109 | return 0 110 | 111 | for normal, contact, other_contact in [(n1, p1, p2), (n2, p2, p1)]: 112 | diff = other_contact - contact 113 | normal_proj = normal.dot(diff) / np.linalg.norm(normal) 114 | if use_abs_value: 115 | normal_proj = abs(normal.dot(diff)) / np.linalg.norm(normal) 116 | 117 | if normal_proj < 0: 118 | return 0 # wrong side 119 | alpha = np.arccos(normal_proj / np.linalg.norm(diff)) 120 | if alpha > np.arctan(friction_coef): 121 | return 0 # outside of friction cone 122 | return 1 123 | 124 | @staticmethod 125 | def force_closure_qp(forces, torques, normals, soft_fingers=False, 126 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 127 | params=None): 128 | """ Checks force closure by solving a quadratic program (whether or not zero is in the convex hull) 129 | 130 | Parameters 131 | ---------- 132 | forces : 3xN :obj:`numpy.ndarray` 133 | set of forces on object in object basis 134 | torques : 3xN :obj:`numpy.ndarray` 135 | set of torques on object in object basis 136 | normals : 3xN :obj:`numpy.ndarray` 137 | surface normals at the contact points 138 | soft_fingers : bool 139 | whether or not to use the soft finger contact model 140 | wrench_norm_thresh : float 141 | threshold to use to determine equivalence of target wrenches 142 | wrench_regularizer : float 143 | small float to make quadratic program positive semidefinite 144 | params : :obj:`GraspQualityConfig` 145 | set of parameters for grasp matrix and contact model 146 | 147 | Returns 148 | ------- 149 | int : 1 if in force closure, 0 otherwise 150 | """ 151 | if params is not None: 152 | if 'wrench_norm_thresh' in params.keys(): 153 | wrench_norm_thresh = params['wrench_norm_thresh'] 154 | if 'wrench_regularizer' in params.keys(): 155 | wrench_regularizer = params['wrench_regularizer'] 156 | 157 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers, params=params) 158 | min_norm, _ = PointGraspMetrics3D.min_norm_vector_in_facet(G, wrench_regularizer=wrench_regularizer) 159 | return 1 * (min_norm < wrench_norm_thresh) # if greater than wrench_norm_thresh, 0 is outside of hull 160 | 161 | @staticmethod 162 | def partial_closure(forces, torques, normals, soft_fingers=False, 163 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 164 | params=None): 165 | """ Evalutes partial closure: whether or not the forces and torques can resist a specific wrench. 166 | Estimates resistance by sollving a quadratic program (whether or not the target wrench is in the convex hull). 167 | 168 | Parameters 169 | ---------- 170 | forces : 3xN :obj:`numpy.ndarray` 171 | set of forces on object in object basis 172 | torques : 3xN :obj:`numpy.ndarray` 173 | set of torques on object in object basis 174 | normals : 3xN :obj:`numpy.ndarray` 175 | surface normals at the contact points 176 | soft_fingers : bool 177 | whether or not to use the soft finger contact model 178 | wrench_norm_thresh : float 179 | threshold to use to determine equivalence of target wrenches 180 | wrench_regularizer : float 181 | small float to make quadratic program positive semidefinite 182 | params : :obj:`GraspQualityConfig` 183 | set of parameters for grasp matrix and contact model 184 | 185 | Returns 186 | ------- 187 | int : 1 if in partial closure, 0 otherwise 188 | """ 189 | force_limit = None 190 | if params is None: 191 | return 0 192 | force_limit = params['force_limits'] 193 | target_wrench = params['target_wrench'] 194 | if 'wrench_norm_thresh' in params.keys(): 195 | wrench_norm_thresh = params['wrench_norm_thresh'] 196 | if 'wrench_regularizer' in params.keys(): 197 | wrench_regularizer = params['wrench_regularizer'] 198 | 199 | # reorganize the grasp matrix for easier constraint enforcement in optimization 200 | num_fingers = normals.shape[1] 201 | num_wrenches_per_finger = forces.shape[1] / num_fingers 202 | G = np.zeros([6,0]) 203 | for i in range(num_fingers): 204 | start_i = num_wrenches_per_finger * i 205 | end_i = num_wrenches_per_finger * (i + 1) 206 | G_i = PointGraspMetrics3D.grasp_matrix(forces[:,start_i:end_i], torques[:,start_i:end_i], normals[:,i:i+1], 207 | soft_fingers, params=params) 208 | G = np.c_[G, G_i] 209 | 210 | wrench_resisted, _ = PointGraspMetrics3D.wrench_in_positive_span(G, target_wrench, force_limit, num_fingers, 211 | wrench_norm_thresh=wrench_norm_thresh, 212 | wrench_regularizer=wrench_regularizer) 213 | return 1 * wrench_resisted 214 | 215 | @staticmethod 216 | def wrench_resistance(forces, torques, normals, soft_fingers=False, 217 | wrench_norm_thresh=1e-3, wrench_regularizer=1e-10, 218 | finger_force_eps=1e-9, params=None): 219 | """ Evalutes wrench resistance: the inverse norm of the contact forces required to resist a target wrench 220 | Estimates resistance by sollving a quadratic program (min normal contact forces to produce a wrench). 221 | 222 | Parameters 223 | ---------- 224 | forces : 3xN :obj:`numpy.ndarray` 225 | set of forces on object in object basis 226 | torques : 3xN :obj:`numpy.ndarray` 227 | set of torques on object in object basis 228 | normals : 3xN :obj:`numpy.ndarray` 229 | surface normals at the contact points 230 | soft_fingers : bool 231 | whether or not to use the soft finger contact model 232 | wrench_norm_thresh : float 233 | threshold to use to determine equivalence of target wrenches 234 | wrench_regularizer : float 235 | small float to make quadratic program positive semidefinite 236 | finger_force_eps : float 237 | small float to prevent numeric issues in wrench resistance metric 238 | params : :obj:`GraspQualityConfig` 239 | set of parameters for grasp matrix and contact model 240 | 241 | Returns 242 | ------- 243 | float : value of wrench resistance metric 244 | """ 245 | force_limit = None 246 | if params is None: 247 | return 0 248 | force_limit = params['force_limits'] 249 | target_wrench = params['target_wrench'] 250 | if 'wrench_norm_thresh' in params.keys(): 251 | wrench_norm_thresh = params['wrench_norm_thresh'] 252 | if 'wrench_regularizer' in params.keys(): 253 | wrench_regularizer = params['wrench_regularizer'] 254 | if 'finger_force_eps' in params.keys(): 255 | finger_force_eps = params['finger_force_eps'] 256 | 257 | # reorganize the grasp matrix for easier constraint enforcement in optimization 258 | num_fingers = normals.shape[1] 259 | num_wrenches_per_finger = forces.shape[1] / num_fingers 260 | G = np.zeros([6,0]) 261 | for i in range(num_fingers): 262 | start_i = num_wrenches_per_finger * i 263 | end_i = num_wrenches_per_finger * (i + 1) 264 | G_i = PointGraspMetrics3D.grasp_matrix(forces[:,start_i:end_i], torques[:,start_i:end_i], normals[:,i:i+1], 265 | soft_fingers, params=params) 266 | G = np.c_[G, G_i] 267 | 268 | # compute metric from finger force norm 269 | Q = 0 270 | wrench_resisted, finger_force_norm = PointGraspMetrics3D.wrench_in_positive_span(G, target_wrench, force_limit, num_fingers, 271 | wrench_norm_thresh=wrench_norm_thresh, 272 | wrench_regularizer=wrench_regularizer) 273 | if wrench_resisted: 274 | Q = 1.0 / (finger_force_norm + finger_force_eps) - 1.0 / (2 * force_limit) 275 | return Q 276 | 277 | @staticmethod 278 | def min_singular(forces, torques, normals, soft_fingers=False, params=None): 279 | """ Min singular value of grasp matrix - measure of wrench that grasp is "weakest" at resisting. 280 | 281 | Parameters 282 | ---------- 283 | forces : 3xN :obj:`numpy.ndarray` 284 | set of forces on object in object basis 285 | torques : 3xN :obj:`numpy.ndarray` 286 | set of torques on object in object basis 287 | normals : 3xN :obj:`numpy.ndarray` 288 | surface normals at the contact points 289 | soft_fingers : bool 290 | whether or not to use the soft finger contact model 291 | params : :obj:`GraspQualityConfig` 292 | set of parameters for grasp matrix and contact model 293 | 294 | Returns 295 | ------- 296 | float : value of smallest singular value 297 | """ 298 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 299 | _, S, _ = np.linalg.svd(G) 300 | min_sig = S[5] 301 | return min_sig 302 | 303 | @staticmethod 304 | def wrench_volume(forces, torques, normals, soft_fingers=False, params=None): 305 | """ Volume of grasp matrix singular values - score of all wrenches that the grasp can resist. 306 | 307 | Parameters 308 | ---------- 309 | forces : 3xN :obj:`numpy.ndarray` 310 | set of forces on object in object basis 311 | torques : 3xN :obj:`numpy.ndarray` 312 | set of torques on object in object basis 313 | normals : 3xN :obj:`numpy.ndarray` 314 | surface normals at the contact points 315 | soft_fingers : bool 316 | whether or not to use the soft finger contact model 317 | params : :obj:`GraspQualityConfig` 318 | set of parameters for grasp matrix and contact model 319 | 320 | Returns 321 | ------- 322 | float : value of wrench volume 323 | """ 324 | k = 1 325 | if params is not None and 'k' in params.keys(): 326 | k = params['k'] 327 | 328 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 329 | _, S, _ = np.linalg.svd(G) 330 | sig = S 331 | return k * np.sqrt(np.prod(sig)) 332 | 333 | @staticmethod 334 | def grasp_isotropy(forces, torques, normals, soft_fingers=False, params=None): 335 | """ Condition number of grasp matrix - ratio of "weakest" wrench that the grasp can exert to the "strongest" one. 336 | 337 | Parameters 338 | ---------- 339 | forces : 3xN :obj:`numpy.ndarray` 340 | set of forces on object in object basis 341 | torques : 3xN :obj:`numpy.ndarray` 342 | set of torques on object in object basis 343 | normals : 3xN :obj:`numpy.ndarray` 344 | surface normals at the contact points 345 | soft_fingers : bool 346 | whether or not to use the soft finger contact model 347 | params : :obj:`GraspQualityConfig` 348 | set of parameters for grasp matrix and contact model 349 | 350 | Returns 351 | ------- 352 | float : value of grasp isotropy metric 353 | """ 354 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, soft_fingers) 355 | _, S, _ = np.linalg.svd(G) 356 | max_sig = S[0] 357 | min_sig = S[5] 358 | isotropy = min_sig / max_sig 359 | if np.isnan(isotropy) or np.isinf(isotropy): 360 | return 0 361 | return isotropy 362 | 363 | @staticmethod 364 | def ferrari_canny_L1(forces, torques, normals, soft_fingers=False, params=None, 365 | wrench_norm_thresh=1e-3, 366 | wrench_regularizer=1e-10): 367 | """ Ferrari & Canny's L1 metric. Also known as the epsilon metric. 368 | 369 | Parameters 370 | ---------- 371 | forces : 3xN :obj:`numpy.ndarray` 372 | set of forces on object in object basis 373 | torques : 3xN :obj:`numpy.ndarray` 374 | set of torques on object in object basis 375 | normals : 3xN :obj:`numpy.ndarray` 376 | surface normals at the contact points 377 | soft_fingers : bool 378 | whether or not to use the soft finger contact model 379 | params : :obj:`GraspQualityConfig` 380 | set of parameters for grasp matrix and contact model 381 | wrench_norm_thresh : float 382 | threshold to use to determine equivalence of target wrenches 383 | wrench_regularizer : float 384 | small float to make quadratic program positive semidefinite 385 | 386 | Returns 387 | ------- 388 | float : value of metric 389 | """ 390 | if params is not None and 'wrench_norm_thresh' in params.keys(): 391 | wrench_norm_thresh = params['wrench_norm_thresh'] 392 | if params is not None and 'wrench_regularizer' in params.keys(): 393 | wrench_regularizer = params['wrench_regularizer'] 394 | 395 | # create grasp matrix 396 | G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals, 397 | soft_fingers, params=params) 398 | s = time.time() 399 | # center grasp matrix for better convex hull comp 400 | hull = cvh.ConvexHull(G.T) 401 | # TODO: suppress ridiculous amount of output for perfectly valid input to qhull 402 | e = time.time() 403 | logging.debug('CVH took %.3f sec' %(e - s)) 404 | 405 | debug = False 406 | if debug: 407 | fig = plt.figure() 408 | torques = G[3:,:].T 409 | ax = Axes3D(fig) 410 | ax.scatter(torques[:,0], torques[:,1], torques[:,2], c='b', s=50) 411 | ax.scatter(0, 0, 0, c='k', s=80) 412 | ax.set_xlim3d(-1.5, 1.5) 413 | ax.set_ylim3d(-1.5, 1.5) 414 | ax.set_zlim3d(-1.5, 1.5) 415 | ax.set_xlabel('tx') 416 | ax.set_ylabel('ty') 417 | ax.set_zlabel('tz') 418 | plt.show() 419 | 420 | if len(hull.vertices) == 0: 421 | logging.warning('Convex hull could not be computed') 422 | return 0.0 423 | 424 | # determine whether or not zero is in the convex hull 425 | s = time.time() 426 | min_norm_in_hull, v = PointGraspMetrics3D.min_norm_vector_in_facet(G, wrench_regularizer=wrench_regularizer) 427 | e = time.time() 428 | logging.debug('Min norm took %.3f sec' %(e - s)) 429 | 430 | # if norm is greater than 0 then forces are outside of hull 431 | if min_norm_in_hull > wrench_norm_thresh: 432 | logging.debug('Zero not in convex hull') 433 | return 0.0 434 | 435 | # if there are fewer nonzeros than D-1 (dim of space minus one) 436 | # then zero is on the boundary and therefore we do not have 437 | # force closure 438 | if np.sum(v > 1e-4) <= G.shape[0]-1: 439 | logging.debug('Zero not in interior of convex hull') 440 | return 0.0 441 | 442 | # find minimum norm vector across all facets of convex hull 443 | s = time.time() 444 | min_dist = sys.float_info.max 445 | closest_facet = None 446 | for v in hull.vertices: 447 | if np.max(np.array(v)) < G.shape[1]: # because of some occasional odd behavior from pyhull 448 | facet = G[:, v] 449 | dist, _ = PointGraspMetrics3D.min_norm_vector_in_facet(facet, wrench_regularizer=wrench_regularizer) 450 | if dist < min_dist: 451 | min_dist = dist 452 | closest_facet = v 453 | e = time.time() 454 | logging.debug('Min dist took %.3f sec for %d vertices' %(e - s, len(hull.vertices))) 455 | 456 | return min_dist 457 | 458 | @staticmethod 459 | def wrench_in_positive_span(wrench_basis, target_wrench, force_limit, num_fingers=1, 460 | wrench_norm_thresh = 1e-4, wrench_regularizer = 1e-10): 461 | """ Check whether a target can be exerted by positive combinations of wrenches in a given basis with L1 norm fonger force limit limit. 462 | 463 | Parameters 464 | ---------- 465 | wrench_basis : 6xN :obj:`numpy.ndarray` 466 | basis for the wrench space 467 | target_wrench : 6x1 :obj:`numpy.ndarray` 468 | target wrench to resist 469 | force_limit : float 470 | L1 upper bound on the forces per finger (aka contact point) 471 | num_fingers : int 472 | number of contacts, used to enforce L1 finger constraint 473 | wrench_norm_thresh : float 474 | threshold to use to determine equivalence of target wrenches 475 | wrench_regularizer : float 476 | small float to make quadratic program positive semidefinite 477 | 478 | Returns 479 | ------- 480 | int 481 | whether or not wrench can be resisted 482 | float 483 | minimum norm of the finger forces required to resist the wrench 484 | """ 485 | num_wrenches = wrench_basis.shape[1] 486 | 487 | # quadratic and linear costs 488 | P = wrench_basis.T.dot(wrench_basis) + wrench_regularizer*np.eye(num_wrenches) 489 | q = -wrench_basis.T.dot(target_wrench) 490 | 491 | # inequalities 492 | lam_geq_zero = -1 * np.eye(num_wrenches) 493 | 494 | num_wrenches_per_finger = num_wrenches / num_fingers 495 | force_constraint = np.zeros([num_fingers, num_wrenches]) 496 | for i in range(num_fingers): 497 | start_i = num_wrenches_per_finger * i 498 | end_i = num_wrenches_per_finger * (i + 1) 499 | force_constraint[i, start_i:end_i] = np.ones(num_wrenches_per_finger) 500 | 501 | G = np.r_[lam_geq_zero, force_constraint] 502 | h = np.zeros(num_wrenches+num_fingers) 503 | for i in range(num_fingers): 504 | h[num_wrenches+i] = force_limit 505 | 506 | # convert to cvx and solve 507 | P = cvx.matrix(P) 508 | q = cvx.matrix(q) 509 | G = cvx.matrix(G) 510 | h = cvx.matrix(h) 511 | sol = cvx.solvers.qp(P, q, G, h) 512 | v = np.array(sol['x']) 513 | 514 | min_dist = np.linalg.norm(wrench_basis.dot(v).ravel() - target_wrench)**2 515 | 516 | # add back in the target wrench 517 | return min_dist < wrench_norm_thresh, np.linalg.norm(v) 518 | 519 | @staticmethod 520 | def min_norm_vector_in_facet(facet, wrench_regularizer=1e-10): 521 | """ Finds the minimum norm point in the convex hull of a given facet (aka simplex) by solving a QP. 522 | 523 | Parameters 524 | ---------- 525 | facet : 6xN :obj:`numpy.ndarray` 526 | vectors forming the facet 527 | wrench_regularizer : float 528 | small float to make quadratic program positive semidefinite 529 | 530 | Returns 531 | ------- 532 | float 533 | minimum norm of any point in the convex hull of the facet 534 | Nx1 :obj:`numpy.ndarray` 535 | vector of coefficients that achieves the minimum 536 | """ 537 | dim = facet.shape[1] # num vertices in facet 538 | 539 | # create alpha weights for vertices of facet 540 | G = facet.T.dot(facet) 541 | grasp_matrix = G + wrench_regularizer * np.eye(G.shape[0]) 542 | 543 | # Solve QP to minimize .5 x'Px + q'x subject to Gx <= h, Ax = b 544 | P = cvx.matrix(2 * grasp_matrix) # quadratic cost for Euclidean dist 545 | q = cvx.matrix(np.zeros((dim, 1))) 546 | G = cvx.matrix(-np.eye(dim)) # greater than zero constraint 547 | h = cvx.matrix(np.zeros((dim, 1))) 548 | A = cvx.matrix(np.ones((1, dim))) # sum constraint to enforce convex 549 | b = cvx.matrix(np.ones(1)) # combinations of vertices 550 | 551 | sol = cvx.solvers.qp(P, q, G, h, A, b) 552 | v = np.array(sol['x']) 553 | min_norm = np.sqrt(sol['primal objective']) 554 | 555 | return abs(min_norm), v 556 | 557 | -------------------------------------------------------------------------------- /src/easydexnet/render.py: -------------------------------------------------------------------------------- 1 | """ 渲染深度图的模块 2 | 这里使用pyrender包进行渲染 3 | 渲染时最重要的就是注意相机的位置和物体的位置 4 | """ 5 | import logging 6 | import numpy as np 7 | import scipy.stats as ss 8 | import pyrender 9 | import cv2 10 | from trimesh.visual.color import hex_to_rgba 11 | from .camare import RandomCamera 12 | from .grasp_2d import Grasp2D 13 | from .colcor import cnames 14 | from .vision import DexScene 15 | 16 | 17 | class RenderScene(DexScene): 18 | """ 渲染器的场景, 从pyrender中继承 19 | """ 20 | 21 | def add_obj(self, mesh, matrix=np.eye(4), offset=False, color=None): 22 | """ 添加一个物体到渲染的环境中 23 | mesh : BashMesh类型的对象 24 | matrix : (4,4)的变换矩阵 25 | offset : 补偿物体位置，在xOy平面上平移物体，使中心与原点对齐 26 | """ 27 | matrix_ = matrix.copy() 28 | if offset: 29 | center = mesh.center_mass 30 | off = matrix_.dot(np.r_[center, 1.0])[:2] 31 | matrix_[:2, 3] = matrix_[:2, 3] - off 32 | tri = mesh.tri_mesh 33 | if color is not None: 34 | if isinstance(color, (str)): 35 | color = hex_to_rgba(cnames[color]) 36 | color[-1] = 200 37 | tri.visual.face_colors = color 38 | tri.visual.vertex_colors = color 39 | render_mesh = pyrender.Mesh.from_trimesh(tri) 40 | n = pyrender.Node(mesh=render_mesh, matrix=matrix_) 41 | self.add_node(n) 42 | return matrix_ 43 | 44 | def add_camera(self, camrea): 45 | """ 向场景中添加一个相机 46 | camrea_matrix : 相机相对于世界坐标系的变换矩阵 47 | model : 相机模型参数数组 48 | """ 49 | model = camrea.model 50 | yfov_ = model[0] 51 | aspectRatio_ = model[2] 52 | znear_ = model[1] 53 | camera_ = pyrender.PerspectiveCamera( 54 | yfov=yfov_, aspectRatio=aspectRatio_, znear=znear_) 55 | self.add(camera_, pose=camrea.pose) 56 | return camera_.get_projection_matrix() 57 | 58 | def add_light(self, pose): 59 | light = pyrender.DirectionalLight(color=[1.0, 1.0, 1.0], intensity=2.0) 60 | # light = pyrender.SpotLight(color=np.ones( 61 | # 3), intensity=3.0, innerConeAngle=np.pi/16.0, outerConeAngle=np.pi/6.0) 62 | self.add(light, pose=pose) 63 | 64 | 65 | class ImageRender(object): 66 | """ 保存了一副完整的深度图和创建深度图时环境的类 67 | 如果没有则渲染在一组给定的相机参数和物体位姿下的深度图 68 | """ 69 | 70 | def __init__(self, mesh, pose, table, config, camera=None, data=None): 71 | if 'camera' in config.keys(): 72 | config = config['camera'] 73 | self._size = np.array( 74 | [config['im_width'], config['im_height']], np.int) 75 | self._obj_color = config['obj_color'] 76 | self._table_color = config['table_color'] 77 | self._mesh = mesh 78 | self._pose = pose 79 | self._camera = camera 80 | self._table = table 81 | self._data = data 82 | self._camera_projection_matrix = None 83 | self._obj_matrix = None 84 | if camera is None: 85 | self._camera = RandomCamera(config) 86 | if data is None: 87 | self._data = self.render_image() 88 | # 视口变换矩阵 89 | self._viewport = self.get_viewport(self._size) 90 | 91 | @property 92 | def camera(self): 93 | return self._camera 94 | 95 | @property 96 | def data(self): 97 | return self._data 98 | 99 | @property 100 | def depth(self): 101 | return self._data[1] 102 | 103 | def get_viewport(self, size): 104 | """ 计算视口变换矩阵 """ 105 | scale = np.array([[size[0]/2, 0, 0], 106 | [0, size[1]/2, 0], 107 | [0, 0, 1]]) 108 | map_m = np.array([[1, 0, 1], 109 | [0, -1, 1], 110 | [0, 0, 1]]) 111 | return scale.dot(map_m) 112 | 113 | def render_image(self): 114 | scene = RenderScene() 115 | scene.add_obj(self._table, color=self._table_color) 116 | self._obj_matrix = scene.add_obj( 117 | self._mesh, self._pose.matrix, True, color=self._obj_color) 118 | self._camera_projection_matrix = scene.add_camera(self._camera) 119 | scene.add_light(self._camera.pose) 120 | self.scene = scene 121 | r = pyrender.OffscreenRenderer(self._size[0], self._size[1]) 122 | rgb, depth = r.render(scene) 123 | return [rgb, depth] 124 | 125 | def render_obj_point(self, point): 126 | """ 计算物体坐标系下的点在该图片下的特征向量 127 | [x, y, z, depth] """ 128 | pose = self._camera.pose 129 | world_to_camera = np.linalg.inv(pose) 130 | point_in_world = self._obj_matrix.dot(np.r_[point, 1.0]) 131 | point_in_camera = world_to_camera.dot(point_in_world) 132 | point_in_image = self._camera_projection_matrix.dot( 133 | point_in_camera) 134 | return point_in_image 135 | 136 | def render_grasp(self, grasp_3d): 137 | """ openGl在进行渲染时的过程参考下面的博客 138 | https://blog.csdn.net/linuxheik/article/details/81747087 139 | https://blog.csdn.net/wangdingqiaoit/article/details/51589825 140 | 这里再上面得到的NDC坐标之后还要进行视口变换 141 | 由于最终的图像是Y轴朝下的，而这里的NDC坐标是Y轴朝上 142 | 为了保证最终的图像不变，这里的y坐标全部要取反 143 | """ 144 | p0_3d, p1_3d = grasp_3d.endpoints 145 | p0_2d = self.render_obj_point(p0_3d) 146 | p1_2d = self.render_obj_point(p1_3d) 147 | p0_2d = p0_2d / p0_2d[-1] 148 | p1_2d = p1_2d / p1_2d[-1] 149 | p0_in_image = self._viewport.dot(np.r_[p0_2d[:2], 1.])[:2] 150 | p1_in_image = self._viewport.dot(np.r_[p1_2d[:2], 1.])[:2] 151 | center_3d = grasp_3d.center 152 | center_2d = self.render_obj_point(center_3d) 153 | v = np.r_[p0_in_image, p1_in_image, center_2d[-1]] 154 | return Grasp2D.from_feature_vec(v) 155 | 156 | 157 | class DataGenerator(object): 158 | """ 数据生成器，从图像中生成最终的训练样本 159 | 1. 平移夹爪中心到图像中心 160 | 2. 旋转夹爪抓取轴与x轴对齐 161 | 3. 裁剪到初步大小 162 | 4. 缩放到最终大小 163 | 注意: Dex-Net源码中这里夹爪宽度并没有与图像对齐,有待改进 164 | """ 165 | 166 | def __init__(self, image, grasp_2d, config): 167 | self._image = image 168 | self._grasp = grasp_2d 169 | if "gqcnn" in config.keys(): 170 | self._config = config["gqcnn"] 171 | 172 | @property 173 | def output(self): 174 | grasp = self._grasp 175 | crop_size = [self._config['crop_width'], self._config['crop_height']] 176 | out_size = [self._config['final_width'], self._config['final_height']] 177 | image = self.transform(self._image, grasp.center_float, grasp.angle) 178 | return self.crop_resize(image, crop_size, out_size) 179 | 180 | @staticmethod 181 | def transform(image, center, angle): 182 | """ 先把图片平移到给定点，再旋转给定角度 183 | 注意:图片保存时维度0是行(即y轴)，维度1是列(即x轴) 184 | """ 185 | angle_ = np.rad2deg(angle) 186 | image_size = np.array(image.shape[:2][::-1]).astype(np.int) 187 | translation = (image_size-1) / 2 - center 188 | trans_map = np.c_[np.eye(2), translation] 189 | rot_map = cv2.getRotationMatrix2D( 190 | tuple(image_size / 2), angle_, 1) 191 | trans_map_aff = np.r_[trans_map, [[0, 0, 1]]] 192 | rot_map_aff = np.r_[rot_map, [[0, 0, 1]]] 193 | full_map = rot_map_aff.dot(trans_map_aff) 194 | full_map = full_map[:2, :] 195 | im_data_tf = cv2.warpAffine(image, full_map, tuple( 196 | image_size), flags=cv2.INTER_NEAREST) 197 | return im_data_tf 198 | 199 | @staticmethod 200 | def crop_resize(image, crop_size, out_size, center=None): 201 | if center is None: 202 | center = (np.array(image.shape[:2][::-1]) - 1) / 2 203 | diag = np.array(crop_size) / 2 204 | start = center - diag 205 | end = center + diag 206 | image_crop = image[int(start[1]):int( 207 | end[1]), int(start[0]):int(end[0])].copy() 208 | image_out = cv2.resize( 209 | image_crop, (int(out_size[0]), int(out_size[0]))) 210 | return image_out 211 | 212 | 213 | class DepthRender(object): 214 | def __init__(self, dex_obj, table, saver, config): 215 | self._dex_obj = dex_obj 216 | self._config = config 217 | self._table = table 218 | self._saver = saver 219 | 220 | @property 221 | def dex_obj(self): 222 | return self._dex_obj 223 | 224 | def render(self): 225 | mesh = self._dex_obj.mesh 226 | table = self._table 227 | config = self._config 228 | saver = self._saver 229 | render_num = config['render']['images_per_stable_pose'] 230 | # 对每个姿势迭代 231 | for pose in self._dex_obj.poses: 232 | vaild_grasps, collision = self.vaild_grasps(pose) 233 | for _ in range(render_num): 234 | render = ImageRender(mesh, pose, table, config) 235 | depth = render.depth 236 | # 对每个抓取 237 | for g, col in zip(vaild_grasps, collision): 238 | quality = g.quality 239 | if quality is None: 240 | logging.error('抓取品质为None') 241 | g_2d = render.render_grasp(g) 242 | out = DataGenerator(depth, g_2d, config).output 243 | saver.add(out, g_2d, quality, col) 244 | 245 | def vaild_grasps(self, pose): 246 | """ 获取该位姿下所有有效的夹爪，和是否碰撞 247 | """ 248 | max_angle = self._config['render']['max_grasp_approch'] 249 | mesh = self._dex_obj.mesh 250 | grasps = self._dex_obj.grasps 251 | vaild_g = [] 252 | collision = [] 253 | for g in grasps: 254 | if g.get_approch(pose)[1] < max_angle: 255 | vaild_g.append(g) 256 | collision.append(g.check_approach(mesh, pose, self._config)) 257 | return vaild_g, collision 258 | -------------------------------------------------------------------------------- /src/easydexnet/stable_poses.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | import numpy as np 5 | 6 | 7 | class StablePoses(object): 8 | def __init__(self, matrix, probability): 9 | self._matrix = matrix 10 | self._probability = probability 11 | 12 | @property 13 | def matrix(self): 14 | return self._matrix 15 | 16 | @property 17 | def probability(self): 18 | return self._probability 19 | 20 | @staticmethod 21 | def from_raw_poses(matrixs, probabilitys): 22 | poses = [] 23 | for matrix, probability in zip(matrixs, probabilitys): 24 | poses.append(StablePoses(matrix, probability)) 25 | return poses 26 | -------------------------------------------------------------------------------- /src/easydexnet/vision.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import trimesh 3 | import pyrender 4 | from .colcor import cnames 5 | 6 | 7 | class DexScene(pyrender.Scene): 8 | def add_obj(self, mesh, matrix=np.eye(4), color='lightblue'): 9 | tri = mesh.tri_mesh 10 | if isinstance(color, (str)): 11 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 12 | color[-1] = 200 13 | tri.visual.face_colors = color 14 | tri.visual.vertex_colors = color 15 | render_mesh = pyrender.Mesh.from_trimesh(tri) 16 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 17 | self.add_node(n) 18 | 19 | def add_grasp(self, grasp, matrix=np.eye(4), radius=0.0025, color='red'): 20 | def vector_to_rotation(vector): 21 | z = np.array(vector) 22 | z = z / np.linalg.norm(z) 23 | x = np.array([1, 0, 0]) 24 | x = x - z*(x.dot(z)/z.dot(z)) 25 | x = x / np.linalg.norm(x) 26 | y = np.cross(z, x) 27 | return np.c_[x, y, z] 28 | grasp_vision = trimesh.creation.capsule(grasp.width, radius) 29 | rotation = vector_to_rotation(grasp.axis) 30 | trasform = np.eye(4) 31 | trasform[:3, :3] = rotation 32 | center = grasp.center - (grasp.width / 2) * grasp.axis 33 | trasform[:3, 3] = center 34 | grasp_vision.apply_transform(trasform) 35 | if isinstance(color, (str)): 36 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 37 | grasp_vision.visual.face_colors = color 38 | grasp_vision.visual.vertex_colors = color 39 | render_mesh = pyrender.Mesh.from_trimesh(grasp_vision) 40 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 41 | self.add_node(n) 42 | 43 | def add_grasp_center(self, grasp, matrix=np.eye(4), radius=0.003, color='black'): 44 | point_vision = trimesh.creation.uv_sphere(radius) 45 | trasform = np.eye(4) 46 | trasform[:3, 3] = grasp.center 47 | point_vision.apply_transform(trasform) 48 | if isinstance(color, (str)): 49 | color = trimesh.visual.color.hex_to_rgba(cnames[color]) 50 | point_vision.visual.face_colors = color 51 | point_vision.visual.vertex_colors = color 52 | render_mesh = pyrender.Mesh.from_trimesh(point_vision) 53 | n = pyrender.Node(mesh=render_mesh, matrix=matrix) 54 | self.add_node(n) 55 | -------------------------------------------------------------------------------- /test/test.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os.path 3 | import logging 4 | import trimesh 5 | import pyrender 6 | import h5py 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from ruamel.yaml import YAML 10 | ROOT_PATH = r'H:\Robot\easy-dexnet' 11 | dex_path = os.path.abspath(os.path.join(ROOT_PATH, 'src')) 12 | # sys.path.append(dex_path) 13 | sys.path = [dex_path]+sys.path 14 | try: 15 | import easydexnet as dex 16 | except Exception as e: 17 | print('导入模块失败', e) 18 | 19 | # 测试一下github的同步功能，这段文字来自于ubuntu 20 | 21 | 22 | TEST_OBJ_FILE = os.path.join(ROOT_PATH, r'data/Waterglass_800_tex.obj') 23 | TEST_LOG_FILE = os.path.join(ROOT_PATH, 'test/test.log') 24 | TEST_CFG_FILE = os.path.join(ROOT_PATH, 'config/test.yaml') 25 | TEST_TABLE_FILE = os.path.join(ROOT_PATH, r'data/table.obj') 26 | HDF5_FILE = r'H:/Robot/Dex-Net/dataSet/example.hdf5' 27 | OBJ_GROUP = 'datasets/mini_dexnet/objects/bar_clamp' 28 | 29 | 30 | def config_logging(file=None, level=logging.DEBUG): 31 | """ 配置全局的日志设置 """ 32 | LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s" 33 | logging.basicConfig(filename=file, level=level, 34 | format=LOG_FORMAT, filemode='w') 35 | 36 | 37 | def load_config(file): 38 | """ 加载配置文件 """ 39 | yaml = YAML(typ='safe') # default, if not specfied, is 'rt' (round-trip) 40 | with open(file, 'r', encoding="utf-8") as f: 41 | config = yaml.load(f) 42 | return config 43 | 44 | def display(mesh, grasps, quality_s=None): 45 | scene = dex.DexScene(ambient_light=[0.02, 0.02, 0.02], 46 | bg_color=[1.0, 1.0, 1.0]) 47 | scene.add_obj(mesh) 48 | if quality_s is None: 49 | quality_s = [1] * len(grasps) 50 | for g,q in zip(grasps, quality_s): 51 | c = q * np.array([255, 0, -255]) + np.array([0, 0, 255]) 52 | c = np.concatenate((c,[255])) 53 | c = c.astype(int) 54 | scene.add_grasp(g, color=c) 55 | scene.add_grasp_center(g) 56 | pyrender.Viewer(scene, use_raymond_lighting=True) 57 | 58 | def plot_grasp2d(grasp_2d, size): 59 | def plot_2p(p0, p1): 60 | x = [p0[0], p1[0]] 61 | y = [p0[1], p1[1]] 62 | plt.plot(x,y) 63 | p0,p1 = grasp_2d.endpoints 64 | # axis = grasp_2d.axis 65 | # axis_T = np.array([-axis[1], axis[0]]) 66 | # axis_T = axis_T / np.linalg.norm(axis_T) 67 | # p00 = p0 + size * axis_T 68 | # p00 = p00.astype(np.int) 69 | # p01 = p0 - size * axis_T 70 | # p01 = p01.astype(np.int) 71 | # p10 = p1 + size * axis_T 72 | # p10 = p10.astype(np.int) 73 | # p11 = p1 - size * axis_T 74 | # p11 = p11.astype(np.int) 75 | plot_2p(p0, p1) 76 | # plot_2p(p00, p01) 77 | # plot_2p(p10, p11) 78 | 79 | def plot_2p(p0, p1): 80 | x = [p0[0], p1[0]] 81 | y = [p0[1], p1[1]] 82 | plt.plot(x,y) 83 | 84 | 85 | def main(): 86 | config_logging(TEST_LOG_FILE) 87 | config = load_config(TEST_CFG_FILE) 88 | 89 | dex_obj = dex.DexObject.from_file(TEST_OBJ_FILE, config) 90 | quality = dex_obj.qualitis 91 | quality_s = (quality - np.min(quality)) / (np.max(quality) - np.min(quality)) 92 | display(dex_obj.mesh, dex_obj.grasps, quality_s) 93 | pose = dex_obj.poses[0] 94 | grasps = [] 95 | quality = [] 96 | for g,q in zip(dex_obj.grasps,quality_s): 97 | if g.check_approach(dex_obj.mesh, pose, config) and \ 98 | g.get_approch(pose)[1] < 40: 99 | grasps.append(g.apply_transform(pose.matrix)) 100 | quality.append(q) 101 | mesh = dex_obj.mesh.apply_transform(pose.matrix) 102 | display(mesh, grasps, quality) 103 | 104 | def test_render(): 105 | config_logging(TEST_LOG_FILE) 106 | config = load_config(TEST_CFG_FILE) 107 | 108 | mesh = dex.BaseMesh.from_file(TEST_OBJ_FILE) 109 | table = dex.BaseMesh.from_file(TEST_TABLE_FILE) 110 | poses = dex.DexObject.get_poses(mesh, 0.05) 111 | # grasps = dex.DexObject.get_grasps(mesh, config) 112 | 113 | pose = poses[0] 114 | depth = [] 115 | rgb = [] 116 | for i in range(10): 117 | render = dex.ImageRender(mesh, pose, table, config) 118 | r, d = render.data 119 | depth.append(d) 120 | rgb.append(r) 121 | np.save('rgb.npy', rgb) 122 | np.save('depth.npy', depth) 123 | 124 | 125 | # vaild_grasps = [] 126 | # for g in grasps: 127 | # if g.check_approach(mesh, pose, config) and \ 128 | # g.get_approch(pose)[1] < 40: 129 | # vaild_grasps.append(g) 130 | 131 | # vaild_grasps_T = [g.apply_transform(pose.matrix) for g in vaild_grasps] 132 | # mesh_T = mesh.apply_transform(pose.matrix) 133 | # display(mesh_T, vaild_grasps_T) 134 | 135 | # render = dex.ImageRender(mesh, pose, table, config) 136 | # for g in vaild_grasps: 137 | # render.scene.add_grasp(g, render._obj_matrix) 138 | # pyrender.Viewer(render.scene, use_raymond_lighting=True) 139 | # rgb, depth = render.data 140 | # plt.imshow(rgb) 141 | # for g in vaild_grasps: 142 | # plot_2p(*render.render_grasp(g).endpoints) 143 | # plt.show() 144 | # display(mesh_T, vaild_grasps_T) 145 | # for g in vaild_grasps: 146 | # g_2d = render.render_grasp(g) 147 | # out = dex.DataGenerator(rgb, g_2d, config).output 148 | # plt.imshow(out) 149 | # plot_2p([9,16], [23,16]) 150 | # plt.show() 151 | 152 | # plt.figure() 153 | # plt.subplot(1,2,1) 154 | # plt.axis('off') 155 | # plt.imshow(rgb) 156 | # plt.subplot(1,2,2) 157 | # plt.axis('off') 158 | # plt.imshow(depth, cmap=plt.cm.gray_r) 159 | # plt.imshow(depth) 160 | # plt.colorbar() 161 | # plt.show() 162 | 163 | def test_dataset(): 164 | config_logging(TEST_LOG_FILE) 165 | config = load_config(TEST_CFG_FILE) 166 | table = dex.BaseMesh.from_file(TEST_TABLE_FILE) 167 | 168 | f = h5py.File(HDF5_FILE) 169 | obj_group = f[OBJ_GROUP] 170 | obj_name = OBJ_GROUP.split('/')[-1] 171 | dex_obj = dex.DexObject.from_hdf5_group(obj_group, config, obj_name) 172 | # ff = h5py.File(r'H:\test.hdf5', 'a') 173 | # dex_obj.to_hdf5_group(ff, config) 174 | quality = dex_obj.qualitis[:25] 175 | quality_s = (quality - np.min(quality)) / (np.max(quality) - np.min(quality)) 176 | display(dex_obj.mesh, dex_obj.grasps[:25], quality_s) 177 | # display(dex_obj.mesh, dex_obj.grasps[:25]) 178 | # for pose in dex_obj.poses: 179 | # render = dex.ImageRender(dex_obj.mesh, pose, table, config) 180 | # pyrender.Viewer(render.scene, use_raymond_lighting=True) 181 | # mesh = dex_obj.mesh.apply_transform(pose.matrix) 182 | # print(mesh.bounding_box()) 183 | # ff.close() 184 | f.close() 185 | 186 | def test_to_hdf5(): 187 | config_logging(TEST_LOG_FILE) 188 | config = load_config(TEST_CFG_FILE) 189 | 190 | f = h5py.File(r'H:\test.hdf5', 'a') 191 | dex_obj = dex.DexObject.from_file(TEST_OBJ_FILE, config) 192 | dex_obj.to_hdf5_group(f, config) 193 | f.close() 194 | 195 | if __name__ == "__main__": 196 | test_dataset() 197 | -------------------------------------------------------------------------------- /tools/add_obj_to_hdf5.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os.path 3 | import logging 4 | import glob 5 | import h5py 6 | from ruamel.yaml import YAML 7 | 8 | ROOT_PATH = r'H:\Robot\easy-dexnet' 9 | dex_path = os.path.abspath(os.path.join(ROOT_PATH, 'src')) 10 | sys.path.append(dex_path) 11 | try: 12 | import easydexnet as dex 13 | except Exception as e: 14 | print('导入模块失败', e) 15 | 16 | TEST_CFG_FILE = os.path.join(ROOT_PATH, 'config/add_obj.yaml') 17 | 18 | def config_logging(file=None, level=logging.DEBUG): 19 | """ 配置全局的日志设置 """ 20 | LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s" 21 | logging.basicConfig(filename=file, level=level, 22 | format=LOG_FORMAT, filemode='w') 23 | 24 | 25 | def load_config(file): 26 | """ 加载配置文件 """ 27 | yaml = YAML(typ='safe') # default, if not specfied, is 'rt' (round-trip) 28 | with open(file, 'r', encoding="utf-8") as f: 29 | config = yaml.load(f) 30 | return config 31 | 32 | def main(): 33 | config = load_config(TEST_CFG_FILE) 34 | logging_path = config['path']['logging_path'] 35 | hdf5_path = config['path']['hdf5_path'] 36 | obj_path = config['path']['obj_path'] 37 | config_logging(logging_path, logging.INFO) 38 | 39 | data = h5py.File(hdf5_path, 'a') 40 | file_list = glob.glob(os.path.join(obj_path, '*.obj')) 41 | file_list = [os.path.basename(obj) for obj in file_list] 42 | if config['obj_list'] == 'all': 43 | obj_list = file_list 44 | else: 45 | obj_list = [obj+'.obj' for obj in config['obj_list'] 46 | if obj+'.obj' in file_list] 47 | 48 | if not config['recover']: 49 | print(config['recover']) 50 | data_list = list(data.keys()) 51 | obj_list = [obj for obj in obj_list if obj[:-4] not in data_list] 52 | print(obj_list) 53 | 54 | for obj in obj_list: 55 | obj_file = os.path.join(obj_path, obj) 56 | dex_obj = dex.DexObject.from_file(obj_file, config) 57 | dex_obj.to_hdf5_group(data, config) 58 | data.close() 59 | 60 | if __name__ == "__main__": 61 | main() -------------------------------------------------------------------------------- /tools/generate.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os.path 3 | import logging 4 | import h5py 5 | from ruamel.yaml import YAML 6 | 7 | ROOT_PATH = r'H:\Robot\easy-dexnet' 8 | dex_path = os.path.abspath(os.path.join(ROOT_PATH, 'src')) 9 | sys.path.append(dex_path) 10 | try: 11 | import easydexnet as dex 12 | except Exception as e: 13 | print('导入模块失败', e) 14 | 15 | TEST_CFG_FILE = os.path.join(ROOT_PATH, 'config/generate.yaml') 16 | OBJ_GROUP = 'datasets/mini_dexnet/objects/' 17 | DEX_DATA = False 18 | 19 | 20 | 21 | def config_logging(file=None, level=logging.DEBUG): 22 | """ 配置全局的日志设置 """ 23 | LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s" 24 | logging.basicConfig(filename=file, level=level, 25 | format=LOG_FORMAT, filemode='w') 26 | 27 | 28 | def load_config(file): 29 | """ 加载配置文件 """ 30 | yaml = YAML(typ='safe') # default, if not specfied, is 'rt' (round-trip) 31 | with open(file, 'r', encoding="utf-8") as f: 32 | config = yaml.load(f) 33 | return config 34 | 35 | 36 | def main(): 37 | config = load_config(TEST_CFG_FILE) 38 | logging_path = config['path']['logging_path'] 39 | data_path = config['path']['data_path'] 40 | out_path = config['path']['out_path'] 41 | table_path = config['path']['table_path'] 42 | config_logging(logging_path, logging.INFO) 43 | 44 | table = dex.BaseMesh.from_file(table_path) 45 | saver = dex.DexSaver(out_path, config) 46 | 47 | data = h5py.File(data_path, 'a') 48 | if DEX_DATA: 49 | data = data[OBJ_GROUP] 50 | if config['obj_list'] == 'all': 51 | obj_list = data.keys() 52 | else: 53 | obj_list = [obj for obj in config['obj_list'] 54 | if obj in list(data.keys())] 55 | for obj_name in obj_list: 56 | obj_group = data[obj_name] 57 | dex_obj = dex.DexObject.from_hdf5_group(obj_group, config, obj_name) 58 | depth_render = dex.DepthRender(dex_obj, table, saver, config) 59 | depth_render.render() 60 | saver.close() 61 | data.close() 62 | 63 | 64 | if __name__ == "__main__": 65 | main() 66 | --------------------------------------------------------------------------------