├── .gitignore ├── InertiaSet.py ├── Joint.py ├── LICENSE ├── Link.py ├── README.md ├── Robot.py ├── SpatialAlgebra.py ├── URDFParser.py ├── __init__.py └── requirements.txt /.gitignore: -------------------------------------------------------------------------------- 1 | # The following is adapted and extended from: https://github.com/github/gitignore 2 | 3 | # Custom debug files 4 | t.txt 5 | t2.txt 6 | t3.txt 7 | 8 | # Prerequisites 9 | *.d 10 | 11 | # Compiled Object files 12 | *.slo 13 | *.lo 14 | *.o 15 | *.obj 16 | 17 | # Precompiled Headers 18 | *.gch 19 | *.pch 20 | 21 | # Compiled Dynamic libraries 22 | *.so 23 | *.dylib 24 | *.dll 25 | 26 | # Fortran module files 27 | *.mod 28 | *.smod 29 | 30 | # Compiled Static libraries 31 | *.lai 32 | *.la 33 | *.a 34 | *.lib 35 | 36 | # Executables 37 | *.exe 38 | *.out 39 | *.app 40 | 41 | # CUDA 42 | *.i 43 | *.ii 44 | *.gpu 45 | *.ptx 46 | *.cubin 47 | *.fatbin 48 | 49 | # Byte-compiled / optimized / DLL files 50 | __pycache__/ 51 | *.py[cod] 52 | *$py.class 53 | 54 | # Distribution / packaging 55 | .Python 56 | .eggs/ 57 | *.egg-info/ 58 | .installed.cfg 59 | *.egg 60 | 61 | # Installer logs 62 | pip-log.txt 63 | pip-delete-this-directory.txt 64 | 65 | # Jupyter Notebook 66 | .ipynb_checkpoints 67 | 68 | # IPython 69 | profile_default/ 70 | ipython_config.py 71 | 72 | # Python Environments 73 | .env 74 | .venv 75 | env/ 76 | venv/ 77 | ENV/ 78 | env.bak/ 79 | venv.bak/ -------------------------------------------------------------------------------- /InertiaSet.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class InertiaSet: 4 | def __init__(self, ixx_in, ixy_in, ixz_in, iyy_in, iyz_in, izz_in): 5 | self.ixx = ixx_in 6 | self.ixy = ixy_in 7 | self.ixz = ixz_in 8 | self.iyy = iyy_in 9 | self.iyz = iyz_in 10 | self.izz = izz_in 11 | 12 | def to_vector(self): 13 | return np.array([self.ixx,self.ixy,self.ixz,self.iyy,self.iyz,self.izz]) 14 | 15 | def to_matrix(self): 16 | return np.array([[self.ixx,self.ixy,self.ixz],[self.ixy,self.iyy,self.iyz],[self.ixz,self.iyz,self.izz]]) 17 | 18 | def is_zero(self): 19 | if self.ixx == 0 and self.ixy == 0 and self.ixz == 0 and \ 20 | self.iyy == 0 and self.iyz == 0 and self.izz == 0: 21 | return True 22 | else: 23 | return False 24 | -------------------------------------------------------------------------------- /Joint.py: -------------------------------------------------------------------------------- 1 | # import numpy as np 2 | import numpy as np 3 | import sympy as sp 4 | from .SpatialAlgebra import Origin, Translation, Rotation 5 | 6 | class Joint: 7 | def __init__(self, name, jid, parent, child): 8 | self.name = name # name 9 | self.jid = jid # temporary ID (replaced by standard DFS parse ordering) 10 | self.urdf_jid = jid # URDF ordered ID 11 | self.bfs_jid = jid # temporary ID (replaced by BFS parse ordering) 12 | self.bfs_level = 0 # temporary level (replaced by BFS parse ordering) 13 | self.origin = Origin() # Fixed origin location 14 | self.jtype = None # type of joint 15 | self.parent = parent # parent link name 16 | self.child = child # child link name 17 | self.theta = sp.symbols("theta") # Free joint variable 18 | self.Xmat_sp = None # Sympy X matrix placeholder 19 | self.Xmat_sp_free = None # Sympy X_free matrix placeholder 20 | self.Smat_sp = None # Sympy S matrix placeholder (usually a vector) 21 | self.damping = 0 # damping placeholder 22 | 23 | def set_id(self, id_in): 24 | self.jid = id_in 25 | 26 | def set_parent(self, parent_name): 27 | self.parent = parent_name 28 | 29 | def set_child(self, child_name): 30 | self.child = child_name 31 | 32 | def set_bfs_id(self, id_in): 33 | self.bfs_id = id_in 34 | 35 | def set_bfs_level(self, level_in): 36 | self.bfs_level = level_in 37 | 38 | def set_origin_xyz(self, x, y = None, z = None): 39 | self.origin.set_translation(x,y,z) 40 | 41 | def set_origin_rpy(self, r, p = None, y = None): 42 | self.origin.set_rotation(r,p,y) 43 | 44 | def set_damping(self, damping): 45 | self.damping = damping 46 | 47 | def set_transformation_matrix(self, matrix_in): 48 | self.Xmat_sp = matrix_in 49 | 50 | def set_type(self, jtype, axis = None): 51 | self.jtype = jtype 52 | self.origin.build_fixed_transform() 53 | if self.jtype == 'revolute': 54 | if axis[2] == 1: 55 | self.Xmat_sp_free = self.origin.rotation.rot(self.origin.rotation.rz(self.theta)) 56 | self.S = np.array([0,0,1,0,0,0]) 57 | elif axis[1] == 1: 58 | self.Xmat_sp_free = self.origin.rotation.rot(self.origin.rotation.ry(self.theta)) 59 | self.S = np.array([0,1,0,0,0,0]) 60 | elif axis[0] == 1: 61 | self.Xmat_sp_free = self.origin.rotation.rot(self.origin.rotation.rx(self.theta)) 62 | self.S = np.array([1,0,0,0,0,0]) 63 | elif self.jtype == 'prismatic': 64 | if axis[2] == 1: 65 | self.Xmat_sp_free = self.origin.translation.xlt(self.origin.translation.skew(0,0,self.theta)) 66 | self.S = np.array([0,0,0,0,0,1]) 67 | elif axis[1] == 1: 68 | self.Xmat_sp_free = self.origin.translation.xlt(self.origin.translation.skew(0,self.theta,0)) 69 | self.S = np.array([0,0,0,0,1,0]) 70 | elif axis[0] == 1: 71 | self.Xmat_sp_free = self.origin.translation.xlt(self.origin.translation.skew(self.theta,0,0)) 72 | self.S = np.array([0,0,0,1,0,0]) 73 | elif self.jtype == 'fixed': 74 | self.Xmat_sp_free = sp.eye(6) 75 | self.S = np.array([0,0,0,0,0,0]) 76 | else: 77 | print('Only revolute and fixed joints currently supported!') 78 | exit() 79 | self.Xmat_sp = self.Xmat_sp_free * self.origin.Xmat_sp_fixed 80 | # remove numerical noise (e.g., URDF's often specify angles as 3.14 or 3.14159 but that isn't exactly PI) 81 | self.Xmat_sp = sp.nsimplify(self.Xmat_sp, tolerance=1e-6, rational=True).evalf() 82 | 83 | def get_transformation_matrix_function(self): 84 | return sp.utilities.lambdify(self.theta, self.Xmat_sp, 'numpy') 85 | 86 | def get_transformation_matrix(self): 87 | return self.Xmat_sp 88 | 89 | def get_joint_subspace(self): 90 | return self.S 91 | 92 | def get_damping(self): 93 | return self.damping 94 | 95 | def get_name(self): 96 | return self.name 97 | 98 | def get_id(self): 99 | return self.jid 100 | 101 | def get_bfs_id(self): 102 | return self.bfs_id 103 | 104 | def get_bfs_level(self): 105 | return self.bfs_level 106 | 107 | def get_parent(self): 108 | return self.parent 109 | 110 | def get_child(self): 111 | return self.child 112 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Hardware Acceleration for Robotics 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 | -------------------------------------------------------------------------------- /Link.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sympy as sp 3 | from .InertiaSet import InertiaSet 4 | from .SpatialAlgebra import Origin, Translation, Rotation 5 | 6 | class Link: 7 | def __init__(self, name, lid): 8 | self.name = name # name 9 | self.lid = lid # temporary ID (replaced by standard DFS parse ordering) 10 | self.urdf_lid = lid # URDF ordered ID 11 | self.bfs_lid = lid # temporary ID (replaced by BFS parse ordering) 12 | self.bfs_level = 0 # temporary level (replaced by BFS parse ordering) 13 | self.parent_id = None # temporary ID (replaced later) 14 | self.origin = Origin() # Fixed origin location 15 | self.mass = None 16 | self.inertia = None 17 | self.spatial_ineratia = None 18 | 19 | def set_id(self, id_in): 20 | self.lid = id_in 21 | 22 | def set_parent_id(self, id_in): 23 | self.parent_id = id_in 24 | 25 | def set_bfs_id(self, id_in): 26 | self.bfs_id = id_in 27 | 28 | def set_bfs_level(self, level_in): 29 | self.bfs_level = level_in 30 | 31 | def set_subtree(self, subtree_in): 32 | self.subtree = subtree_in 33 | 34 | def set_origin_xyz(self, x, y = None, z = None): 35 | self.origin.set_translation(x,y,z) 36 | 37 | def set_origin_rpy(self, r, p = None, y = None): 38 | self.origin.set_rotation(r,p,y) 39 | 40 | def set_inertia(self, mass, ixx, ixy, ixz, iyy, iyz, izz): 41 | self.mass = mass 42 | self.inertia = InertiaSet(ixx,ixy,ixz,iyy,iyz,izz) 43 | self.build_spatial_inertia() 44 | 45 | def set_spatial_inertia(self, inertia_in): 46 | self.spatial_ineratia = inertia_in 47 | 48 | def build_spatial_inertia(self): 49 | if self.inertia is None or self.origin.translation is None: 50 | print("[!Error] Set origin and inertia first!") 51 | # I6x6 = I3x3 + mccT mc I3x3 = Ixx Ixy Ixz c = 0 -cz cy 52 | # mcT mI Ixy Iyy Iyz cz 0 -cx 53 | # Ixz Iyz Izz -cy cx 0 54 | rx = sp.nsimplify(self.origin.translation.rx, tolerance=1e-12, rational=True) 55 | com_trans = np.reshape(np.array([expr.evalf() for expr in rx]),(3,3)) 56 | 57 | mc = self.mass*com_trans 58 | mccT = np.matmul(mc,com_trans.transpose()) 59 | 60 | topLeft = self.inertia.to_matrix() + mccT 61 | top = np.hstack((topLeft,mc)) 62 | bottom = np.hstack((mc.transpose(),self.mass*np.eye(3))) 63 | self.spatial_ineratia = np.vstack((top,bottom)).astype(float) 64 | # remove numerical noise (e.g., URDF's often specify angles as 3.14 or 3.14159 but that isn't exactly PI) 65 | self.spatial_ineratia[np.isclose(self.spatial_ineratia, np.zeros((6,6)), 1e-10, 1e-10)] = 0 66 | 67 | def get_spatial_inertia(self): 68 | return self.spatial_ineratia 69 | 70 | def get_name(self): 71 | return self.name 72 | 73 | def get_id(self): 74 | return self.lid 75 | 76 | def get_parent_id(self): 77 | return self.parent_id 78 | 79 | def get_bfs_id(self): 80 | return self.bfs_id 81 | 82 | def get_bfs_level(self): 83 | return self.bfs_level 84 | 85 | def get_subtree(self): 86 | return self.subtree 87 | 88 | def is_world_base_frame(self): 89 | if self.mass == 0 and self.inertia.is_zero(): 90 | return True 91 | else: 92 | return False 93 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # URDFParser 2 | 3 | A simple parser libaray for URDF Files. That returns a ```robot``` object which can be used to access links, joints, transformation matrices, etc. 4 | 5 | ## Usage: 6 | ```python 7 | parser = URDFParser() 8 | robot = parser.parse(urdf_filepath, alpha_tie_breaker = False) 9 | ``` 10 | Where the tie breaker is used to order joints with the same parent link. 11 | ```python 12 | alpha_tie_breaker=False # URDF ordering used 13 | alpha_tie_breaker=True # Joint name ordering used 14 | ``` 15 | 16 | ## Instalation Instructions: 17 | There are 4 required packages ```beautifulsoup4, lxml, numpy, sympy``` which can be automatically installed by running: 18 | ```shell 19 | pip3 install -r requirements.txt 20 | ``` 21 | 22 | ## Robot API: 23 | 24 | The main API is as follows where **XXX** can be replaced by: 25 | + **joint**: a joint object (see API below) 26 | + **link**: a link object (see API below) 27 | + **Xmat**: a sympy transformation matrix with one free variable as defined by its joint 28 | + **Xmat_Func**: a function that returns a numpy matrix when passed a value for the free variable 29 | + **Imat**: a numpy 6x6 inertia matrix 30 | + **S**: a numpy 6x1 motion subspace matrix 31 | 32 | ```python 33 | # A single object by its ID or by its name as defined in the URDF 34 | get_XXX_by_id(lid) # jid for joints 35 | get_XXX_by_name(name) 36 | # A list of the objects that occur in the given bfs level 37 | get_XXX_by_bfs_level(name) 38 | # A list of the object ordered by their IDs or by their names as defined in the URDF 39 | # Note: The base link/inertia exists at index -1 and so will appear at the beginning of the list 40 | get_XXXs_ordered_by_id(reverse = False) 41 | get_XXXs_ordered_by_name(reverse = False) 42 | # A dictionary of objects by their ID or by their name as defined in the URDF 43 | # Note: The base link/inertia exists at index -1 44 | get_XXXs_dict_by_id() 45 | get_XXXs_dict_by_name() 46 | ``` 47 | 48 | The API also includes the following functions: 49 | ```python 50 | # get the robot name 51 | get_name() 52 | # get the robot type (if applicable) 53 | is_serial_chain() 54 | # get the number of positions and velocities in the robot state as well as numbers of links and joints 55 | # note: links should be joints + 1 when including the base, num_joints = num_pos 56 | # num_vel = num_pos for fixed base (and is one larger with quaternion) 57 | get_num_pos() 58 | get_num_vel() 59 | get_num_cntrl() 60 | get_num_joints() 61 | get_num_links() 62 | get_num_links_effective() # num_links - 1 (base link is not used in many RBD algorithms when fixed) 63 | # get the max bfs_level 64 | get_max_bfs_level() 65 | # get the IDs at a given bfs level and the bfs level for a given id 66 | get_ids_by_bfs_level(level) 67 | get_bfs_level_by_id(jid) 68 | # get the ID of the parent(s) of a given link(s) by id 69 | get_parent_id(lid) 70 | get_parent_ids(lids) 71 | get_unique_parent_ids(lids) # remove duplicates 72 | # get the full list of parents ordered by id 73 | get_parent_id_array() 74 | # test if there is a repeated parent by ids 75 | has_repeated_parents(jids) 76 | # get the subtree IDs for a given id and total count and test if in a subtree 77 | get_subtree_by_id(jid) 78 | get_total_subtree_count() 79 | get_is_in_subtree_of(jid,jid_of) 80 | # get the ancestor IDs for a given id and total count and test if an ancestor 81 | get_ancestors_by_id(jid) 82 | get_total_ancestor_count() 83 | get_is_ancestor_of(jid,jid_of) 84 | # get all joints that have parent link name as the parent or child link name as the child 85 | get_joints_by_parent_name(parent_name) 86 | get_joints_by_child_name(child_name) 87 | # get the joint that has parent link name as the parent and child link name as the child 88 | get_joint_by_parent_child_name(parent_name,child_name) 89 | # see if the following joints have the same S (useful for codegen) 90 | are_Ss_identical(jids) 91 | # get the velocity damping coefficients 92 | get_damping_by_id(jid) 93 | ``` 94 | 95 | ## Joint API: 96 | ```python 97 | # get the name, id, and bfs of the joint 98 | get_name() 99 | get_id() 100 | get_bfs_id() 101 | get_bfs_level() 102 | # get the parent and child link name 103 | get_parent() 104 | get_child() 105 | # get the Xmat or Xmat_Func for this joint as defined above 106 | get_transformation_matrix() 107 | get_transformation_matrix_function() 108 | # get the S for this joint as defined above 109 | get_joint_subspace() 110 | # get the velocity damping coefficent for this joint 111 | get_damping() 112 | ``` 113 | 114 | ## Link API: 115 | ```python 116 | # get the name, id, and bfs of the link 117 | get_name() 118 | get_id() 119 | get_bfs_id() 120 | get_bfs_level() 121 | # get the link's spatial inertia matrix 122 | get_spatial_inertia() 123 | ``` 124 | -------------------------------------------------------------------------------- /Robot.py: -------------------------------------------------------------------------------- 1 | from .Link import Link 2 | from .Joint import Joint 3 | 4 | class Robot: 5 | # initialization 6 | def __init__(self, name): 7 | self.name = name 8 | self.links = [] 9 | self.joints = [] 10 | 11 | def next_none(self, iterable): 12 | try: 13 | return next(iterable) 14 | except: 15 | return None 16 | 17 | ################# 18 | # Setters # 19 | ################# 20 | 21 | def add_joint(self, joint): 22 | self.joints.append(joint) 23 | 24 | def add_link(self, link): 25 | self.links.append(link) 26 | 27 | def remove_joint(self, joint): 28 | self.joints.remove(joint) 29 | 30 | def remove_link(self, link): 31 | self.links.remove(link) 32 | 33 | ######################### 34 | # Generic Getters # 35 | ######################### 36 | 37 | def get_num_pos(self): 38 | return self.get_num_joints() 39 | 40 | def get_num_vel(self): 41 | return self.get_num_joints() 42 | 43 | def get_num_cntrl(self): 44 | return self.get_num_joints() 45 | 46 | def get_name(self): 47 | return self.name 48 | 49 | def is_serial_chain(self): 50 | return all([jid - self.get_parent_id(jid) == 1 for jid in range(self.get_num_joints())]) 51 | 52 | def get_parent_id(self, lid): 53 | return self.get_link_by_id(lid).get_parent_id() 54 | 55 | def get_parent_ids(self, lids): 56 | return [self.get_parent_id(lid) for lid in lids] 57 | 58 | def get_unique_parent_ids(self, lids): 59 | return list(set(self.get_parent_ids(lids))) 60 | 61 | def get_parent_id_array(self): 62 | return [tpl[1] for tpl in sorted([(link.get_id(),link.get_parent_id()) for link in self.links], key=lambda tpl: tpl[0])[1:]] 63 | 64 | def has_repeated_parents(self, jids): 65 | return len(self.get_parent_ids(jids)) != len(self.get_unique_parent_ids(jids)) 66 | 67 | def get_subtree_by_id(self, lid): 68 | return sorted(self.get_link_by_id(lid).get_subtree()) 69 | 70 | def get_total_subtree_count(self): 71 | return sum([len(self.get_subtree_by_id(lid)) for lid in range(self.get_num_joints())]) 72 | 73 | def get_ancestors_by_id(self, jid): 74 | ancestors = [] 75 | curr_id = jid 76 | while True: 77 | curr_id = self.get_parent_id(curr_id) 78 | if curr_id == -1: 79 | break 80 | else: 81 | ancestors.append(curr_id) 82 | return ancestors 83 | 84 | def get_total_ancestor_count(self): 85 | return sum([len(self.get_ancestors_by_id(jid)) for jid in range(self.get_num_joints())]) 86 | 87 | def get_is_ancestor_of(self, jid, jid_of): 88 | return jid in self.get_ancestors_by_id(jid_of) 89 | 90 | def get_is_in_subtree_of(self, jid, jid_of): 91 | return jid in self.get_subtree_by_id(jid_of) 92 | 93 | def get_max_bfs_level(self): 94 | return sorted(self.joints, key=lambda joint: joint.bfs_level, reverse = True)[0].bfs_level 95 | 96 | def get_ids_by_bfs_level(self, level): 97 | return [joint.jid for joint in self.get_joints_by_bfs_level(level)] 98 | 99 | def get_bfs_level_by_id(self, jid): 100 | return(self.get_joint_by_id(jid).get_bfs_level()) 101 | 102 | def get_max_bfs_width(self): 103 | return max([len(self.get_ids_by_bfs_level(level)) for level in range(self.get_max_bfs_level() + 1)]) 104 | 105 | 106 | ############### 107 | # Joint # 108 | ############### 109 | 110 | def get_num_joints(self): 111 | return len(self.joints) 112 | 113 | def get_joint_by_id(self, jid): 114 | return self.next_none(filter(lambda fjoint: fjoint.jid == jid, self.joints)) 115 | 116 | def get_joint_by_name(self, name): 117 | return self.next_none(filter(lambda fjoint: fjoint.name == name, self.joints)) 118 | 119 | def get_joints_by_bfs_level(self, level): 120 | return list(filter(lambda fjoint: fjoint.bfs_level == level, self.joints)) 121 | 122 | def get_joints_ordered_by_id(self, reverse = False): 123 | return sorted(self.joints, key=lambda item: item.jid, reverse = reverse) 124 | 125 | def get_joints_ordered_by_name(self, reverse = False): 126 | return sorted(self.joints, key=lambda item: item.name, reverse = reverse) 127 | 128 | def get_joints_dict_by_id(self): 129 | return {joint.jid:joint for joint in self.joints} 130 | 131 | def get_joints_dict_by_name(self): 132 | return {joint.name:joint for joint in self.joints} 133 | 134 | def get_joints_by_parent_name(self, parent_name): 135 | return list(filter(lambda fjoint: fjoint.parent == parent_name, self.joints)) 136 | 137 | def get_joints_by_child_name(self, child_name): 138 | return list(filter(lambda fjoint: fjoint.child == child_name, self.joints)) 139 | 140 | def get_joint_by_parent_child_name(self, parent_name, child_name): 141 | return self.next_none(filter(lambda fjoint: fjoint.parent == parent_name and fjoint.child == child_name, self.joints)) 142 | 143 | def get_damping_by_id(self, jid): 144 | return self.get_joint_by_id(jid).get_damping() 145 | 146 | ############## 147 | # Link # 148 | ############## 149 | 150 | def get_num_links(self): 151 | return len(self.links) 152 | 153 | def get_num_links_effective(self): 154 | # subtracting base link from total # of links 155 | return get_num_links() - 1 156 | 157 | def get_link_by_id(self, lid): 158 | return self.next_none(filter(lambda flink: flink.lid == lid, self.links)) 159 | 160 | def get_link_by_name(self, name): 161 | return self.next_none(filter(lambda flink: flink.name == name, self.links)) 162 | 163 | def get_links_by_bfs_level(self, level): 164 | return list(filter(lambda flink: flink.bfs_level == level, self.links)) 165 | 166 | def get_links_ordered_by_id(self, reverse = False): 167 | return sorted(self.links, key=lambda item: item.lid, reverse = reverse) 168 | 169 | def get_links_ordered_by_name(self, reverse = False): 170 | return sorted(self.links, key=lambda item: item.name, reverse = reverse) 171 | 172 | def get_links_dict_by_id(self): 173 | return {link.lid:link for link in self.links} 174 | 175 | def get_links_dict_by_name(self): 176 | return {link.name:link for link in self.links} 177 | 178 | ############## 179 | # XMAT # 180 | ############## 181 | 182 | def get_Xmat_by_id(self, jid): 183 | return self.get_joint_by_id(jid).get_transformation_matrix() 184 | 185 | def get_Xmat_by_name(self, name): 186 | return self.get_joint_by_name(name).get_transformation_matrix() 187 | 188 | def get_Xmats_by_bfs_level(self, level): 189 | return [joint.get_transformation_matrix() for joint in self.get_joints_by_bfs_level(level)] 190 | 191 | def get_Xmats_ordered_by_id(self, reverse = False): 192 | return [joint.get_transformation_matrix() for joint in self.get_joints_ordered_by_id(reverse)] 193 | 194 | def get_Xmats_ordered_by_name(self, reverse = False): 195 | return [joint.get_transformation_matrix() for joint in self.get_joints_ordered_by_name(reverse)] 196 | 197 | def get_Xmats_dict_by_id(self): 198 | return {joint.jid:joint.get_transformation_matrix() for joint in self.joints} 199 | 200 | def get_Xmats_dict_by_name(self): 201 | return {joint.name:joint.get_transformation_matrix() for joint in self.joints} 202 | 203 | ################### 204 | # XMAT_Func # 205 | ################### 206 | 207 | def get_Xmat_Func_by_id(self, jid): 208 | return self.get_joint_by_id(jid).get_transformation_matrix_function() 209 | 210 | def get_Xmat_Func_by_name(self, name): 211 | return self.get_joint_by_name(name).get_transformation_matrix_function() 212 | 213 | def get_Xmat_Funcs_by_bfs_level(self, level): 214 | return [joint.get_transformation_matrix_function() for joint in self.get_joints_by_bfs_level(level)] 215 | 216 | def get_Xmat_Funcs_ordered_by_id(self, reverse = False): 217 | return [joint.get_transformation_matrix_function() for joint in self.get_joints_ordered_by_id(reverse)] 218 | 219 | def get_Xmat_Funcs_ordered_by_name(self, reverse = False): 220 | return [joint.get_transformation_matrix_function() for joint in self.get_joints_ordered_by_name(reverse)] 221 | 222 | def get_Xmat_Funcs_dict_by_id(self): 223 | return {joint.jid:joint.get_transformation_matrix_function() for joint in self.joints} 224 | 225 | def get_Xmat_Funcs_dict_by_name(self): 226 | return {joint.name:joint.get_transformation_matrix_function() for joint in self.joints} 227 | 228 | ############## 229 | # IMAT # 230 | ############## 231 | 232 | def get_Imat_by_id(self, lid): 233 | return self.get_link_by_id(lid).get_spatial_inertia() 234 | 235 | def get_Imat_by_name(self, name): 236 | return self.get_joint_by_name(name).get_spatial_inertia() 237 | 238 | def get_Imats_by_bfs_level(self, level): 239 | return [link.get_spatial_inertia() for link in self.get_links_by_bfs_level()] 240 | 241 | def get_Imats_ordered_by_id(self, reverse = False): 242 | return [link.get_spatial_inertia() for link in self.get_links_ordered_by_id(reverse)] 243 | 244 | def get_Imats_ordered_by_name(self, reverse = False): 245 | return [link.get_spatial_inertia() for link in self.get_links_ordered_by_name(reverse)] 246 | 247 | def get_Imats_dict_by_id(self): 248 | return {link.lid:link.get_spatial_inertia() for link in self.links} 249 | 250 | def get_Imats_dict_by_name(self): 251 | return {link.name:link.get_spatial_inertia() for link in self.links} 252 | 253 | ############### 254 | # S # 255 | ############### 256 | 257 | def get_S_by_id(self, jid): 258 | return self.get_joint_by_id(jid).get_joint_subspace() 259 | 260 | def get_S_by_name(self, name): 261 | return self.get_joint_by_name(name).get_joint_subspace() 262 | 263 | def get_S_by_bfs_level(self, level): 264 | return [joint.get_joint_subspace() for joint in self.get_joints_by_bfs_level(level)] 265 | 266 | def get_Ss_ordered_by_id(self, reverse = False): 267 | return [joint.get_joint_subspace() for joint in self.get_joints_ordered_by_id(reverse)] 268 | 269 | def get_Ss_ordered_by_name(self, reverse = False): 270 | return [joint.get_joint_subspace() for joint in self.get_joints_ordered_by_name(reverse)] 271 | 272 | def get_Ss_dict_by_id(self): 273 | return {joint.jid:joint.get_joint_subspace() for joint in self.joints} 274 | 275 | def get_Ss_dict_by_name(self): 276 | return {joint.name:joint.get_joint_subspace() for joint in self.joints} 277 | 278 | def are_Ss_identical(self,jids): 279 | return all(all(self.get_S_by_id(jid) == self.get_S_by_id(jids[0])) for jid in jids) -------------------------------------------------------------------------------- /SpatialAlgebra.py: -------------------------------------------------------------------------------- 1 | import sympy as sp 2 | import copy 3 | 4 | class Translation: 5 | def __init__(self, x, y = None, z = None): 6 | if y == None: # passed in as tuple 7 | self.y = x[1] 8 | self.z = x[2] 9 | self.x = x[0] 10 | else: 11 | self.x = x 12 | self.y = y 13 | self.z = z 14 | self.rx = self.skew(self.x,self.y,self.z) 15 | self.Xmat_sp_fixed = self.xlt(self.rx) 16 | self.tx_hom = sp.Matrix([[1,0,0,self.x],[0,1,0,self.y],[0,0,1,self.z],[0,0,0,1]]) 17 | self.tx_hom_inv = sp.Matrix([[1,0,0,-self.x],[0,1,0,-self.y],[0,0,1,-self.z],[0,0,0,1]]) 18 | 19 | def skew(self, x, y, z): 20 | return sp.Matrix([[0,-z,y],[z,0,-x],[-y,x,0]]) 21 | 22 | def xlt(self, rx): 23 | col1 = sp.Matrix.vstack(sp.eye(3), -rx) 24 | col2 = sp.Matrix.vstack(sp.zeros(3, 3), sp.eye(3)) 25 | return sp.Matrix.hstack(col1, col2) 26 | 27 | class Rotation: 28 | def __init__(self, r, p = None, y = None): 29 | if p == None: # passed in as tuple 30 | self.p = r[1] 31 | self.y = r[2] 32 | self.r = r[0] 33 | else: 34 | self.r = r 35 | self.p = p 36 | self.y = y 37 | 38 | roll_mat = self.rx(self.r) 39 | pitch_mat = self.ry(self.p) 40 | yaw_mat = self.rz(self.y) 41 | self.E = roll_mat * pitch_mat * yaw_mat 42 | self.Xmat_sp_fixed = self.rot(self.E) 43 | self.E_hom = sp.Matrix.vstack(copy.deepcopy(self.E),sp.Matrix([[0,0,0]])) 44 | self.E_hom = sp.Matrix.hstack(self.E_hom,sp.Matrix([[0],[0],[0],[1]])) 45 | self.E_hom_inv = self.E_hom.transpose() 46 | 47 | def rx(self, theta): 48 | c = sp.cos(theta) 49 | s = sp.sin(theta) 50 | E = sp.Matrix([[1, 0, 0], [0, c, s], [0, -s, c]]) 51 | return E 52 | 53 | def ry(self, theta): 54 | c = sp.cos(theta) 55 | s = sp.sin(theta) 56 | E = sp.Matrix([[c, 0, -s], [0, 1, 0], [s, 0, c]]) 57 | return E 58 | 59 | def rz(self, theta): 60 | c = sp.cos(theta) 61 | s = sp.sin(theta) 62 | E = sp.Matrix([[c, s, 0], [-s, c, 0], [0, 0, 1]]) 63 | return E 64 | 65 | def rot(self, E): 66 | z = sp.zeros(3, 3) 67 | col1 = sp.Matrix.vstack(E, z) 68 | col2 = sp.Matrix.vstack(z, E) 69 | return sp.Matrix.hstack(col1, col2) 70 | 71 | class Origin: 72 | def __init__(self): 73 | self.translation = None 74 | self.rotation = None 75 | self.Xmat_sp_fixed = None 76 | self.Xmat_sp_fixed_hom = None 77 | 78 | def set_translation(self, x, y = None, z = None): 79 | self.translation = Translation(x,y,z) 80 | 81 | def set_rotation(self, r, p = None, y = None): 82 | self.rotation = Rotation(r,p,y) 83 | 84 | def build_fixed_transform(self): 85 | if self.translation is None or self.rotation is None: 86 | print("[!Error] First set the origin translation and rotation!") 87 | else: 88 | self.Xmat_sp_fixed = self.rotation.Xmat_sp_fixed * self.translation.Xmat_sp_fixed 89 | self.Xmat_sp_fixed_hom = self.rotation.E_hom * self.translation.tx_hom 90 | self.Xmat_sp_fixed_hom_inv = self.rotation.E_hom_inv * self.translation.tx_hom_inv 91 | 92 | -------------------------------------------------------------------------------- /URDFParser.py: -------------------------------------------------------------------------------- 1 | from bs4 import BeautifulSoup 2 | import numpy as np 3 | import sympy as sp 4 | import copy 5 | from .Robot import Robot 6 | from .Link import Link 7 | from .Joint import Joint 8 | 9 | class URDFParser: 10 | def __init__(self): 11 | pass 12 | 13 | def parse(self, filename, alpha_tie_breaker = False): 14 | try: 15 | # parse the file 16 | urdf_file = open(filename, "r") 17 | self.soup = BeautifulSoup(urdf_file.read(),"xml").find("robot") 18 | # set up the robot object 19 | self.robot = Robot(self.soup["name"]) 20 | # collect links 21 | self.parse_links() 22 | # collect joints 23 | self.parse_joints() 24 | # remove all fixed joints, renumber links and joints, and build parent and subtree lists 25 | self.renumber_linksJoints(alpha_tie_breaker) 26 | # report joint ordering to user 27 | self.print_joint_order() 28 | # return the robot object 29 | return copy.deepcopy(self.robot) 30 | except: 31 | return None 32 | 33 | def to_float(self, string_arr): 34 | try: 35 | return [float(value) for value in string_arr] 36 | except: 37 | return string_arr 38 | 39 | def parse_links(self): 40 | lid = 0 41 | for raw_link in self.soup.find_all('link', recursive=False): 42 | # construct link object 43 | curr_link = Link(raw_link["name"],lid) 44 | lid = lid + 1 45 | # parse origin 46 | raw_origin = raw_link.find("origin") 47 | if raw_origin == None: 48 | print("Link [" + curr_link.name + "] does not have an origin. Assuming this is the fixed world base frame. Else there is an error with your URDF file.") 49 | curr_link.set_origin_xyz([0, 0, 0]) 50 | curr_link.set_origin_rpy([0, 0, 0]) 51 | else: 52 | curr_link.set_origin_xyz(self.to_float(raw_origin["xyz"].split(" "))) 53 | curr_link.set_origin_rpy(self.to_float(raw_origin["rpy"].split(" "))) 54 | # parse inertial properties 55 | raw_inertial = raw_link.find("inertial") 56 | if raw_inertial == None: 57 | print("Link [" + curr_link.name + "] does not have inertial properties. Assuming this is the fixed world base frame. Else there is an error with your URDF file.") 58 | curr_link.set_inertia(0, 0, 0, 0, 0, 0, 0) 59 | else: 60 | # get mass and inertia values 61 | raw_inertia = raw_inertial.find("inertia") 62 | curr_link.set_inertia(float(raw_inertial.find("mass")["value"]), \ 63 | float(raw_inertia["ixx"]), \ 64 | float(raw_inertia["ixy"]), \ 65 | float(raw_inertia["ixz"]), \ 66 | float(raw_inertia["iyy"]), \ 67 | float(raw_inertia["iyz"]), \ 68 | float(raw_inertia["izz"])) 69 | # store 70 | self.robot.add_link(copy.deepcopy(curr_link)) 71 | 72 | def parse_joints(self): 73 | jid = 0 74 | for raw_joint in self.soup.find_all('joint', recursive=False): 75 | # construct joint object 76 | curr_joint = Joint(raw_joint["name"], jid, \ 77 | raw_joint.find("parent")["link"], \ 78 | raw_joint.find("child")["link"]) 79 | jid += 1 80 | # get origin position and rotation 81 | curr_joint.set_origin_xyz(self.to_float(raw_joint.find("origin")["xyz"].split(" "))) 82 | curr_joint.set_origin_rpy(self.to_float(raw_joint.find("origin")["rpy"].split(" "))) 83 | # set joint type and axis of motion for joints if applicable 84 | raw_axis = raw_joint.find("axis") 85 | if raw_axis is None: 86 | curr_joint.set_type(raw_joint["type"]) 87 | else: 88 | curr_joint.set_type(raw_joint["type"],self.to_float(raw_axis["xyz"].split(" "))) 89 | raw_dynamics = raw_joint.find("dynamics") 90 | if raw_dynamics is None: 91 | curr_joint.set_damping(0) 92 | else: 93 | curr_joint.set_damping(float(raw_dynamics["damping"])) 94 | # store 95 | self.robot.add_joint(copy.deepcopy(curr_joint)) 96 | 97 | def remove_fixed_joints(self): 98 | for curr_joint in self.robot.get_joints_ordered_by_id(): 99 | if curr_joint.jtype == "fixed": 100 | # updated fixed transforms and parents of grandchild_joints 101 | # to account for the additional fixed transform 102 | # X_grandchild = X_granchild * X_child 103 | for gcjoint in self.robot.get_joints_by_parent_name(curr_joint.child): 104 | gcjoint.set_parent(curr_joint.get_parent()) 105 | gcjoint.set_transformation_matrix(gcjoint.get_transformation_matrix() * curr_joint.get_transformation_matrix()) 106 | # combine inertia tensors of child and parent at parent 107 | # note: if X is the transform from A to B the I_B = X^T I_A X 108 | # note2: inertias in the same from add so I_parent_final = I_parent + X^T I_child X 109 | child_link = self.robot.get_link_by_name(curr_joint.child) 110 | parent_link = self.robot.get_link_by_name(curr_joint.parent) 111 | child_I = child_link.get_spatial_inertia() 112 | curr_Xmat = np.reshape(np.array(curr_joint.get_transformation_matrix()).astype(float),(6,6)) 113 | transformed_Imat = np.matmul(np.matmul(np.transpose(curr_Xmat),child_I),curr_Xmat) 114 | parent_link.set_spatial_inertia(parent_link.get_spatial_inertia() + transformed_Imat) 115 | 116 | # delete the bypassed fixed joint and link 117 | self.robot.remove_joint(curr_joint) 118 | self.robot.remove_link(child_link) 119 | 120 | def build_subtree_lists(self): 121 | subtree_lid_lists = {} 122 | # initialize all subtrees to include itself 123 | for lid in self.robot.get_links_dict_by_id().keys(): 124 | subtree_lid_lists[lid] = [lid] 125 | # start at the leaves and build up! 126 | for curr_joint in self.robot.get_joints_ordered_by_id(reverse=True): 127 | parent_lid = self.robot.get_link_by_name(curr_joint.parent).get_id() 128 | child_lid = self.robot.get_link_by_name(curr_joint.child).get_id() 129 | # add the child's subtree list to the parent (includes the child) 130 | if child_lid in subtree_lid_lists.keys(): 131 | subtree_lid_lists[parent_lid] = list(set(subtree_lid_lists[parent_lid]).union(set(subtree_lid_lists[child_lid]))) 132 | # save to the links 133 | for link in self.robot.links: 134 | curr_subtree = subtree_lid_lists[link.get_id()] 135 | link.set_subtree(copy.deepcopy(curr_subtree)) 136 | 137 | def dfs_order_update(self, parent_name, alpha_tie_breaker = False, next_lid = 0, next_jid = 0): 138 | while True: 139 | child_joints = self.robot.get_joints_by_parent_name(parent_name) 140 | parent_id = self.robot.get_link_by_name(parent_name).lid 141 | if alpha_tie_breaker: 142 | child_joints.sort(key=lambda joint: joint.name) 143 | for curr_joint in child_joints: 144 | # save the new id 145 | curr_joint.set_id(next_jid) 146 | # save the next_lid to the child 147 | child = self.robot.get_link_by_name(curr_joint.child) 148 | child.set_id(next_lid) 149 | child.set_parent_id(parent_id) 150 | # recurse 151 | next_lid, next_jid = self.dfs_order_update(child.name, alpha_tie_breaker, next_lid + 1, next_jid + 1) 152 | # return to parent 153 | return next_lid, next_jid 154 | 155 | def bfs_order(self, root_name): 156 | # initialize 157 | next_lid = 0 158 | next_jid = 0 159 | next_parent_names = [(root_name,-1)] 160 | self.robot.get_link_by_name(root_name).set_bfs_id(-1) 161 | self.robot.get_link_by_name(root_name).set_bfs_level(-1) 162 | # until there are no parent to parse 163 | while len(next_parent_names) != 0: 164 | # get the next parent and save its level 165 | (parent_name, parent_level) = next_parent_names.pop(0) 166 | next_level = parent_level + 1 167 | # then until there are no children to parse (of that parent) 168 | child_joints = self.robot.get_joints_by_parent_name(parent_name) 169 | while len(child_joints) != 0: 170 | # update the current link 171 | curr_joint = child_joints.pop(0) 172 | curr_joint.set_bfs_id(next_jid) 173 | curr_joint.set_bfs_level(next_level) 174 | # append the child to the list of future possible parents 175 | curr_child_name = curr_joint.get_child() 176 | next_parent_names.append((curr_child_name,next_level)) 177 | # update the child 178 | curr_link = self.robot.get_link_by_name(curr_child_name) 179 | curr_link.set_bfs_id(next_lid) 180 | curr_link.set_bfs_level(next_level) 181 | # update the global lid, jid 182 | next_lid += 1 183 | next_jid += 1 184 | 185 | 186 | def renumber_linksJoints(self, alpha_tie_breaker = False): 187 | # remove all fixed joints where applicable (merge links) 188 | self.remove_fixed_joints() 189 | # find the root link 190 | link_names = set([link.name for link in self.robot.get_links_ordered_by_id()]) 191 | links_that_are_children = set([joint.get_child() for joint in self.robot.get_joints_ordered_by_id()]) 192 | root_link_name = list(link_names.difference(links_that_are_children))[0] 193 | # start renumbering at -1 as the base link is fixed by default 194 | self.robot.get_link_by_name(root_link_name).set_id(-1) 195 | # generate the standard dfs ordering of joints/links 196 | self.dfs_order_update(root_link_name, alpha_tie_breaker) 197 | # also save a bfs parse ordering and levels of joints/links 198 | self.bfs_order(root_link_name) 199 | # build subtree lists 200 | self.build_subtree_lists() 201 | 202 | def print_joint_order(self): 203 | print("------------------------------------------") 204 | print("Assumed Input Joint Configuration Ordering") 205 | print("------------------------------------------") 206 | for curr_joint in self.robot.get_joints_ordered_by_id(): 207 | print(curr_joint.get_name()) 208 | print("----------------------------") 209 | print("Total of n = " + str(self.robot.get_num_pos()) + " joints") 210 | print("----------------------------") -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | from .URDFParser import URDFParser 2 | from .Robot import Robot 3 | from .Link import Link 4 | from .Joint import Joint 5 | from .InertiaSet import InertiaSet 6 | from .SpatialAlgebra import Origin, Translation, Rotation -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy >= 1.17.4 2 | sympy >= 1.7.1 3 | beautifulsoup4 >= 4.9.3 4 | lxml >= 4.2.1 --------------------------------------------------------------------------------