def test_new_transmission_multiple_joints(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<transmission name="simple_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="foo_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<joint name="bar_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="foo_motor">
<mechanicalReduction>50.0</mechanicalReduction>
</actuator>
</transmission>
</robot>'''
self.parse_and_compare(xml)
robot = urdf.Robot(name='test', version='1.0')
trans = urdf.Transmission(name='simple_trans')
trans.type = 'transmission_interface/SimpleTransmission'
joint = urdf.TransmissionJoint(name='foo_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
joint = urdf.TransmissionJoint(name='bar_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
actuator = urdf.Actuator(name='foo_motor')
actuator.mechanicalReduction = 50.0
trans.add_aggregate('actuator', actuator)
robot.add_aggregate('transmission', trans)
self.xml_and_compare(robot, xml)
def add_transmission_plugins(robot, effort=True):
joints = get_all_joints(robot)
mov_joints = [
joint for typ, joint in joints.items()
if typ in ['revolute', 'continuous', 'prismatic']
]
m_joints = []
for ls in mov_joints:
if ls is not None:
for item in ls:
m_joints.append(item)
else:
continue
for joint in m_joints:
# Transmission
t = urdf.Transmission(joint + "_trans")
t.type = "transmission_interface/SimpleTransmission"
transjoint = urdf.TransmissionJoint(name=joint)
transjoint.add_aggregate("hardwareInterface",
"hardware_interface/EffortJointInterface")
t.add_aggregate("joint", transjoint)
actuator = urdf.Actuator(joint + "_motor")
actuator.mechanicalReduction = 1
t.add_aggregate("actuator", actuator)
robot.add_aggregate("transmission", t)
return robot
def generate_robot(namespace):
""" Generate Inverted Pendulum URDF """
# Robot
robot = urdf.Robot(name)
# Base
l_world = urdf.Link("world")
robot.add_link(l_world)
# Frame
frame_geometry = urdf.Cylinder(radius=l_f_rad, length=l_f_len)
l_frame = urdf.Link("l_frame")
l_frame.collision = frame_collision(geometry=frame_geometry)
l_frame.inertial = frame_inertial()
l_frame.visual = frame_visual(geometry=frame_geometry)
robot.add_link(l_frame)
# World Joint
j_world = urdf.Joint(name="fixed",
parent="world",
child="l_frame",
joint_type="fixed",
origin=urdf.Pose([0, 0, 0], [0, 0, 0]))
robot.add_joint(j_world)
# Links
l_link = [urdf.Link("l_{}".format(i)) for i in range(n_links)]
j_link = [urdf.Joint("j_{}".format(i)) for i in range(n_links)]
for i in range(n_links):
l_y = (0.1 if i == 0 else 2 * l_link_rad)
l_z = ((l_f_len - 0.05) if i == 0 else (l_link_len - 0.05))
l_pos = urdf.Pose(xyz=[0, l_y, l_z], rpy=[0, 0, 0])
l_link[i].visual = default_visual()
l_link[i].inertial = default_inertial()
l_link[i].collision = default_collision()
robot.add_link(l_link[i])
j_link[i] = urdf.Joint(
name="j_{}".format(i),
parent=("l_frame" if i == 0 else "l_{}".format(i - 1)),
child="l_{}".format(i),
origin=l_pos,
joint_type="continuous",
axis=[0, 1, 0],
dynamics=default_dynamics())
robot.add_joint(j_link[i])
for joint in j_link:
# Transmission
t = urdf.Transmission(joint.name + "_trans")
t.type = "transmission_interface/SimpleTransmission"
transjoint = urdf.TransmissionJoint(name=joint.name)
transjoint.add_aggregate("hardwareInterface", "EffortJointInterface")
t.add_aggregate("joint", transjoint)
actuator = urdf.Actuator(joint.name + "_motor")
actuator.mechanicalReduction = 1
t.add_aggregate("actuator", actuator)
robot.add_aggregate("transmission", t)
# joint force-torque sensors
if use_ftSensors:
joints = get_continous_joints(robot)
for joint_i, joint in enumerate(joints):
# Provide feedback
a = etree.Element("gazebo", {"reference": joint.name})
b = etree.SubElement(a, "provideFeedback")
b.text = "true"
robot.add_aggregate("gazebo", a)
# Sensor
a = etree.Element("gazebo")
b = etree.SubElement(
a, "plugin", {
"name": "force_torque_plugin_" + joint.name,
"filename": "libgazebo_ros_ft_sensor.so"
})
c = etree.SubElement(b, "alwaysOn")
c.text = "true"
d = etree.SubElement(b, "updateRate")
d.text = "100"
d = etree.SubElement(b, "jointName")
d.text = joint.name
d = etree.SubElement(b, "topicName")
d.text = namespace + "ftSensors/" + joint.name
robot.add_aggregate("gazebo", a)
# Gazebo Color Plugin for all links
for link_i, link in enumerate(get_all_links(robot)):
a = etree.Element("gazebo", {"reference": link.name})
b = etree.SubElement(a, "material")
b.text = "Gazebo/Black"
robot.add_aggregate("gazebo", a)
# Gazebo Color Plugin for Frame
a = etree.Element("gazebo", {"reference": "l_frame"})
b = etree.SubElement(a, "material")
b.text = "Gazebo/Orange"
robot.add_aggregate("gazebo", a)
# Gazebo plugin
a = etree.Element("gazebo")
b = etree.SubElement(a, "plugin", {
"name": "gazebo_ros_control",
"filename": "libgazebo_ros_control.so"
})
c = etree.SubElement(b, "robotNamespace")
c.text = namespace
# d = etree.SubElement(b, "robotParam")
# d.text = "/robot_description"
robot.add_aggregate("gazebo", a)
return robot