Source code for director.affordanceurdf

from director import transformUtils
from director import affordanceitems
from urdf_parser_py import urdf


[docs]def geometryFromAffordance(aff):

    if isinstance(aff, affordanceitems.SphereAffordanceItem):
        radius = aff.getProperty('Radius')
        return urdf.Sphere(radius=radius)

    if isinstance(aff, affordanceitems.BoxAffordanceItem):
        dimensions = aff.getProperty('Dimensions')
        return urdf.Box(size=dimensions)

    if isinstance(aff, affordanceitems.CylinderAffordanceItem):
        return urdf.Cylinder(length=aff.getProperty('Length'), radius=aff.getProperty('Radius'))

    if isinstance(aff, affordanceitems.CapsuleAffordanceItem):
        return urdf.Cylinder(length=aff.getProperty('Length'), radius=aff.getProperty('Radius'))

    if isinstance(aff, affordanceitems.CapsuleRingAffordanceItem):
        raise Exception('not supported yet')

    if isinstance(aff, affordanceitems.MeshAffordanceItem):
        filename = aff.getProperty('Filename')
        filename = affordanceitems.MeshAffordanceItem.getMeshManager().getFilesystemFilename(filename)
        return urdf.Mesh(filename=filename, scale=[1.0, 1.0, 1.0])


[docs]def stringWithAffordanceId(inputStr, aff):
    return inputStr % aff.getProperty('uuid')


[docs]def stringWithAffordanceName(inputStr, aff):
    return inputStr % aff.getProperty('Name')


[docs]def colorFromAffordance(aff):
    color = aff.getProperty('Color')
    return urdf.Color(color[0], color[1], color[2] ,1)


[docs]def materialFromAffordance(aff):
    color = colorFromAffordance(aff)
    return urdf.Material(name=stringWithAffordanceId('material_%s', aff), color=color, texture=None)


[docs]def poseFromAffordance(aff):

    t = aff.getChildFrame().transform
    position, quat = transformUtils.poseFromTransform(t)
    rpy = transformUtils.rollPitchYawFromTransform(t)
    return urdf.Pose(position, rpy)


[docs]def linkFromAffordance(aff):

    geometry = geometryFromAffordance(aff)
    material = materialFromAffordance(aff)
    pose = poseFromAffordance(aff)

    visual = urdf.Visual(geometry=geometry, material=material, origin=pose)
    collision =  urdf.Collision(geometry=geometry, origin=pose)
    return urdf.Link(name=stringWithAffordanceId('link_%s', aff), visual=visual, collision=collision)


[docs]def urdfStringFromAffordances(affordanceList):
    r = urdf.Robot(name='affordance_environment')
    for aff in affordanceList:
        r.add_link(linkFromAffordance(aff))
    return r.to_xml_string()