├── geolib ├── __init__.py ├── imgui.ini ├── objloader.py ├── cuboid.py ├── bbox.py ├── objcenter.py ├── objfile.py └── coupling.obj ├── urdf_parser_py ├── __init__.py ├── xml_reflection │ ├── __init__.py │ ├── basics.py │ └── core.py ├── test.py ├── sdf.py └── urdf.py ├── imgs ├── example1.png ├── example2.png └── example3.png ├── requirements.txt ├── LICENSE ├── README.md ├── pgm_loader.py ├── inverse_kinematics.py ├── .gitignore ├── vis_lib.py ├── gripper_data └── robotiq3f │ └── robotiq_3f_test.urdf ├── math_utils.py └── gripper_files.py /geolib/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /urdf_parser_py/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /urdf_parser_py/xml_reflection/__init__.py: -------------------------------------------------------------------------------- 1 | from urdf_parser_py.xml_reflection.core import * 2 | -------------------------------------------------------------------------------- /imgs/example1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linsats/Python-Parser-for-Robotic-Gripper/HEAD/imgs/example1.png -------------------------------------------------------------------------------- /imgs/example2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linsats/Python-Parser-for-Robotic-Gripper/HEAD/imgs/example2.png -------------------------------------------------------------------------------- /imgs/example3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linsats/Python-Parser-for-Robotic-Gripper/HEAD/imgs/example3.png -------------------------------------------------------------------------------- /urdf_parser_py/test.py: -------------------------------------------------------------------------------- 1 | import os 2 | from urdf_parser_py import urdf 3 | 4 | xmls = [line for line in open('/home/lins/MetaGrasp/grippers/robotiq_arg85_description.URDF')] 5 | urdf.Robot.from_xml_string(xmls) 6 | 7 | -------------------------------------------------------------------------------- /geolib/imgui.ini: -------------------------------------------------------------------------------- 1 | [Debug] 2 | Pos=60,60 3 | Size=400,400 4 | Collapsed=0 5 | 6 | [Splash Screen Banner] 7 | Pos=522,448 8 | Size=1344,756 9 | Collapsed=0 10 | 11 | [nostreaming_popup] 12 | Pos=692,328 13 | Size=1004,50 14 | Collapsed=0 15 | 16 | [Notification parent window] 17 | Pos=60,60 18 | Size=32,32 19 | Collapsed=0 20 | 21 | [Stream of 3] 22 | Pos=845,146 23 | Size=496,32 24 | Collapsed=0 25 | 26 | [Stream of 0] 27 | Pos=343,146 28 | Size=496,32 29 | Collapsed=0 30 | 31 | [numbered_ruler] 32 | Pos=810,196 33 | Size=32,342 34 | Collapsed=0 35 | 36 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # This file may be used to create an environment using: 2 | # $ conda create --name --file 3 | # platform: linux-64 4 | _libgcc_mutex=0.1=main 5 | ca-certificates=2020.7.22=0 6 | certifi=2018.8.24=py35_1 7 | libedit=3.1.20191231=h14c3975_1 8 | libffi=3.2.1=hd88cf55_4 9 | libgcc-ng=9.1.0=hdf63c60_0 10 | libstdcxx-ng=9.1.0=hdf63c60_0 11 | ncurses=6.2=he6710b0_1 12 | openssl=1.0.2u=h7b6447c_0 13 | pip=10.0.1=py35_0 14 | python=3.5.6=hc3d631a_0 15 | readline=7.0=h7b6447c_5 16 | setuptools=40.2.0=py35_0 17 | sqlite=3.33.0=h62c20be_0 18 | tk=8.6.10=hbc83047_0 19 | wheel=0.35.1=py_0 20 | xz=5.2.5=h7b6447c_0 21 | zlib=1.2.11=h7b6447c_3 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Lin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A Python URDF Parser For Robotic Gripper in UniGrasp Project 2 | UniGrasp: Learning a Unified Model to Grasp with Multifingered Robotic Hands 3 | : https://sites.google.com/view/unigrasp 4 | 5 | ## Functions 6 | 1.Load and Visualize URDF of Robotic Hands. 7 | 8 | 2.Forward Kinematics 9 | 10 | 3.Inverse Kienmatics 11 | 12 | 4.Sample Point Clouds 13 | 14 | 15 | 16 | 17 | ## Get Started 18 | 1.Initialize repository 19 | ``` 20 | git clone https://github.com/linsats/Python-Parser-for-Robotic-Gripper.git 21 | ``` 22 | 23 | 2.Install requirement 24 | ``` 25 | conda create --name gripper --file requirements.txt 26 | ``` 27 | 28 | 3.Activate Conda 29 | ``` 30 | conda activate gripper 31 | ``` 32 | 33 | 4.Run 34 | ``` 35 | python gripper_files.py 36 | 37 | ``` 38 | 39 | 40 | If you think our work is useful, please consider citing use with 41 | ``` 42 | @ARTICLE{8972562, 43 | author={L. {Shao} and F. {Ferreira} and M. {Jorda} and V. {Nambiar} and J. {Luo} and E. {Solowjow} and J. A. {Ojea} and O. {Khatib} and J. {Bohg}}, 44 | journal={IEEE Robotics and Automation Letters}, 45 | title={UniGrasp: Learning a Unified Model to Grasp With Multifingered Robotic Hands}, 46 | year={2020}, 47 | volume={5}, 48 | number={2}, 49 | pages={2286-2293}, 50 | doi={10.1109/LRA.2020.2969946}, 51 | ISSN={2377-3774}, 52 | month={April},} 53 | ``` 54 | 55 | -------------------------------------------------------------------------------- /geolib/objloader.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def obj_loader(file_name,normalize=False): 4 | """It loads the vertices, vertice normals, the faces of a wavefront obj file. 5 | """ 6 | vertices = [] 7 | faces = [] 8 | vnormals = [] 9 | 10 | with open(file_name,'r') as fin: 11 | for line in fin: 12 | if line.startswith('#'): 13 | continue 14 | values = line.split() 15 | if len(values) < 1: 16 | continue 17 | if values[0] == 'v': 18 | v = list(map(float,values[1:4])) 19 | vertices.append(v) 20 | elif values[0] == 'vn': 21 | vn = list(map(float,values[1:4])) 22 | vnormals.append(vn) 23 | elif values[0] == 'f': 24 | face = [] 25 | for v in values[1:]: 26 | w = v.split('/') 27 | face.append(int(w[0])) 28 | faces.append(face) 29 | 30 | vertices = np.array(vertices) 31 | faces = np.array(faces) 32 | if len(vnormals): 33 | vnormals = np.array(vnormals) 34 | faces = faces-1 35 | if normalize: 36 | bbox_max = np.max(vertices,axis=0) 37 | bbox_min = np.min(vertices,axis=0) 38 | bbox_center = 0.5 * (bbox_max + bbox_min) 39 | bbox_rad = np.linalg.norm(bbox_max - bbox_center) 40 | vertices -= bbox_center 41 | vertices /= (bbox_rad*2.0) 42 | if np.any(faces < 0): 43 | print('Negative face indexing in obj file') 44 | 45 | return vertices, faces, vnormals 46 | 47 | if __name__ == "__main__": 48 | v,f,vn = obj_loader('/Users/lins/GraspNet/mesh_processing/reorient_faces_coherently/vase.obj',normalize=True) 49 | -------------------------------------------------------------------------------- /pgm_loader.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.ndimage 3 | 4 | def read_pgm_xyz(filename): 5 | """Return image data from a PGM file generated by blensor. """ 6 | fx = 472.92840576171875 7 | fy = fx 8 | with open(filename, 'rb') as f: 9 | f.readline() 10 | f.readline() 11 | width_height = f.readline().strip().split() 12 | if len(width_height) > 1: 13 | width, height = map(int,width_height) 14 | print("width",width,height) 15 | value_max_range = float(f.readline()) 16 | image_ = [float(line.strip()) for line in f.readlines()] 17 | if len(image_) == height * width: 18 | nx,ny = (width,height) 19 | x_index = np.linspace(0,width-1,width) 20 | y_index = np.linspace(0,height-1,height) 21 | xx,yy = np.meshgrid(x_index,y_index) 22 | xx -= float(width)/2 23 | yy -= float(height)/2 24 | xx /= fx 25 | yy /= fy 26 | 27 | cam_z = np.reshape(image_,(height, width)) 28 | cam_z = cam_z / value_max_range * 1.5 29 | cam_x = xx * cam_z 30 | cam_y = yy * cam_z 31 | image_z = np.flipud(cam_z) 32 | image_y = np.flipud(cam_y) 33 | image_x = np.flipud(cam_x) 34 | image_z = image_z[240-120:240+120, 320-160:320+160] 35 | image_x = image_x[240-120:240+120, 320-160:320+160] 36 | image_y = image_y[240-120:240+120, 320-160:320+160] 37 | 38 | zoom_scale = 1#0.25 39 | image_x = scipy.ndimage.zoom(image_x, zoom_scale, order=1) 40 | image_y = scipy.ndimage.zoom(image_y, zoom_scale, order=1) 41 | image_z = scipy.ndimage.zoom(image_z, zoom_scale, order=1) 42 | image = np.dstack((image_x,image_y,image_z)) 43 | print("image",image.shape) 44 | return image 45 | 46 | return np.zeros((480,640,3)) 47 | 48 | 49 | -------------------------------------------------------------------------------- /inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | import scipy.optimize 2 | import numpy as np 3 | 4 | def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, regularization_parameter=None,max_iter=None,rot=False): 5 | """Compute the inverse kinematic on the specified target with an optimization method. 6 | Parameters: 7 | --- 8 | chain: ikpy.chain.Chain: The chain used for the Inverse kinematics. 9 | target: numpy.array : The desired target. 10 | starting_nodes_angles: numpy.array : The initial pose of your chain. 11 | regualarization_parameter: float : The coefficient of the regularization. 12 | max_iter: int: Maximum number of iterations for the optimization algorithm.. 13 | """ 14 | # Only get the position 15 | target = target_frame[:3,3] 16 | # Compute squared distance to target: 17 | if not rot: 18 | def optimize_target(x): 19 | #y = chain.active_to_full(x, starting_nodes_angles) 20 | squared_distance = np.linalg.norm(chain.forward_kinematics(x)[:3,3] - target) 21 | return squared_distance 22 | else: 23 | def optimize_target(x): 24 | cal_frame = chain.forward_kinematics(x) 25 | squared_distance = np.linalg.norm(cal_frame[:3,3] - target_frame[:3,3]) 26 | squared_rot = np.linalg.norm(cal_frame[:3,:3] - target_frame[:3,3]) 27 | return squared_distance + squared_rot 28 | 29 | # If a regularization is selected 30 | if regularization_parameter is not None: 31 | def optimize_total(x): 32 | regularization = np.linalg.norm(x - starting_nodes_angles[chain.first_active_joint:]) 33 | return optimize_target(x) + regularization_parameter * regularization 34 | else: 35 | def optimize_total(x): 36 | return optimize_target(x) 37 | 38 | # Compute bounds 39 | real_bounds = [] 40 | real_bounds.append((0,0)) 41 | for joint in chain.joints[1:]: 42 | if joint.limit is not None: 43 | real_bounds.append((joint.limit.lower,joint.limit.upper)) 44 | else: 45 | real_bounds.append((0,0)) 46 | 47 | print(real_bounds) 48 | # real_bounds 49 | real_bounds = chain.active_from_full(real_bounds) 50 | 51 | options = {} 52 | # Manage iterations maximum 53 | if max_iter is not None: 54 | options["maxiter"] = max_iter 55 | 56 | # Utilisation 57 | res = scipy.optimize.minimize(optimize_total, chain.active_from_full(starting_nodes_angles),method="L-BFGS-B",bounds=real_bounds) 58 | return res.x 59 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /urdf_parser_py/xml_reflection/basics.py: -------------------------------------------------------------------------------- 1 | import string 2 | import yaml 3 | import collections 4 | from lxml import etree 5 | 6 | # Different implementations mix well it seems 7 | # @todo Do not use this? 8 | from xml.etree.ElementTree import ElementTree 9 | 10 | 11 | def xml_string(rootXml, addHeader=True): 12 | # Meh 13 | xmlString = etree.tostring(rootXml, pretty_print=True) 14 | if addHeader: 15 | xmlString = '\n' + xmlString 16 | return xmlString 17 | 18 | 19 | def dict_sub(obj, keys): 20 | return dict((key, obj[key]) for key in keys) 21 | 22 | 23 | def node_add(doc, sub): 24 | if sub is None: 25 | return None 26 | if type(sub) == str: 27 | return etree.SubElement(doc, sub) 28 | elif isinstance(sub, etree._Element): 29 | doc.append(sub) # This screws up the rest of the tree for prettyprint 30 | return sub 31 | else: 32 | raise Exception('Invalid sub value') 33 | 34 | 35 | def pfloat(x): 36 | return str(x).rstrip('.') 37 | 38 | 39 | def xml_children(node): 40 | children = node.getchildren() 41 | 42 | def predicate(node): 43 | return not isinstance(node, etree._Comment) 44 | return list(filter(predicate, children)) 45 | 46 | 47 | def isstring(obj): 48 | try: 49 | return isinstance(obj, basestring) 50 | except NameError: 51 | return isinstance(obj, str) 52 | 53 | 54 | def to_yaml(obj): 55 | """ Simplify yaml representation for pretty printing """ 56 | # Is there a better way to do this by adding a representation with 57 | # yaml.Dumper? 58 | # Ordered dict: http://pyyaml.org/ticket/29#comment:11 59 | if obj is None or isstring(obj): 60 | out = str(obj) 61 | elif type(obj) in [int, float, bool]: 62 | return obj 63 | elif hasattr(obj, 'to_yaml'): 64 | out = obj.to_yaml() 65 | elif isinstance(obj, etree._Element): 66 | out = etree.tostring(obj, pretty_print=True) 67 | elif type(obj) == dict: 68 | out = {} 69 | for (var, value) in obj.items(): 70 | out[str(var)] = to_yaml(value) 71 | elif hasattr(obj, 'tolist'): 72 | # For numpy objects 73 | out = to_yaml(obj.tolist()) 74 | elif isinstance(obj, collections.Iterable): 75 | out = [to_yaml(item) for item in obj] 76 | else: 77 | out = str(obj) 78 | return out 79 | 80 | 81 | class SelectiveReflection(object): 82 | def get_refl_vars(self): 83 | return list(vars(self).keys()) 84 | 85 | 86 | class YamlReflection(SelectiveReflection): 87 | def to_yaml(self): 88 | raw = dict((var, getattr(self, var)) for var in self.get_refl_vars()) 89 | return to_yaml(raw) 90 | 91 | def __str__(self): 92 | # Good idea? Will it remove other important things? 93 | return yaml.dump(self.to_yaml()).rstrip() 94 | -------------------------------------------------------------------------------- /urdf_parser_py/sdf.py: -------------------------------------------------------------------------------- 1 | from urdf_parser_py.xml_reflection.basics import * 2 | import urdf_parser_py.xml_reflection as xmlr 3 | 4 | # What is the scope of plugins? Model, World, Sensor? 5 | 6 | xmlr.start_namespace('sdf') 7 | 8 | 9 | class Pose(xmlr.Object): 10 | def __init__(self, vec=None, extra=None): 11 | self.xyz = None 12 | self.rpy = None 13 | if vec is not None: 14 | assert isinstance(vec, list) 15 | count = len(vec) 16 | if len == 3: 17 | xyz = vec 18 | else: 19 | self.from_vec(vec) 20 | elif extra is not None: 21 | assert xyz is None, "Cannot specify 6-length vector and 3-length vector" # noqa 22 | assert len(extra) == 3, "Invalid length" 23 | self.rpy = extra 24 | 25 | def from_vec(self, vec): 26 | assert len(vec) == 6, "Invalid length" 27 | self.xyz = vec[:3] 28 | self.rpy = vec[3:6] 29 | 30 | def as_vec(self): 31 | xyz = self.xyz if self.xyz else [0, 0, 0] 32 | rpy = self.rpy if self.rpy else [0, 0, 0] 33 | return xyz + rpy 34 | 35 | def read_xml(self, node): 36 | # Better way to do this? Define type? 37 | vec = get_type('vector6').read_xml(node) 38 | self.load_vec(vec) 39 | 40 | def write_xml(self, node): 41 | vec = self.as_vec() 42 | get_type('vector6').write_xml(node, vec) 43 | 44 | def check_valid(self): 45 | assert self.xyz is not None or self.rpy is not None 46 | 47 | 48 | name_attribute = xmlr.Attribute('name', str) 49 | pose_element = xmlr.Element('pose', Pose, False) 50 | 51 | 52 | class Entity(xmlr.Object): 53 | def __init__(self, name=None, pose=None): 54 | self.name = name 55 | self.pose = pose 56 | 57 | 58 | xmlr.reflect(Entity, params=[ 59 | name_attribute, 60 | pose_element 61 | ]) 62 | 63 | 64 | class Inertia(xmlr.Object): 65 | KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] 66 | 67 | def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): 68 | self.ixx = ixx 69 | self.ixy = ixy 70 | self.ixz = ixz 71 | self.iyy = iyy 72 | self.iyz = iyz 73 | self.izz = izz 74 | 75 | def to_matrix(self): 76 | return [ 77 | [self.ixx, self.ixy, self.ixz], 78 | [self.ixy, self.iyy, self.iyz], 79 | [self.ixz, self.iyz, self.izz]] 80 | 81 | 82 | xmlr.reflect(Inertia, 83 | params=[xmlr.Element(key, float) for key in Inertia.KEYS]) 84 | 85 | # Pretty much copy-paste... Better method? 86 | # Use multiple inheritance to separate the objects out so they are unique? 87 | 88 | 89 | class Inertial(xmlr.Object): 90 | def __init__(self, mass=0.0, inertia=None, pose=None): 91 | self.mass = mass 92 | self.inertia = inertia 93 | self.pose = pose 94 | 95 | 96 | xmlr.reflect(Inertial, params=[ 97 | xmlr.Element('mass', float), 98 | xmlr.Element('inertia', Inertia), 99 | pose_element 100 | ]) 101 | 102 | 103 | class Link(Entity): 104 | def __init__(self, name=None, pose=None, inertial=None, kinematic=False): 105 | Entity.__init__(self, name, pose) 106 | self.inertial = inertial 107 | self.kinematic = kinematic 108 | 109 | 110 | xmlr.reflect(Link, parent_cls=Entity, params=[ 111 | xmlr.Element('inertial', Inertial), 112 | xmlr.Attribute('kinematic', bool, False), 113 | xmlr.AggregateElement('visual', Visual, var='visuals'), 114 | xmlr.AggregateElement('collision', Collision, var='collisions') 115 | ]) 116 | 117 | 118 | class Model(Entity): 119 | def __init__(self, name=None, pose=None): 120 | Entity.__init__(self, name, pose) 121 | self.links = [] 122 | self.joints = [] 123 | self.plugins = [] 124 | 125 | 126 | xmlr.reflect(Model, parent_cls=Entity, params=[ 127 | xmlr.AggregateElement('link', Link, var='links'), 128 | xmlr.AggregateElement('joint', Joint, var='joints'), 129 | xmlr.AggregateElement('plugin', Plugin, var='plugins') 130 | ]) 131 | 132 | xmlr.end_namespace('sdf') 133 | -------------------------------------------------------------------------------- /geolib/cuboid.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | 5 | class Cuboid(object): 6 | def __init__(self,extrema=None,corner_points=None): 7 | """ Constructor: 8 | @params: extrema: A numpy array with shape (6,1) [xmin,ymin,zmin,xmax,ymax,zmax] 9 | @params: corner_points: A numpy array with shape (8,3) 10 | """ 11 | if extrema is not None: 12 | self.extrema = extrema 13 | 14 | if corner_points is not None: 15 | extrema = np.zeros((6,1)) 16 | extrema[0:3,0] 17 | extrema[0:3,0]=np.min(corner_points,axis=0) 18 | extrema[3:6,0]=np.max(corner_points,axis=0) 19 | self.extrema = extrema 20 | 21 | self.corner_points = self._corner_points() 22 | 23 | def _corner_points(self): 24 | [xmin, ymin, zmin, xmax, ymax, zmax] = self.extrema 25 | c1 = np.array([xmin, ymin, zmin]) 26 | c2 = np.array([xmax, ymin, zmin]) 27 | c3 = np.array([xmax, ymax, zmin]) 28 | c4 = np.array([xmin, ymax, zmin]) 29 | c5 = np.array([xmin, ymin, zmax]) 30 | c6 = np.array([xmax, ymin, zmax]) 31 | c7 = np.array([xmax, ymax, zmax]) 32 | c8 = np.array([xmin, ymax, zmax]) 33 | return np.vstack([c1, c2, c3, c4, c5, c6, c7, c8]) 34 | 35 | def diagonal_length(self): 36 | return np.linalg.norm(self.extrema[:3] - self.extrema[3:]) 37 | 38 | def center(self): 39 | return np.sum(self._corner_points(), axis=0) / 8.0 40 | 41 | def is_point_inside(self, point): 42 | [xmin, ymin, zmin, xmax, ymax, zmax] = self.extrema 43 | tmp_p = np.copy(point).reshape((3,)) 44 | xp,yp,zp = tmp_p 45 | return xmin < xp and ymin < yp and zmin < zp and xmax > xp and ymax > yp and zmax > zp 46 | 47 | def are_points_inside(self, points): 48 | [xmin, ymin, zmin, xmax, ymax, zmax] = np.array(self.extrema).reshape((6,)) 49 | tmp_p = np.copy(points).reshape((-1,3)) 50 | xp = tmp_p[:,0] 51 | yp = tmp_p[:,1] 52 | zp = tmp_p[:,2] 53 | index = np.array([xmin < xp, ymin < yp, zmin < zp, xmax > xp, ymax > yp, zmax >zp]).transpose() 54 | return np.all(index,axis=1) 55 | 56 | def are_plane_intersect_v2(self, plane): 57 | plane_center = np.mean(plane,axis=0) 58 | cuboid_center_tmp = np.mean(self._corner_points(),axis=0) 59 | plane_normal = np.cross((plane[0]-plane[1]),(plane[0]-plane[2])) 60 | plane_normal = plane_normal/(1e-8+np.linalg.norm(plane_normal)) 61 | cuboid_center_plane_center = cuboid_center_tmp - plane_center 62 | ortho_com = np.sum(cuboid_center_plane_center * plane_normal) 63 | cuboid_center_2_plane_center = cuboid_center_plane_center - ortho_com * plane_normal 64 | cuboid_center_in_plane = plane_center + cuboid_center_2_plane_center 65 | tmp_corners = self._corner_points() - cuboid_center_in_plane 66 | tmp_corners = tmp_corners / (1e-8 +np.linalg.norm(tmp_corners,axis=1,keepdims=True)) 67 | flag = np.sign(np.sum(tmp_corners * plane_normal,axis=1)) 68 | if np.all(flag > 0) or np.all(flag < 0): 69 | return False 70 | else: 71 | return True 72 | 73 | def are_plane_intersect(self, plane): 74 | plane_center = np.mean(plane,axis=0) 75 | plane_normal = np.cross((plane[0]-plane[1]),(plane[0]-plane[2])) 76 | tmp_corners = self._corner_points() - plane_center 77 | tmp_corners = tmp_corners / (1e-8 +np.linalg.norm(tmp_corners,axis=1,keepdims=True)) 78 | flag = np.sign(np.sum(tmp_corners * plane_normal,axis=1)) 79 | if np.all(flag > 0) or np.all(flag < 0): 80 | return False 81 | else: 82 | return True 83 | 84 | def volume(self): 85 | [xmin, ymin, zmin, xmax, ymax, zmax] = self.extrema.reshape((6,)) 86 | return (xmax - xmin) * (ymax - ymin) * (zmax - zmin) 87 | 88 | def dim(self): 89 | [xmin, ymin, zmin, xmax, ymax, zmax] = self.extrema.reshape((6,)) 90 | return [xmax-xmin,ymax-ymin,zmax-zmin] 91 | 92 | if __name__ == '__main__': 93 | c = Cuboid(extrema=[-1.0,-1.0,0.0,1.0,1.0,1.0]) 94 | table = np.zeros((4,3)) 95 | table[0] = np.array([-0.1,0.0,-0.01]) 96 | table[1] = np.array([0.1,0.0,-0.01]) 97 | table[2] = np.array([0.0,-0.1,-0.01]) 98 | table[3] = np.array([0.0,0.1,-0.01]) 99 | print(c.are_plane_intersect(table)) 100 | -------------------------------------------------------------------------------- /vis_lib.py: -------------------------------------------------------------------------------- 1 | from mayavi import mlab as mayalab 2 | import numpy as np 3 | import os 4 | 5 | def plot_pc(pcs,color=None,scale_factor=.05,mode='point'): 6 | if color == 'red': 7 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(1,0,0)) 8 | print("color",color) 9 | elif color == 'blue': 10 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,0,1)) 11 | elif color == 'green': 12 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,0)) 13 | elif color == 'ycan': 14 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,1)) 15 | else: 16 | print("unkown color") 17 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=color) 18 | 19 | 20 | def plot_pc_with_normal(pcs,pcs_n,scale_factor=1.0,color='red'): 21 | if color == 'red': 22 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], color=(1,0,0), mode='arrow',scale_factor=1.0) 23 | elif color == 'blue': 24 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], color=(0,0,1), mode='arrow',scale_factor=1.0) 25 | elif color == 'green': 26 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], color=(0,1,0), mode='arrow',scale_factor=1.0) 27 | 28 | 29 | def plot_origin(): 30 | origin_pc = np.array([0.0,0.0,0.0]).reshape((-1,3)) 31 | plot_pc(origin_pc,color='ycan',mode='sphere',scale_factor=.01) 32 | origin_pcs = np.tile(origin_pc,(3,1)) 33 | origin_pcns = np.eye(3) * 0.01 34 | plot_pc_with_normal(origin_pcs,origin_pcns) 35 | 36 | 37 | if __name__ == '__main__': 38 | #save_dir = '/home/lins/MetaGrasp/Data/BlensorResult/2056' 39 | #gripper_name = '056_rho0.384015_azi1.000000_ele89.505854_theta0.092894_xcam0.000000_ycam0.000000_zcam0.384015_scale0.146439_xdim0.084960_ydim0.084567_zdim0.08411000000_pcn_new.npz.npy' 40 | 41 | #gripper_name ='339_rho0.308024_azi6.000000_ele89.850030_theta-0.013403_xcam0.000000_ycam0.000000_zcam0.308024_scale0.061975_xdim0.048725_ydim0.036192_zdim0.01252500000_pcn.npz' 42 | gripper = np.load(os.path.join("robotiq2f_open.npy")) 43 | #plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.002) 44 | plot_pc(gripper,color=(209/255.0,64/255.0,109/255.0),mode='sphere',scale_factor=0.002) 45 | plot_origin() 46 | mayalab.show() 47 | #sle = np.array([1494,1806]) 48 | #plot_pc(gripper[sle],color='red',mode='sphere',scale_factor=0.002) 49 | #mayalab.show() 50 | 51 | #save_dir = '/home/lins/MetaGrasp/meta_grasping/saved_results/interp' 52 | #save_dir = '/home/lins/MetaGrasp/Data/Gripper/Data3' 53 | # #save_dir_gt = '/home/lins/MetaGrasp/Data/Gripper/Data' 54 | 55 | save_dir = '/home/lins/MetaGrasp/Data/Gripper/Data_DB/G5/f2_5_close.npy' 56 | a = np.load(save_dir) 57 | plot_pc(a) 58 | save_dirb = '/home/lins/MetaGrasp/Data/Gripper/Data_DB/G3/f2_3_close.npy' 59 | b = np.load(save_dirb) 60 | plot_pc(b,color='red') 61 | mayalab.show() 62 | #for i in range(10001,10300): 63 | # gripper_name = 'f2_'+str(i)+'_middel.npy' 64 | #print(gripper_name) 65 | # gripper = np.load(os.path.join(save_dir,gripper_name)) 66 | # plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.002) 67 | # plot_origin() 68 | # mayalab.show() 69 | #save_dir_gt = '/home/lins/MetaGrasp/Data/Gripper/Data' 70 | #gripper_gt = np.load(os.path.join(save_dir_gt,gripper_name)) 71 | #plot_pc(gripper_gt,color='red',mode='sphere',scale_factor=0.002) 72 | 73 | if 0: 74 | for i in range(0,199): 75 | save_dir = '/home/lins/MetaGrasp/Data/Gripper/Data_noR' 76 | #save_dir = '/home/lins/MetaGrasp/meta_grasping/saved_results/recon_old' 77 | gripper_name = 'robotiq_3f_'+str(i)+'.npy' 78 | print(gripper_name) 79 | gripper = np.load(os.path.join(save_dir,gripper_name)) 80 | plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 81 | 82 | plot_origin() 83 | mayalab.show() 84 | 85 | 86 | if 0: 87 | save_dir = '/home/lins/MetaGrasp/meta_grasping/saved_results/interp' 88 | gripper_name = 'kinova_kg3_0.npy' 89 | print(gripper_name) 90 | gripper = np.load(os.path.join(save_dir,gripper_name)) 91 | plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 92 | 93 | plot_origin() 94 | mayalab.show() 95 | 96 | gripper_name = 'robotiq_3f_1.npy' 97 | print(gripper_name) 98 | gripper = np.load(os.path.join(save_dir,gripper_name)) 99 | plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 100 | plot_origin() 101 | mayalab.show() 102 | 103 | save_dir = '/home/lins/MetaGrasp/meta_grasping/saved_results/interp' 104 | gripper_name = 'middle0.npy' 105 | print(gripper_name) 106 | gripper = np.load(os.path.join(save_dir,gripper_name)) 107 | plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 108 | 109 | plot_origin() 110 | mayalab.show() 111 | 112 | gripper_name = 'middle1.npy' 113 | print(gripper_name) 114 | gripper = np.load(os.path.join(save_dir,gripper_name)) 115 | plot_pc(gripper,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 116 | plot_origin() 117 | mayalab.show() 118 | 119 | save_dir = '/home/lins/MetaGrasp/Data/Gripper/Data_noR' 120 | gripper_name1 = 'kinova_kg3_0.npy' 121 | print(gripper_name) 122 | gripper1 = np.load(os.path.join(save_dir,gripper_name1)) 123 | plot_pc(gripper1,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 124 | plot_origin() 125 | mayalab.show() 126 | 127 | gripper_name2 = 'robotiq_3f_1.npy' 128 | print(gripper_name) 129 | gripper2 = np.load(os.path.join(save_dir,gripper_name2)) 130 | plot_pc(gripper2,color=(139/255.0,177/255.0,212/255.0),mode='sphere',scale_factor=0.01) 131 | 132 | plot_origin() 133 | mayalab.show() 134 | -------------------------------------------------------------------------------- /geolib/bbox.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mayavi import mlab as mayalab 3 | 4 | def plot_pc_with_normal(pcs,pcs_n,scale_factor=1.0): 5 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], mode='arrow',scale_factor=1.0) 6 | 7 | def plot_pc(pcs,color=None,scale_factor=.05,mode='point'): 8 | if color == 'r': 9 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(1,0,0)) 10 | elif color == 'blue': 11 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,0,1)) 12 | elif color == 'green': 13 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,0)) 14 | elif color == 'ycan': 15 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,1)) 16 | else: 17 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor) 18 | 19 | 20 | class Bbox(object): 21 | def __init__(self,extrema=None,corner_points=None,frame_length=None): 22 | """ 23 | z -x 24 | | / 25 | | / 26 | | / 27 | 0-----> y 28 | 29 | c5 30 | | --/--> c8 31 | | / 32 | | / 33 | c6 c1-----> c4 34 | | / 35 | | / 36 | |/ 37 | c2 -----> c3 38 | 39 | """ 40 | if extrema is not None: 41 | [xmin, ymin, zmin, xmax, ymax, zmax] = extrema 42 | self.c1 = np.array([xmin, ymin, zmin]) 43 | self.c2 = np.array([xmax, ymin, zmin]) 44 | self.c3 = np.array([xmax, ymax, zmin]) 45 | self.c4 = np.array([xmin, ymax, zmin]) 46 | self.c5 = np.array([xmin, ymin, zmax]) 47 | self.c6 = np.array([xmax, ymin, zmax]) 48 | self.c7 = np.array([xmax, ymax, zmax]) 49 | self.c8 = np.array([xmin, ymax, zmax]) 50 | self.corner_points = np.vstack([self.c1,self.c2,self.c3,self.c4,self.c5,self.c6,self.c7,self.c8]) 51 | self.frame_length = np.array([xmax-xmin,ymax-ymin,zmax-zmin]) 52 | else: 53 | self.corner_points = corner_points 54 | self.frame_length = frame_length 55 | self.frame_rot = np.eye(4) 56 | self.transformation(np.eye(4)) 57 | 58 | def transformation(self,T): 59 | self.corner_points_4d = np.hstack([np.copy(self.corner_points),np.ones((8,1))]) 60 | self.corner_points_transformed = self.corner_points_4d.dot(T.transpose())[:,0:3] 61 | 62 | def is_point_inside(self, point, transformed=True): 63 | if transformed: 64 | self.basis = np.zeros((3,3)) 65 | self.basis[:,0] = self.corner_points_transformed[1]-self.corner_points_transformed[0] 66 | self.basis[:,1] = self.corner_points_transformed[3]-self.corner_points_transformed[0] 67 | self.basis[:,2] = self.corner_points_transformed[4]-self.corner_points_transformed[0] 68 | self.basis[:,0] = self.basis[:,0] / (np.linalg.norm(self.basis[:,0]) + 1e-16) 69 | self.basis[:,1] = self.basis[:,1] / (np.linalg.norm(self.basis[:,1]) + 1e-16) 70 | self.basis[:,2] = self.basis[:,2] / (np.linalg.norm(self.basis[:,2]) + 1e-16) 71 | 72 | point = point - self.corner_points_transformed[0] 73 | point = point.dot(self.basis) 74 | if point[0] < self.frame_length[0] and point[1] < self.frame_length[1] and point[2] < self.frame_length[2] and point[0] > 0 and point[1] > 0 and point[2] > 0: 75 | return True 76 | else: 77 | return False 78 | 79 | def points_inside(self, points_o, transformed=True): 80 | if transformed: 81 | self.basis = np.zeros((3,3)) 82 | self.basis[:,0] = self.corner_points_transformed[1]-self.corner_points_transformed[0] 83 | self.basis[:,1] = self.corner_points_transformed[3]-self.corner_points_transformed[0] 84 | self.basis[:,2] = self.corner_points_transformed[4]-self.corner_points_transformed[0] 85 | self.basis[:,0] = self.basis[:,0] / (np.linalg.norm(self.basis[:,0]) + 1e-16) 86 | self.basis[:,1] = self.basis[:,1] / (np.linalg.norm(self.basis[:,1]) + 1e-16) 87 | self.basis[:,2] = self.basis[:,2] / (np.linalg.norm(self.basis[:,2]) + 1e-16) 88 | 89 | transformed_o = np.copy(self.corner_points_transformed[0]).reshape((1,3)) 90 | 91 | points = points_o - transformed_o 92 | points = points.dot(self.basis) 93 | points_check = np.hstack([points[:,0:1] < self.frame_length[0], points[:,1:2] < self.frame_length[1] , points[:,2:3] < self.frame_length[2] , points[:,0:1] > 0 ,points[:,1:2] > 0, points[:,2:3] > 0]) 94 | points_flag = np.all(points_check,axis=1) 95 | if np.any(points_flag): 96 | #pct = np.vstack([transformed_o,transformed_o,transformed_o]) 97 | #plot_pc_with_normal(pct,self.basis.transpose() * 0.01) 98 | #plot_pc(self.corner_points_transformed,color='ycan',scale_factor=0.01,mode='sphere') 99 | #plot_pc(points_o[points_flag],color='r',scale_factor=0.01,mode='sphere') 100 | #print(points_check[points_flag]) 101 | #print(points[points_flag]) 102 | return True 103 | else: 104 | return False 105 | 106 | def rect_intersect(self,table=None): 107 | if np.any(self.corner_points_transformed[:,2] < 0.0): 108 | return True 109 | else: 110 | return False 111 | 112 | def plot(self,transformed=True): 113 | if transformed: 114 | p1 = self.corner_points_transformed[0] 115 | p2 = self.corner_points_transformed[1] 116 | p3 = self.corner_points_transformed[2] 117 | p4 = self.corner_points_transformed[3] 118 | 119 | p5 = self.corner_points_transformed[0] 120 | p6 = self.corner_points_transformed[1] 121 | p7 = self.corner_points_transformed[2] 122 | p8 = self.corner_points_transformed[3] 123 | 124 | p9 = self.corner_points_transformed[4] 125 | p10 = self.corner_points_transformed[5] 126 | p11 = self.corner_points_transformed[6] 127 | p12 = self.corner_points_transformed[7] 128 | 129 | c1 = p1 - p2 130 | c2 = p2 - p3 131 | c3 = p3 - p4 132 | c4 = p4 - p1 133 | 134 | c5 = p1 - p9 135 | c6 = p2 - p10 136 | c7 = p3 - p11 137 | c8 = p4 - p12 138 | 139 | c9 = p9 - p10 140 | c10 = p10 - p11 141 | c11 = p11 - p12 142 | c12 = p12 - p9 143 | 144 | ps = np.vstack([p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12]) 145 | cs = np.vstack([c1,c2,c3,c4,c5,c6,c7,c8,c9,c10,c11,c12]) 146 | cs = cs * -1.0 147 | mayalab.quiver3d(ps[:,0],ps[:,1],ps[:,2],cs[:,0],cs[:,1],cs[:,2],mode='2ddash',scale_factor=1.0) 148 | 149 | 150 | 151 | if __name__ == '__main__': 152 | tmp = Bbox(extrema=np.array([-1.0,-1.0,-1.0,1.0,1.0,1.0])*0.1) 153 | tmp.transformation(np.eye(4)) 154 | print(tmp.is_point_inside(np.array([0.0,0,0]))) 155 | #tmp.plot() 156 | #mayalab.show() 157 | -------------------------------------------------------------------------------- /gripper_data/robotiq3f/robotiq_3f_test.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | -------------------------------------------------------------------------------- /geolib/objcenter.py: -------------------------------------------------------------------------------- 1 | import warnings 2 | import numpy as np 3 | from objloader import obj_loader 4 | from math import sin, cos 5 | 6 | def plot_pc(pcs,color=None,scale_factor=.05,mode='point'): 7 | if color == 'red': 8 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(1,0,0)) 9 | elif color == 'blue': 10 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,0,1)) 11 | elif color == 'green': 12 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,0)) 13 | elif color == 'ycan': 14 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,1)) 15 | else: 16 | print("unkown color") 17 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=color) 18 | 19 | def plot_pc_with_normal(pcs,pcs_n,scale_factor=1.0): 20 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], mode='arrow',scale_factor=1.0) 21 | 22 | 23 | def plot_origin(): 24 | origin_pc = np.array([0.0,0.0,0.0]).reshape((-1,3)) 25 | plot_pc(origin_pc,color='ycan',mode='sphere',scale_factor=.01) 26 | origin_pcs = np.tile(origin_pc,(3,1)) 27 | origin_pcns = np.eye(3) * 0.01 28 | plot_pc_with_normal(origin_pcs,origin_pcns) 29 | 30 | 31 | try: 32 | from mayavi import mlab as mayalab 33 | except: 34 | warnings.warn('Mayavi library was not found.') 35 | 36 | class OBJ(object): 37 | def __init__(self, vertices=None, faces=None, vnormals=None, normal_normalized=True,normalize=False,file_name=None,scale=1.0,rotation_degree=0): 38 | self.vertices = None 39 | self.faces = None 40 | self.vnormals = None 41 | self.fnormals = None 42 | self.fareas = None 43 | self.seed = 42 44 | self.normalize = normalize 45 | 46 | if file_name is not None: 47 | self.vertices, self.faces, self.vnormals = obj_loader(file_name,self.normalize)[:3] 48 | else: 49 | self.vertices = vertices 50 | self.faces = faces 51 | self.vnormals = vnormals 52 | 53 | if abs(scale - 1.0) > 0.0000001: 54 | self.vertices *= scale 55 | 56 | if abs(rotation_degree - 0.0) > 0.0001: 57 | new_x = self.vertices[:,0]*cos(rotation_degree / 180.0 * np.pi) + self.vertices[:,1]*sin(rotation_degree / 180.0 * np.pi) 58 | new_y = self.vertices[:,0]*sin(rotation_degree / 180.0 * np.pi) - self.vertices[:,1]*cos(rotation_degree / 180.0 * np.pi) 59 | self.vertices = np.vstack([new_x,new_y*-1.0,self.vertices[:,2]]).T 60 | 61 | self.num_points = len(self.vertices) 62 | self.num_faces = len(self.faces) 63 | 64 | if normal_normalized: 65 | self.cal_vnormals() 66 | 67 | def centers(self): 68 | return np.mean(self.vertices,axis=0) 69 | 70 | def cal_fareas(self): 71 | self.fareas = 0.5 * np.linalg.norm( np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]),axis=1) 72 | return np.copy(self.fareas) 73 | 74 | def cal_fnormals(self): 75 | N = np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]) 76 | row_norms = np.linalg.norm(N,axis=1) 77 | self.fareas = 0.5 * row_norms 78 | N = (N.T / row_norms).T 79 | self.fnormals = N 80 | 81 | def cal_vnormals(self): 82 | if self.vnormals.shape != self.vertices.shape: 83 | N = np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]) 84 | self.vnormals = np.empty_like(self.vertices) 85 | self.vnormals[self.faces[:,0],:] = N 86 | self.vnormals[self.faces[:,1],:] += N 87 | self.vnormals[self.faces[:,2],:] += N 88 | row_norms = np.linalg.norm(self.vnormals,axis=1) 89 | self.vnormals = (self.vnormals.T / row_norms).T 90 | 91 | def write(self,file_name): 92 | self.faces = self.faces + 1 93 | with open(file_name,'w') as fw: 94 | for v in self.vertices: 95 | fw.write('v %f %f %f\n' % (v[0],v[1],v[2])) 96 | 97 | if self.vnormals.shape == self.vertices.shape: 98 | for vn in self.vnormals: 99 | fw.write('vn %f %f %f\n' % (vn[0],vn[1],vn[2])) 100 | 101 | for f in self.faces: 102 | fw.write('f %d %d %d\n' % (f[0],f[1],f[2])) 103 | 104 | def sample_points(self,n_samples,seed=None,with_normal=False): 105 | if seed is not None: 106 | np.random.seed(seed) 107 | else: 108 | np.random.seed(self.seed) 109 | 110 | face_areas = self.cal_fareas() 111 | face_areas = face_areas / np.sum(face_areas) 112 | 113 | n_samples_per_face = np.round(n_samples * face_areas) 114 | 115 | n_samples_per_face = n_samples_per_face.astype(np.int) 116 | n_samples_s = int(np.sum(n_samples_per_face)) 117 | 118 | diff = n_samples_s - n_samples 119 | indices = np.arange(self.num_faces) 120 | 121 | if diff > 0: 122 | rand_faces = np.random.choice(indices[n_samples_per_face >= 1], abs(diff), replace=False) 123 | n_samples_per_face[rand_faces] = n_samples_per_face[rand_faces] - 1 124 | elif diff < 0: 125 | rand_faces = np.random.choice(indices, abs(diff), replace=False) 126 | n_samples_per_face[rand_faces] = n_samples_per_face[rand_faces] + 1 127 | 128 | # Create a vector that contains the face indices 129 | sample_face_idx = np.zeros((n_samples, ), dtype=int) 130 | 131 | acc = 0 132 | for face_idx, _n_sample in enumerate(n_samples_per_face): 133 | sample_face_idx[acc:acc + _n_sample] = face_idx 134 | acc += _n_sample 135 | 136 | r = np.random.rand(n_samples, 2) 137 | 138 | A = self.vertices[self.faces[sample_face_idx, 0], :] 139 | B = self.vertices[self.faces[sample_face_idx, 1], :] 140 | C = self.vertices[self.faces[sample_face_idx, 2], :] 141 | P = (1 - np.sqrt(r[:, 0:1])) * A + np.sqrt(r[:, 0:1]) * (1 - r[:, 1:]) * B + \ 142 | np.sqrt(r[:, 0:1]) * r[:, 1:] * C 143 | 144 | if with_normal: 145 | self.cal_vnormals() 146 | An = self.vnormals[self.faces[sample_face_idx,0],:] 147 | Bn = self.vnormals[self.faces[sample_face_idx,1],:] 148 | Cn = self.vnormals[self.faces[sample_face_idx,2],:] 149 | 150 | An[np.isnan(An)] = 0.0 151 | Bn[np.isnan(Bn)] = 0.0 152 | Cn[np.isnan(Cn)] = 0.0 153 | 154 | Pn = (1 - np.sqrt(r[:, 0:1])) * An + np.sqrt(r[:, 0:1]) * (1 - r[:, 1:]) * Bn + \ 155 | np.sqrt(r[:, 0:1]) * r[:, 1:] * Cn 156 | row_norms = np.linalg.norm(Pn, axis=1) 157 | row_norms[row_norms == 0] = 1 158 | Pn = (Pn.T / row_norms).T 159 | return P, Pn, sample_face_idx 160 | else: 161 | return P, sample_face_idx 162 | 163 | def plot_normals(self): 164 | mayalab.quiver3d(self.vertices[:, 0], self.vertices[:, 1], self.vertices[:, 2], self.vnormals[:, 0], self.vnormals[:, 1], self.vnormals[:, 2], mode='arrow') 165 | 166 | def plot_sample_points(self): 167 | self.s_pc,self.s_pc_n = self.sample_points(1000,with_normal=True)[0:2] 168 | mayalab.quiver3d(self.s_pc[:, 0], self.s_pc[:, 1], self.s_pc[:, 2], self.s_pc_n[:, 0], self.s_pc_n[:, 1], self.s_pc_n[:, 2], mode='arrow') 169 | 170 | def plot(self,triangle_function=np.array([]),vertex_function=np.array([]),show=True,*args,**kwargs): 171 | mayalab.triangular_mesh(self.vertices[:, 0], self.vertices[:, 1], self.vertices[:, 2], self.faces, color=(209/255.0,64/255.0,109/255.0), *args, **kwargs) 172 | 173 | if __name__ == "__main__": 174 | #test = OBJ(file_name='/home/lins/MetaGrasp/Data/Benchmarks_n/003.obj',scale=0.0001,rotation_degree=-45.0,normalize=False) 175 | #test = OBJ(file_name="/home/lins/bullet3/examples/pybullet/gym/pybullet_data/Panda/robotiq_2f_85/visual/robotiq_gripper_coupling.obj",rotation_degree=0.0,normalize=False,scale=0.001) 176 | test = OBJ(file_name="/home/lins/Downloads/model.obj",rotation_degree=0.0,normalize=False) 177 | test.vertices -= test.centers() 178 | #test.cal_vnormals() 179 | #tmp = test.sample_points(10000,with_normal=False)[0] 180 | #print(tmp.shape) 181 | #plot_pc(tmp,color=(209/255.0,64/255.0,109/255.0),mode='sphere',scale_factor=0.005) 182 | #tmp = np.copy(test.vnormals[:,2]) 183 | #test.vnormals[:,2] = np.copy(test.vnormals[:,1]) 184 | #test.vnormals[:,1] = np.copy(tmp) 185 | #test.vnormals *= -1.0 186 | 187 | #test.vertices[:,2] *= -1.0 188 | #tmp = np.copy(test.vertices[:,2]) 189 | #test.vertices[:,2] = np.copy(test.vertices[:,1]) 190 | #test.vertices[:,1] = np.copy(tmp) 191 | 192 | #test.vertices[:,0] -= 0.04 193 | #test.vertices[:,1] -= 0.04 194 | 195 | #print(test.vnormals.shape) 196 | #test.sample_points(1000,with_normal=True) 197 | #test.plot() 198 | #test.plot_normals() 199 | #test.plot_sample_points() 200 | #plot_origin() 201 | #mayalab.show() 202 | test.write('Shovel.obj') 203 | -------------------------------------------------------------------------------- /geolib/objfile.py: -------------------------------------------------------------------------------- 1 | import warnings 2 | import numpy as np 3 | from .objloader import obj_loader 4 | from math import sin, cos 5 | 6 | def plot_pc(pcs,color=None,scale_factor=.05,mode='point'): 7 | if color == 'red': 8 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(1,0,0)) 9 | elif color == 'blue': 10 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,0,1)) 11 | elif color == 'green': 12 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,0)) 13 | elif color == 'ycan': 14 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=(0,1,1)) 15 | else: 16 | print("unkown color") 17 | mayalab.points3d(pcs[:,0],pcs[:,1],pcs[:,2],mode=mode,scale_factor=scale_factor,color=color) 18 | 19 | def plot_pc_with_normal(pcs,pcs_n,scale_factor=1.0): 20 | mayalab.quiver3d(pcs[:, 0], pcs[:, 1], pcs[:, 2], pcs_n[:, 0], pcs_n[:, 1], pcs_n[:, 2], mode='arrow',scale_factor=1.0) 21 | 22 | 23 | def plot_origin(): 24 | origin_pc = np.array([0.0,0.0,0.0]).reshape((-1,3)) 25 | plot_pc(origin_pc,color='ycan',mode='sphere',scale_factor=.01) 26 | origin_pcs = np.tile(origin_pc,(3,1)) 27 | origin_pcns = np.eye(3) * 0.01 28 | plot_pc_with_normal(origin_pcs,origin_pcns) 29 | 30 | 31 | try: 32 | from mayavi import mlab as mayalab 33 | except: 34 | warnings.warn('Mayavi library was not found.') 35 | 36 | class OBJ(object): 37 | def __init__(self, vertices=None, faces=None, vnormals=None, normal_normalized=False,normalize=False,file_name=None,scale=1.0,rotation_degree=0): 38 | self.vertices = None 39 | self.faces = None 40 | self.vnormals = None 41 | self.fnormals = None 42 | self.fareas = None 43 | self.seed = 42 44 | self.normalize = normalize 45 | 46 | if file_name is not None: 47 | self.vertices, self.faces, self.vnormals = obj_loader(file_name,self.normalize)[:3] 48 | else: 49 | self.vertices = vertices 50 | self.faces = faces 51 | self.vnormals = vnormals 52 | 53 | if abs(scale - 1.0) > 0.0000001: 54 | self.vertices *= scale 55 | 56 | if abs(rotation_degree - 0.0) > 0.0001: 57 | new_x = self.vertices[:,0]*cos(rotation_degree / 180.0 * np.pi) + self.vertices[:,1]*sin(rotation_degree / 180.0 * np.pi) 58 | new_y = self.vertices[:,0]*sin(rotation_degree / 180.0 * np.pi) - self.vertices[:,1]*cos(rotation_degree / 180.0 * np.pi) 59 | self.vertices = np.vstack([new_x,new_y*-1.0,self.vertices[:,2]]).T 60 | 61 | self.num_points = len(self.vertices) 62 | self.num_faces = len(self.faces) 63 | 64 | if normal_normalized: 65 | self.cal_vnormals() 66 | 67 | def centers(self): 68 | return np.mean(self.vertices,axis=0) 69 | 70 | def cal_fareas(self): 71 | self.fareas = 0.5 * np.linalg.norm( np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]),axis=1) 72 | return np.copy(self.fareas) 73 | 74 | def cal_fnormals(self): 75 | N = np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]) 76 | row_norms = np.linalg.norm(N,axis=1) 77 | self.fareas = 0.5 * row_norms 78 | N = (N.T / row_norms).T 79 | self.fnormals = N 80 | 81 | def cal_vnormals(self): 82 | if self.vnormals.shape != self.vertices.shape: 83 | N = np.cross(self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,1],:], self.vertices[self.faces[:,0],:] - self.vertices[self.faces[:,2],:]) 84 | self.vnormals = np.empty_like(self.vertices) 85 | self.vnormals[self.faces[:,0],:] = N 86 | self.vnormals[self.faces[:,1],:] += N 87 | self.vnormals[self.faces[:,2],:] += N 88 | row_norms = np.linalg.norm(self.vnormals,axis=1) 89 | self.vnormals = (self.vnormals.T / row_norms).T 90 | 91 | def write(self,file_name): 92 | self.faces = self.faces + 1 93 | with open(file_name,'w') as fw: 94 | for v in self.vertices: 95 | fw.write('v %f %f %f\n' % (v[0],v[1],v[2])) 96 | 97 | if self.vnormals.shape == self.vertices.shape: 98 | for vn in self.vnormals: 99 | fw.write('vn %f %f %f\n' % (vn[0],vn[1],vn[2])) 100 | 101 | for f in self.faces: 102 | fw.write('f %d %d %d\n' % (f[0],f[1],f[2])) 103 | 104 | def sample_points(self,n_samples,seed=None,with_normal=False): 105 | if seed is not None: 106 | np.random.seed(seed) 107 | else: 108 | np.random.seed(self.seed) 109 | 110 | face_areas = self.cal_fareas() 111 | face_areas = face_areas / np.sum(face_areas) 112 | 113 | n_samples_per_face = np.round(n_samples * face_areas) 114 | 115 | n_samples_per_face = n_samples_per_face.astype(np.int) 116 | n_samples_s = int(np.sum(n_samples_per_face)) 117 | 118 | diff = n_samples_s - n_samples 119 | indices = np.arange(self.num_faces) 120 | 121 | if diff > 0: 122 | rand_faces = np.random.choice(indices[n_samples_per_face >= 1], abs(diff), replace=False) 123 | n_samples_per_face[rand_faces] = n_samples_per_face[rand_faces] - 1 124 | elif diff < 0: 125 | rand_faces = np.random.choice(indices, abs(diff), replace=False) 126 | n_samples_per_face[rand_faces] = n_samples_per_face[rand_faces] + 1 127 | 128 | # Create a vector that contains the face indices 129 | sample_face_idx = np.zeros((n_samples, ), dtype=int) 130 | 131 | acc = 0 132 | for face_idx, _n_sample in enumerate(n_samples_per_face): 133 | sample_face_idx[acc:acc + _n_sample] = face_idx 134 | acc += _n_sample 135 | 136 | r = np.random.rand(n_samples, 2) 137 | 138 | A = self.vertices[self.faces[sample_face_idx, 0], :] 139 | B = self.vertices[self.faces[sample_face_idx, 1], :] 140 | C = self.vertices[self.faces[sample_face_idx, 2], :] 141 | P = (1 - np.sqrt(r[:, 0:1])) * A + np.sqrt(r[:, 0:1]) * (1 - r[:, 1:]) * B + \ 142 | np.sqrt(r[:, 0:1]) * r[:, 1:] * C 143 | 144 | if with_normal: 145 | self.cal_vnormals() 146 | An = self.vnormals[self.faces[sample_face_idx,0],:] 147 | Bn = self.vnormals[self.faces[sample_face_idx,1],:] 148 | Cn = self.vnormals[self.faces[sample_face_idx,2],:] 149 | 150 | An[np.isnan(An)] = 0.0 151 | Bn[np.isnan(Bn)] = 0.0 152 | Cn[np.isnan(Cn)] = 0.0 153 | 154 | Pn = (1 - np.sqrt(r[:, 0:1])) * An + np.sqrt(r[:, 0:1]) * (1 - r[:, 1:]) * Bn + \ 155 | np.sqrt(r[:, 0:1]) * r[:, 1:] * Cn 156 | row_norms = np.linalg.norm(Pn, axis=1) 157 | row_norms[row_norms == 0] = 1 158 | Pn = (Pn.T / row_norms).T 159 | return P, Pn, sample_face_idx 160 | else: 161 | return P, sample_face_idx 162 | 163 | def plot_normals(self): 164 | mayalab.quiver3d(self.vertices[:, 0], self.vertices[:, 1], self.vertices[:, 2], self.vnormals[:, 0], self.vnormals[:, 1], self.vnormals[:, 2], mode='arrow') 165 | 166 | def plot_sample_points(self): 167 | self.s_pc,self.s_pc_n = self.sample_points(1000,with_normal=True)[0:2] 168 | mayalab.quiver3d(self.s_pc[:, 0], self.s_pc[:, 1], self.s_pc[:, 2], self.s_pc_n[:, 0], self.s_pc_n[:, 1], self.s_pc_n[:, 2], mode='arrow') 169 | 170 | def plot(self,triangle_function=np.array([]),vertex_function=np.array([]),show=True,*args,**kwargs): 171 | mayalab.triangular_mesh(self.vertices[:, 0], self.vertices[:, 1], self.vertices[:, 2], self.faces, color=(209/255.0,64/255.0,109/255.0), *args, **kwargs) 172 | 173 | if __name__ == "__main__": 174 | #test = OBJ(file_name='/home/lins/MetaGrasp/Data/Benchmarks_n/003.obj',scale=0.0001,rotation_degree=-45.0,normalize=False) 175 | #test = OBJ(file_name="/home/lins/bullet3/examples/pybullet/gym/pybullet_data/Panda/robotiq_2f_85/visual/robotiq_gripper_coupling.obj",rotation_degree=0.0,normalize=False,scale=0.001) 176 | test = OBJ(file_name="/home/lins/MetaGrasp/grippers/meshes/robotiq_3f/coupling.obj",rotation_degree=0.0,normalize=False,normal_normalized=False) 177 | test.vertices *= 1.16#-= test.centers() 178 | #test.cal_vnormals() 179 | #tmp = test.sample_points(10000,with_normal=False)[0] 180 | #print(tmp.shape) 181 | #plot_pc(tmp,color=(209/255.0,64/255.0,109/255.0),mode='sphere',scale_factor=0.005) 182 | #tmp = np.copy(test.vnormals[:,2]) 183 | #test.vnormals[:,2] = np.copy(test.vnormals[:,1]) 184 | #test.vnormals[:,1] = np.copy(tmp) 185 | #test.vnormals *= -1.0 186 | 187 | #test.vertices[:,2] *= -1.0 188 | #tmp = np.copy(test.vertices[:,2]) 189 | #test.vertices[:,2] = np.copy(test.vertices[:,1]) 190 | #test.vertices[:,1] = np.copy(tmp) 191 | 192 | #test.vertices[:,0] -= 0.04 193 | #test.vertices[:,1] -= 0.04 194 | 195 | #print(test.vnormals.shape) 196 | #test.sample_points(1000,with_normal=True) 197 | #test.plot() 198 | #test.plot_normals() 199 | #test.plot_sample_points() 200 | #plot_origin() 201 | #mayalab.show() 202 | test.write('link_3.0_tip_new.obj') 203 | -------------------------------------------------------------------------------- /math_utils.py: -------------------------------------------------------------------------------- 1 | import math 2 | import sys 3 | import os 4 | import numpy as np 5 | from numpy.linalg import inv 6 | from math import * 7 | import numpy 8 | _EPS = 1e-8 9 | 10 | # axis sequences for Euler angles 11 | _NEXT_AXIS = [1, 2, 0, 1] 12 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 13 | _AXES2TUPLE = { 14 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 15 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 16 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 17 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 18 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 19 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 20 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 21 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 22 | 23 | _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) 24 | 25 | def vector_norm(data, axis=None, out=None): 26 | data = numpy.array(data, dtype=numpy.float64, copy=True) 27 | if out is None: 28 | if data.ndim == 1: 29 | return math.sqrt(numpy.dot(data, data)) 30 | data *= data 31 | out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 32 | numpy.sqrt(out, out) 33 | return out 34 | else: 35 | data *= data 36 | numpy.sum(data, axis=axis, out=out) 37 | numpy.sqrt(out, out) 38 | 39 | def quaternion_about_axis(angle, axis): 40 | q = numpy.array([0.0, axis[0], axis[1], axis[2]]) 41 | qlen = vector_norm(q) 42 | if qlen > _EPS: 43 | q *= math.sin(angle/2.0) / qlen 44 | q[0] = math.cos(angle/2.0) 45 | return q 46 | 47 | def quaternion_matrix(quaternion): 48 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 49 | n = numpy.dot(q, q) 50 | if n < _EPS: 51 | return numpy.identity(4) 52 | q *= math.sqrt(2.0 / n) 53 | q = numpy.outer(q, q) 54 | return numpy.array([ 55 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 56 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 57 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 58 | [ 0.0, 0.0, 0.0, 1.0]]) 59 | 60 | 61 | def angleaxis_rotmat(angle,axis): 62 | quater = quaternion_about_axis(angle,axis) 63 | return quaternion_matrix(quater) 64 | 65 | def rpy_rotmat(rpy): 66 | rotmat = np.zeros((3,3)) 67 | roll = rpy[0] 68 | pitch = rpy[1] 69 | yaw = rpy[2] 70 | rotmat[0,0] = np.cos(yaw) * np.cos(pitch) 71 | rotmat[0,1] = np.cos(yaw) * np.sin(pitch) * np.sin(roll) - np.sin(yaw) * np.cos(roll) 72 | rotmat[0,2] = np.cos(yaw) * np.sin(pitch) * np.cos(roll) + np.sin(yaw) * np.sin(roll) 73 | rotmat[1,0] = np.sin(yaw) * np.cos(pitch) 74 | rotmat[1,1] = np.sin(yaw) * np.sin(pitch) * np.sin(roll) + np.cos(yaw) * np.cos(roll) 75 | rotmat[1,2] = np.sin(yaw) * np.sin(pitch) * np.cos(roll) - np.cos(yaw) * np.sin(roll) 76 | rotmat[2,0] = - np.sin(pitch) 77 | rotmat[2,1] = np.cos(pitch) * np.sin(roll) 78 | rotmat[2,2] = np.cos(pitch) * np.cos(roll) 79 | return rotmat 80 | 81 | 82 | def euler_from_matrix(matrix, axes='sxyz'): 83 | """Return Euler angles from rotation matrix for specified axis sequence. 84 | 85 | axes : One of 24 axis sequences as string or encoded tuple 86 | 87 | Note that many Euler angle triplets can describe one matrix. 88 | 89 | >>> R0 = euler_matrix(1, 2, 3, 'syxz') 90 | >>> al, be, ga = euler_from_matrix(R0, 'syxz') 91 | >>> R1 = euler_matrix(al, be, ga, 'syxz') 92 | >>> numpy.allclose(R0, R1) 93 | True 94 | >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) 95 | >>> for axes in _AXES2TUPLE.keys(): 96 | ... R0 = euler_matrix(axes=axes, *angles) 97 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 98 | ... if not numpy.allclose(R0, R1): print(axes, "failed") 99 | 100 | """ 101 | try: 102 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 103 | except (AttributeError, KeyError): 104 | _TUPLE2AXES[axes] # noqa: validation 105 | firstaxis, parity, repetition, frame = axes 106 | 107 | i = firstaxis 108 | j = _NEXT_AXIS[i+parity] 109 | k = _NEXT_AXIS[i-parity+1] 110 | 111 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 112 | if repetition: 113 | sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 114 | if sy > _EPS: 115 | ax = math.atan2( M[i, j], M[i, k]) 116 | ay = math.atan2( sy, M[i, i]) 117 | az = math.atan2( M[j, i], -M[k, i]) 118 | else: 119 | ax = math.atan2(-M[j, k], M[j, j]) 120 | ay = math.atan2( sy, M[i, i]) 121 | az = 0.0 122 | else: 123 | cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 124 | if cy > _EPS: 125 | ax = math.atan2( M[k, j], M[k, k]) 126 | ay = math.atan2(-M[k, i], cy) 127 | az = math.atan2( M[j, i], M[i, i]) 128 | else: 129 | ax = math.atan2(-M[j, k], M[j, j]) 130 | ay = math.atan2(-M[k, i], cy) 131 | az = 0.0 132 | 133 | if parity: 134 | ax, ay, az = -ax, -ay, -az 135 | if frame: 136 | ax, az = az, ax 137 | return ax, ay, az 138 | 139 | 140 | 141 | 142 | 143 | def tran_rot(filepath): 144 | rot = np.zeros((3,3)) 145 | tran = np.zeros((3,)) 146 | lines = [line.strip() for line in open(filepath)] 147 | for idx, line in enumerate(lines): 148 | tmp = str(line).split('(')[1].split(')')[0].split() 149 | tmp = [float(x.split(',')[0]) for x in tmp] 150 | if idx < 3: 151 | rot[idx,:] = np.array(tmp[0:3]) 152 | tran[idx] = tmp[3] 153 | return tran,rot 154 | 155 | def obj_centened_camera_pos(dist, azimuth_deg, elevation_deg): 156 | ''' 157 | Args: 158 | dist : distance from camera position to the origin 159 | azimuth_deg: azimuth degree in 360 degresses starting from positive x axis 160 | elevation_deg: elevation_deg in 360 degresses 161 | Returns: 162 | (x,y,z) : coordinates in xyz 163 | ''' 164 | phi = float(elevation_deg) / 180 * math.pi 165 | theta = float(azimuth_deg) / 180 * math.pi 166 | x = (dist * math.cos(theta) * math.cos(phi)) 167 | y = (dist * math.sin(theta) * math.cos(phi)) 168 | z = (dist * math.sin(phi)) 169 | return (x, y, z) 170 | 171 | def quaternionFromYawPitchRoll(yaw, pitch, roll): 172 | ''' 173 | Calculate quaternion from yaw, pitch, roll 174 | Args: 175 | yaw: rotation degress along z axis 176 | pitch: rotation degrees along y axis 177 | roll: rotation degress along x axis 178 | Return: 179 | quaternion 180 | ''' 181 | c1 = math.cos(yaw / 2.0) 182 | c2 = math.cos(pitch / 2.0) 183 | c3 = math.cos(roll / 2.0) 184 | s1 = math.sin(yaw / 2.0) 185 | s2 = math.sin(pitch / 2.0) 186 | s3 = math.sin(roll / 2.0) 187 | q1 = c1 * c2 * c3 + s1 * s2 * s3 188 | q2 = c1 * c2 * s3 - s1 * s2 * c3 189 | q3 = c1 * s2 * c3 + s1 * c2 * s3 190 | q4 = s1 * c2 * c3 - c1 * s2 * s3 191 | return (q1, q2, q3, q4) 192 | 193 | def camPosToQuaternion(cx, cy, cz): 194 | q1a = 0 195 | q1b = 0 196 | q1c = math.sqrt(2) / 2 197 | q1d = math.sqrt(2) / 2 198 | camDist = math.sqrt(cx * cx + cy * cy + cz * cz) 199 | cx = cx / camDist 200 | cy = cy / camDist 201 | cz = cz / camDist 202 | t = math.sqrt(cx * cx + cy * cy) 203 | tx = cx / t 204 | ty = cy / t 205 | yaw = math.acos(ty) 206 | if tx > 0: 207 | yaw = 2 * math.pi - yaw 208 | pitch = 0 209 | tmp = min(max(tx*cx + ty*cy, -1),1) 210 | roll = math.acos(tmp) 211 | if cz < 0: 212 | roll = -roll 213 | q2a, q2b, q2c, q2d = quaternionFromYawPitchRoll(yaw, pitch, roll) 214 | q1 = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d 215 | q2 = q1b * q2a + q1a * q2b + q1d * q2c - q1c * q2d 216 | q3 = q1c * q2a - q1d * q2b + q1a * q2c + q1b * q2d 217 | q4 = q1d * q2a + q1c * q2b - q1b * q2c + q1a * q2d 218 | return (q1, q2, q3, q4) 219 | 220 | def camRotQuaternion(cx, cy, cz, theta): 221 | theta = theta / 180.0 * math.pi 222 | camDist = math.sqrt(cx * cx + cy * cy + cz * cz) 223 | cx = -cx / camDist 224 | cy = -cy / camDist 225 | cz = -cz / camDist 226 | q1 = math.cos(theta * 0.5) 227 | q2 = -cx * math.sin(theta * 0.5) 228 | q3 = -cy * math.sin(theta * 0.5) 229 | q4 = -cz * math.sin(theta * 0.5) 230 | return (q1, q2, q3, q4) 231 | 232 | def quaternionProduct(qx, qy): 233 | ''' 234 | qx * qy 235 | first rotation qy and then rotate qx 236 | ''' 237 | a = qx[0] 238 | b = qx[1] 239 | c = qx[2] 240 | d = qx[3] 241 | e = qy[0] 242 | f = qy[1] 243 | g = qy[2] 244 | h = qy[3] 245 | q1 = a * e - b * f - c * g - d * h 246 | q2 = a * f + b * e + c * h - d * g 247 | q3 = a * g - b * h + c * e + d * f 248 | q4 = a * h + b * g - c * f + d * e 249 | return (q1, q2, q3, q4) 250 | 251 | 252 | def quaternion_matrix(quaternion): 253 | """Return homogeneous rotation matrix from quaternion. 254 | 255 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 256 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 257 | True 258 | >>> M = quaternion_matrix([1, 0, 0, 0]) 259 | >>> numpy.allclose(M, numpy.identity(4)) 260 | True 261 | >>> M = quaternion_matrix([0, 1, 0, 0]) 262 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 263 | True 264 | 265 | """ 266 | _EPS = np.finfo(float).eps * 4.0 267 | q = np.array(quaternion, dtype=np.float64, copy=True) 268 | n = np.dot(q, q) 269 | if n < _EPS: 270 | return np.identity(4) 271 | q *= math.sqrt(2.0 / n) 272 | q = np.outer(q, q) 273 | return np.array([ 274 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 275 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 276 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 277 | [ 0.0, 0.0, 0.0, 1.0]]) 278 | 279 | 280 | -------------------------------------------------------------------------------- /urdf_parser_py/urdf.py: -------------------------------------------------------------------------------- 1 | from urdf_parser_py.xml_reflection.basics import * 2 | import urdf_parser_py.xml_reflection as xmlr 3 | 4 | # Add a 'namespace' for names to avoid a conflict between URDF and SDF? 5 | # A type registry? How to scope that? Just make a 'global' type pointer? 6 | # Or just qualify names? urdf.geometric, sdf.geometric 7 | 8 | xmlr.start_namespace('urdf') 9 | 10 | xmlr.add_type('element_link', xmlr.SimpleElementType('link', str)) 11 | xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3')) 12 | 13 | verbose = True 14 | 15 | 16 | class Pose(xmlr.Object): 17 | def __init__(self, xyz=None, rpy=None): 18 | self.xyz = xyz 19 | self.rpy = rpy 20 | 21 | def check_valid(self): 22 | assert (self.xyz is None or len(self.xyz) == 3) and \ 23 | (self.rpy is None or len(self.rpy) == 3) 24 | 25 | # Aliases for backwards compatibility 26 | @property 27 | def rotation(self): return self.rpy 28 | 29 | @rotation.setter 30 | def rotation(self, value): self.rpy = value 31 | 32 | @property 33 | def position(self): return self.xyz 34 | 35 | @position.setter 36 | def position(self, value): self.xyz = value 37 | 38 | 39 | xmlr.reflect(Pose, params=[ 40 | xmlr.Attribute('xyz', 'vector3', False, default=[0, 0, 0]), 41 | xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0]) 42 | ]) 43 | 44 | 45 | # Common stuff 46 | name_attribute = xmlr.Attribute('name', str) 47 | origin_element = xmlr.Element('origin', Pose, False) 48 | 49 | 50 | class Color(xmlr.Object): 51 | def __init__(self, *args): 52 | # What about named colors? 53 | count = len(args) 54 | if count == 4 or count == 3: 55 | self.rgba = args 56 | elif count == 1: 57 | self.rgba = args[0] 58 | elif count == 0: 59 | self.rgba = None 60 | if self.rgba is not None: 61 | if len(self.rgba) == 3: 62 | self.rgba += [1.] 63 | if len(self.rgba) != 4: 64 | raise Exception('Invalid color argument count') 65 | 66 | 67 | xmlr.reflect(Color, params=[ 68 | xmlr.Attribute('rgba', 'vector4') 69 | ]) 70 | 71 | 72 | class JointDynamics(xmlr.Object): 73 | def __init__(self, damping=None, friction=None): 74 | self.damping = damping 75 | self.friction = friction 76 | 77 | 78 | xmlr.reflect(JointDynamics, params=[ 79 | xmlr.Attribute('damping', float, False), 80 | xmlr.Attribute('friction', float, False) 81 | ]) 82 | 83 | 84 | class Box(xmlr.Object): 85 | def __init__(self, size=None): 86 | self.size = size 87 | 88 | 89 | xmlr.reflect(Box, params=[ 90 | xmlr.Attribute('size', 'vector3') 91 | ]) 92 | 93 | 94 | class Cylinder(xmlr.Object): 95 | def __init__(self, radius=0.0, length=0.0): 96 | self.radius = radius 97 | self.length = length 98 | 99 | 100 | xmlr.reflect(Cylinder, params=[ 101 | xmlr.Attribute('radius', float), 102 | xmlr.Attribute('length', float) 103 | ]) 104 | 105 | 106 | class Sphere(xmlr.Object): 107 | def __init__(self, radius=0.0): 108 | self.radius = radius 109 | 110 | 111 | xmlr.reflect(Sphere, params=[ 112 | xmlr.Attribute('radius', float) 113 | ]) 114 | 115 | 116 | class Mesh(xmlr.Object): 117 | def __init__(self, filename=None, scale=None): 118 | self.filename = filename 119 | self.scale = scale 120 | 121 | 122 | xmlr.reflect(Mesh, params=[ 123 | xmlr.Attribute('filename', str), 124 | xmlr.Attribute('scale', 'vector3', required=False) 125 | ]) 126 | 127 | 128 | class GeometricType(xmlr.ValueType): 129 | def __init__(self): 130 | self.factory = xmlr.FactoryType('geometric', { 131 | 'box': Box, 132 | 'cylinder': Cylinder, 133 | 'sphere': Sphere, 134 | 'mesh': Mesh 135 | }) 136 | 137 | def from_xml(self, node): 138 | children = xml_children(node) 139 | assert len(children) == 1, 'One element only for geometric' 140 | return self.factory.from_xml(children[0]) 141 | 142 | def write_xml(self, node, obj): 143 | name = self.factory.get_name(obj) 144 | child = node_add(node, name) 145 | obj.write_xml(child) 146 | 147 | 148 | xmlr.add_type('geometric', GeometricType()) 149 | 150 | 151 | class Collision(xmlr.Object): 152 | def __init__(self, geometry=None, origin=None): 153 | self.geometry = geometry 154 | self.origin = origin 155 | 156 | 157 | xmlr.reflect(Collision, params=[ 158 | origin_element, 159 | xmlr.Element('geometry', 'geometric') 160 | ]) 161 | 162 | 163 | class Texture(xmlr.Object): 164 | def __init__(self, filename=None): 165 | self.filename = filename 166 | 167 | 168 | xmlr.reflect(Texture, params=[ 169 | xmlr.Attribute('filename', str) 170 | ]) 171 | 172 | 173 | class Material(xmlr.Object): 174 | def __init__(self, name=None, color=None, texture=None): 175 | self.name = name 176 | self.color = color 177 | self.texture = texture 178 | 179 | def check_valid(self): 180 | if self.color is None and self.texture is None: 181 | xmlr.on_error("Material has neither a color nor texture.") 182 | 183 | 184 | xmlr.reflect(Material, params=[ 185 | name_attribute, 186 | xmlr.Element('color', Color, False), 187 | xmlr.Element('texture', Texture, False) 188 | ]) 189 | 190 | 191 | class LinkMaterial(Material): 192 | def check_valid(self): 193 | pass 194 | 195 | 196 | class Visual(xmlr.Object): 197 | def __init__(self, geometry=None, material=None, origin=None): 198 | self.geometry = geometry 199 | self.material = material 200 | self.origin = origin 201 | 202 | 203 | xmlr.reflect(Visual, params=[ 204 | origin_element, 205 | xmlr.Element('geometry', 'geometric'), 206 | xmlr.Element('material', LinkMaterial, False) 207 | ]) 208 | 209 | 210 | class Inertia(xmlr.Object): 211 | KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] 212 | 213 | def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): 214 | self.ixx = ixx 215 | self.ixy = ixy 216 | self.ixz = ixz 217 | self.iyy = iyy 218 | self.iyz = iyz 219 | self.izz = izz 220 | 221 | def to_matrix(self): 222 | return [ 223 | [self.ixx, self.ixy, self.ixz], 224 | [self.ixy, self.iyy, self.iyz], 225 | [self.ixz, self.iyz, self.izz]] 226 | 227 | 228 | xmlr.reflect(Inertia, 229 | params=[xmlr.Attribute(key, float) for key in Inertia.KEYS]) 230 | 231 | 232 | class Inertial(xmlr.Object): 233 | def __init__(self, mass=0.0, inertia=None, origin=None): 234 | self.mass = mass 235 | self.inertia = inertia 236 | self.origin = origin 237 | 238 | 239 | xmlr.reflect(Inertial, params=[ 240 | origin_element, 241 | xmlr.Element('mass', 'element_value'), 242 | xmlr.Element('inertia', Inertia, False) 243 | ]) 244 | 245 | 246 | # FIXME: we are missing the reference position here. 247 | class JointCalibration(xmlr.Object): 248 | def __init__(self, rising=None, falling=None): 249 | self.rising = rising 250 | self.falling = falling 251 | 252 | 253 | xmlr.reflect(JointCalibration, params=[ 254 | xmlr.Attribute('rising', float, False, 0), 255 | xmlr.Attribute('falling', float, False, 0) 256 | ]) 257 | 258 | 259 | class JointLimit(xmlr.Object): 260 | def __init__(self, effort=None, velocity=None, lower=None, upper=None): 261 | self.effort = effort 262 | self.velocity = velocity 263 | self.lower = lower 264 | self.upper = upper 265 | 266 | 267 | xmlr.reflect(JointLimit, params=[ 268 | xmlr.Attribute('effort', float), 269 | xmlr.Attribute('lower', float, False, 0), 270 | xmlr.Attribute('upper', float, False, 0), 271 | xmlr.Attribute('velocity', float) 272 | ]) 273 | 274 | # FIXME: we are missing __str__ here. 275 | 276 | 277 | class JointMimic(xmlr.Object): 278 | def __init__(self, joint_name=None, multiplier=None, offset=None): 279 | self.joint = joint_name 280 | self.multiplier = multiplier 281 | self.offset = offset 282 | 283 | 284 | xmlr.reflect(JointMimic, params=[ 285 | xmlr.Attribute('joint', str), 286 | xmlr.Attribute('multiplier', float, False), 287 | xmlr.Attribute('offset', float, False) 288 | ]) 289 | 290 | 291 | class SafetyController(xmlr.Object): 292 | def __init__(self, velocity=None, position=None, lower=None, upper=None): 293 | self.k_velocity = velocity 294 | self.k_position = position 295 | self.soft_lower_limit = lower 296 | self.soft_upper_limit = upper 297 | 298 | 299 | xmlr.reflect(SafetyController, params=[ 300 | xmlr.Attribute('k_velocity', float), 301 | xmlr.Attribute('k_position', float, False, 0), 302 | xmlr.Attribute('soft_lower_limit', float, False, 0), 303 | xmlr.Attribute('soft_upper_limit', float, False, 0) 304 | ]) 305 | 306 | 307 | class Joint(xmlr.Object): 308 | TYPES = ['unknown', 'revolute', 'continuous', 'prismatic', 309 | 'floating', 'planar', 'fixed'] 310 | 311 | def __init__(self, name=None, parent=None, child=None, joint_type=None, 312 | axis=None, origin=None, 313 | limit=None, dynamics=None, safety_controller=None, 314 | calibration=None, mimic=None): 315 | self.name = name 316 | self.parent = parent 317 | self.child = child 318 | self.type = joint_type 319 | self.axis = axis 320 | self.origin = origin 321 | self.limit = limit 322 | self.dynamics = dynamics 323 | self.safety_controller = safety_controller 324 | self.calibration = calibration 325 | self.mimic = mimic 326 | 327 | def check_valid(self): 328 | assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type) # noqa 329 | 330 | # Aliases 331 | @property 332 | def joint_type(self): return self.type 333 | 334 | @joint_type.setter 335 | def joint_type(self, value): self.type = value 336 | 337 | xmlr.reflect(Joint, params=[ 338 | name_attribute, 339 | xmlr.Attribute('type', str), 340 | origin_element, 341 | xmlr.Element('axis', 'element_xyz', False), 342 | xmlr.Element('parent', 'element_link'), 343 | xmlr.Element('child', 'element_link'), 344 | xmlr.Element('limit', JointLimit, False), 345 | xmlr.Element('dynamics', JointDynamics, False), 346 | xmlr.Element('safety_controller', SafetyController, False), 347 | xmlr.Element('calibration', JointCalibration, False), 348 | xmlr.Element('mimic', JointMimic, False), 349 | ]) 350 | 351 | 352 | class Link(xmlr.Object): 353 | def __init__(self, name=None, visual=None, inertial=None, collision=None, 354 | origin=None): 355 | self.name = name 356 | self.visual = visual 357 | self.inertial = inertial 358 | self.collision = collision 359 | self.origin = origin 360 | 361 | xmlr.reflect(Link, params=[ 362 | name_attribute, 363 | origin_element, 364 | xmlr.Element('inertial', Inertial, False), 365 | xmlr.Element('visual', Visual, False), 366 | xmlr.Element('collision', Collision, False) 367 | ]) 368 | 369 | 370 | class PR2Transmission(xmlr.Object): 371 | def __init__(self, name=None, joint=None, actuator=None, type=None, 372 | mechanicalReduction=1): 373 | self.name = name 374 | self.type = type 375 | self.joint = joint 376 | self.actuator = actuator 377 | self.mechanicalReduction = mechanicalReduction 378 | 379 | 380 | xmlr.reflect(PR2Transmission, tag='pr2_transmission', params=[ 381 | name_attribute, 382 | xmlr.Attribute('type', str), 383 | xmlr.Element('joint', 'element_name'), 384 | xmlr.Element('actuator', 'element_name'), 385 | xmlr.Element('mechanicalReduction', float) 386 | ]) 387 | 388 | 389 | class Actuator(xmlr.Object): 390 | def __init__(self, name=None, mechanicalReduction=1): 391 | self.name = name 392 | self.mechanicalReduction = None 393 | 394 | 395 | xmlr.reflect(Actuator, tag='actuator', params=[ 396 | name_attribute, 397 | xmlr.Element('mechanicalReduction', float, required=False) 398 | ]) 399 | 400 | 401 | class TransmissionJoint(xmlr.Object): 402 | def __init__(self, name=None): 403 | self.aggregate_init() 404 | self.name = name 405 | self.hardwareInterfaces = [] 406 | 407 | def check_valid(self): 408 | assert len(self.hardwareInterfaces) > 0, "no hardwareInterface defined" 409 | 410 | 411 | xmlr.reflect(TransmissionJoint, tag='joint', params=[ 412 | name_attribute, 413 | xmlr.AggregateElement('hardwareInterface', str), 414 | ]) 415 | 416 | 417 | class Transmission(xmlr.Object): 418 | """ New format: http://wiki.ros.org/urdf/XML/Transmission """ 419 | 420 | def __init__(self, name=None): 421 | self.aggregate_init() 422 | self.name = name 423 | self.joints = [] 424 | self.actuators = [] 425 | 426 | def check_valid(self): 427 | assert len(self.joints) > 0, "no joint defined" 428 | assert len(self.actuators) > 0, "no actuator defined" 429 | 430 | 431 | xmlr.reflect(Transmission, tag='new_transmission', params=[ 432 | name_attribute, 433 | xmlr.Element('type', str), 434 | xmlr.AggregateElement('joint', TransmissionJoint), 435 | xmlr.AggregateElement('actuator', Actuator) 436 | ]) 437 | 438 | xmlr.add_type('transmission', 439 | xmlr.DuckTypedFactory('transmission', 440 | [Transmission, PR2Transmission])) 441 | 442 | 443 | class Robot(xmlr.Object): 444 | def __init__(self, name=None): 445 | self.aggregate_init() 446 | 447 | self.name = name 448 | self.joints = [] 449 | self.links = [] 450 | self.materials = [] 451 | self.gazebos = [] 452 | self.transmissions = [] 453 | 454 | self.joint_map = {} 455 | self.link_map = {} 456 | 457 | self.parent_map = {} 458 | self.child_map = {} 459 | 460 | def add_aggregate(self, typeName, elem): 461 | xmlr.Object.add_aggregate(self, typeName, elem) 462 | 463 | if typeName == 'joint': 464 | joint = elem 465 | self.joint_map[joint.name] = joint 466 | self.parent_map[joint.child] = (joint.name, joint.parent) 467 | if joint.parent in self.child_map: 468 | self.child_map[joint.parent].append((joint.name, joint.child)) 469 | else: 470 | self.child_map[joint.parent] = [(joint.name, joint.child)] 471 | elif typeName == 'link': 472 | link = elem 473 | self.link_map[link.name] = link 474 | 475 | def add_link(self, link): 476 | self.add_aggregate('link', link) 477 | 478 | def add_joint(self, joint): 479 | self.add_aggregate('joint', joint) 480 | 481 | def get_chain(self, root, tip, joints=True, links=True, fixed=True): 482 | chain = [] 483 | if links: 484 | chain.append(tip) 485 | link = tip 486 | while link != root: 487 | (joint, parent) = self.parent_map[link] 488 | if joints: 489 | if fixed or self.joint_map[joint].joint_type != 'fixed': 490 | chain.append(joint) 491 | if links: 492 | chain.append(parent) 493 | link = parent 494 | chain.reverse() 495 | return chain 496 | 497 | def get_root(self): 498 | root = None 499 | for link in self.link_map: 500 | if link not in self.parent_map: 501 | assert root is None, "Multiple roots detected, invalid URDF." 502 | root = link 503 | assert root is not None, "No roots detected, invalid URDF." 504 | return root 505 | 506 | @classmethod 507 | def from_parameter_server(cls, key='robot_description'): 508 | """ 509 | Retrieve the robot model on the parameter server 510 | and parse it to create a URDF robot structure. 511 | 512 | Warning: this requires roscore to be running. 513 | """ 514 | # Could move this into xml_reflection 515 | import rospy 516 | return cls.from_xml_string(rospy.get_param(key)) 517 | 518 | 519 | xmlr.reflect(Robot, tag='robot', params=[ 520 | xmlr.Attribute('name', str, False), # Is 'name' a required attribute? 521 | xmlr.AggregateElement('link', Link), 522 | xmlr.AggregateElement('joint', Joint), 523 | xmlr.AggregateElement('gazebo', xmlr.RawType()), 524 | xmlr.AggregateElement('transmission', 'transmission'), 525 | xmlr.AggregateElement('material', Material) 526 | ]) 527 | 528 | # Make an alias 529 | URDF = Robot 530 | 531 | xmlr.end_namespace() 532 | -------------------------------------------------------------------------------- /urdf_parser_py/xml_reflection/core.py: -------------------------------------------------------------------------------- 1 | from urdf_parser_py.xml_reflection.basics import * 2 | import sys 3 | import copy 4 | 5 | # @todo Get rid of "import *" 6 | # @todo Make this work with decorators 7 | 8 | # Is this reflection or serialization? I think it's serialization... 9 | # Rename? 10 | 11 | # Do parent operations after, to allow child to 'override' parameters? 12 | # Need to make sure that duplicate entires do not get into the 'unset*' lists 13 | 14 | 15 | def reflect(cls, *args, **kwargs): 16 | """ 17 | Simple wrapper to add XML reflection to an xml_reflection.Object class 18 | """ 19 | cls.XML_REFL = Reflection(*args, **kwargs) 20 | 21 | # Rename 'write_xml' to 'write_xml' to have paired 'load/dump', and make 22 | # 'pre_dump' and 'post_load'? 23 | # When dumping to yaml, include tag name? 24 | 25 | # How to incorporate line number and all that jazz? 26 | 27 | 28 | def on_error(message): 29 | """ What to do on an error. This can be changed to raise an exception. """ 30 | sys.stderr.write(message + '\n') 31 | 32 | 33 | skip_default = False 34 | # defaultIfMatching = True # Not implemeneted yet 35 | 36 | # Registering Types 37 | value_types = {} 38 | value_type_prefix = '' 39 | 40 | 41 | def start_namespace(namespace): 42 | """ 43 | Basic mechanism to prevent conflicts for string types for URDF and SDF 44 | @note Does not handle nesting! 45 | """ 46 | global value_type_prefix 47 | value_type_prefix = namespace + '.' 48 | 49 | 50 | def end_namespace(): 51 | global value_type_prefix 52 | value_type_prefix = '' 53 | 54 | 55 | def add_type(key, value): 56 | if isinstance(key, str): 57 | key = value_type_prefix + key 58 | assert key not in value_types 59 | value_types[key] = value 60 | 61 | 62 | def get_type(cur_type): 63 | """ Can wrap value types if needed """ 64 | if value_type_prefix and isinstance(cur_type, str): 65 | # See if it exists in current 'namespace' 66 | curKey = value_type_prefix + cur_type 67 | value_type = value_types.get(curKey) 68 | else: 69 | value_type = None 70 | if value_type is None: 71 | # Try again, in 'global' scope 72 | value_type = value_types.get(cur_type) 73 | if value_type is None: 74 | value_type = make_type(cur_type) 75 | add_type(cur_type, value_type) 76 | return value_type 77 | 78 | 79 | def make_type(cur_type): 80 | if isinstance(cur_type, ValueType): 81 | return cur_type 82 | elif isinstance(cur_type, str): 83 | if cur_type.startswith('vector'): 84 | extra = cur_type[6:] 85 | if extra: 86 | count = float(extra) 87 | else: 88 | count = None 89 | return VectorType(count) 90 | else: 91 | raise Exception("Invalid value type: {}".format(cur_type)) 92 | elif cur_type == list: 93 | return ListType() 94 | elif issubclass(cur_type, Object): 95 | return ObjectType(cur_type) 96 | elif cur_type in [str, float]: 97 | return BasicType(cur_type) 98 | else: 99 | raise Exception("Invalid type: {}".format(cur_type)) 100 | 101 | 102 | class ValueType(object): 103 | """ Primitive value type """ 104 | 105 | def from_xml(self, node): 106 | return self.from_string(node.text) 107 | 108 | def write_xml(self, node, value): 109 | """ 110 | If type has 'write_xml', this function should expect to have it's own 111 | XML already created i.e., In Axis.to_sdf(self, node), 'node' would be 112 | the 'axis' element. 113 | @todo Add function that makes an XML node completely independently? 114 | """ 115 | node.text = self.to_string(value) 116 | 117 | def equals(self, a, b): 118 | return a == b 119 | 120 | 121 | class BasicType(ValueType): 122 | def __init__(self, cur_type): 123 | self.type = cur_type 124 | 125 | def to_string(self, value): 126 | return str(value) 127 | 128 | def from_string(self, value): 129 | return self.type(value) 130 | 131 | 132 | class ListType(ValueType): 133 | def to_string(self, values): 134 | return ' '.join(values) 135 | 136 | def from_string(self, text): 137 | return text.split() 138 | 139 | def equals(self, aValues, bValues): 140 | return len(aValues) == len(bValues) and all(a == b for (a, b) in zip(aValues, bValues)) # noqa 141 | 142 | 143 | class VectorType(ListType): 144 | def __init__(self, count=None): 145 | self.count = count 146 | 147 | def check(self, values): 148 | if self.count is not None: 149 | assert len(values) == self.count, "Invalid vector length" 150 | 151 | def to_string(self, values): 152 | self.check(values) 153 | raw = list(map(str, values)) 154 | return ListType.to_string(self, raw) 155 | 156 | def from_string(self, text): 157 | raw = ListType.from_string(self, text) 158 | self.check(raw) 159 | return list(map(float, raw)) 160 | 161 | 162 | class RawType(ValueType): 163 | """ 164 | Simple, raw XML value. Need to bugfix putting this back into a document 165 | """ 166 | 167 | def from_xml(self, node): 168 | return node 169 | 170 | def write_xml(self, node, value): 171 | # @todo rying to insert an element at root level seems to screw up 172 | # pretty printing 173 | children = xml_children(value) 174 | list(map(node.append, children)) 175 | # Copy attributes 176 | for (attrib_key, attrib_value) in value.attrib.iteritems(): 177 | node.set(attrib_key, attrib_value) 178 | 179 | 180 | class SimpleElementType(ValueType): 181 | """ 182 | Extractor that retrieves data from an element, given a 183 | specified attribute, casted to value_type. 184 | """ 185 | 186 | def __init__(self, attribute, value_type): 187 | self.attribute = attribute 188 | self.value_type = get_type(value_type) 189 | 190 | def from_xml(self, node): 191 | text = node.get(self.attribute) 192 | return self.value_type.from_string(text) 193 | 194 | def write_xml(self, node, value): 195 | text = self.value_type.to_string(value) 196 | node.set(self.attribute, text) 197 | 198 | 199 | class ObjectType(ValueType): 200 | def __init__(self, cur_type): 201 | self.type = cur_type 202 | 203 | def from_xml(self, node): 204 | obj = self.type() 205 | obj.read_xml(node) 206 | return obj 207 | 208 | def write_xml(self, node, obj): 209 | obj.write_xml(node) 210 | 211 | 212 | class FactoryType(ValueType): 213 | def __init__(self, name, typeMap): 214 | self.name = name 215 | self.typeMap = typeMap 216 | self.nameMap = {} 217 | for (key, value) in typeMap.items(): 218 | # Reverse lookup 219 | self.nameMap[value] = key 220 | 221 | def from_xml(self, node): 222 | cur_type = self.typeMap.get(node.tag) 223 | if cur_type is None: 224 | raise Exception("Invalid {} tag: {}".format(self.name, node.tag)) 225 | value_type = get_type(cur_type) 226 | return value_type.from_xml(node) 227 | 228 | def get_name(self, obj): 229 | cur_type = type(obj) 230 | name = self.nameMap.get(cur_type) 231 | if name is None: 232 | raise Exception("Invalid {} type: {}".format(self.name, cur_type)) 233 | return name 234 | 235 | def write_xml(self, node, obj): 236 | obj.write_xml(node) 237 | 238 | 239 | class DuckTypedFactory(ValueType): 240 | def __init__(self, name, typeOrder): 241 | self.name = name 242 | assert len(typeOrder) > 0 243 | self.type_order = typeOrder 244 | 245 | def from_xml(self, node): 246 | error_set = [] 247 | for value_type in self.type_order: 248 | try: 249 | return value_type.from_xml(node) 250 | except Exception as e: 251 | error_set.append((value_type, e)) 252 | # Should have returned, we encountered errors 253 | out = "Could not perform duck-typed parsing." 254 | for (value_type, e) in error_set: 255 | out += "\nValue Type: {}\nException: {}\n".format(value_type, e) 256 | raise Exception(out) 257 | 258 | def write_xml(self, node, obj): 259 | obj.write_xml(node) 260 | 261 | 262 | class Param(object): 263 | """ Mirroring Gazebo's SDF api 264 | 265 | @param xml_var: Xml name 266 | @todo If the value_type is an object with a tag defined in it's 267 | reflection, allow it to act as the default tag name? 268 | @param var: Python class variable name. By default it's the same as the 269 | XML name 270 | """ 271 | 272 | def __init__(self, xml_var, value_type, required=True, default=None, 273 | var=None): 274 | self.xml_var = xml_var 275 | if var is None: 276 | self.var = xml_var 277 | else: 278 | self.var = var 279 | self.type = None 280 | self.value_type = get_type(value_type) 281 | self.default = default 282 | if required: 283 | assert default is None, "Default does not make sense for a required field" # noqa 284 | self.required = required 285 | self.is_aggregate = False 286 | 287 | def set_default(self, obj): 288 | if self.required: 289 | raise Exception("Required {} not set in XML: {}".format(self.type, self.xml_var)) # noqa 290 | elif not skip_default: 291 | setattr(obj, self.var, self.default) 292 | 293 | 294 | class Attribute(Param): 295 | def __init__(self, xml_var, value_type, required=True, default=None, 296 | var=None): 297 | Param.__init__(self, xml_var, value_type, required, default, var) 298 | self.type = 'attribute' 299 | 300 | def set_from_string(self, obj, value): 301 | """ Node is the parent node in this case """ 302 | # Duplicate attributes cannot occur at this point 303 | setattr(obj, self.var, self.value_type.from_string(value)) 304 | 305 | def add_to_xml(self, obj, node): 306 | value = getattr(obj, self.var) 307 | # Do not set with default value if value is None 308 | if value is None: 309 | if self.required: 310 | raise Exception("Required attribute not set in object: {}".format(self.var)) # noqa 311 | elif not skip_default: 312 | value = self.default 313 | # Allow value type to handle None? 314 | if value is not None: 315 | node.set(self.xml_var, self.value_type.to_string(value)) 316 | 317 | # Add option if this requires a header? 318 | # Like .... ??? 319 | # Not really... This would be a specific list type, not really aggregate 320 | 321 | 322 | class Element(Param): 323 | def __init__(self, xml_var, value_type, required=True, default=None, 324 | var=None, is_raw=False): 325 | Param.__init__(self, xml_var, value_type, required, default, var) 326 | self.type = 'element' 327 | self.is_raw = is_raw 328 | 329 | def set_from_xml(self, obj, node): 330 | value = self.value_type.from_xml(node) 331 | setattr(obj, self.var, value) 332 | 333 | def add_to_xml(self, obj, parent): 334 | value = getattr(obj, self.xml_var) 335 | if value is None: 336 | if self.required: 337 | raise Exception("Required element not defined in object: {}".format(self.var)) # noqa 338 | elif not skip_default: 339 | value = self.default 340 | if value is not None: 341 | self.add_scalar_to_xml(parent, value) 342 | 343 | def add_scalar_to_xml(self, parent, value): 344 | if self.is_raw: 345 | node = parent 346 | else: 347 | node = node_add(parent, self.xml_var) 348 | self.value_type.write_xml(node, value) 349 | 350 | 351 | class AggregateElement(Element): 352 | def __init__(self, xml_var, value_type, var=None, is_raw=False): 353 | if var is None: 354 | var = xml_var + 's' 355 | Element.__init__(self, xml_var, value_type, required=False, var=var, 356 | is_raw=is_raw) 357 | self.is_aggregate = True 358 | 359 | def add_from_xml(self, obj, node): 360 | value = self.value_type.from_xml(node) 361 | obj.add_aggregate(self.xml_var, value) 362 | 363 | def set_default(self, obj): 364 | pass 365 | 366 | 367 | class Info: 368 | """ Small container for keeping track of what's been consumed """ 369 | 370 | def __init__(self, node): 371 | self.attributes = list(node.attrib.keys()) 372 | self.children = xml_children(node) 373 | 374 | 375 | class Reflection(object): 376 | def __init__(self, params=[], parent_cls=None, tag=None): 377 | """ Construct a XML reflection thing 378 | @param parent_cls: Parent class, to use it's reflection as well. 379 | @param tag: Only necessary if you intend to use Object.write_xml_doc() 380 | This does not override the name supplied in the reflection 381 | definition thing. 382 | """ 383 | if parent_cls is not None: 384 | self.parent = parent_cls.XML_REFL 385 | else: 386 | self.parent = None 387 | self.tag = tag 388 | 389 | # Laziness for now 390 | attributes = [] 391 | elements = [] 392 | for param in params: 393 | if isinstance(param, Element): 394 | elements.append(param) 395 | else: 396 | attributes.append(param) 397 | 398 | self.vars = [] 399 | self.paramMap = {} 400 | 401 | self.attributes = attributes 402 | self.attribute_map = {} 403 | self.required_attribute_names = [] 404 | for attribute in attributes: 405 | self.attribute_map[attribute.xml_var] = attribute 406 | self.paramMap[attribute.xml_var] = attribute 407 | self.vars.append(attribute.var) 408 | if attribute.required: 409 | self.required_attribute_names.append(attribute.xml_var) 410 | 411 | self.elements = [] 412 | self.element_map = {} 413 | self.required_element_names = [] 414 | self.aggregates = [] 415 | self.scalars = [] 416 | self.scalarNames = [] 417 | for element in elements: 418 | self.element_map[element.xml_var] = element 419 | self.paramMap[element.xml_var] = element 420 | self.vars.append(element.var) 421 | if element.required: 422 | self.required_element_names.append(element.xml_var) 423 | if element.is_aggregate: 424 | self.aggregates.append(element) 425 | else: 426 | self.scalars.append(element) 427 | self.scalarNames.append(element.xml_var) 428 | 429 | def set_from_xml(self, obj, node, info=None): 430 | is_final = False 431 | if info is None: 432 | is_final = True 433 | info = Info(node) 434 | 435 | if self.parent: 436 | self.parent.set_from_xml(obj, node, info) 437 | 438 | # Make this a map instead? Faster access? {name: isSet} ? 439 | unset_attributes = list(self.attribute_map.keys()) 440 | unset_scalars = copy.copy(self.scalarNames) 441 | # Better method? Queues? 442 | for xml_var in copy.copy(info.attributes): 443 | attribute = self.attribute_map.get(xml_var) 444 | if attribute is not None: 445 | value = node.attrib[xml_var] 446 | attribute.set_from_string(obj, value) 447 | unset_attributes.remove(xml_var) 448 | info.attributes.remove(xml_var) 449 | 450 | # Parse unconsumed nodes 451 | for child in copy.copy(info.children): 452 | tag = child.tag 453 | element = self.element_map.get(tag) 454 | if element is not None: 455 | if element.is_aggregate: 456 | element.add_from_xml(obj, child) 457 | else: 458 | if tag in unset_scalars: 459 | element.set_from_xml(obj, child) 460 | unset_scalars.remove(tag) 461 | else: 462 | on_error("Scalar element defined multiple times: {}".format(tag)) # noqa 463 | info.children.remove(child) 464 | 465 | for attribute in map(self.attribute_map.get, unset_attributes): 466 | attribute.set_default(obj) 467 | 468 | for element in map(self.element_map.get, unset_scalars): 469 | element.set_default(obj) 470 | 471 | if is_final: 472 | for xml_var in info.attributes: 473 | on_error('Unknown attribute: {}'.format(xml_var)) 474 | for node in info.children: 475 | on_error('Unknown tag: {}'.format(node.tag)) 476 | 477 | def add_to_xml(self, obj, node): 478 | if self.parent: 479 | self.parent.add_to_xml(obj, node) 480 | for attribute in self.attributes: 481 | attribute.add_to_xml(obj, node) 482 | for element in self.scalars: 483 | element.add_to_xml(obj, node) 484 | # Now add in aggregates 485 | if self.aggregates: 486 | obj.add_aggregates_to_xml(node) 487 | 488 | 489 | class Object(YamlReflection): 490 | """ Raw python object for yaml / xml representation """ 491 | XML_REFL = None 492 | 493 | def get_refl_vars(self): 494 | return self.XML_REFL.vars 495 | 496 | def check_valid(self): 497 | pass 498 | 499 | def pre_write_xml(self): 500 | """ If anything needs to be converted prior to dumping to xml 501 | i.e., getting the names of objects and such """ 502 | pass 503 | 504 | def write_xml(self, node): 505 | """ Adds contents directly to XML node """ 506 | self.check_valid() 507 | self.pre_write_xml() 508 | self.XML_REFL.add_to_xml(self, node) 509 | 510 | def to_xml(self): 511 | """ Creates an overarching tag and adds its contents to the node """ 512 | tag = self.XML_REFL.tag 513 | assert tag is not None, "Must define 'tag' in reflection to use this function" # noqa 514 | doc = etree.Element(tag) 515 | self.write_xml(doc) 516 | return doc 517 | 518 | def to_xml_string(self, addHeader=True): 519 | return xml_string(self.to_xml(), addHeader) 520 | 521 | def post_read_xml(self): 522 | pass 523 | 524 | def read_xml(self, node): 525 | self.XML_REFL.set_from_xml(self, node) 526 | self.post_read_xml() 527 | self.check_valid() 528 | 529 | @classmethod 530 | def from_xml(cls, node): 531 | cur_type = get_type(cls) 532 | return cur_type.from_xml(node) 533 | 534 | @classmethod 535 | def from_xml_string(cls, xml_string): 536 | node = etree.fromstring(xml_string) 537 | return cls.from_xml(node) 538 | 539 | @classmethod 540 | def from_xml_file(cls, file_path): 541 | xml_string = open(file_path, 'r').read() 542 | return cls.from_xml_string(xml_string) 543 | 544 | # Confusing distinction between loading code in object and reflection 545 | # registry thing... 546 | 547 | def get_aggregate_list(self, xml_var): 548 | var = self.XML_REFL.paramMap[xml_var].var 549 | values = getattr(self, var) 550 | assert isinstance(values, list) 551 | return values 552 | 553 | def aggregate_init(self): 554 | """ Must be called in constructor! """ 555 | self.aggregate_order = [] 556 | # Store this info in the loaded object??? Nah 557 | self.aggregate_type = {} 558 | 559 | def add_aggregate(self, xml_var, obj): 560 | """ NOTE: One must keep careful track of aggregate types for this system. 561 | Can use 'lump_aggregates()' before writing if you don't care. """ 562 | self.get_aggregate_list(xml_var).append(obj) 563 | self.aggregate_order.append(obj) 564 | self.aggregate_type[obj] = xml_var 565 | 566 | def add_aggregates_to_xml(self, node): 567 | for value in self.aggregate_order: 568 | typeName = self.aggregate_type[value] 569 | element = self.XML_REFL.element_map[typeName] 570 | element.add_scalar_to_xml(node, value) 571 | 572 | def remove_aggregate(self, obj): 573 | self.aggregate_order.remove(obj) 574 | xml_var = self.aggregate_type[obj] 575 | del self.aggregate_type[obj] 576 | self.get_aggregate_list(xml_var).remove(obj) 577 | 578 | def lump_aggregates(self): 579 | """ Put all aggregate types together, just because """ 580 | self.aggregate_init() 581 | for param in self.XML_REFL.aggregates: 582 | for obj in self.get_aggregate_list(param.xml_var): 583 | self.add_aggregate(param.var, obj) 584 | 585 | """ Compatibility """ 586 | 587 | def parse(self, xml_string): 588 | node = etree.fromstring(xml_string) 589 | self.read_xml(node) 590 | return self 591 | 592 | 593 | # Really common types 594 | # Better name: element_with_name? Attributed element? 595 | add_type('element_name', SimpleElementType('name', str)) 596 | add_type('element_value', SimpleElementType('value', float)) 597 | 598 | # Add in common vector types so they aren't absorbed into the namespaces 599 | get_type('vector3') 600 | get_type('vector4') 601 | get_type('vector6') 602 | -------------------------------------------------------------------------------- /gripper_files.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import argparse 3 | import os 4 | import numpy as np 5 | import numpy 6 | from geolib.objfile import OBJ 7 | from geolib.cuboid import Cuboid 8 | from urdf_parser_py.urdf import URDF 9 | from mayavi import mlab as mayalab 10 | import math 11 | import scipy.optimize 12 | 13 | import inverse_kinematics as ik 14 | from geolib.objfile import OBJ 15 | from geolib.bbox import Bbox 16 | 17 | from vis_lib import * 18 | from math_utils import * 19 | 20 | #BASE_DIR = os.path.dirname(os.path.abspath(__file__)) 21 | #GRIPPER_DIR = os.path.join(BASE_DIR,'../',"grippers") 22 | #MESH_DIR = "./gripper_data/robotiq3f/meshes" 23 | 24 | class Pose(object): 25 | def __init__(self, pose_in_urdf): 26 | self.xyz = pose_in_urdf.xyz 27 | self.rpy = pose_in_urdf.rpy 28 | self.T = np.zeros((4,4)) 29 | self.T[:3,3] = self.xyz 30 | self.T[3,3] = 1.0 31 | self.T[:3,:3] = rpy_rotmat(self.rpy) 32 | 33 | 34 | class JointLimit(object): 35 | def __init__(self,jointlimit_in_urdf): 36 | self.effort = jointlimit_in_urdf.effort 37 | self.velocity = jointlimit_in_urdf.velocity 38 | self.lower = jointlimit_in_urdf.lower 39 | self.upper = jointlimit_in_urdf.upper 40 | 41 | 42 | class Mesh(object): 43 | def __init__(self,filename=None,scale=None): 44 | self.filename = os.path.join(MESH_DIR,filename.strip()) 45 | self.scale = scale 46 | print("filename",self.filename) 47 | self.mesh = OBJ(file_name=self.filename) 48 | self.pc = self.mesh.sample_points(4096,with_normal=False)[0] 49 | 50 | 51 | class Visual(object): 52 | def __init__(self,visual_in_urdf): 53 | self.geometry = visual_in_urdf.geometry 54 | self.material = visual_in_urdf.material 55 | if visual_in_urdf.origin != None: 56 | self.origin = Pose(visual_in_urdf.origin) 57 | self.T = self.origin.T 58 | else: 59 | self.origin = None 60 | self.T = np.eye(4) 61 | self.obj = Mesh(self.geometry.filename,scale=self.geometry.scale) 62 | self.obj.len_pc = len(self.obj.pc) 63 | self.obj.pc_4d = np.hstack([self.obj.pc,np.ones((self.obj.len_pc,1))]) 64 | self.obj.pc_T = self.obj.pc_4d.dot(self.T.transpose())[:,0:3] 65 | 66 | 67 | class Collision(object): 68 | def __init__(self,visual): 69 | self.epi = 0.005 70 | self.xmax = np.max(visual.obj.pc[:,0]) - self.epi 71 | self.xmin = np.min(visual.obj.pc[:,0]) + self.epi 72 | self.ymax = np.max(visual.obj.pc[:,1]) - self.epi 73 | self.ymin = np.min(visual.obj.pc[:,1]) + self.epi 74 | self.zmax = np.max(visual.obj.pc[:,2]) - self.epi 75 | self.zmin = np.min(visual.obj.pc[:,2]) + self.epi 76 | self.geometry = Bbox(extrema=np.array([self.xmin,self.ymin,self.zmin,self.xmax,self.ymax,self.zmax])) 77 | #print("self.geometry",self.geometry.corner_points) 78 | 79 | class Link(object): 80 | def __init__(self,link_in_urdf): 81 | self.name = link_in_urdf.name 82 | self.visual = Visual(link_in_urdf.visual) 83 | self.collision = Collision(self.visual) 84 | if link_in_urdf.origin != None: 85 | self.origin = link_in_urdf.origin 86 | self.T = self.origin.T 87 | else: 88 | self.origin = None 89 | self.T = np.eye(4) 90 | self.base_T = np.eye(4) 91 | 92 | def get_transformation_matrix(self,value): 93 | return self.visual.T 94 | 95 | 96 | class Joint(object): 97 | def __init__(self,joint_in_urdf): 98 | self.name = joint_in_urdf.name 99 | self.parent = joint_in_urdf.parent 100 | self.child = joint_in_urdf.child 101 | self.type = joint_in_urdf.joint_type 102 | self.axis = joint_in_urdf.axis 103 | if joint_in_urdf.origin != None: 104 | self.origin = Pose(joint_in_urdf.origin) 105 | self.T = self.origin.T 106 | else: 107 | self.origin = None 108 | self.T = np.eye(4) 109 | if joint_in_urdf.limit != None: 110 | self.limit = JointLimit(joint_in_urdf.limit) 111 | else: 112 | self.limit = None 113 | self.default_joint_value = 0 114 | 115 | def get_transformation_matrix(self,joint_angle): 116 | frame = np.copy(self.T) 117 | #print("joint_angle",joint_angle,"self.type",self.type,"self.axis",self.axis) 118 | if self.type == 'fixed': 119 | if self.axis is not None: 120 | rotmat = angleaxis_rotmat(self.default_joint_value,self.axis) 121 | return np.dot(frame,rotmat) 122 | else: 123 | return frame 124 | else: 125 | rotmat = angleaxis_rotmat(joint_angle,self.axis) 126 | frame = np.dot(frame,rotmat) 127 | return frame 128 | 129 | 130 | class Chain(object): 131 | """ 132 | The base Chain class 133 | Parameters 134 | ---- 135 | links: list : List of the links for the chain 136 | active_links_mask:list : A list of boolean indicating that whether or not the corresponding link is active 137 | name: str: The name of the Chain 138 | """ 139 | def __init__(self, joints, active_joints_mask=None, name="chain", tip_pc=None, gripper=None, profile=''"",**kwargs): 140 | self.name = name 141 | self.joints = joints 142 | self.gripper = gripper 143 | 144 | 145 | # If the active_links_mask is not given, set it to True for every link 146 | if active_joints_mask is not None: 147 | self.active_joints_mask = np.array(active_joints_mask) 148 | # Always set the last link to True 149 | #self.active_joints_mask[-1] = False 150 | else: 151 | self.active_joints_mask = np.array([True] * len(joints)) 152 | self.initial_position = np.zeros((len(self.joints),)) 153 | self.tip_pc = tip_pc 154 | 155 | def forward_kinematics(self, joint_values, full_kinematics=False): 156 | """ 157 | Returns the transformation matrix of the forward kinematics. 158 | 159 | Parameters: 160 | joints: list : The list of the positions of each joint. Noet: Inactive joints must not be in the list. 161 | full_kinematics: bool: Returns the transformation matrices of each joint. 162 | 163 | Returns: 164 | ----- 165 | frame_matrix: 166 | The transformation matrix. 167 | """ 168 | frame_matrix = np.eye(4) 169 | if self.tip_pc is not None: 170 | frame_matrix[:3,3] = self.tip_pc 171 | 172 | if full_kinematics: 173 | frame_matrixes = [] 174 | 175 | self.joints_value = self.active_to_full(joint_values,self.initial_position) 176 | 177 | for index, joint in enumerate(self.joints): 178 | # Compute iteratively the position 179 | # NB: Use asarray to avoid old sympy problems 180 | if self.active_joints_mask[index]: 181 | frame_matrix = np.dot(np.asarray(joint.get_transformation_matrix(self.joints_value[index])),frame_matrix) 182 | else: 183 | frame_matrix = np.dot(np.asarray(joint.get_transformation_matrix(0)),frame_matrix) 184 | 185 | if full_kinematics: 186 | # rotation_axes = np.dot(frame_matrix, link.rotation) 187 | frame_matrixes.append(frame_matrix) 188 | 189 | # Return the matrix, or matric3es 190 | if full_kinematics: 191 | return frame_matrixes 192 | else: 193 | return frame_matrix 194 | 195 | 196 | def inverse_kinematics(self,target,initial_position=None,**kwargs): 197 | """Computes the inverse kinematic on the speicified target. 198 | Parameters: 199 | ---------- 200 | target: numpy.array 201 | The frame target of the inverse kinematic, in meters. It must be 4x4 transformation matrix 202 | initial_position: numpy.array 203 | Optional: the initial position of each joint of the chain. Defaults to 0 for each joint. 204 | 205 | Returns: 206 | ------- 207 | The list of the position of each joint according to the target. Note: Inactive joints are in the list. 208 | """ 209 | target = np.array(target) 210 | if target.shape != (4,4): 211 | raise ValueError("Your target must be a 4x4 transformation matrix") 212 | 213 | if initial_position is None: 214 | initial_position = [0] * len(self.joints) 215 | else: 216 | if len(initial_position) != len(self.joints): 217 | initial_position = self.active_to_full(self.active_joints_mask,[0] * len(self.joints)) 218 | 219 | ik_res = ik.inverse_kinematic_optimization(self, target, starting_nodes_angles=initial_position, **kwargs) 220 | return ik_res 221 | 222 | def active_from_full(self,joints): 223 | return np.compress(self.active_joints_mask,joints,axis=0) 224 | 225 | def active_to_full(self,active_joints,intial_position): 226 | full_joints = np.array(intial_position,copy=True,dtype=np.float) 227 | np.place(full_joints,self.active_joints_mask,active_joints) 228 | return full_joints 229 | 230 | 231 | class Gripper(object): 232 | def __init__(self,gripper_in_urdf,tip_pc,mesh_top_dir=None): 233 | self.name = gripper_in_urdf.name 234 | self.mesh_top_dir = mesh_top_dir 235 | self.tip_pc = tip_pc 236 | 237 | self.links = [] 238 | for link in gripper_in_urdf.links: 239 | self.links.append(Link(link)) 240 | 241 | self.joints = [] 242 | for joint in gripper_in_urdf.joints: 243 | self.joints.append(Joint(joint)) 244 | 245 | self.joint_map = {} 246 | self.parent_map = {} 247 | self.child_map = {} 248 | self.link_map = {} 249 | 250 | self.q_joint_map = {} 251 | self.q_limit_map = {} 252 | self.q_T_map = {} 253 | self.q_value_map = {} 254 | self.q_axis_map = {} 255 | self.q_type_map = {} 256 | 257 | self.chain_map = {} 258 | 259 | for elem in self.joints: 260 | self.joint_map[elem.name] = elem 261 | self.parent_map[elem.child] = (elem.name, elem.parent) 262 | if elem.parent in self.child_map: 263 | self.child_map[elem.parent].append((elem.name, elem.child)) 264 | else: 265 | self.child_map[elem.parent] = [(elem.name, elem.child)] 266 | 267 | for elem in self.links: 268 | self.link_map[elem.name] = elem 269 | 270 | for elem in self.joints: 271 | if elem.type == 'revolute' or elem.type == 'prismatic': 272 | self.q_limit_map[elem.name] = elem.limit 273 | self.q_value_map[elem.name] = 0.0 #np.random.uniform(low=elem.limit.lower,high=elem.limit.upper,size=1)[0] 274 | self.q_axis_map[elem.name] = elem.axis 275 | self.q_type_map[elem.name] = elem.type 276 | 277 | self.root = self.get_root() 278 | 279 | for link in self.link_map: 280 | if link in self.parent_map: 281 | _, active_joints_mask, link_joint_chain = self.get_chain(self.root,link,links=False,return_active_joints=True) 282 | self.chain_map[link] = Chain(joints=link_joint_chain,name=link,active_joints_mask=active_joints_mask,tip_pc=self.tip_pc) 283 | 284 | 285 | def cal_configure(self): 286 | for elem in self.q_value_map: 287 | if self.q_type_map[elem] == 'revolute': 288 | rotmat = angleaxis_rotmat(self.q_value_map[elem],self.q_axis_map[elem]) 289 | self.q_T_map[elem] = rotmat 290 | 291 | 292 | def get_chain(self,root,tip,joints=True,links=True,fixed=True,tip_flag=True,return_active_joints=False): 293 | chain = [] 294 | active_joints_mask = [] 295 | chain_obj = [] 296 | chain_obj.append(self.link_map[tip]) 297 | active_joints_mask.append(False) 298 | if links: 299 | chain.append(tip) 300 | else: 301 | if tip_flag: 302 | chain.append(tip) 303 | link = tip 304 | while link != root: 305 | (joint, parent) = self.parent_map[link] 306 | if joints: 307 | #if fixed or self.joint_map[joint].type != 'fixed': 308 | chain.append(joint) 309 | chain_obj.append(self.joint_map[joint]) 310 | if self.joint_map[joint].type == 'fixed': 311 | active_joints_mask.append(False) 312 | else: 313 | active_joints_mask.append(True) 314 | if links: 315 | chain.append(parent) 316 | link = parent 317 | if not return_active_joints: 318 | return chain 319 | else: 320 | return chain, active_joints_mask, chain_obj 321 | 322 | 323 | def get_root(self): 324 | root = None 325 | for link in self.link_map: 326 | if link not in self.parent_map: 327 | root = link 328 | return root 329 | 330 | 331 | def get_all_T0(self): 332 | self.cal_configure() 333 | self.root = self.get_root() 334 | self.T0_map = {} 335 | for link in self.link_map: 336 | if link in self.parent_map: 337 | link_joint_chain = self.get_chain(self.root,link,links=False) 338 | T0 = np.eye(4) 339 | for elem in link_joint_chain: 340 | if elem in self.link_map: 341 | T_tmp = np.copy(self.link_map[elem].visual.T) 342 | if elem in self.joint_map: 343 | if elem in self.q_type_map: 344 | T_q_map = np.copy(self.q_T_map[elem]) 345 | T_tmp = np.copy(self.joint_map[elem].T).dot(T_q_map) 346 | else: 347 | T_tmp = np.copy(self.joint_map[elem].get_transformation_matrix(0)) 348 | T0 = T_tmp.dot(T0) 349 | self.T0_map[link] = self.link_map[self.root].base_T.dot(T0) 350 | self.T0_map['base_link'] = self.link_map[self.root].base_T.dot(self.link_map['base_link'].visual.T) 351 | 352 | def cal_all_bbox(self): 353 | self.cal_configure() 354 | self.get_all_T0() 355 | for link_name in self.link_map: 356 | link = self.link_map[link_name] 357 | link.collision.geometry.transformation(self.T0_map[link_name]) 358 | 359 | def vis_all_bbox(self): 360 | self.cal_all_bbox() 361 | for link_name in self.link_map: 362 | link = self.link_map[link_name] 363 | link.collision.geometry.plot() 364 | 365 | def vis(self): 366 | self.get_all_T0() 367 | self.pc_whole = [] 368 | for link_name in self.link_map: 369 | if link_name in self.parent_map: 370 | link_pc = self.link_map[link_name].visual.obj.pc_4d.dot(self.T0_map[link_name].transpose())[:,0:3] 371 | self.pc_whole.append(link_pc) 372 | self.pc_whole.append(self.link_map[self.get_root()].visual.obj.pc_4d.dot(self.link_map[self.get_root()].base_T.transpose())[:,0:3]) 373 | self.pc_whole = np.array(self.pc_whole).reshape((-1,3)) 374 | plot_pc(self.pc_whole) 375 | #mayalab.show() 376 | 377 | 378 | def inverse_kinematic(self,target_tip_tf,chain_1,chain_2,chain_3,rot=False,initial_frame=None,opt_time=2,lower_stop_thres=0.018,upper_stop_thres=0.036): 379 | 380 | if not rot: 381 | def optimize_target(x): 382 | base_frame = np.eye(4) 383 | base_frame[:3,:3] = rpy_rotmat(x[:3]) 384 | base_frame[:3,-1] = x[3:6] 385 | num_dof_1 = 2 386 | cal_frame_1 = base_frame.dot(chain_1.forward_kinematics(x[6:6+num_dof_1])) 387 | target_frame_1 = target_tip_tf[0] 388 | dist1 = np.linalg.norm(cal_frame_1[:3,3] - target_frame_1[:3,3]) 389 | num_dof_2 = 2 390 | cal_frame_2 = base_frame.dot(chain_2.forward_kinematics(x[6+num_dof_1:6+num_dof_1+num_dof_2])) 391 | target_frame_2 = target_tip_tf[1] 392 | dist2 = np.linalg.norm(cal_frame_2[:3,3] - target_frame_2[:3,3]) 393 | num_dof_3 = 1 394 | cal_frame_3 = base_frame.dot(chain_3.forward_kinematics(x[6+num_dof_1+num_dof_2:6+num_dof_1+num_dof_2+num_dof_3])) 395 | target_frame_3 = target_tip_tf[2] 396 | dist3 = np.linalg.norm(cal_frame_3[:3,3] - target_frame_3[:3,3]) 397 | #print("predi",cal_frame_3[:3,3],"target",target_frame_3[:3,-1]) 398 | #print("dist1",dist1,"dist2",dist2,"dist3",dist3) 399 | return dist3 + dist2 + dist1 400 | else: 401 | def optimize_target(x): 402 | base_frame = np.eye(4) 403 | base_frame[:3,:3] = rpy_rotmat(x[:3]) 404 | base_frame[:3,-1] = x[3:6] 405 | num_dof_1 = 2 406 | cal_frame_1 = base_frame.dot(chain_1.forward_kinematics(x[6:6+num_dof_1])) 407 | target_frame_1 = target_tip_tf[0] 408 | dist1 = np.linalg.norm(cal_frame_1[:3,3] - target_frame_1[:3,3]) 409 | dist_rot1 = np.linalg.norm(cal_frame_1[:3,1] - target_frame_1[:3,1]) 410 | num_dof_2 = 2 411 | cal_frame_2 = base_frame.dot(chain_2.forward_kinematics(x[6+num_dof_1:6+num_dof_1+num_dof_2])) 412 | target_frame_2 = target_tip_tf[1] 413 | dist2 = np.linalg.norm(cal_frame_2[:3,3] - target_frame_2[:3,3]) 414 | dist_rot2 = np.linalg.norm(cal_frame_2[:3,1] - target_frame_2[:3,1]) 415 | num_dof_3 = 1 416 | cal_frame_3 = base_frame.dot(chain_3.forward_kinematics(x[6+num_dof_1+num_dof_2:6+num_dof_1+num_dof_2+num_dof_3])) 417 | target_frame_3 = target_tip_tf[2] 418 | dist3 = np.linalg.norm(cal_frame_3[:3,3] - target_frame_3[:3,3]) 419 | dist_rot3 = np.linalg.norm(cal_frame_3[:3,1] - target_frame_3[:3,1]) 420 | #print("dist1",dist1,"distrot1",dist_rot1,"dist2",dist2,"dist_rot2",dist_rot2,"dist3",dist3,"dist_rot3",dist_rot3) 421 | return dist3 + dist2 + dist1 + dist_rot1 * 0.01 + dist_rot2 * 0.01 + dist_rot3 * 0.01 422 | 423 | if not rot: 424 | def optimize_target_vis(x): 425 | base_frame = np.eye(4) 426 | base_frame[:3,:3] = rpy_rotmat(x[:3]) 427 | base_frame[:3,-1] = x[3:6] 428 | num_dof_1 = 2 429 | cal_frame_1 = base_frame.dot(chain_1.forward_kinematics(x[6:6+num_dof_1])) 430 | target_frame_1 = target_tip_tf[0] 431 | dist1 = np.linalg.norm(cal_frame_1[:3,3] - target_frame_1[:3,3]) 432 | num_dof_2 = 2 433 | cal_frame_2 = base_frame.dot(chain_2.forward_kinematics(x[6+num_dof_1:6+num_dof_1+num_dof_2])) 434 | target_frame_2 = target_tip_tf[1] 435 | dist2 = np.linalg.norm(cal_frame_2[:3,3] - target_frame_2[:3,3]) 436 | num_dof_3 = 1 437 | cal_frame_3 = base_frame.dot(chain_3.forward_kinematics(x[6+num_dof_1+num_dof_2:6+num_dof_1+num_dof_2+num_dof_3])) 438 | target_frame_3 = target_tip_tf[2] 439 | dist3 = np.linalg.norm(cal_frame_3[:3,3] - target_frame_3[:3,3]) 440 | #print("dist1",dist1,"dist2",dist2,"dist3",dist3) 441 | return dist3 + dist2 + dist1 442 | else: 443 | def optimize_target_vis(x): 444 | base_frame = np.eye(4) 445 | base_frame[:3,:3] = rpy_rotmat(x[:3]) 446 | base_frame[:3,-1] = x[3:6] 447 | num_dof_1 = 2 448 | cal_frame_1 = base_frame.dot(chain_1.forward_kinematics(x[6:6+num_dof_1])) 449 | target_frame_1 = target_tip_tf[0] 450 | dist1 = np.linalg.norm(cal_frame_1[:3,3] - target_frame_1[:3,3]) 451 | dist_rot1 = np.linalg.norm(cal_frame_1[:3,1] - target_frame_1[:3,1]) 452 | num_dof_2 = 2 453 | cal_frame_2 = base_frame.dot(chain_2.forward_kinematics(x[6+num_dof_1:6+num_dof_1+num_dof_2])) 454 | target_frame_2 = target_tip_tf[1] 455 | dist2 = np.linalg.norm(cal_frame_2[:3,3] - target_frame_2[:3,3]) 456 | dist_rot2 = np.linalg.norm(cal_frame_2[:3,1] - target_frame_2[:3,1]) 457 | num_dof_3 = 1 458 | cal_frame_3 = base_frame.dot(chain_3.forward_kinematics(x[6+num_dof_1+num_dof_2:6+num_dof_1+num_dof_2+num_dof_3])) 459 | target_frame_3 = target_tip_tf[2] 460 | dist3 = np.linalg.norm(cal_frame_3[:3,3] - target_frame_3[:3,3]) 461 | dist_rot3 = np.linalg.norm(cal_frame_3[:3,1] - target_frame_3[:3,1]) 462 | print("dist1",dist1,"distrot1",dist_rot1,"dist2",dist2,"dist_rot2",dist_rot2,"dist3",dist3,"dist_rot3",dist_rot3) 463 | return dist3 + dist2 + dist1 + dist_rot1 * 0.01 + dist_rot2 * 0.01 + dist_rot3 * 0.01 464 | 465 | 466 | # compute bounds 467 | if initial_frame is None: 468 | #real_bounds = [(-3.14,3.14)] * (3) 469 | #real_bounds += [(-0.25,0.25)] * (3) 470 | #real_bounds += [(0,0)] * 3 471 | real_bounds = [(0,0)] * 6 472 | else: 473 | initial_rpy = euler_from_matrix(initial_frame[:3,:3]) 474 | real_bounds = [(-3.14/10 + initial_rpy[0], 3.14/10 + initial_rpy[0])] 475 | real_bounds += [(-3.14/10 + initial_rpy[1], 3.14/10 + initial_rpy[1])] 476 | real_bounds += [(-3.14/10 + initial_rpy[2], 3.14/10 + initial_rpy[2])] 477 | 478 | real_bounds += [(-0.05 + initial_frame[0,3],0.05 + initial_frame[0,3])] 479 | real_bounds += [(-0.05 + initial_frame[1,3],0.05 + initial_frame[1,3])] 480 | real_bounds += [(-0.05 + initial_frame[2,3],0.05 + initial_frame[2,3])] 481 | 482 | 483 | for idx,joint in enumerate(chain_1.joints): 484 | if chain_1.active_joints_mask[idx]: 485 | real_bounds.append((joint.limit.lower,joint.limit.upper)) 486 | 487 | for idx, joint in enumerate(chain_2.joints): 488 | if chain_2.active_joints_mask[idx]: 489 | real_bounds.append((joint.limit.lower,joint.limit.upper)) 490 | 491 | for idx, joint in enumerate(chain_3.joints): 492 | if chain_3.active_joints_mask[idx]: 493 | real_bounds.append((joint.limit.lower,joint.limit.upper)) 494 | 495 | min_fun = 1000000.0 496 | min_res_x = None 497 | 498 | for i in range(opt_time): 499 | print("opti",i) 500 | initial_pos = [] 501 | for (l,h) in real_bounds: 502 | if h == l: 503 | initial_pos.append(l) 504 | else: 505 | initial_pos.append(np.random.uniform(l,h)) 506 | #inital_pos = (6 + 4 + 4 + 3) * [0] 507 | res = scipy.optimize.minimize(optimize_target, initial_pos, method="L-BFGS-B",bounds=real_bounds) 508 | #print(res.fun) 509 | if res.fun < min_fun: 510 | min_fun = res.fun 511 | min_res_x = res.x 512 | #if min_fun < lower_stop_thres or min_fun > upper_stop_thres: 513 | # break 514 | print("min_fun",min_fun,"min_x",min_res_x) 515 | print("min_fun",min_fun, "min_res_x",min_res_x,"actual_",optimize_target_vis(min_res_x)) 516 | return min_res_x, min_fun 517 | 518 | 519 | if __name__ == "__main__": 520 | ###### Example of loading and visualizing the robotiq3f 521 | 522 | GRIPPER_DIR = "./gripper_data/robotiq3f" 523 | MESH_DIR = "./gripper_data/robotiq3f/meshes" 524 | 525 | ### Gripper Model 526 | urdf_file = os.path.join(GRIPPER_DIR,"robotiq_3f_test.urdf") 527 | with open(urdf_file,'r') as myfile: 528 | urdf_strings = myfile.read().replace('\n','') 529 | gripper_in_urdf = URDF.from_xml_string(urdf_strings) 530 | 531 | tip_pc = np.array([0.033,0.0079,0]) 532 | gripper = Gripper(gripper_in_urdf=gripper_in_urdf,tip_pc=tip_pc,mesh_top_dir=MESH_DIR) 533 | gripper.joint_map['finger_1_joint_3'].default_joint_value = -0.6632 534 | gripper.joint_map['finger_2_joint_3'].default_joint_value = -0.6632 535 | gripper.joint_map['finger_middle_joint_3'].default_joint_value = -0.6632 536 | 537 | gripper.get_root() 538 | gripper.get_all_T0() 539 | gripper.vis() 540 | mayalab.show() 541 | 542 | ##### Forward Kinematics 543 | gripper.q_value_map['finger_1_joint_1'] = 0.9 544 | gripper.q_value_map['palm_finger_1_joint'] = 0.9 545 | gripper.q_value_map['finger_2_joint_1'] = 0.9 546 | gripper.q_value_map['palm_finger_2_joint'] = 0.9 547 | gripper.q_value_map['finger_middle_joint_1'] = 0.9 548 | 549 | gripper.vis() 550 | mayalab.show() 551 | 552 | ######Inverse Kinematics 553 | f1_tip = gripper.chain_map['finger_1_link_3'] 554 | f2_tip = gripper.chain_map['finger_2_link_3'] 555 | f3_tip = gripper.chain_map['finger_middle_link_3'] 556 | 557 | target_base_frame = np.eye(4) 558 | gripper.link_map['base_link'].base_T = np.copy(target_base_frame) 559 | 560 | target_frame_1 = f1_tip.forward_kinematics([0.4,0.1]) 561 | target_frame_2 = f2_tip.forward_kinematics([0.5,-0.1]) 562 | target_frame_3 = f3_tip.forward_kinematics([0.1]) 563 | 564 | target_tip_1 = target_base_frame.dot(target_frame_1) 565 | target_tip_2 = target_base_frame.dot(target_frame_2) 566 | target_tip_3 = target_base_frame.dot(target_frame_3) 567 | 568 | f1_tip_pc = target_tip_1[:3,3] 569 | f1_tip_pc = np.array(f1_tip_pc).reshape((-1,3)) 570 | f1_tip_n = np.copy(target_tip_1[:3,1]) 571 | f1_tip_n = np.array(f1_tip_n).reshape((-1,3)) 572 | plot_pc(f1_tip_pc,color='red',mode='sphere',scale_factor=.01) 573 | plot_pc_with_normal(f1_tip_pc,f1_tip_n * 0.01,scale_factor=0.1) 574 | 575 | f2_tip_pc = target_tip_2[:3,3] 576 | f2_tip_pc = np.array(f2_tip_pc).reshape((-1,3)) 577 | f2_tip_n = np.copy(target_tip_2[:3,1]) 578 | f2_tip_n = np.array(f2_tip_n).reshape((-1,3)) 579 | plot_pc(f2_tip_pc,color='green',mode='sphere',scale_factor=.01) 580 | plot_pc_with_normal(f2_tip_pc,f2_tip_n * 0.01,scale_factor=0.1) 581 | 582 | f3_tip_pc = target_tip_3[:3,3] 583 | f3_tip_pc = np.array(f3_tip_pc).reshape((-1,3)) 584 | f3_tip_n = np.copy(target_tip_3[:3,1]) 585 | f3_tip_n = np.array(f3_tip_n).reshape((-1,3)) 586 | plot_pc(f3_tip_pc,color='blue',mode='sphere',scale_factor=.01) 587 | plot_pc_with_normal(f3_tip_pc,f3_tip_n * 0.01,scale_factor=0.1) 588 | 589 | target_tips = [] 590 | target_tips.append(target_tip_1) 591 | target_tips.append(target_tip_2) 592 | target_tips.append(target_tip_3) 593 | 594 | plot_origin() 595 | 596 | #mayalab.show() 597 | 598 | q_list,_ = gripper.inverse_kinematic(target_tips,gripper.chain_map['finger_1_link_3'],gripper.chain_map['finger_2_link_3'],gripper.chain_map['finger_middle_link_3'],rot=True) 599 | gripper.link_map['base_link'].base_T = np.eye(4) 600 | gripper.link_map['base_link'].base_T[:3,:3] = np.copy(rpy_rotmat(q_list[:3])) 601 | gripper.link_map['base_link'].base_T[:3,-1] = np.copy(q_list[3:6]) 602 | 603 | count = 6 604 | for idx,joint in enumerate(f1_tip.joints): 605 | if f1_tip.active_joints_mask[idx]: 606 | gripper.q_value_map[joint.name] = q_list[count] 607 | print("joint.name",joint.name,q_list[count],count) 608 | count = count + 1 609 | predict_f1_tip = gripper.link_map['base_link'].base_T.dot(gripper.chain_map['finger_1_link_3'].forward_kinematics([q_list[6],q_list[7]])) 610 | predict_f1_tip = np.array(predict_f1_tip[:3,-1]).reshape((-1,3)) 611 | plot_pc(predict_f1_tip,color='red',mode='cube',scale_factor=.005) 612 | 613 | for idx,joint in enumerate(f2_tip.joints): 614 | if f2_tip.active_joints_mask[idx]: 615 | gripper.q_value_map[joint.name] = q_list[count] 616 | print("joint.name", joint.name, q_list[count], count) 617 | count = count + 1 618 | 619 | predict_f2_tip = gripper.link_map['base_link'].base_T.dot(gripper.chain_map['finger_2_link_3'].forward_kinematics([q_list[8],q_list[9]])) 620 | predict_f2_tip = np.array(predict_f2_tip[:3,-1]).reshape((-1,3)) 621 | plot_pc(predict_f2_tip,color='green',mode='cube',scale_factor=.005) 622 | 623 | for idx,joint in enumerate(f3_tip.joints): 624 | if f3_tip.active_joints_mask[idx]: 625 | gripper.q_value_map[joint.name] = q_list[count] 626 | print("joint.name", joint.name, q_list[count], count) 627 | count = count + 1 628 | predict_f3_tip = gripper.link_map['base_link'].base_T.dot(gripper.chain_map['finger_middle_link_3'].forward_kinematics([q_list[10]])) 629 | predict_f3_tip = np.array(predict_f3_tip[:3,-1]).reshape((-1,3)) 630 | plot_pc(predict_f3_tip,color='blue',mode='cube',scale_factor=.005) 631 | 632 | #gripper.get_all_T0() 633 | gripper.vis() 634 | mayalab.show() 635 | 636 | 637 | #### Sample Point Clouds 638 | gripper.pc_whole 639 | 640 | 641 | -------------------------------------------------------------------------------- /geolib/coupling.obj: -------------------------------------------------------------------------------- 1 | v -0.000000 -0.040000 0.000000 2 | v 0.004181 -0.039781 0.025000 3 | v 0.004181 -0.039781 0.000000 4 | v 0.004181 -0.039781 0.000000 5 | v 0.004181 -0.039781 0.025000 6 | v 0.008316 -0.039126 0.025000 7 | v 0.004181 -0.039781 0.000000 8 | v 0.008316 -0.039126 0.025000 9 | v 0.008316 -0.039126 0.000000 10 | v 0.008316 -0.039126 0.000000 11 | v 0.008316 -0.039126 0.025000 12 | v 0.012361 -0.038042 0.025000 13 | v 0.008316 -0.039126 0.000000 14 | v 0.012361 -0.038042 0.025000 15 | v 0.012361 -0.038042 0.000000 16 | v 0.012361 -0.038042 0.000000 17 | v 0.012361 -0.038042 0.025000 18 | v 0.016269 -0.036542 0.025000 19 | v 0.012361 -0.038042 0.000000 20 | v 0.016269 -0.036542 0.025000 21 | v 0.016269 -0.036542 0.000000 22 | v 0.016269 -0.036542 0.000000 23 | v 0.016269 -0.036542 0.025000 24 | v 0.020000 -0.034641 0.025000 25 | v 0.016269 -0.036542 0.000000 26 | v 0.020000 -0.034641 0.025000 27 | v 0.020000 -0.034641 0.000000 28 | v 0.020000 -0.034641 0.000000 29 | v 0.020000 -0.034641 0.025000 30 | v 0.023511 -0.032361 0.025000 31 | v 0.020000 -0.034641 0.000000 32 | v 0.023511 -0.032361 0.025000 33 | v 0.023511 -0.032361 0.000000 34 | v 0.023511 -0.032361 0.000000 35 | v 0.023511 -0.032361 0.025000 36 | v 0.026765 -0.029726 0.025000 37 | v 0.023511 -0.032361 0.000000 38 | v 0.026765 -0.029726 0.025000 39 | v 0.026765 -0.029726 0.000000 40 | v 0.026765 -0.029726 0.000000 41 | v 0.026765 -0.029726 0.025000 42 | v 0.029726 -0.026765 0.025000 43 | v 0.026765 -0.029726 0.000000 44 | v 0.029726 -0.026765 0.025000 45 | v 0.029726 -0.026765 0.000000 46 | v 0.029726 -0.026765 0.000000 47 | v 0.029726 -0.026765 0.025000 48 | v 0.032361 -0.023511 0.025000 49 | v 0.029726 -0.026765 0.000000 50 | v 0.032361 -0.023511 0.025000 51 | v 0.032361 -0.023511 0.000000 52 | v 0.032361 -0.023511 0.000000 53 | v 0.032361 -0.023511 0.025000 54 | v 0.034641 -0.020000 0.025000 55 | v 0.032361 -0.023511 0.000000 56 | v 0.034641 -0.020000 0.025000 57 | v 0.034641 -0.020000 0.000000 58 | v 0.034641 -0.020000 0.000000 59 | v 0.034641 -0.020000 0.025000 60 | v 0.036542 -0.016269 0.025000 61 | v 0.034641 -0.020000 0.000000 62 | v 0.036542 -0.016269 0.025000 63 | v 0.036542 -0.016269 0.000000 64 | v 0.036542 -0.016269 0.000000 65 | v 0.036542 -0.016269 0.025000 66 | v 0.038042 -0.012361 0.025000 67 | v 0.036542 -0.016269 0.000000 68 | v 0.038042 -0.012361 0.025000 69 | v 0.038042 -0.012361 0.000000 70 | v 0.038042 -0.012361 0.000000 71 | v 0.038042 -0.012361 0.025000 72 | v 0.039126 -0.008316 0.025000 73 | v 0.038042 -0.012361 0.000000 74 | v 0.039126 -0.008316 0.025000 75 | v 0.039126 -0.008316 0.000000 76 | v 0.039126 -0.008316 0.000000 77 | v 0.039126 -0.008316 0.025000 78 | v 0.039781 -0.004181 0.025000 79 | v 0.039126 -0.008316 0.000000 80 | v 0.039781 -0.004181 0.025000 81 | v 0.039781 -0.004181 0.000000 82 | v 0.039781 -0.004181 0.000000 83 | v 0.039781 -0.004181 0.025000 84 | v 0.040000 -0.000000 0.025000 85 | v 0.039781 -0.004181 0.000000 86 | v 0.040000 -0.000000 0.025000 87 | v 0.040000 -0.000000 0.000000 88 | v 0.040000 -0.000000 0.000000 89 | v 0.040000 -0.000000 0.025000 90 | v 0.039781 0.004181 0.025000 91 | v 0.040000 -0.000000 0.000000 92 | v 0.039781 0.004181 0.025000 93 | v 0.039781 0.004181 0.000000 94 | v 0.039781 0.004181 0.000000 95 | v 0.039781 0.004181 0.025000 96 | v 0.039126 0.008316 0.025000 97 | v 0.039781 0.004181 0.000000 98 | v 0.039126 0.008316 0.025000 99 | v 0.039126 0.008316 0.000000 100 | v 0.039126 0.008316 0.000000 101 | v 0.039126 0.008316 0.025000 102 | v 0.038042 0.012361 0.025000 103 | v 0.039126 0.008316 0.000000 104 | v 0.038042 0.012361 0.025000 105 | v 0.038042 0.012361 0.000000 106 | v 0.038042 0.012361 0.000000 107 | v 0.038042 0.012361 0.025000 108 | v 0.036542 0.016269 0.025000 109 | v 0.038042 0.012361 0.000000 110 | v 0.036542 0.016269 0.025000 111 | v 0.036542 0.016269 0.000000 112 | v 0.036542 0.016269 0.000000 113 | v 0.036542 0.016269 0.025000 114 | v 0.034641 0.020000 0.025000 115 | v 0.036542 0.016269 0.000000 116 | v 0.034641 0.020000 0.025000 117 | v 0.034641 0.020000 0.000000 118 | v 0.034641 0.020000 0.000000 119 | v 0.034641 0.020000 0.025000 120 | v 0.032361 0.023511 0.025000 121 | v 0.034641 0.020000 0.000000 122 | v 0.032361 0.023511 0.025000 123 | v 0.032361 0.023511 0.000000 124 | v 0.032361 0.023511 0.000000 125 | v 0.032361 0.023511 0.025000 126 | v 0.029726 0.026765 0.025000 127 | v 0.032361 0.023511 0.000000 128 | v 0.029726 0.026765 0.025000 129 | v 0.029726 0.026765 0.000000 130 | v 0.029726 0.026765 0.000000 131 | v 0.029726 0.026765 0.025000 132 | v 0.026765 0.029726 0.025000 133 | v 0.029726 0.026765 0.000000 134 | v 0.026765 0.029726 0.025000 135 | v 0.026765 0.029726 0.000000 136 | v 0.026765 0.029726 0.000000 137 | v 0.026765 0.029726 0.025000 138 | v 0.023511 0.032361 0.025000 139 | v 0.026765 0.029726 0.000000 140 | v 0.023511 0.032361 0.025000 141 | v 0.023511 0.032361 0.000000 142 | v 0.023511 0.032361 0.000000 143 | v 0.023511 0.032361 0.025000 144 | v 0.020000 0.034641 0.025000 145 | v 0.023511 0.032361 0.000000 146 | v 0.020000 0.034641 0.025000 147 | v 0.020000 0.034641 0.000000 148 | v 0.020000 0.034641 0.000000 149 | v 0.020000 0.034641 0.025000 150 | v 0.016269 0.036542 0.025000 151 | v 0.020000 0.034641 0.000000 152 | v 0.016269 0.036542 0.025000 153 | v 0.016269 0.036542 0.000000 154 | v 0.016269 0.036542 0.000000 155 | v 0.016269 0.036542 0.025000 156 | v 0.012361 0.038042 0.025000 157 | v 0.016269 0.036542 0.000000 158 | v 0.012361 0.038042 0.025000 159 | v 0.012361 0.038042 0.000000 160 | v 0.012361 0.038042 0.000000 161 | v 0.012361 0.038042 0.025000 162 | v 0.008316 0.039126 0.025000 163 | v 0.012361 0.038042 0.000000 164 | v 0.008316 0.039126 0.025000 165 | v 0.008316 0.039126 0.000000 166 | v 0.008316 0.039126 0.000000 167 | v 0.008316 0.039126 0.025000 168 | v 0.004181 0.039781 0.025000 169 | v 0.008316 0.039126 0.000000 170 | v 0.004181 0.039781 0.025000 171 | v 0.004181 0.039781 0.000000 172 | v 0.004181 0.039781 0.000000 173 | v 0.004181 0.039781 0.025000 174 | v -0.000000 0.040000 0.025000 175 | v 0.004181 0.039781 0.000000 176 | v -0.000000 0.040000 0.025000 177 | v -0.000000 0.040000 0.000000 178 | v -0.000000 0.040000 0.000000 179 | v -0.000000 0.040000 0.025000 180 | v -0.004181 0.039781 0.025000 181 | v -0.000000 0.040000 0.000000 182 | v -0.004181 0.039781 0.025000 183 | v -0.004181 0.039781 0.000000 184 | v -0.004181 0.039781 0.000000 185 | v -0.004181 0.039781 0.025000 186 | v -0.008316 0.039126 0.025000 187 | v -0.004181 0.039781 0.000000 188 | v -0.008316 0.039126 0.025000 189 | v -0.008316 0.039126 0.000000 190 | v -0.008316 0.039126 0.000000 191 | v -0.008316 0.039126 0.025000 192 | v -0.012361 0.038042 0.025000 193 | v -0.008316 0.039126 0.000000 194 | v -0.012361 0.038042 0.025000 195 | v -0.012361 0.038042 0.000000 196 | v -0.012361 0.038042 0.000000 197 | v -0.012361 0.038042 0.025000 198 | v -0.016269 0.036542 0.025000 199 | v -0.012361 0.038042 0.000000 200 | v -0.016269 0.036542 0.025000 201 | v -0.016269 0.036542 0.000000 202 | v -0.016269 0.036542 0.000000 203 | v -0.016269 0.036542 0.025000 204 | v -0.020000 0.034641 0.025000 205 | v -0.016269 0.036542 0.000000 206 | v -0.020000 0.034641 0.025000 207 | v -0.020000 0.034641 0.000000 208 | v -0.020000 0.034641 0.000000 209 | v -0.020000 0.034641 0.025000 210 | v -0.023511 0.032361 0.025000 211 | v -0.020000 0.034641 0.000000 212 | v -0.023511 0.032361 0.025000 213 | v -0.023511 0.032361 0.000000 214 | v -0.023511 0.032361 0.000000 215 | v -0.023511 0.032361 0.025000 216 | v -0.026765 0.029726 0.025000 217 | v -0.023511 0.032361 0.000000 218 | v -0.026765 0.029726 0.025000 219 | v -0.026765 0.029726 0.000000 220 | v -0.026765 0.029726 0.000000 221 | v -0.026765 0.029726 0.025000 222 | v -0.029726 0.026765 0.025000 223 | v -0.026765 0.029726 0.000000 224 | v -0.029726 0.026765 0.025000 225 | v -0.029726 0.026765 0.000000 226 | v -0.029726 0.026765 0.000000 227 | v -0.029726 0.026765 0.025000 228 | v -0.032361 0.023511 0.025000 229 | v -0.029726 0.026765 0.000000 230 | v -0.032361 0.023511 0.025000 231 | v -0.032361 0.023511 0.000000 232 | v -0.032361 0.023511 0.000000 233 | v -0.032361 0.023511 0.025000 234 | v -0.034641 0.020000 0.025000 235 | v -0.032361 0.023511 0.000000 236 | v -0.034641 0.020000 0.025000 237 | v -0.034641 0.020000 0.000000 238 | v -0.034641 0.020000 0.000000 239 | v -0.034641 0.020000 0.025000 240 | v -0.036542 0.016269 0.025000 241 | v -0.034641 0.020000 0.000000 242 | v -0.036542 0.016269 0.025000 243 | v -0.036542 0.016269 0.000000 244 | v -0.036542 0.016269 0.000000 245 | v -0.036542 0.016269 0.025000 246 | v -0.038042 0.012361 0.025000 247 | v -0.036542 0.016269 0.000000 248 | v -0.038042 0.012361 0.025000 249 | v -0.038042 0.012361 0.000000 250 | v -0.038042 0.012361 0.000000 251 | v -0.038042 0.012361 0.025000 252 | v -0.039126 0.008316 0.025000 253 | v -0.038042 0.012361 0.000000 254 | v -0.039126 0.008316 0.025000 255 | v -0.039126 0.008316 0.000000 256 | v -0.039126 0.008316 0.000000 257 | v -0.039126 0.008316 0.025000 258 | v -0.039781 0.004181 0.025000 259 | v -0.039126 0.008316 0.000000 260 | v -0.039781 0.004181 0.025000 261 | v -0.039781 0.004181 0.000000 262 | v -0.039781 0.004181 0.000000 263 | v -0.039781 0.004181 0.025000 264 | v -0.040000 -0.000000 0.025000 265 | v -0.039781 0.004181 0.000000 266 | v -0.040000 -0.000000 0.025000 267 | v -0.040000 -0.000000 0.000000 268 | v -0.040000 -0.000000 0.000000 269 | v -0.040000 -0.000000 0.025000 270 | v -0.039781 -0.004181 0.025000 271 | v -0.040000 -0.000000 0.000000 272 | v -0.039781 -0.004181 0.025000 273 | v -0.039781 -0.004181 0.000000 274 | v -0.039781 -0.004181 0.000000 275 | v -0.039781 -0.004181 0.025000 276 | v -0.039126 -0.008316 0.025000 277 | v -0.039781 -0.004181 0.000000 278 | v -0.039126 -0.008316 0.025000 279 | v -0.039126 -0.008316 0.000000 280 | v -0.039126 -0.008316 0.000000 281 | v -0.039126 -0.008316 0.025000 282 | v -0.038042 -0.012361 0.025000 283 | v -0.039126 -0.008316 0.000000 284 | v -0.038042 -0.012361 0.025000 285 | v -0.038042 -0.012361 0.000000 286 | v -0.038042 -0.012361 0.000000 287 | v -0.038042 -0.012361 0.025000 288 | v -0.036542 -0.016269 0.025000 289 | v -0.038042 -0.012361 0.000000 290 | v -0.036542 -0.016269 0.025000 291 | v -0.036542 -0.016269 0.000000 292 | v -0.036542 -0.016269 0.000000 293 | v -0.036542 -0.016269 0.025000 294 | v -0.034641 -0.020000 0.025000 295 | v -0.036542 -0.016269 0.000000 296 | v -0.034641 -0.020000 0.025000 297 | v -0.034641 -0.020000 0.000000 298 | v -0.034641 -0.020000 0.000000 299 | v -0.034641 -0.020000 0.025000 300 | v -0.032361 -0.023511 0.025000 301 | v -0.034641 -0.020000 0.000000 302 | v -0.032361 -0.023511 0.025000 303 | v -0.032361 -0.023511 0.000000 304 | v -0.032361 -0.023511 0.000000 305 | v -0.032361 -0.023511 0.025000 306 | v -0.029726 -0.026765 0.025000 307 | v -0.032361 -0.023511 0.000000 308 | v -0.029726 -0.026765 0.025000 309 | v -0.029726 -0.026765 0.000000 310 | v -0.029726 -0.026765 0.000000 311 | v -0.029726 -0.026765 0.025000 312 | v -0.026765 -0.029726 0.025000 313 | v -0.029726 -0.026765 0.000000 314 | v -0.026765 -0.029726 0.025000 315 | v -0.026765 -0.029726 0.000000 316 | v -0.026765 -0.029726 0.000000 317 | v -0.026765 -0.029726 0.025000 318 | v -0.023511 -0.032361 0.025000 319 | v -0.026765 -0.029726 0.000000 320 | v -0.023511 -0.032361 0.025000 321 | v -0.023511 -0.032361 0.000000 322 | v -0.023511 -0.032361 0.000000 323 | v -0.023511 -0.032361 0.025000 324 | v -0.020000 -0.034641 0.025000 325 | v -0.023511 -0.032361 0.000000 326 | v -0.020000 -0.034641 0.025000 327 | v -0.020000 -0.034641 0.000000 328 | v -0.020000 -0.034641 0.000000 329 | v -0.020000 -0.034641 0.025000 330 | v -0.016269 -0.036542 0.025000 331 | v -0.020000 -0.034641 0.000000 332 | v -0.016269 -0.036542 0.025000 333 | v -0.016269 -0.036542 0.000000 334 | v -0.016269 -0.036542 0.000000 335 | v -0.016269 -0.036542 0.025000 336 | v -0.012361 -0.038042 0.025000 337 | v -0.016269 -0.036542 0.000000 338 | v -0.012361 -0.038042 0.025000 339 | v -0.012361 -0.038042 0.000000 340 | v -0.012361 -0.038042 0.000000 341 | v -0.012361 -0.038042 0.025000 342 | v -0.008316 -0.039126 0.025000 343 | v -0.012361 -0.038042 0.000000 344 | v -0.008316 -0.039126 0.025000 345 | v -0.008316 -0.039126 0.000000 346 | v -0.008316 -0.039126 0.000000 347 | v -0.008316 -0.039126 0.025000 348 | v -0.004181 -0.039781 0.025000 349 | v -0.008316 -0.039126 0.000000 350 | v -0.004181 -0.039781 0.025000 351 | v -0.004181 -0.039781 0.000000 352 | v -0.004181 -0.039781 0.000000 353 | v -0.004181 -0.039781 0.025000 354 | v -0.000000 -0.040000 0.025000 355 | v -0.004181 -0.039781 0.000000 356 | v -0.000000 -0.040000 0.025000 357 | v -0.000000 -0.040000 0.000000 358 | v -0.000000 -0.040000 0.000000 359 | v -0.000000 -0.040000 0.025000 360 | v 0.004181 -0.039781 0.025000 361 | v -0.008316 0.039126 0.025000 362 | v -0.004181 0.039781 0.025000 363 | v -0.000000 -0.000000 0.025000 364 | v -0.000000 -0.000000 0.025000 365 | v -0.004181 0.039781 0.025000 366 | v -0.000000 0.040000 0.025000 367 | v -0.000000 -0.000000 0.025000 368 | v -0.000000 0.040000 0.025000 369 | v 0.004181 0.039781 0.025000 370 | v -0.020000 0.034641 0.025000 371 | v -0.016269 0.036542 0.025000 372 | v -0.000000 -0.000000 0.025000 373 | v -0.000000 -0.000000 0.025000 374 | v -0.016269 0.036542 0.025000 375 | v -0.012361 0.038042 0.025000 376 | v -0.000000 -0.000000 0.025000 377 | v -0.012361 0.038042 0.025000 378 | v -0.008316 0.039126 0.025000 379 | v -0.029726 0.026765 0.025000 380 | v -0.026765 0.029726 0.025000 381 | v -0.000000 -0.000000 0.025000 382 | v -0.000000 -0.000000 0.025000 383 | v -0.026765 0.029726 0.025000 384 | v -0.023511 0.032361 0.025000 385 | v -0.000000 -0.000000 0.025000 386 | v -0.023511 0.032361 0.025000 387 | v -0.020000 0.034641 0.025000 388 | v -0.036542 0.016269 0.025000 389 | v -0.034641 0.020000 0.025000 390 | v -0.000000 -0.000000 0.025000 391 | v -0.000000 -0.000000 0.025000 392 | v -0.034641 0.020000 0.025000 393 | v -0.032361 0.023511 0.025000 394 | v -0.000000 -0.000000 0.025000 395 | v -0.032361 0.023511 0.025000 396 | v -0.029726 0.026765 0.025000 397 | v -0.039781 0.004181 0.025000 398 | v -0.039126 0.008316 0.025000 399 | v -0.000000 -0.000000 0.025000 400 | v -0.000000 -0.000000 0.025000 401 | v -0.039126 0.008316 0.025000 402 | v -0.038042 0.012361 0.025000 403 | v -0.000000 -0.000000 0.025000 404 | v -0.038042 0.012361 0.025000 405 | v -0.036542 0.016269 0.025000 406 | v -0.039126 -0.008316 0.025000 407 | v -0.039781 -0.004181 0.025000 408 | v -0.000000 -0.000000 0.025000 409 | v -0.000000 -0.000000 0.025000 410 | v -0.039781 -0.004181 0.025000 411 | v -0.040000 -0.000000 0.025000 412 | v -0.000000 -0.000000 0.025000 413 | v -0.040000 -0.000000 0.025000 414 | v -0.039781 0.004181 0.025000 415 | v -0.034641 -0.020000 0.025000 416 | v -0.036542 -0.016269 0.025000 417 | v -0.000000 -0.000000 0.025000 418 | v -0.000000 -0.000000 0.025000 419 | v -0.036542 -0.016269 0.025000 420 | v -0.038042 -0.012361 0.025000 421 | v -0.000000 -0.000000 0.025000 422 | v -0.038042 -0.012361 0.025000 423 | v -0.039126 -0.008316 0.025000 424 | v -0.026765 -0.029726 0.025000 425 | v -0.029726 -0.026765 0.025000 426 | v -0.000000 -0.000000 0.025000 427 | v -0.000000 -0.000000 0.025000 428 | v -0.029726 -0.026765 0.025000 429 | v -0.032361 -0.023511 0.025000 430 | v -0.000000 -0.000000 0.025000 431 | v -0.032361 -0.023511 0.025000 432 | v -0.034641 -0.020000 0.025000 433 | v -0.016269 -0.036542 0.025000 434 | v -0.020000 -0.034641 0.025000 435 | v -0.000000 -0.000000 0.025000 436 | v -0.000000 -0.000000 0.025000 437 | v -0.020000 -0.034641 0.025000 438 | v -0.023511 -0.032361 0.025000 439 | v -0.000000 -0.000000 0.025000 440 | v -0.023511 -0.032361 0.025000 441 | v -0.026765 -0.029726 0.025000 442 | v -0.004181 -0.039781 0.025000 443 | v -0.008316 -0.039126 0.025000 444 | v -0.000000 -0.000000 0.025000 445 | v -0.000000 -0.000000 0.025000 446 | v -0.008316 -0.039126 0.025000 447 | v -0.012361 -0.038042 0.025000 448 | v -0.000000 -0.000000 0.025000 449 | v -0.012361 -0.038042 0.025000 450 | v -0.016269 -0.036542 0.025000 451 | v 0.008316 -0.039126 0.025000 452 | v 0.004181 -0.039781 0.025000 453 | v -0.000000 -0.000000 0.025000 454 | v -0.000000 -0.000000 0.025000 455 | v 0.004181 -0.039781 0.025000 456 | v -0.000000 -0.040000 0.025000 457 | v -0.000000 -0.000000 0.025000 458 | v -0.000000 -0.040000 0.025000 459 | v -0.004181 -0.039781 0.025000 460 | v 0.020000 -0.034641 0.025000 461 | v 0.016269 -0.036542 0.025000 462 | v -0.000000 -0.000000 0.025000 463 | v -0.000000 -0.000000 0.025000 464 | v 0.016269 -0.036542 0.025000 465 | v 0.012361 -0.038042 0.025000 466 | v -0.000000 -0.000000 0.025000 467 | v 0.012361 -0.038042 0.025000 468 | v 0.008316 -0.039126 0.025000 469 | v 0.029726 -0.026765 0.025000 470 | v 0.026765 -0.029726 0.025000 471 | v -0.000000 -0.000000 0.025000 472 | v -0.000000 -0.000000 0.025000 473 | v 0.026765 -0.029726 0.025000 474 | v 0.023511 -0.032361 0.025000 475 | v -0.000000 -0.000000 0.025000 476 | v 0.023511 -0.032361 0.025000 477 | v 0.020000 -0.034641 0.025000 478 | v 0.036542 -0.016269 0.025000 479 | v 0.034641 -0.020000 0.025000 480 | v -0.000000 -0.000000 0.025000 481 | v -0.000000 -0.000000 0.025000 482 | v 0.034641 -0.020000 0.025000 483 | v 0.032361 -0.023511 0.025000 484 | v -0.000000 -0.000000 0.025000 485 | v 0.032361 -0.023511 0.025000 486 | v 0.029726 -0.026765 0.025000 487 | v 0.039781 -0.004181 0.025000 488 | v 0.039126 -0.008316 0.025000 489 | v -0.000000 -0.000000 0.025000 490 | v -0.000000 -0.000000 0.025000 491 | v 0.039126 -0.008316 0.025000 492 | v 0.038042 -0.012361 0.025000 493 | v -0.000000 -0.000000 0.025000 494 | v 0.038042 -0.012361 0.025000 495 | v 0.036542 -0.016269 0.025000 496 | v 0.039126 0.008316 0.025000 497 | v 0.039781 0.004181 0.025000 498 | v -0.000000 -0.000000 0.025000 499 | v -0.000000 -0.000000 0.025000 500 | v 0.039781 0.004181 0.025000 501 | v 0.040000 -0.000000 0.025000 502 | v -0.000000 -0.000000 0.025000 503 | v 0.040000 -0.000000 0.025000 504 | v 0.039781 -0.004181 0.025000 505 | v 0.034641 0.020000 0.025000 506 | v 0.036542 0.016269 0.025000 507 | v -0.000000 -0.000000 0.025000 508 | v -0.000000 -0.000000 0.025000 509 | v 0.036542 0.016269 0.025000 510 | v 0.038042 0.012361 0.025000 511 | v -0.000000 -0.000000 0.025000 512 | v 0.038042 0.012361 0.025000 513 | v 0.039126 0.008316 0.025000 514 | v 0.026765 0.029726 0.025000 515 | v 0.029726 0.026765 0.025000 516 | v -0.000000 -0.000000 0.025000 517 | v -0.000000 -0.000000 0.025000 518 | v 0.029726 0.026765 0.025000 519 | v 0.032361 0.023511 0.025000 520 | v -0.000000 -0.000000 0.025000 521 | v 0.032361 0.023511 0.025000 522 | v 0.034641 0.020000 0.025000 523 | v 0.016269 0.036542 0.025000 524 | v 0.020000 0.034641 0.025000 525 | v -0.000000 -0.000000 0.025000 526 | v -0.000000 -0.000000 0.025000 527 | v 0.020000 0.034641 0.025000 528 | v 0.023511 0.032361 0.025000 529 | v -0.000000 -0.000000 0.025000 530 | v 0.023511 0.032361 0.025000 531 | v 0.026765 0.029726 0.025000 532 | v 0.004181 0.039781 0.025000 533 | v 0.008316 0.039126 0.025000 534 | v -0.000000 -0.000000 0.025000 535 | v -0.000000 -0.000000 0.025000 536 | v 0.008316 0.039126 0.025000 537 | v 0.012361 0.038042 0.025000 538 | v -0.000000 -0.000000 0.025000 539 | v 0.012361 0.038042 0.025000 540 | v 0.016269 0.036542 0.025000 541 | v 0.008316 0.039126 0.000000 542 | v 0.004181 0.039781 0.000000 543 | v -0.000000 -0.000000 0.000000 544 | v -0.000000 -0.000000 0.000000 545 | v 0.004181 0.039781 0.000000 546 | v -0.000000 0.040000 0.000000 547 | v -0.000000 -0.000000 0.000000 548 | v -0.000000 0.040000 0.000000 549 | v -0.004181 0.039781 0.000000 550 | v 0.020000 0.034641 0.000000 551 | v 0.016269 0.036542 0.000000 552 | v -0.000000 -0.000000 0.000000 553 | v -0.000000 -0.000000 0.000000 554 | v 0.016269 0.036542 0.000000 555 | v 0.012361 0.038042 0.000000 556 | v -0.000000 -0.000000 0.000000 557 | v 0.012361 0.038042 0.000000 558 | v 0.008316 0.039126 0.000000 559 | v 0.029726 0.026765 0.000000 560 | v 0.026765 0.029726 0.000000 561 | v -0.000000 -0.000000 0.000000 562 | v -0.000000 -0.000000 0.000000 563 | v 0.026765 0.029726 0.000000 564 | v 0.023511 0.032361 0.000000 565 | v -0.000000 -0.000000 0.000000 566 | v 0.023511 0.032361 0.000000 567 | v 0.020000 0.034641 0.000000 568 | v 0.036542 0.016269 0.000000 569 | v 0.034641 0.020000 0.000000 570 | v -0.000000 -0.000000 0.000000 571 | v -0.000000 -0.000000 0.000000 572 | v 0.034641 0.020000 0.000000 573 | v 0.032361 0.023511 0.000000 574 | v -0.000000 -0.000000 0.000000 575 | v 0.032361 0.023511 0.000000 576 | v 0.029726 0.026765 0.000000 577 | v 0.039781 0.004181 0.000000 578 | v 0.039126 0.008316 0.000000 579 | v -0.000000 -0.000000 0.000000 580 | v -0.000000 -0.000000 0.000000 581 | v 0.039126 0.008316 0.000000 582 | v 0.038042 0.012361 0.000000 583 | v -0.000000 -0.000000 0.000000 584 | v 0.038042 0.012361 0.000000 585 | v 0.036542 0.016269 0.000000 586 | v 0.039126 -0.008316 0.000000 587 | v 0.039781 -0.004181 0.000000 588 | v -0.000000 -0.000000 0.000000 589 | v -0.000000 -0.000000 0.000000 590 | v 0.039781 -0.004181 0.000000 591 | v 0.040000 -0.000000 0.000000 592 | v -0.000000 -0.000000 0.000000 593 | v 0.040000 -0.000000 0.000000 594 | v 0.039781 0.004181 0.000000 595 | v 0.034641 -0.020000 0.000000 596 | v 0.036542 -0.016269 0.000000 597 | v -0.000000 -0.000000 0.000000 598 | v -0.000000 -0.000000 0.000000 599 | v 0.036542 -0.016269 0.000000 600 | v 0.038042 -0.012361 0.000000 601 | v -0.000000 -0.000000 0.000000 602 | v 0.038042 -0.012361 0.000000 603 | v 0.039126 -0.008316 0.000000 604 | v 0.026765 -0.029726 0.000000 605 | v 0.029726 -0.026765 0.000000 606 | v -0.000000 -0.000000 0.000000 607 | v -0.000000 -0.000000 0.000000 608 | v 0.029726 -0.026765 0.000000 609 | v 0.032361 -0.023511 0.000000 610 | v -0.000000 -0.000000 0.000000 611 | v 0.032361 -0.023511 0.000000 612 | v 0.034641 -0.020000 0.000000 613 | v 0.016269 -0.036542 0.000000 614 | v 0.020000 -0.034641 0.000000 615 | v -0.000000 -0.000000 0.000000 616 | v -0.000000 -0.000000 0.000000 617 | v 0.020000 -0.034641 0.000000 618 | v 0.023511 -0.032361 0.000000 619 | v -0.000000 -0.000000 0.000000 620 | v 0.023511 -0.032361 0.000000 621 | v 0.026765 -0.029726 0.000000 622 | v 0.004181 -0.039781 0.000000 623 | v 0.008316 -0.039126 0.000000 624 | v -0.000000 -0.000000 0.000000 625 | v -0.000000 -0.000000 0.000000 626 | v 0.008316 -0.039126 0.000000 627 | v 0.012361 -0.038042 0.000000 628 | v -0.000000 -0.000000 0.000000 629 | v 0.012361 -0.038042 0.000000 630 | v 0.016269 -0.036542 0.000000 631 | v -0.008316 -0.039126 0.000000 632 | v -0.004181 -0.039781 0.000000 633 | v -0.000000 -0.000000 0.000000 634 | v -0.000000 -0.000000 0.000000 635 | v -0.004181 -0.039781 0.000000 636 | v -0.000000 -0.040000 0.000000 637 | v -0.000000 -0.000000 0.000000 638 | v -0.000000 -0.040000 0.000000 639 | v 0.004181 -0.039781 0.000000 640 | v -0.020000 -0.034641 0.000000 641 | v -0.016269 -0.036542 0.000000 642 | v -0.000000 -0.000000 0.000000 643 | v -0.000000 -0.000000 0.000000 644 | v -0.016269 -0.036542 0.000000 645 | v -0.012361 -0.038042 0.000000 646 | v -0.000000 -0.000000 0.000000 647 | v -0.012361 -0.038042 0.000000 648 | v -0.008316 -0.039126 0.000000 649 | v -0.029726 -0.026765 0.000000 650 | v -0.026765 -0.029726 0.000000 651 | v -0.000000 -0.000000 0.000000 652 | v -0.000000 -0.000000 0.000000 653 | v -0.026765 -0.029726 0.000000 654 | v -0.023511 -0.032361 0.000000 655 | v -0.000000 -0.000000 0.000000 656 | v -0.023511 -0.032361 0.000000 657 | v -0.020000 -0.034641 0.000000 658 | v -0.036542 -0.016269 0.000000 659 | v -0.034641 -0.020000 0.000000 660 | v -0.000000 -0.000000 0.000000 661 | v -0.000000 -0.000000 0.000000 662 | v -0.034641 -0.020000 0.000000 663 | v -0.032361 -0.023511 0.000000 664 | v -0.000000 -0.000000 0.000000 665 | v -0.032361 -0.023511 0.000000 666 | v -0.029726 -0.026765 0.000000 667 | v -0.039781 -0.004181 0.000000 668 | v -0.039126 -0.008316 0.000000 669 | v -0.000000 -0.000000 0.000000 670 | v -0.000000 -0.000000 0.000000 671 | v -0.039126 -0.008316 0.000000 672 | v -0.038042 -0.012361 0.000000 673 | v -0.000000 -0.000000 0.000000 674 | v -0.038042 -0.012361 0.000000 675 | v -0.036542 -0.016269 0.000000 676 | v -0.039126 0.008316 0.000000 677 | v -0.039781 0.004181 0.000000 678 | v -0.000000 -0.000000 0.000000 679 | v -0.000000 -0.000000 0.000000 680 | v -0.039781 0.004181 0.000000 681 | v -0.040000 -0.000000 0.000000 682 | v -0.000000 -0.000000 0.000000 683 | v -0.040000 -0.000000 0.000000 684 | v -0.039781 -0.004181 0.000000 685 | v -0.034641 0.020000 0.000000 686 | v -0.036542 0.016269 0.000000 687 | v -0.000000 -0.000000 0.000000 688 | v -0.000000 -0.000000 0.000000 689 | v -0.036542 0.016269 0.000000 690 | v -0.038042 0.012361 0.000000 691 | v -0.000000 -0.000000 0.000000 692 | v -0.038042 0.012361 0.000000 693 | v -0.039126 0.008316 0.000000 694 | v -0.026765 0.029726 0.000000 695 | v -0.029726 0.026765 0.000000 696 | v -0.000000 -0.000000 0.000000 697 | v -0.000000 -0.000000 0.000000 698 | v -0.029726 0.026765 0.000000 699 | v -0.032361 0.023511 0.000000 700 | v -0.000000 -0.000000 0.000000 701 | v -0.032361 0.023511 0.000000 702 | v -0.034641 0.020000 0.000000 703 | v -0.016269 0.036542 0.000000 704 | v -0.020000 0.034641 0.000000 705 | v -0.000000 -0.000000 0.000000 706 | v -0.000000 -0.000000 0.000000 707 | v -0.020000 0.034641 0.000000 708 | v -0.023511 0.032361 0.000000 709 | v -0.000000 -0.000000 0.000000 710 | v -0.023511 0.032361 0.000000 711 | v -0.026765 0.029726 0.000000 712 | v -0.004181 0.039781 0.000000 713 | v -0.008316 0.039126 0.000000 714 | v -0.000000 -0.000000 0.000000 715 | v -0.000000 -0.000000 0.000000 716 | v -0.008316 0.039126 0.000000 717 | v -0.012361 0.038042 0.000000 718 | v -0.000000 -0.000000 0.000000 719 | v -0.012361 0.038042 0.000000 720 | v -0.016269 0.036542 0.000000 721 | vn 0.069771 -0.997563 0.000000 722 | vn 0.069771 -0.997563 0.000000 723 | vn 0.069771 -0.997563 0.000000 724 | vn 0.139159 -0.990270 0.000000 725 | vn 0.139159 -0.990270 0.000000 726 | vn 0.139159 -0.990270 0.000000 727 | vn 0.173662 -0.984805 0.000000 728 | vn 0.173662 -0.984805 0.000000 729 | vn 0.173662 -0.984805 0.000000 730 | vn 0.241908 -0.970299 0.000000 731 | vn 0.241908 -0.970299 0.000000 732 | vn 0.241908 -0.970299 0.000000 733 | vn 0.275651 -0.961258 0.000000 734 | vn 0.275651 -0.961258 0.000000 735 | vn 0.275651 -0.961258 0.000000 736 | vn 0.342007 -0.939697 0.000000 737 | vn 0.342007 -0.939697 0.000000 738 | vn 0.342007 -0.939697 0.000000 739 | vn 0.374620 -0.927179 0.000000 740 | vn 0.374620 -0.927179 0.000000 741 | vn 0.374620 -0.927179 0.000000 742 | vn 0.438358 -0.898800 0.000000 743 | vn 0.438358 -0.898800 0.000000 744 | vn 0.438358 -0.898800 0.000000 745 | vn 0.469484 -0.882941 0.000000 746 | vn 0.469484 -0.882941 0.000000 747 | vn 0.469484 -0.882941 0.000000 748 | vn 0.529907 -0.848056 0.000000 749 | vn 0.529907 -0.848056 0.000000 750 | vn 0.529907 -0.848056 0.000000 751 | vn 0.559205 -0.829030 0.000000 752 | vn 0.559205 -0.829030 0.000000 753 | vn 0.559205 -0.829030 0.000000 754 | vn 0.615650 -0.788020 0.000000 755 | vn 0.615650 -0.788020 0.000000 756 | vn 0.615650 -0.788020 0.000000 757 | vn 0.642798 -0.766035 0.000000 758 | vn 0.642798 -0.766035 0.000000 759 | vn 0.642798 -0.766035 0.000000 760 | vn 0.694648 -0.719350 0.000000 761 | vn 0.694648 -0.719350 0.000000 762 | vn 0.694648 -0.719350 0.000000 763 | vn 0.719350 -0.694648 0.000000 764 | vn 0.719350 -0.694648 0.000000 765 | vn 0.719350 -0.694648 0.000000 766 | vn 0.766035 -0.642798 0.000000 767 | vn 0.766035 -0.642798 0.000000 768 | vn 0.766035 -0.642798 0.000000 769 | vn 0.788020 -0.615650 0.000000 770 | vn 0.788020 -0.615650 0.000000 771 | vn 0.788020 -0.615650 0.000000 772 | vn 0.829030 -0.559205 0.000000 773 | vn 0.829030 -0.559205 0.000000 774 | vn 0.829030 -0.559205 0.000000 775 | vn 0.848056 -0.529907 0.000000 776 | vn 0.848056 -0.529907 0.000000 777 | vn 0.848056 -0.529907 0.000000 778 | vn 0.882941 -0.469484 0.000000 779 | vn 0.882941 -0.469484 0.000000 780 | vn 0.882941 -0.469484 0.000000 781 | vn 0.898800 -0.438358 0.000000 782 | vn 0.898800 -0.438358 0.000000 783 | vn 0.898800 -0.438358 0.000000 784 | vn 0.927179 -0.374620 0.000000 785 | vn 0.927179 -0.374620 0.000000 786 | vn 0.927179 -0.374620 0.000000 787 | vn 0.939697 -0.342007 0.000000 788 | vn 0.939697 -0.342007 0.000000 789 | vn 0.939697 -0.342007 0.000000 790 | vn 0.961258 -0.275651 0.000000 791 | vn 0.961258 -0.275651 0.000000 792 | vn 0.961258 -0.275651 0.000000 793 | vn 0.970299 -0.241908 0.000000 794 | vn 0.970299 -0.241908 0.000000 795 | vn 0.970299 -0.241908 0.000000 796 | vn 0.984805 -0.173662 0.000000 797 | vn 0.984805 -0.173662 0.000000 798 | vn 0.984805 -0.173662 0.000000 799 | vn 0.990270 -0.139159 0.000000 800 | vn 0.990270 -0.139159 0.000000 801 | vn 0.990270 -0.139159 0.000000 802 | vn 0.997563 -0.069771 0.000000 803 | vn 0.997563 -0.069771 0.000000 804 | vn 0.997563 -0.069771 0.000000 805 | vn 0.999391 -0.034885 0.000000 806 | vn 0.999391 -0.034885 0.000000 807 | vn 0.999391 -0.034885 0.000000 808 | vn 0.999391 0.034885 0.000000 809 | vn 0.999391 0.034885 0.000000 810 | vn 0.999391 0.034885 0.000000 811 | vn 0.997563 0.069771 0.000000 812 | vn 0.997563 0.069771 0.000000 813 | vn 0.997563 0.069771 0.000000 814 | vn 0.990270 0.139159 0.000000 815 | vn 0.990270 0.139159 0.000000 816 | vn 0.990270 0.139159 0.000000 817 | vn 0.984805 0.173662 0.000000 818 | vn 0.984805 0.173662 0.000000 819 | vn 0.984805 0.173662 0.000000 820 | vn 0.970299 0.241908 0.000000 821 | vn 0.970299 0.241908 0.000000 822 | vn 0.970299 0.241908 0.000000 823 | vn 0.961258 0.275651 0.000000 824 | vn 0.961258 0.275651 0.000000 825 | vn 0.961258 0.275651 0.000000 826 | vn 0.939697 0.342007 0.000000 827 | vn 0.939697 0.342007 0.000000 828 | vn 0.939697 0.342007 0.000000 829 | vn 0.927179 0.374620 0.000000 830 | vn 0.927179 0.374620 0.000000 831 | vn 0.927179 0.374620 0.000000 832 | vn 0.898800 0.438358 0.000000 833 | vn 0.898800 0.438358 0.000000 834 | vn 0.898800 0.438358 0.000000 835 | vn 0.882941 0.469484 0.000000 836 | vn 0.882941 0.469484 0.000000 837 | vn 0.882941 0.469484 0.000000 838 | vn 0.848056 0.529907 0.000000 839 | vn 0.848056 0.529907 0.000000 840 | vn 0.848056 0.529907 0.000000 841 | vn 0.829030 0.559205 0.000000 842 | vn 0.829030 0.559205 0.000000 843 | vn 0.829030 0.559205 0.000000 844 | vn 0.788020 0.615650 0.000000 845 | vn 0.788020 0.615650 0.000000 846 | vn 0.788020 0.615650 0.000000 847 | vn 0.766035 0.642798 0.000000 848 | vn 0.766035 0.642798 0.000000 849 | vn 0.766035 0.642798 0.000000 850 | vn 0.719350 0.694648 0.000000 851 | vn 0.719350 0.694648 0.000000 852 | vn 0.719350 0.694648 0.000000 853 | vn 0.694648 0.719350 0.000000 854 | vn 0.694648 0.719350 0.000000 855 | vn 0.694648 0.719350 0.000000 856 | vn 0.642798 0.766035 0.000000 857 | vn 0.642798 0.766035 0.000000 858 | vn 0.642798 0.766035 0.000000 859 | vn 0.615650 0.788020 0.000000 860 | vn 0.615650 0.788020 0.000000 861 | vn 0.615650 0.788020 0.000000 862 | vn 0.559205 0.829030 0.000000 863 | vn 0.559205 0.829030 0.000000 864 | vn 0.559205 0.829030 0.000000 865 | vn 0.529907 0.848056 0.000000 866 | vn 0.529907 0.848056 0.000000 867 | vn 0.529907 0.848056 0.000000 868 | vn 0.469484 0.882941 0.000000 869 | vn 0.469484 0.882941 0.000000 870 | vn 0.469484 0.882941 0.000000 871 | vn 0.438358 0.898800 0.000000 872 | vn 0.438358 0.898800 0.000000 873 | vn 0.438358 0.898800 0.000000 874 | vn 0.374620 0.927179 0.000000 875 | vn 0.374620 0.927179 0.000000 876 | vn 0.374620 0.927179 0.000000 877 | vn 0.342007 0.939697 0.000000 878 | vn 0.342007 0.939697 0.000000 879 | vn 0.342007 0.939697 0.000000 880 | vn 0.275651 0.961258 0.000000 881 | vn 0.275651 0.961258 0.000000 882 | vn 0.275651 0.961258 0.000000 883 | vn 0.241908 0.970299 0.000000 884 | vn 0.241908 0.970299 0.000000 885 | vn 0.241908 0.970299 0.000000 886 | vn 0.173662 0.984805 0.000000 887 | vn 0.173662 0.984805 0.000000 888 | vn 0.173662 0.984805 0.000000 889 | vn 0.139159 0.990270 0.000000 890 | vn 0.139159 0.990270 0.000000 891 | vn 0.139159 0.990270 0.000000 892 | vn 0.069771 0.997563 0.000000 893 | vn 0.069771 0.997563 0.000000 894 | vn 0.069771 0.997563 0.000000 895 | vn 0.034885 0.999391 0.000000 896 | vn 0.034885 0.999391 0.000000 897 | vn 0.034885 0.999391 0.000000 898 | vn -0.034885 0.999391 0.000000 899 | vn -0.034885 0.999391 0.000000 900 | vn -0.034885 0.999391 0.000000 901 | vn -0.069771 0.997563 0.000000 902 | vn -0.069771 0.997563 0.000000 903 | vn -0.069771 0.997563 0.000000 904 | vn -0.139159 0.990270 0.000000 905 | vn -0.139159 0.990270 0.000000 906 | vn -0.139159 0.990270 0.000000 907 | vn -0.173662 0.984805 0.000000 908 | vn -0.173662 0.984805 0.000000 909 | vn -0.173662 0.984805 0.000000 910 | vn -0.241908 0.970299 0.000000 911 | vn -0.241908 0.970299 0.000000 912 | vn -0.241908 0.970299 0.000000 913 | vn -0.275651 0.961258 0.000000 914 | vn -0.275651 0.961258 0.000000 915 | vn -0.275651 0.961258 0.000000 916 | vn -0.342007 0.939697 0.000000 917 | vn -0.342007 0.939697 0.000000 918 | vn -0.342007 0.939697 0.000000 919 | vn -0.374620 0.927179 0.000000 920 | vn -0.374620 0.927179 0.000000 921 | vn -0.374620 0.927179 0.000000 922 | vn -0.438358 0.898800 0.000000 923 | vn -0.438358 0.898800 0.000000 924 | vn -0.438358 0.898800 0.000000 925 | vn -0.469484 0.882941 0.000000 926 | vn -0.469484 0.882941 0.000000 927 | vn -0.469484 0.882941 0.000000 928 | vn -0.529907 0.848056 0.000000 929 | vn -0.529907 0.848056 0.000000 930 | vn -0.529907 0.848056 0.000000 931 | vn -0.559205 0.829030 0.000000 932 | vn -0.559205 0.829030 0.000000 933 | vn -0.559205 0.829030 0.000000 934 | vn -0.615650 0.788020 0.000000 935 | vn -0.615650 0.788020 0.000000 936 | vn -0.615650 0.788020 0.000000 937 | vn -0.642798 0.766035 0.000000 938 | vn -0.642798 0.766035 0.000000 939 | vn -0.642798 0.766035 0.000000 940 | vn -0.694648 0.719350 0.000000 941 | vn -0.694648 0.719350 0.000000 942 | vn -0.694648 0.719350 0.000000 943 | vn -0.719350 0.694648 0.000000 944 | vn -0.719350 0.694648 0.000000 945 | vn -0.719350 0.694648 0.000000 946 | vn -0.766035 0.642798 0.000000 947 | vn -0.766035 0.642798 0.000000 948 | vn -0.766035 0.642798 0.000000 949 | vn -0.788020 0.615650 0.000000 950 | vn -0.788020 0.615650 0.000000 951 | vn -0.788020 0.615650 0.000000 952 | vn -0.829030 0.559205 0.000000 953 | vn -0.829030 0.559205 0.000000 954 | vn -0.829030 0.559205 0.000000 955 | vn -0.848056 0.529907 0.000000 956 | vn -0.848056 0.529907 0.000000 957 | vn -0.848056 0.529907 0.000000 958 | vn -0.882941 0.469484 0.000000 959 | vn -0.882941 0.469484 0.000000 960 | vn -0.882941 0.469484 0.000000 961 | vn -0.898800 0.438358 0.000000 962 | vn -0.898800 0.438358 0.000000 963 | vn -0.898800 0.438358 0.000000 964 | vn -0.927179 0.374620 0.000000 965 | vn -0.927179 0.374620 0.000000 966 | vn -0.927179 0.374620 0.000000 967 | vn -0.939697 0.342007 0.000000 968 | vn -0.939697 0.342007 0.000000 969 | vn -0.939697 0.342007 0.000000 970 | vn -0.961258 0.275651 0.000000 971 | vn -0.961258 0.275651 0.000000 972 | vn -0.961258 0.275651 0.000000 973 | vn -0.970299 0.241908 0.000000 974 | vn -0.970299 0.241908 0.000000 975 | vn -0.970299 0.241908 0.000000 976 | vn -0.984805 0.173662 0.000000 977 | vn -0.984805 0.173662 0.000000 978 | vn -0.984805 0.173662 0.000000 979 | vn -0.990270 0.139159 0.000000 980 | vn -0.990270 0.139159 0.000000 981 | vn -0.990270 0.139159 0.000000 982 | vn -0.997563 0.069771 0.000000 983 | vn -0.997563 0.069771 0.000000 984 | vn -0.997563 0.069771 0.000000 985 | vn -0.999391 0.034885 0.000000 986 | vn -0.999391 0.034885 0.000000 987 | vn -0.999391 0.034885 0.000000 988 | vn -0.999391 -0.034885 0.000000 989 | vn -0.999391 -0.034885 0.000000 990 | vn -0.999391 -0.034885 0.000000 991 | vn -0.997563 -0.069771 0.000000 992 | vn -0.997563 -0.069771 0.000000 993 | vn -0.997563 -0.069771 0.000000 994 | vn -0.990270 -0.139159 0.000000 995 | vn -0.990270 -0.139159 0.000000 996 | vn -0.990270 -0.139159 0.000000 997 | vn -0.984805 -0.173662 0.000000 998 | vn -0.984805 -0.173662 0.000000 999 | vn -0.984805 -0.173662 0.000000 1000 | vn -0.970299 -0.241908 0.000000 1001 | vn -0.970299 -0.241908 0.000000 1002 | vn -0.970299 -0.241908 0.000000 1003 | vn -0.961258 -0.275651 0.000000 1004 | vn -0.961258 -0.275651 0.000000 1005 | vn -0.961258 -0.275651 0.000000 1006 | vn -0.939697 -0.342007 0.000000 1007 | vn -0.939697 -0.342007 0.000000 1008 | vn -0.939697 -0.342007 0.000000 1009 | vn -0.927179 -0.374620 0.000000 1010 | vn -0.927179 -0.374620 0.000000 1011 | vn -0.927179 -0.374620 0.000000 1012 | vn -0.898800 -0.438358 0.000000 1013 | vn -0.898800 -0.438358 0.000000 1014 | vn -0.898800 -0.438358 0.000000 1015 | vn -0.882941 -0.469484 0.000000 1016 | vn -0.882941 -0.469484 0.000000 1017 | vn -0.882941 -0.469484 0.000000 1018 | vn -0.848056 -0.529907 0.000000 1019 | vn -0.848056 -0.529907 0.000000 1020 | vn -0.848056 -0.529907 0.000000 1021 | vn -0.829030 -0.559205 0.000000 1022 | vn -0.829030 -0.559205 0.000000 1023 | vn -0.829030 -0.559205 0.000000 1024 | vn -0.788020 -0.615650 0.000000 1025 | vn -0.788020 -0.615650 0.000000 1026 | vn -0.788020 -0.615650 0.000000 1027 | vn -0.766035 -0.642798 0.000000 1028 | vn -0.766035 -0.642798 0.000000 1029 | vn -0.766035 -0.642798 0.000000 1030 | vn -0.719350 -0.694648 0.000000 1031 | vn -0.719350 -0.694648 0.000000 1032 | vn -0.719350 -0.694648 0.000000 1033 | vn -0.694648 -0.719350 0.000000 1034 | vn -0.694648 -0.719350 0.000000 1035 | vn -0.694648 -0.719350 0.000000 1036 | vn -0.642798 -0.766035 0.000000 1037 | vn -0.642798 -0.766035 0.000000 1038 | vn -0.642798 -0.766035 0.000000 1039 | vn -0.615650 -0.788020 0.000000 1040 | vn -0.615650 -0.788020 0.000000 1041 | vn -0.615650 -0.788020 0.000000 1042 | vn -0.559205 -0.829030 0.000000 1043 | vn -0.559205 -0.829030 0.000000 1044 | vn -0.559205 -0.829030 0.000000 1045 | vn -0.529907 -0.848056 0.000000 1046 | vn -0.529907 -0.848056 0.000000 1047 | vn -0.529907 -0.848056 0.000000 1048 | vn -0.469484 -0.882941 0.000000 1049 | vn -0.469484 -0.882941 0.000000 1050 | vn -0.469484 -0.882941 0.000000 1051 | vn -0.438358 -0.898800 0.000000 1052 | vn -0.438358 -0.898800 0.000000 1053 | vn -0.438358 -0.898800 0.000000 1054 | vn -0.374620 -0.927179 0.000000 1055 | vn -0.374620 -0.927179 0.000000 1056 | vn -0.374620 -0.927179 0.000000 1057 | vn -0.342007 -0.939697 0.000000 1058 | vn -0.342007 -0.939697 0.000000 1059 | vn -0.342007 -0.939697 0.000000 1060 | vn -0.275651 -0.961258 0.000000 1061 | vn -0.275651 -0.961258 0.000000 1062 | vn -0.275651 -0.961258 0.000000 1063 | vn -0.241908 -0.970299 0.000000 1064 | vn -0.241908 -0.970299 0.000000 1065 | vn -0.241908 -0.970299 0.000000 1066 | vn -0.173662 -0.984805 0.000000 1067 | vn -0.173662 -0.984805 0.000000 1068 | vn -0.173662 -0.984805 0.000000 1069 | vn -0.139159 -0.990270 0.000000 1070 | vn -0.139159 -0.990270 0.000000 1071 | vn -0.139159 -0.990270 0.000000 1072 | vn -0.069771 -0.997563 0.000000 1073 | vn -0.069771 -0.997563 0.000000 1074 | vn -0.069771 -0.997563 0.000000 1075 | vn -0.034885 -0.999391 0.000000 1076 | vn -0.034885 -0.999391 0.000000 1077 | vn -0.034885 -0.999391 0.000000 1078 | vn 0.034885 -0.999391 0.000000 1079 | vn 0.034885 -0.999391 0.000000 1080 | vn 0.034885 -0.999391 0.000000 1081 | vn 0.000000 0.000000 1.000000 1082 | vn 0.000000 0.000000 1.000000 1083 | vn 0.000000 0.000000 1.000000 1084 | vn 0.000000 0.000000 1.000000 1085 | vn 0.000000 0.000000 1.000000 1086 | vn 0.000000 0.000000 1.000000 1087 | vn 0.000000 0.000000 1.000000 1088 | vn 0.000000 0.000000 1.000000 1089 | vn 0.000000 0.000000 1.000000 1090 | vn 0.000000 0.000000 1.000000 1091 | vn 0.000000 0.000000 1.000000 1092 | vn 0.000000 0.000000 1.000000 1093 | vn 0.000000 0.000000 1.000000 1094 | vn 0.000000 0.000000 1.000000 1095 | vn 0.000000 0.000000 1.000000 1096 | vn 0.000000 0.000000 1.000000 1097 | vn 0.000000 0.000000 1.000000 1098 | vn 0.000000 0.000000 1.000000 1099 | vn 0.000000 0.000000 1.000000 1100 | vn 0.000000 0.000000 1.000000 1101 | vn 0.000000 0.000000 1.000000 1102 | vn 0.000000 0.000000 1.000000 1103 | vn 0.000000 0.000000 1.000000 1104 | vn 0.000000 0.000000 1.000000 1105 | vn 0.000000 0.000000 1.000000 1106 | vn 0.000000 0.000000 1.000000 1107 | vn 0.000000 0.000000 1.000000 1108 | vn 0.000000 0.000000 1.000000 1109 | vn 0.000000 0.000000 1.000000 1110 | vn 0.000000 0.000000 1.000000 1111 | vn 0.000000 0.000000 1.000000 1112 | vn 0.000000 0.000000 1.000000 1113 | vn 0.000000 0.000000 1.000000 1114 | vn 0.000000 0.000000 1.000000 1115 | vn 0.000000 0.000000 1.000000 1116 | vn 0.000000 0.000000 1.000000 1117 | vn 0.000000 0.000000 1.000000 1118 | vn 0.000000 0.000000 1.000000 1119 | vn 0.000000 0.000000 1.000000 1120 | vn 0.000000 0.000000 1.000000 1121 | vn 0.000000 0.000000 1.000000 1122 | vn 0.000000 0.000000 1.000000 1123 | vn 0.000000 0.000000 1.000000 1124 | vn 0.000000 0.000000 1.000000 1125 | vn 0.000000 0.000000 1.000000 1126 | vn 0.000000 0.000000 1.000000 1127 | vn 0.000000 0.000000 1.000000 1128 | vn 0.000000 0.000000 1.000000 1129 | vn 0.000000 0.000000 1.000000 1130 | vn 0.000000 0.000000 1.000000 1131 | vn 0.000000 0.000000 1.000000 1132 | vn 0.000000 0.000000 1.000000 1133 | vn 0.000000 0.000000 1.000000 1134 | vn 0.000000 0.000000 1.000000 1135 | vn 0.000000 0.000000 1.000000 1136 | vn 0.000000 0.000000 1.000000 1137 | vn 0.000000 0.000000 1.000000 1138 | vn 0.000000 0.000000 1.000000 1139 | vn 0.000000 0.000000 1.000000 1140 | vn 0.000000 0.000000 1.000000 1141 | vn 0.000000 0.000000 1.000000 1142 | vn 0.000000 0.000000 1.000000 1143 | vn 0.000000 0.000000 1.000000 1144 | vn 0.000000 0.000000 1.000000 1145 | vn 0.000000 0.000000 1.000000 1146 | vn 0.000000 0.000000 1.000000 1147 | vn 0.000000 0.000000 1.000000 1148 | vn 0.000000 0.000000 1.000000 1149 | vn 0.000000 0.000000 1.000000 1150 | vn 0.000000 0.000000 1.000000 1151 | vn 0.000000 0.000000 1.000000 1152 | vn 0.000000 0.000000 1.000000 1153 | vn 0.000000 0.000000 1.000000 1154 | vn 0.000000 0.000000 1.000000 1155 | vn 0.000000 0.000000 1.000000 1156 | vn 0.000000 0.000000 1.000000 1157 | vn 0.000000 0.000000 1.000000 1158 | vn 0.000000 0.000000 1.000000 1159 | vn 0.000000 0.000000 1.000000 1160 | vn 0.000000 0.000000 1.000000 1161 | vn 0.000000 0.000000 1.000000 1162 | vn 0.000000 0.000000 1.000000 1163 | vn 0.000000 0.000000 1.000000 1164 | vn 0.000000 0.000000 1.000000 1165 | vn 0.000000 0.000000 1.000000 1166 | vn 0.000000 0.000000 1.000000 1167 | vn 0.000000 0.000000 1.000000 1168 | vn 0.000000 0.000000 1.000000 1169 | vn 0.000000 0.000000 1.000000 1170 | vn 0.000000 0.000000 1.000000 1171 | vn 0.000000 0.000000 1.000000 1172 | vn 0.000000 0.000000 1.000000 1173 | vn 0.000000 0.000000 1.000000 1174 | vn 0.000000 0.000000 1.000000 1175 | vn 0.000000 0.000000 1.000000 1176 | vn 0.000000 0.000000 1.000000 1177 | vn 0.000000 0.000000 1.000000 1178 | vn 0.000000 0.000000 1.000000 1179 | vn 0.000000 0.000000 1.000000 1180 | vn 0.000000 0.000000 1.000000 1181 | vn 0.000000 0.000000 1.000000 1182 | vn 0.000000 0.000000 1.000000 1183 | vn 0.000000 0.000000 1.000000 1184 | vn 0.000000 0.000000 1.000000 1185 | vn 0.000000 0.000000 1.000000 1186 | vn 0.000000 0.000000 1.000000 1187 | vn 0.000000 0.000000 1.000000 1188 | vn 0.000000 0.000000 1.000000 1189 | vn 0.000000 0.000000 1.000000 1190 | vn 0.000000 0.000000 1.000000 1191 | vn 0.000000 0.000000 1.000000 1192 | vn 0.000000 0.000000 1.000000 1193 | vn 0.000000 0.000000 1.000000 1194 | vn 0.000000 0.000000 1.000000 1195 | vn 0.000000 0.000000 1.000000 1196 | vn 0.000000 0.000000 1.000000 1197 | vn 0.000000 0.000000 1.000000 1198 | vn 0.000000 0.000000 1.000000 1199 | vn 0.000000 0.000000 1.000000 1200 | vn 0.000000 0.000000 1.000000 1201 | vn 0.000000 0.000000 1.000000 1202 | vn 0.000000 0.000000 1.000000 1203 | vn 0.000000 0.000000 1.000000 1204 | vn 0.000000 0.000000 1.000000 1205 | vn 0.000000 0.000000 1.000000 1206 | vn 0.000000 0.000000 1.000000 1207 | vn 0.000000 0.000000 1.000000 1208 | vn 0.000000 0.000000 1.000000 1209 | vn 0.000000 0.000000 1.000000 1210 | vn 0.000000 0.000000 1.000000 1211 | vn 0.000000 0.000000 1.000000 1212 | vn 0.000000 0.000000 1.000000 1213 | vn 0.000000 0.000000 1.000000 1214 | vn 0.000000 0.000000 1.000000 1215 | vn 0.000000 0.000000 1.000000 1216 | vn 0.000000 0.000000 1.000000 1217 | vn 0.000000 0.000000 1.000000 1218 | vn 0.000000 0.000000 1.000000 1219 | vn 0.000000 0.000000 1.000000 1220 | vn 0.000000 0.000000 1.000000 1221 | vn 0.000000 0.000000 1.000000 1222 | vn 0.000000 0.000000 1.000000 1223 | vn 0.000000 0.000000 1.000000 1224 | vn 0.000000 0.000000 1.000000 1225 | vn 0.000000 0.000000 1.000000 1226 | vn 0.000000 0.000000 1.000000 1227 | vn 0.000000 0.000000 1.000000 1228 | vn 0.000000 0.000000 1.000000 1229 | vn 0.000000 0.000000 1.000000 1230 | vn 0.000000 0.000000 1.000000 1231 | vn 0.000000 0.000000 1.000000 1232 | vn 0.000000 0.000000 1.000000 1233 | vn 0.000000 0.000000 1.000000 1234 | vn 0.000000 0.000000 1.000000 1235 | vn 0.000000 0.000000 1.000000 1236 | vn 0.000000 0.000000 1.000000 1237 | vn 0.000000 0.000000 1.000000 1238 | vn 0.000000 0.000000 1.000000 1239 | vn 0.000000 0.000000 1.000000 1240 | vn 0.000000 0.000000 1.000000 1241 | vn 0.000000 0.000000 1.000000 1242 | vn 0.000000 0.000000 1.000000 1243 | vn 0.000000 0.000000 1.000000 1244 | vn 0.000000 0.000000 1.000000 1245 | vn 0.000000 0.000000 1.000000 1246 | vn 0.000000 0.000000 1.000000 1247 | vn 0.000000 0.000000 1.000000 1248 | vn 0.000000 0.000000 1.000000 1249 | vn 0.000000 0.000000 1.000000 1250 | vn 0.000000 0.000000 1.000000 1251 | vn 0.000000 0.000000 1.000000 1252 | vn 0.000000 0.000000 1.000000 1253 | vn 0.000000 0.000000 1.000000 1254 | vn 0.000000 0.000000 1.000000 1255 | vn 0.000000 0.000000 1.000000 1256 | vn 0.000000 0.000000 1.000000 1257 | vn 0.000000 0.000000 1.000000 1258 | vn 0.000000 0.000000 1.000000 1259 | vn 0.000000 0.000000 1.000000 1260 | vn 0.000000 0.000000 1.000000 1261 | vn 0.000000 0.000000 -1.000000 1262 | vn 0.000000 0.000000 -1.000000 1263 | vn 0.000000 0.000000 -1.000000 1264 | vn 0.000000 0.000000 -1.000000 1265 | vn 0.000000 0.000000 -1.000000 1266 | vn 0.000000 0.000000 -1.000000 1267 | vn 0.000000 0.000000 -1.000000 1268 | vn 0.000000 0.000000 -1.000000 1269 | vn 0.000000 0.000000 -1.000000 1270 | vn 0.000000 0.000000 -1.000000 1271 | vn 0.000000 0.000000 -1.000000 1272 | vn 0.000000 0.000000 -1.000000 1273 | vn 0.000000 0.000000 -1.000000 1274 | vn 0.000000 0.000000 -1.000000 1275 | vn 0.000000 0.000000 -1.000000 1276 | vn 0.000000 0.000000 -1.000000 1277 | vn 0.000000 0.000000 -1.000000 1278 | vn 0.000000 0.000000 -1.000000 1279 | vn 0.000000 0.000000 -1.000000 1280 | vn 0.000000 0.000000 -1.000000 1281 | vn 0.000000 0.000000 -1.000000 1282 | vn 0.000000 0.000000 -1.000000 1283 | vn 0.000000 0.000000 -1.000000 1284 | vn 0.000000 0.000000 -1.000000 1285 | vn 0.000000 0.000000 -1.000000 1286 | vn 0.000000 0.000000 -1.000000 1287 | vn 0.000000 0.000000 -1.000000 1288 | vn 0.000000 0.000000 -1.000000 1289 | vn 0.000000 0.000000 -1.000000 1290 | vn 0.000000 0.000000 -1.000000 1291 | vn 0.000000 0.000000 -1.000000 1292 | vn 0.000000 0.000000 -1.000000 1293 | vn 0.000000 0.000000 -1.000000 1294 | vn 0.000000 0.000000 -1.000000 1295 | vn 0.000000 0.000000 -1.000000 1296 | vn 0.000000 0.000000 -1.000000 1297 | vn 0.000000 0.000000 -1.000000 1298 | vn 0.000000 0.000000 -1.000000 1299 | vn 0.000000 0.000000 -1.000000 1300 | vn 0.000000 0.000000 -1.000000 1301 | vn 0.000000 0.000000 -1.000000 1302 | vn 0.000000 0.000000 -1.000000 1303 | vn 0.000000 0.000000 -1.000000 1304 | vn 0.000000 0.000000 -1.000000 1305 | vn 0.000000 0.000000 -1.000000 1306 | vn 0.000000 0.000000 -1.000000 1307 | vn 0.000000 0.000000 -1.000000 1308 | vn 0.000000 0.000000 -1.000000 1309 | vn 0.000000 0.000000 -1.000000 1310 | vn 0.000000 0.000000 -1.000000 1311 | vn 0.000000 0.000000 -1.000000 1312 | vn 0.000000 0.000000 -1.000000 1313 | vn 0.000000 0.000000 -1.000000 1314 | vn 0.000000 0.000000 -1.000000 1315 | vn 0.000000 0.000000 -1.000000 1316 | vn 0.000000 0.000000 -1.000000 1317 | vn 0.000000 0.000000 -1.000000 1318 | vn 0.000000 0.000000 -1.000000 1319 | vn 0.000000 0.000000 -1.000000 1320 | vn 0.000000 0.000000 -1.000000 1321 | vn 0.000000 0.000000 -1.000000 1322 | vn 0.000000 0.000000 -1.000000 1323 | vn 0.000000 0.000000 -1.000000 1324 | vn 0.000000 0.000000 -1.000000 1325 | vn 0.000000 0.000000 -1.000000 1326 | vn 0.000000 0.000000 -1.000000 1327 | vn 0.000000 0.000000 -1.000000 1328 | vn 0.000000 0.000000 -1.000000 1329 | vn 0.000000 0.000000 -1.000000 1330 | vn 0.000000 0.000000 -1.000000 1331 | vn 0.000000 0.000000 -1.000000 1332 | vn 0.000000 0.000000 -1.000000 1333 | vn 0.000000 0.000000 -1.000000 1334 | vn 0.000000 0.000000 -1.000000 1335 | vn 0.000000 0.000000 -1.000000 1336 | vn 0.000000 0.000000 -1.000000 1337 | vn 0.000000 0.000000 -1.000000 1338 | vn 0.000000 0.000000 -1.000000 1339 | vn 0.000000 0.000000 -1.000000 1340 | vn 0.000000 0.000000 -1.000000 1341 | vn 0.000000 0.000000 -1.000000 1342 | vn 0.000000 0.000000 -1.000000 1343 | vn 0.000000 0.000000 -1.000000 1344 | vn 0.000000 0.000000 -1.000000 1345 | vn 0.000000 0.000000 -1.000000 1346 | vn 0.000000 0.000000 -1.000000 1347 | vn 0.000000 0.000000 -1.000000 1348 | vn 0.000000 0.000000 -1.000000 1349 | vn 0.000000 0.000000 -1.000000 1350 | vn 0.000000 0.000000 -1.000000 1351 | vn 0.000000 0.000000 -1.000000 1352 | vn 0.000000 0.000000 -1.000000 1353 | vn 0.000000 0.000000 -1.000000 1354 | vn 0.000000 0.000000 -1.000000 1355 | vn 0.000000 0.000000 -1.000000 1356 | vn 0.000000 0.000000 -1.000000 1357 | vn 0.000000 0.000000 -1.000000 1358 | vn 0.000000 0.000000 -1.000000 1359 | vn 0.000000 0.000000 -1.000000 1360 | vn 0.000000 0.000000 -1.000000 1361 | vn 0.000000 0.000000 -1.000000 1362 | vn 0.000000 0.000000 -1.000000 1363 | vn 0.000000 0.000000 -1.000000 1364 | vn 0.000000 0.000000 -1.000000 1365 | vn 0.000000 0.000000 -1.000000 1366 | vn 0.000000 0.000000 -1.000000 1367 | vn 0.000000 0.000000 -1.000000 1368 | vn 0.000000 0.000000 -1.000000 1369 | vn 0.000000 0.000000 -1.000000 1370 | vn 0.000000 0.000000 -1.000000 1371 | vn 0.000000 0.000000 -1.000000 1372 | vn 0.000000 0.000000 -1.000000 1373 | vn 0.000000 0.000000 -1.000000 1374 | vn 0.000000 0.000000 -1.000000 1375 | vn 0.000000 0.000000 -1.000000 1376 | vn 0.000000 0.000000 -1.000000 1377 | vn 0.000000 0.000000 -1.000000 1378 | vn 0.000000 0.000000 -1.000000 1379 | vn 0.000000 0.000000 -1.000000 1380 | vn 0.000000 0.000000 -1.000000 1381 | vn 0.000000 0.000000 -1.000000 1382 | vn 0.000000 0.000000 -1.000000 1383 | vn 0.000000 0.000000 -1.000000 1384 | vn 0.000000 0.000000 -1.000000 1385 | vn 0.000000 0.000000 -1.000000 1386 | vn 0.000000 0.000000 -1.000000 1387 | vn 0.000000 0.000000 -1.000000 1388 | vn 0.000000 0.000000 -1.000000 1389 | vn 0.000000 0.000000 -1.000000 1390 | vn 0.000000 0.000000 -1.000000 1391 | vn 0.000000 0.000000 -1.000000 1392 | vn 0.000000 0.000000 -1.000000 1393 | vn 0.000000 0.000000 -1.000000 1394 | vn 0.000000 0.000000 -1.000000 1395 | vn 0.000000 0.000000 -1.000000 1396 | vn 0.000000 0.000000 -1.000000 1397 | vn 0.000000 0.000000 -1.000000 1398 | vn 0.000000 0.000000 -1.000000 1399 | vn 0.000000 0.000000 -1.000000 1400 | vn 0.000000 0.000000 -1.000000 1401 | vn 0.000000 0.000000 -1.000000 1402 | vn 0.000000 0.000000 -1.000000 1403 | vn 0.000000 0.000000 -1.000000 1404 | vn 0.000000 0.000000 -1.000000 1405 | vn 0.000000 0.000000 -1.000000 1406 | vn 0.000000 0.000000 -1.000000 1407 | vn 0.000000 0.000000 -1.000000 1408 | vn 0.000000 0.000000 -1.000000 1409 | vn 0.000000 0.000000 -1.000000 1410 | vn 0.000000 0.000000 -1.000000 1411 | vn 0.000000 0.000000 -1.000000 1412 | vn 0.000000 0.000000 -1.000000 1413 | vn 0.000000 0.000000 -1.000000 1414 | vn 0.000000 0.000000 -1.000000 1415 | vn 0.000000 0.000000 -1.000000 1416 | vn 0.000000 0.000000 -1.000000 1417 | vn 0.000000 0.000000 -1.000000 1418 | vn 0.000000 0.000000 -1.000000 1419 | vn 0.000000 0.000000 -1.000000 1420 | vn 0.000000 0.000000 -1.000000 1421 | vn 0.000000 0.000000 -1.000000 1422 | vn 0.000000 0.000000 -1.000000 1423 | vn 0.000000 0.000000 -1.000000 1424 | vn 0.000000 0.000000 -1.000000 1425 | vn 0.000000 0.000000 -1.000000 1426 | vn 0.000000 0.000000 -1.000000 1427 | vn 0.000000 0.000000 -1.000000 1428 | vn 0.000000 0.000000 -1.000000 1429 | vn 0.000000 0.000000 -1.000000 1430 | vn 0.000000 0.000000 -1.000000 1431 | vn 0.000000 0.000000 -1.000000 1432 | vn 0.000000 0.000000 -1.000000 1433 | vn 0.000000 0.000000 -1.000000 1434 | vn 0.000000 0.000000 -1.000000 1435 | vn 0.000000 0.000000 -1.000000 1436 | vn 0.000000 0.000000 -1.000000 1437 | vn 0.000000 0.000000 -1.000000 1438 | vn 0.000000 0.000000 -1.000000 1439 | vn 0.000000 0.000000 -1.000000 1440 | vn 0.000000 0.000000 -1.000000 1441 | f 1 2 3 1442 | f 4 5 6 1443 | f 7 8 9 1444 | f 10 11 12 1445 | f 13 14 15 1446 | f 16 17 18 1447 | f 19 20 21 1448 | f 22 23 24 1449 | f 25 26 27 1450 | f 28 29 30 1451 | f 31 32 33 1452 | f 34 35 36 1453 | f 37 38 39 1454 | f 40 41 42 1455 | f 43 44 45 1456 | f 46 47 48 1457 | f 49 50 51 1458 | f 52 53 54 1459 | f 55 56 57 1460 | f 58 59 60 1461 | f 61 62 63 1462 | f 64 65 66 1463 | f 67 68 69 1464 | f 70 71 72 1465 | f 73 74 75 1466 | f 76 77 78 1467 | f 79 80 81 1468 | f 82 83 84 1469 | f 85 86 87 1470 | f 88 89 90 1471 | f 91 92 93 1472 | f 94 95 96 1473 | f 97 98 99 1474 | f 100 101 102 1475 | f 103 104 105 1476 | f 106 107 108 1477 | f 109 110 111 1478 | f 112 113 114 1479 | f 115 116 117 1480 | f 118 119 120 1481 | f 121 122 123 1482 | f 124 125 126 1483 | f 127 128 129 1484 | f 130 131 132 1485 | f 133 134 135 1486 | f 136 137 138 1487 | f 139 140 141 1488 | f 142 143 144 1489 | f 145 146 147 1490 | f 148 149 150 1491 | f 151 152 153 1492 | f 154 155 156 1493 | f 157 158 159 1494 | f 160 161 162 1495 | f 163 164 165 1496 | f 166 167 168 1497 | f 169 170 171 1498 | f 172 173 174 1499 | f 175 176 177 1500 | f 178 179 180 1501 | f 181 182 183 1502 | f 184 185 186 1503 | f 187 188 189 1504 | f 190 191 192 1505 | f 193 194 195 1506 | f 196 197 198 1507 | f 199 200 201 1508 | f 202 203 204 1509 | f 205 206 207 1510 | f 208 209 210 1511 | f 211 212 213 1512 | f 214 215 216 1513 | f 217 218 219 1514 | f 220 221 222 1515 | f 223 224 225 1516 | f 226 227 228 1517 | f 229 230 231 1518 | f 232 233 234 1519 | f 235 236 237 1520 | f 238 239 240 1521 | f 241 242 243 1522 | f 244 245 246 1523 | f 247 248 249 1524 | f 250 251 252 1525 | f 253 254 255 1526 | f 256 257 258 1527 | f 259 260 261 1528 | f 262 263 264 1529 | f 265 266 267 1530 | f 268 269 270 1531 | f 271 272 273 1532 | f 274 275 276 1533 | f 277 278 279 1534 | f 280 281 282 1535 | f 283 284 285 1536 | f 286 287 288 1537 | f 289 290 291 1538 | f 292 293 294 1539 | f 295 296 297 1540 | f 298 299 300 1541 | f 301 302 303 1542 | f 304 305 306 1543 | f 307 308 309 1544 | f 310 311 312 1545 | f 313 314 315 1546 | f 316 317 318 1547 | f 319 320 321 1548 | f 322 323 324 1549 | f 325 326 327 1550 | f 328 329 330 1551 | f 331 332 333 1552 | f 334 335 336 1553 | f 337 338 339 1554 | f 340 341 342 1555 | f 343 344 345 1556 | f 346 347 348 1557 | f 349 350 351 1558 | f 352 353 354 1559 | f 355 356 357 1560 | f 358 359 360 1561 | f 361 362 363 1562 | f 364 365 366 1563 | f 367 368 369 1564 | f 370 371 372 1565 | f 373 374 375 1566 | f 376 377 378 1567 | f 379 380 381 1568 | f 382 383 384 1569 | f 385 386 387 1570 | f 388 389 390 1571 | f 391 392 393 1572 | f 394 395 396 1573 | f 397 398 399 1574 | f 400 401 402 1575 | f 403 404 405 1576 | f 406 407 408 1577 | f 409 410 411 1578 | f 412 413 414 1579 | f 415 416 417 1580 | f 418 419 420 1581 | f 421 422 423 1582 | f 424 425 426 1583 | f 427 428 429 1584 | f 430 431 432 1585 | f 433 434 435 1586 | f 436 437 438 1587 | f 439 440 441 1588 | f 442 443 444 1589 | f 445 446 447 1590 | f 448 449 450 1591 | f 451 452 453 1592 | f 454 455 456 1593 | f 457 458 459 1594 | f 460 461 462 1595 | f 463 464 465 1596 | f 466 467 468 1597 | f 469 470 471 1598 | f 472 473 474 1599 | f 475 476 477 1600 | f 478 479 480 1601 | f 481 482 483 1602 | f 484 485 486 1603 | f 487 488 489 1604 | f 490 491 492 1605 | f 493 494 495 1606 | f 496 497 498 1607 | f 499 500 501 1608 | f 502 503 504 1609 | f 505 506 507 1610 | f 508 509 510 1611 | f 511 512 513 1612 | f 514 515 516 1613 | f 517 518 519 1614 | f 520 521 522 1615 | f 523 524 525 1616 | f 526 527 528 1617 | f 529 530 531 1618 | f 532 533 534 1619 | f 535 536 537 1620 | f 538 539 540 1621 | f 541 542 543 1622 | f 544 545 546 1623 | f 547 548 549 1624 | f 550 551 552 1625 | f 553 554 555 1626 | f 556 557 558 1627 | f 559 560 561 1628 | f 562 563 564 1629 | f 565 566 567 1630 | f 568 569 570 1631 | f 571 572 573 1632 | f 574 575 576 1633 | f 577 578 579 1634 | f 580 581 582 1635 | f 583 584 585 1636 | f 586 587 588 1637 | f 589 590 591 1638 | f 592 593 594 1639 | f 595 596 597 1640 | f 598 599 600 1641 | f 601 602 603 1642 | f 604 605 606 1643 | f 607 608 609 1644 | f 610 611 612 1645 | f 613 614 615 1646 | f 616 617 618 1647 | f 619 620 621 1648 | f 622 623 624 1649 | f 625 626 627 1650 | f 628 629 630 1651 | f 631 632 633 1652 | f 634 635 636 1653 | f 637 638 639 1654 | f 640 641 642 1655 | f 643 644 645 1656 | f 646 647 648 1657 | f 649 650 651 1658 | f 652 653 654 1659 | f 655 656 657 1660 | f 658 659 660 1661 | f 661 662 663 1662 | f 664 665 666 1663 | f 667 668 669 1664 | f 670 671 672 1665 | f 673 674 675 1666 | f 676 677 678 1667 | f 679 680 681 1668 | f 682 683 684 1669 | f 685 686 687 1670 | f 688 689 690 1671 | f 691 692 693 1672 | f 694 695 696 1673 | f 697 698 699 1674 | f 700 701 702 1675 | f 703 704 705 1676 | f 706 707 708 1677 | f 709 710 711 1678 | f 712 713 714 1679 | f 715 716 717 1680 | f 718 719 720 1681 | --------------------------------------------------------------------------------