def parse_hand_groups(self, manipulator):
previous = self.hand_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'group':
# Check it is not a subgroup
if len(elt.childNodes) > 0:
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
elif elt.tagName == 'group_state':
for index, group_element in enumerate(
elt.getElementsByTagName("joint")):
attribute_name = group_element.getAttribute("name")
attribute_name = attribute_name.split("_")[1]
if attribute_name in ["WRJ1", "WRJ2"
] and manipulator.has_arm:
group_element.parentNode.removeChild(group_element)
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_arm_collisions(self, manipulator):
comment = [manipulator.arm.internal_name + " collisions"]
self.add_comments(comment)
previous = self.arm_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'disable_collisions':
elt.getAttributeNode("link1").nodeValue = (
manipulator.arm.prefix + elt.getAttribute("link1"))
elt.getAttributeNode("link2").nodeValue = (
manipulator.arm.prefix + elt.getAttribute("link2"))
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement = deepcopy(elt)
previous = elt
elt = xacro.next_element(previous)
# Add collisions between arm and hand
if manipulator.has_hand:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
robot_urdf = URDF.from_xml_string(urdf_str)
robot = URDF.from_xml_string(urdf_str)
arm_chain = robot.get_chain("world",
manipulator.hand.prefix +
"forearm",
joints=False,
fixed=False)
newElement.getAttributeNode("link1").nodeValue = arm_chain[-2]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement.getAttributeNode("link1").nodeValue = arm_chain[-3]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement.getAttributeNode("link1").nodeValue = arm_chain[-4]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
def parse_hand_end_effectors(self, manipulator):
previous = self.hand_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'end_effector':
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_hand_collisions(self, manipulator):
comment = [manipulator.hand.internal_name + " collisions"]
self.add_comments(comment)
previous = self.hand_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'disable_collisions':
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_hand_virtual_joint(self, manipulator):
comment = [
"VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an "
+
"external frame of reference (considered fixed with respect to the robot)"
]
self.add_comments(comment)
previous = self.hand_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'virtual_joint':
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_arm_end_effectors(self, manipulator):
previous = self.arm_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'end_effector':
elt.getAttributeNode(
"name"
).nodeValue = manipulator.arm.prefix + elt.getAttribute("name")
elt.getAttributeNode("parent_link").nodeValue = (
manipulator.arm.prefix + elt.getAttribute("parent_link"))
elt.getAttributeNode(
"group").nodeValue = manipulator.arm.internal_name
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement = deepcopy(elt)
newElement.getAttributeNode(
"name"
).nodeValue = manipulator.arm.internal_name + "_and_wrist_ee"
newElement.getAttributeNode(
"parent_link").nodeValue = manipulator.hand.prefix + "palm"
newElement.getAttributeNode(
"group"
).nodeValue = manipulator.arm.internal_name + "_and_wrist"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement = deepcopy(elt)
newElement.getAttributeNode(
"name"
).nodeValue = manipulator.arm.internal_name + "_and_hand_ee"
newElement.getAttributeNode(
"parent_link").nodeValue = manipulator.hand.prefix + "palm"
newElement.getAttributeNode(
"group"
).nodeValue = manipulator.arm.internal_name + "_and_hand"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_arm_virtual_joint(self, manipulator):
comment = [
"VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an "
+
"external frame of reference (considered fixed with respect to the robot)"
]
self.add_comments(comment)
previous = self.arm_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'virtual_joint':
elt.getAttributeNode(
"name"
).nodeValue = "world_to_" + manipulator.arm.internal_name
elt.getAttributeNode(
"child_link"
).nodeValue = manipulator.arm.prefix + elt.getAttribute(
"child_link")
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)
def parse_arm_groups(self, manipulator):
previous = self.arm_srdf_xml.documentElement
elt = xacro.next_element(previous)
while elt:
if elt.tagName == 'group':
# Check it is not a subgroup
if len(elt.childNodes) > 0:
group_name = elt.getAttribute('name')
if group_name == manipulator.arm.main_group or group_name in manipulator.arm.other_groups:
if group_name == manipulator.arm.main_group:
elt.setAttribute('name',
manipulator.arm.internal_name)
else:
elt.setAttribute(
'name', manipulator.arm.prefix + group_name)
for index, group_element in enumerate(
elt.getElementsByTagName("chain")):
attributes = ["base_link", "tip_link"]
for attribute in attributes:
node_attribute = group_element.getAttributeNode(
attribute)
node_attribute.nodeValue = (
manipulator.arm.prefix +
group_element.getAttribute(attribute))
for tagName in ["joint", "link", "group"]:
for index, group_element in enumerate(
elt.getElementsByTagName(tagName)):
attribute_name = group_element.getAttribute(
"name")
node_attribute = group_element.getAttributeNode(
"name")
node_attribute.nodeValue = (
manipulator.arm.prefix + attribute_name)
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
if group_name == manipulator.arm.main_group and (
manipulator.has_hand
and not manipulator.hand.is_lite):
elt.setAttribute(
'name',
manipulator.arm.internal_name + "_and_wrist")
for group_element in elt.getElementsByTagName("chain"):
node_attribute = group_element.getAttributeNode(
"tip_link")
node_attribute.nodeValue = (
manipulator.hand.prefix + "palm")
if len(elt.getElementsByTagName("joint")) > 0:
node = deepcopy(
elt.getElementsByTagName("joint")[0])
node_attribute = node.getAttributeNode("name")
node_attribute.nodeValue = (
manipulator.hand.prefix + "WRJ2")
newatt = elt.appendChild(node)
node = deepcopy(
elt.getElementsByTagName("joint")[0])
node_attribute = node.getAttributeNode("name")
node_attribute.nodeValue = (
manipulator.hand.prefix + "WRJ1")
newatt = elt.appendChild(node)
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
if group_name == manipulator.arm.main_group and (
manipulator.has_hand):
new_group = xml.dom.minidom.Document().createElement(
'group')
new_group.setAttribute(
"name",
manipulator.arm.internal_name + "_and_hand")
if manipulator.hand.is_lite:
arm_group = xml.dom.minidom.Document(
).createElement('group name="' +
manipulator.arm.internal_name +
'"')
else:
arm_group = xml.dom.minidom.Document(
).createElement('group name="' +
manipulator.arm.internal_name +
'_and_wrist' + '"')
new_group.appendChild(arm_group)
hand_group = xml.dom.minidom.Document().createElement(
'group name="' + manipulator.hand.internal_name +
'"')
new_group.appendChild(hand_group)
new_group.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
elif elt.tagName == 'group_state':
group_state_name = elt.getAttribute('name')
group_name = elt.getAttribute('group')
if group_state_name in manipulator.arm.group_states and (
group_name == manipulator.arm.main_group
or group_name in manipulator.arm.other_groups):
elt.setAttribute('name',
manipulator.arm.prefix + group_state_name)
if group_name == manipulator.arm.main_group:
elt.setAttribute('group',
manipulator.arm.internal_name)
else:
elt.setAttribute('group',
manipulator.arm.prefix + group_name)
for index, group_element in enumerate(
elt.getElementsByTagName("joint")):
attribute_name = group_element.getAttribute("name")
if attribute_name in ["WRJ1", "WRJ2"]:
if manipulator.has_hand:
if not manipulator.hand.is_lite:
node_attribute = group_element.getAttributeNode(
"name")
node_attribute.nodeValue = (
manipulator.hand.prefix +
attribute_name)
else:
group_element.parentNode.removeChild(
group_element)
else:
node_attribute = group_element.getAttributeNode(
"name")
node_attribute.nodeValue = (
manipulator.arm.prefix + attribute_name)
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
previous = elt
elt = xacro.next_element(previous)