def add_joint(
robot_root,
joint_name,
parent_name,
child_name,
joint_definition: SMJointDefinition,
origin, # add doc. origin is [origin_x, origin_y, origin_x, origin_r, origin_p, origin_y]; rpy: roll pitch yaw
):
"""adds a joint to the xml tree of robot_root."""
# todo: add asserts?
joint = ET.SubElement(robot_root, "joint", {
"name": joint_name,
"type": joint_definition.joint_type
})
joint_parent = ET.SubElement(joint, "parent", {"link": parent_name})
joint_child = ET.SubElement(joint, "child", {"link": child_name})
origin_dict = {
"xyz": spaced_str(origin[:3]),
"rpy": spaced_str(origin[3:])
}
joint_origin = ET.SubElement(joint, "origin", origin_dict)
if joint_definition.axis:
axis_dict = {"xyz": spaced_str(joint_definition.axis)}
joint_axis = ET.SubElement(joint, "axis", axis_dict)
if joint_definition.limits:
joint_limits = ET.SubElement(joint, "limit",
joint_definition.limits_dict)
return joint
def add_link(robot_root, link_name, link_definition: SMLinkDefinition, origin):
"""adds a link to the xml tree of robot_root
link_type: either "box", "cylinder", or "sphere"
"""
# todo: assertions, in particular that it is a valid SMLinkDefinition? add ability to have a dict or json file that defines an SMLinkDefinition.
# todo: the visual geometry scaler should maybe happen directly in link definition, and not here...
mass_dict = {"value": str(link_definition.mass)}
origin_dict = {
"xyz": spaced_str(origin[:3]),
"rpy": spaced_str(origin[3:])
}
link = ET.SubElement(robot_root, "link", {"name": link_name})
visual = ET.SubElement(link, "visual")
vis_orig = ET.SubElement(visual, "origin", origin_dict)
vis_geom = ET.SubElement(visual, "geometry")
vis_box = ET.SubElement(
vis_geom,
link_definition.shape_type,
link_definition.visual_shape_dimensions_dict,
)
vis_mat = ET.SubElement(visual, "material",
{"name": link_definition.material_name})
vis_mat_col = ET.SubElement(
vis_mat, "color", {"rgba": spaced_str(link_definition.material_color)})
collision = ET.SubElement(link, "collision")
col_orig = ET.SubElement(collision, "origin", origin_dict)
col_geom = ET.SubElement(collision, "geometry")
col_box = ET.SubElement(
col_geom,
link_definition.shape_type,
link_definition.contact_shape_dimensions_dict,
)
inertial = ET.SubElement(link, "inertial")
inner_mass = ET.SubElement(inertial, "mass", mass_dict)
inner_in = ET.SubElement(inertial, "inertia",
link_definition.inertial_value_dict)
return link
def add_link(robot_root, link_name, link_definition: SMLinkDefinition, origin):
"""adds a link to the xml tree of robot_root
link_type: either "box", "cylinder", or "sphere"
"""
# todo: assertions, in particular that it is a valid SMLinkDefinition? add ability to have a dict or json file that defines an SMLinkDefinition.
mass_dict = {"value": str(link_definition.mass)}
origin_dict = {
"xyz": spaced_str(origin[:3]),
"rpy": spaced_str(origin[3:])
}
# origin_dict = {"xyz": spaced_str(origin[:3]), "rpy": spaced_str(list(growth_axis))} # xx trial only todo: fix/remove
link = ET.SubElement(robot_root, "link", {"name": link_name})
visual = ET.SubElement(link, "visual")
vis_orig = ET.SubElement(visual, "origin", origin_dict)
vis_geom = ET.SubElement(visual, "geometry")
vis_box = ET.SubElement(vis_geom, link_definition.shape_type,
link_definition.shape_dimensions_dict)
vis_mat = ET.SubElement(visual, "material",
{"name": link_definition.material_name})
vis_mat_col = ET.SubElement(
vis_mat, "color", {"rgba": spaced_str(link_definition.material_color)})
collision = ET.SubElement(link, "collision")
col_orig = ET.SubElement(collision, "origin", origin_dict)
col_geom = ET.SubElement(collision, "geometry")
col_box = ET.SubElement(col_geom, link_definition.shape_type,
link_definition.shape_dimensions_dict)
inertial = ET.SubElement(link, "inertial")
inner_mass = ET.SubElement(inertial, "mass", mass_dict)
inner_in = ET.SubElement(inertial, "inertia",
link_definition.inertial_value_dict)
return link