55"""
66
77import numpy as np
8- from spatialmath import base
98import roboticstoolbox as rtb
109import copy
1110import os
@@ -1612,7 +1611,7 @@ def __init__(self, name, links, joints=None,
16121611 # why arent the other things validated
16131612 try :
16141613 self ._validate_transmissions ()
1615- except BaseException :
1614+ except Exception :
16161615 pass
16171616
16181617 self .name = name
@@ -1711,18 +1710,18 @@ def __init__(self, name, links, joints=None,
17111710
17121711 # joint limit
17131712 try :
1714- qlim = [joint .limit .lower , joint .limit .upper ]
1713+ joint . qlim = [joint .limit .lower , joint .limit .upper ]
17151714 except AttributeError :
17161715 # no joint limits provided
1717- qlim = None
1716+ pass
17181717
17191718 # joint friction
17201719 try :
17211720 if joint .dynamics .friction is not None :
17221721 childlink .B = joint .dynamics .friction
17231722
17241723 # TODO Add damping
1725- joint .dynamics .damping
1724+ # joint.dynamics.damping
17261725 except AttributeError :
17271726 pass
17281727
@@ -1737,136 +1736,6 @@ def __init__(self, name, links, joints=None,
17371736 # TODO, why did you put the base_link on the end?
17381737 # easy to do it here
17391738
1740- # for i in range(len(elinks)):
1741- # found = False
1742- # for joint in range(len(elinks)):
1743- # if i != joint:
1744- # if self.joints[i].parent == self.joints[joint].child:
1745- # elinks[i]._parent = elinks[joint]
1746- # found = True
1747-
1748- # if not found:
1749- # link = self._link_map[self.joints[i].parent]
1750- # base_link = rtb.ELink(
1751- # rtb.ETS(),
1752- # name=link.name)
1753- # elinks[i]._parent = base_link
1754- # try:
1755- # for visual in link.visuals:
1756- # base_link.geometry.append(visual.geometry.ob)
1757- # except AttributeError: # pragma nocover
1758- # pass
1759-
1760- # for joint in self.joints:
1761- # # each joint has a reference to a parent and child link
1762-
1763- # ets = rtb.ETS()
1764- # T = sm.SE3(joint.origin)
1765- # trans = T.t
1766- # rot = joint.rpy
1767- # var = None
1768-
1769- # ets = ETS.SE3(trans, rot)
1770- # # if trans[0] != 0:
1771- # # ets = ets * rtb.ETS.tx(trans[0])
1772-
1773- # # if trans[1] != 0:
1774- # # ets = ets * rtb.ETS.ty(trans[1])
1775-
1776- # # if trans[2] != 0:
1777- # # ets = ets * rtb.ETS.tz(trans[2])
1778-
1779- # # if rot[0] != 0:
1780- # # ets = ets * rtb.ETS.rx(rot[0])
1781-
1782- # # if rot[1] != 0:
1783- # # ets = ets * rtb.ETS.ry(rot[1])
1784-
1785- # # if rot[2] != 0:
1786- # # ets = ets * rtb.ETS.rz(rot[2])
1787-
1788- # if joint.joint_type == 'revolute' or \
1789- # joint.joint_type == 'continuous': # pragma nocover
1790- # if joint.axis[0] == 1:
1791- # var = rtb.ETS.rx()
1792- # elif joint.axis[0] == -1:
1793- # var = rtb.ETS.rx(flip=True)
1794- # elif joint.axis[1] == 1:
1795- # var = rtb.ETS.ry()
1796- # elif joint.axis[1] == -1:
1797- # var = rtb.ETS.ry(flip=True)
1798- # elif joint.axis[2] == 1:
1799- # var = rtb.ETS.rz()
1800- # elif joint.axis[2] == -1:
1801- # var = rtb.ETS.rz(flip=True)
1802- # elif joint.joint_type == 'prismatic': # pragma nocover
1803- # if joint.axis[0] == 1:
1804- # var = rtb.ETS.tx()
1805- # elif joint.axis[0] == -1:
1806- # var = rtb.ETS.tx(flip=True)
1807- # elif joint.axis[1] == 1:
1808- # var = rtb.ETS.ty()
1809- # elif joint.axis[1] == -1:
1810- # var = rtb.ETS.ty(flip=True)
1811- # elif joint.axis[2] == 1:
1812- # var = rtb.ETS.tz()
1813- # elif joint.axis[2] == -1:
1814- # var = rtb.ETS.tz(flip=True)
1815-
1816- # try:
1817- # qlim = [joint.limit.lower, joint.limit.upper]
1818- # except AttributeError:
1819- # qlim = [0, 0]
1820-
1821- # elinks.append(
1822- # rtb.ELink(
1823- # ets,
1824- # var,
1825- # name=joint.name,
1826- # qlim=qlim
1827- # )
1828- # )
1829- # elinks_dict[elinks[-1].name] = elinks[-1]
1830-
1831- # elinks.append(base_link)
1832- # self.elinks = elinks
1833-
1834- # Store the visuals, collisions, and inertials
1835- # for i in range(len(joints)):
1836- # link = self._link_map[joints[i].child]
1837- # elinks[i].r = link.inertial.origin[:3, 3]
1838- # elinks[i].m = link.inertial.mass
1839- # elinks[i].inertia = link.inertial.inertia
1840-
1841- # try:
1842- # if self.joints[i].dynamics.friction is not None:
1843- # elinks[i].B = self.joints[i].dynamics.friction
1844-
1845- # # TODO Add damping
1846- # self.joints[i].dynamics.damping
1847- # except AttributeError:
1848- # pass
1849-
1850- # try:
1851- # for visual in link.visuals:
1852- # elinks[i].geometry.append(visual.geometry.ob)
1853- # except AttributeError: # pragma nocover
1854- # pass
1855-
1856- # try:
1857- # colls = []
1858- # for col in link.collisions:
1859- # colls.append(col.geometry.ob)
1860- # elinks[i].collision = colls
1861- # except AttributeError: # pragma nocover
1862- # pass
1863-
1864- # # Apply gear ratio
1865- # self._validate_transmissions()
1866- # for t in self.transmissions:
1867- # elinks_dict[self.joint_map[t.joints[0].name].name].G = \
1868- # t.actuators[0].mechanicalReduction
1869-
18701739 @property
18711740 def name (self ):
18721741 """str : The name of the URDF.
@@ -2003,7 +1872,7 @@ def loadstr(str_obj, file_obj):
20031872 path , _ = os .path .split (file_obj )
20041873
20051874 else : # pragma nocover
2006- parser = ET .XMLParser (remove_comments = True , remove_blank_text = True )
1875+ parser = ET .XMLParser ()
20071876 tree = ET .parse (file_obj , parser = parser )
20081877 path , _ = os .path .split (file_obj .name )
20091878
0 commit comments