Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def parse(self, sphere):
if not sphere.has_attr("radius"):
raise UrdfException("Sphere has no radius.")
self.radius = float(sphere["radius"])
def parse(self, cylinder):
if not cylinder.has_attr("radius"):
raise UrdfException("Cylinder has no radius.")
self.radius = float(cylinder["radius"])
if not cylinder.has_attr("length"):
raise UrdfException("Cylinder has no length.")
self.length = float(cylinder["length"])
def _parse_joint(self, joint, links):
"""Create joint object."""
j = Joint()
if not joint.has_attr("name"):
raise UrdfException("Joint name is missing.")
j.joint_name = joint["name"]
if not joint.has_attr("type"):
raise UrdfException("Joint type is missing in joint '%s'."
% j.joint_name)
parent = joint.find("parent")
if parent is None:
raise UrdfException("No parent specified in joint '%s'"
% j.joint_name)
if not parent.has_attr("link"):
raise UrdfException("No parent link name given in joint '%s'."
% j.joint_name)
j.parent = parent["link"]
if j.parent not in links:
raise UrdfException("Parent link '%s' of joint '%s' is not "
j.joint_name = joint["name"]
if not joint.has_attr("type"):
raise UrdfException("Joint type is missing in joint '%s'."
% j.joint_name)
parent = joint.find("parent")
if parent is None:
raise UrdfException("No parent specified in joint '%s'"
% j.joint_name)
if not parent.has_attr("link"):
raise UrdfException("No parent link name given in joint '%s'."
% j.joint_name)
j.parent = parent["link"]
if j.parent not in links:
raise UrdfException("Parent link '%s' of joint '%s' is not "
"defined." % (j.parent, j.joint_name))
child = joint.find("child")
if child is None:
raise UrdfException("No child specified in joint '%s'"
% j.joint_name)
if not child.has_attr("link"):
raise UrdfException("No child link name given in joint '%s'."
% j.joint_name)
j.child = child["link"]
if j.child not in links:
raise UrdfException("Child link '%s' of joint '%s' is not "
"defined." % (j.child, j.joint_name))
j.joint_type = joint["type"]
def _parse_geometry(self, child, name):
"""Parse geometric primitives (box, cylinder, sphere).
Meshes are not supported!
"""
geometry = child.find("geometry")
if geometry is None:
raise UrdfException("Missing geometry tag in link '%s'" % name)
result = []
# meshes are not supported
for shape_type in ["box", "cylinder", "sphere"]:
shapes = geometry.findAll(shape_type)
Cls = shape_classes[shape_type]
for shape in shapes:
shape_object = Cls(name)
shape_object.parse(shape)
result.append(shape_object)
return result
def parse(self, cylinder):
if not cylinder.has_attr("radius"):
raise UrdfException("Cylinder has no radius.")
self.radius = float(cylinder["radius"])
if not cylinder.has_attr("length"):
raise UrdfException("Cylinder has no length.")
self.length = float(cylinder["length"])
def load_urdf(self, urdf_xml):
"""Load URDF file into transformation manager.
Parameters
----------
urdf_xml : string
Robot definition in URDF
"""
urdf = BeautifulSoup(urdf_xml, "xml")
# URDF XML schema:
# https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd
robot = urdf.find("robot")
if robot is None:
raise UrdfException("Robot tag is missing.")
if not robot.has_attr("name"):
raise UrdfException("Attribute 'name' is missing in robot tag.")
robot_name = robot["name"]
links = [self._parse_link(link)
for link in robot.findAll("link", recursive=False)]
joints = [self._parse_joint(joint, links)
for joint in robot.findAll("joint", recursive=False)]
self.add_transform(links[0], robot_name, np.eye(4))
for joint in joints:
if joint.joint_type == "revolute":
self.add_joint(
joint.joint_name, joint.child, joint.parent,
Parameters
----------
urdf_xml : string
Robot definition in URDF
"""
urdf = BeautifulSoup(urdf_xml, "xml")
# URDF XML schema:
# https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd
robot = urdf.find("robot")
if robot is None:
raise UrdfException("Robot tag is missing.")
if not robot.has_attr("name"):
raise UrdfException("Attribute 'name' is missing in robot tag.")
robot_name = robot["name"]
links = [self._parse_link(link)
for link in robot.findAll("link", recursive=False)]
joints = [self._parse_joint(joint, links)
for joint in robot.findAll("joint", recursive=False)]
self.add_transform(links[0], robot_name, np.eye(4))
for joint in joints:
if joint.joint_type == "revolute":
self.add_joint(
joint.joint_name, joint.child, joint.parent,
joint.child2parent, joint.joint_axis, joint.limits)
else:
self.add_transform(
raise UrdfException("No parent specified in joint '%s'"
% j.joint_name)
if not parent.has_attr("link"):
raise UrdfException("No parent link name given in joint '%s'."
% j.joint_name)
j.parent = parent["link"]
if j.parent not in links:
raise UrdfException("Parent link '%s' of joint '%s' is not "
"defined." % (j.parent, j.joint_name))
child = joint.find("child")
if child is None:
raise UrdfException("No child specified in joint '%s'"
% j.joint_name)
if not child.has_attr("link"):
raise UrdfException("No child link name given in joint '%s'."
% j.joint_name)
j.child = child["link"]
if j.child not in links:
raise UrdfException("Child link '%s' of joint '%s' is not "
"defined." % (j.child, j.joint_name))
j.joint_type = joint["type"]
if j.joint_type in ["planar", "floating", "continuous", "prismatic"]:
raise UrdfException("Unsupported joint type '%s'" % j.joint_type)
elif j.joint_type not in ["revolute", "fixed"]:
raise UrdfException("Joint type '%s' is not allowed in a URDF "
"document." % j.joint_type)
j.child2parent = self._parse_origin(joint)