def createJoint(joint, linkobj=None):
"""Adds joint data to 'link' object.
Args:
joint (dict): dictionary containing the joint definition
linkobj (bpy.types.Object): the obj of phobostype 'link' that receives the joint
Returns:
"""
# add joint information
if not linkobj:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(
joint['name']), 'ERROR')
return
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# get hold of object
bUtils.toggleLayer(list(linkobj.layers).index(True),
True) # any layer containing the object
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']),
'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints
for param in ['effort', 'velocity']:
try:
if 'limits' in joint:
linkobj['joint/max' + param] = joint['limits'][param]
except KeyError:
log("Joint limits incomplete for joint {0}".format(joint['name']),
'ERROR')
try:
lower = joint['limits']['lower']
upper = joint['limits']['upper']
except KeyError:
lower = 0.0
upper = 0.0
setJointConstraints(linkobj, joint['type'], lower, upper)
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
def buildRobotDictionary():
"""Builds a python dictionary representation of a Blender robot model for export and inspection."""
objectlist = bpy.context.selected_objects
#notifications, faulty_objects = robotupdate.updateModel(bpy.context.selected_objects)
#print(notifications)
robot = {'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'groups': {},
'chains': {},
'lights': {}
}
#save timestamped version of model
robot["date"] = datetime.now().strftime("%Y%m%d_%H:%M")
root = selectionUtils.getRoot(bpy.context.selected_objects[0])
if root.phobostype != 'link':
raise Exception("Found no 'link' object as root of the robot model.")
else:
if 'modelname' in root:
robot['modelname'] = root["modelname"]
else:
robot['modelname'] = 'unnamed_robot'
# digest all the links to derive link and joint information
print('\nParsing links, joints and motors...')
for obj in bpy.context.selected_objects:
if obj.phobostype == 'link':
link, joint, motor = deriveKinematics(obj)
robot['links'][namingUtils.getObjectName(obj, phobostype="link")] = link # it's important that this is really the object's name
if joint: # joint can be None if link is a root
robot['joints'][joint['name']] = joint
if motor:
robot['motors'][joint['name']] = motor
obj.select = False
# add inertial information to link
print('\n\nParsing inertials...')
for l in robot['links']:
#link = bpy.data.objects[l] NEW NAMING!
link = selectionUtils.getObjectByName(l)[0] if selectionUtils.getObjectByName(l) is not None else "ERROR!"
inertials = selectionUtils.getImmediateChildren(link, ['inertial'])
if len(inertials) == 1:
props, parent = deriveDictEntry(inertials[0])
if not (props is None or parent is None): # this may be the case if there is inertia information missing
robot['links'][namingUtils.getObjectName(parent)]['inertial'] = props
inertials[0].select = False
elif len(inertials) > 1:
for i in inertials:
if namingUtils.getObjectName(i, phobostype="inertial") == 'inertial_' + l:
props, parent = deriveDictEntry(i)
robot['links'][namingUtils.getObjectName(parent, phobostype="link")]['inertial'] = props
# FIXME: this has to be re-implemented
#if linkinertial == None:
# mass, com, inertia = inertia.fuseInertiaData(inertials)
# parent = inertials[0].parent
# matrix_local = mathutils.Matrix.Translation(mathutils.Vector(com))
# pose = {}
# pose['matrix'] = [list(vector) for vector in list(matrix_local)]
# pose['translation'] = list(matrix_local.to_translation())
# pose['rotation_euler'] = list(matrix_local.to_euler())
# pose['rotation_quaternion'] = list(matrix_local.to_quaternion())
# props = {'mass': mass, 'pose': pose, 'inertia': inertia}
# robot['links'][parent.name]['inertial'] = props
for i in inertials:
i.select = False
# complete link information by parsing visuals and collision objects
print('\n\nParsing visual and collision (approximation) objects...')
for obj in bpy.context.selected_objects:
print("Parsing object " + namingUtils.getObjectName(obj))
if obj.phobostype in ['visual', 'collision']:
props, parent = deriveDictEntry(obj)
robot['links'][namingUtils.getObjectName(parent, phobostype="link")][obj.phobostype][namingUtils.getObjectName(obj, phobostype=obj.phobostype)] = props
obj.select = False
elif obj.phobostype == 'approxsphere':
props, parent = deriveDictEntry(obj)
robot['links'][namingUtils.getObjectName(parent)]['approxcollision'].append(props)
obj.select = False
# combine collision information for links
for linkname in robot['links']:
link = robot['links'][linkname]
bitmask = 0
for collname in link['collision']:
try:
bitmask = bitmask | link['collision'][collname]['bitmask']
except KeyError:
pass
link['collision_bitmask'] = bitmask
# parse sensors and controllers
print('\n\nParsing sensors and controllers...')
for obj in bpy.context.selected_objects:
if obj.phobostype in ['sensor', 'controller']:
robot[obj.phobostype+'s'][namingUtils.getObjectName(obj)] = deriveDictEntry(obj)
obj.select = False
# parse materials
print('\n\nParsing materials...')
robot['materials'] = collectMaterials(objectlist)
for obj in objectlist:
if obj.phobostype == 'visual' and len(obj.data.materials) > 0:
mat = obj.data.materials[0]
if not namingUtils.getObjectName(mat) in robot['materials']:
robot['materials'][namingUtils.getObjectName(mat)] = deriveMaterial(mat) #this should actually never happen
robot['links'][namingUtils.getObjectName(obj.parent)]['visual'][namingUtils.getObjectName(obj, phobostype="visual")]['material'] = namingUtils.getObjectName(mat)
# gather information on groups of objects
print('\n\nParsing groups...')
for group in bpy.data.groups: # TODO: get rid of the "data" part
if len(group.objects) > 0 and namingUtils.getObjectName(group) != "RigidBodyWorld":
robot['groups'][namingUtils.getObjectName(group)] = deriveGroupEntry(group)
# gather information on chains of objects
print('\n\nParsing chains...')
chains = []
for obj in bpy.data.objects:
if obj.phobostype == 'link' and 'endChain' in obj:
chains.extend(deriveChainEntry(obj))
for chain in chains:
robot['chains'][chain['name']] = chain
# gather information on global lights
print('\n\nParsing lights...')
for obj in bpy.context.selected_objects:
if obj.phobostype == 'light':
robot['lights'][namingUtils.getObjectName(obj)] = deriveLight(obj)
robot['poses'] = deriveStoredPoses()
#shorten numbers in dictionary to n decimalPlaces and return it
print('\n\nRounding numbers...')
epsilon = 10**(-bpy.data.worlds[0].decimalPlaces) # TODO: implement this separately
return generalUtils.epsilonToZero(robot, epsilon, bpy.data.worlds[0].decimalPlaces)
def createJoint(joint, linkobj=None, links=None):
"""Adds joint data to a link object.
If the linkobj is not specified, it is derived from the **child** entry in the joint (object is
searched in the current scene). This only works if the search for the child yields a single
object. Alternatively, it is possible to provide the model dictionary of links. In this case,
the link object is searched in the dictionary (make sure the **object** keys of the dictionary
are set properly).
These entries are mandatory for the dictionary:
| **name**: name of the joint
These entries are optional:
| **axis**: tuple which specifies the axis of the editbone
| **limits**: limits of the joint movement
| **lower**: lower limit (defaults to 0.)
| **upper**: upper limit (defaults to 0.)
| **effort**: maximum effort for the joint
| **velocity**: maximum velocity for the joint
Furthermore any generic properties, prepended by a ``$`` will be added as custom properties to
the joint. E.g. ``$test/etc`` would be put to joint/test/etc. However, these properties are
extracted only in the first layer of hierarchy.
Args:
joint(dict): dictionary containing the joint definition
linkobj(bpy.types.Object, optional): link object receiving joint (Default value = None)
links(dict, optional): model dictionary of links (Default value = None)
Returns:
None: None
"""
# try deriving link object from joint['child']
if not linkobj:
# link dictionary provided -> search for child link object
if (
links
and 'child' in joint
and joint['child'] in links
and 'object' in links[joint['child']]
):
linkobj = links[joint['child']]['object']
# search for child link in scene
else:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(joint['name']),
'ERROR',
)
return
# make sure the proper joint name is kept
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# select the link object
bUtils.toggleLayer(list(linkobj.layers).index(True), True)
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']), 'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints to the joint
if 'limits' in joint:
for param in ['effort', 'velocity']:
if param in joint['limits']:
linkobj['joint/max' + param] = joint['limits'][param]
else:
log(
"Joint limits incomplete for joint {}. Missing {}.".format(
joint['name'], param
),
'ERROR',
)
if all(elem in joint['limits'] for elem in ['lower', 'upper']):
lower = joint['limits']['lower']
upper = joint['limits']['upper']
else:
log("Joint limits upper/lower is missing! Defaulted to [-1e-5, 1e-5].", 'WARNING')
lower = -1e-5
upper = 1e-5
else:
log("Joint limits upper/lower is missing! Defaulted both to [-1e-5, 1e-5].", 'WARNING')
lower = -1e-5
upper = 1e-5
setJointConstraints(linkobj, joint['type'], lower, upper)
# add generic properties
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
log("Assigned joint information to {}.".format(linkobj.name), 'DEBUG')
def createJoint(joint, linkobj=None, links=None):
"""Adds joint data to a link object.
If the linkobj is not specified, it is derived from the **child** entry in the joint (object is
searched in the current scene). This only works if the search for the child yields a single
object. Alternatively, it is possible to provide the model dictionary of links. In this case,
the link object is searched in the dictionary (make sure the **object** keys of the dictionary
are set properly).
These entries are mandatory for the dictionary:
| **name**: name of the joint
These entries are optional:
| **axis**: tuple which specifies the axis of the editbone
| **limits**: limits of the joint movement
| **lower**: lower limit (defaults to 0.)
| **upper**: upper limit (defaults to 0.)
| **effort**: maximum effort for the joint
| **velocity**: maximum velocity for the joint
Furthermore any generic properties, prepended by a ``$`` will be added as custom properties to
the joint. E.g. ``$test/etc`` would be put to joint/test/etc. However, these properties are
extracted only in the first layer of hierarchy.
Args:
joint(dict): dictionary containing the joint definition
linkobj(bpy.types.Object, optional): link object receiving joint (Default value = None)
links(dict, optional): model dictionary of links (Default value = None)
Returns:
None: None
"""
# try deriving link object from joint['child']
if not linkobj:
# link dictionary provided -> search for child link object
if (links and 'child' in joint and joint['child'] in links
and 'object' in links[joint['child']]):
linkobj = links[joint['child']]['object']
# search for child link in scene
else:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(
joint['name']),
'ERROR',
)
return
# make sure the proper joint name is kept
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# select the link object
bUtils.toggleLayer(list(linkobj.layers).index(True), True)
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']),
'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints to the joint
if 'limits' in joint:
for param in ['effort', 'velocity']:
if param in joint['limits']:
linkobj['joint/max' + param] = joint['limits'][param]
else:
log(
"Joint limits incomplete for joint {}. Missing {}.".format(
joint['name'], param),
'ERROR',
)
if all(elem in joint['limits'] for elem in ['lower', 'upper']):
lower = joint['limits']['lower']
upper = joint['limits']['upper']
else:
log(
"Joint limits upper/lower is missing! Defaulted to [-1e-5, 1e-5].",
'WARNING')
lower = -1e-5
upper = 1e-5
else:
log(
"Joint limits upper/lower is missing! Defaulted both to [-1e-5, 1e-5].",
'WARNING')
lower = -1e-5
upper = 1e-5
setJointConstraints(linkobj, joint['type'], lower, upper)
# add generic properties
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
log("Assigned joint information to {}.".format(linkobj.name), 'DEBUG')
def deriveMotor(obj, jointdict=None):
"""Derives motor information from an object.
Args:
obj(bpy_types.Object): Blender object to derive the motor from
jointdict(dict, optional): phobos representation of the respective joint (Default value = None)
Returns:
: dict -- phobos representation of a motor
"""
import phobos.model.models as models
import phobos.model.controllers as controllermodel
props = models.initObjectProperties(obj, phobostype='motor')
# return None if no motor is attached (there will always be at least a name in the props)
if len(props) < 2:
return None
# make sure the parent is a joint
if not obj.parent or obj.parent.phobostype != 'link' or 'joint/type' not in obj.parent:
log(
"Can not derive motor from {}. Insufficient requirements from parent object!"
.format(obj.name),
'ERROR',
)
return None
props['joint'] = nUtils.getObjectName(obj.parent, phobostype='joint')
# todo: transfer joint limits to motor properties
# check for a mimic motor
for k in (obj.parent).keys():
# Check for mimic motor
if "mimic" in k:
# Find the name
mimic_driver = sUtils.getObjectByName(
(obj.parent)['joint/mimic_joint'], phobostypes=['link'])
c_motor = sUtils.getImmediateChildren(mimic_driver,
phobostypes=['motor'])
props['mimic_motor'] = nUtils.getObjectName(c_motor[0],
phobostype='motor')
props['mimic_multiplier'] = (obj.parent)['joint/mimic_multiplier']
props['mimic_offset'] = (obj.parent)['joint/mimic_offset']
break
# try to derive the motor controller
controllerobjs = [
control for control in obj.children
if control.phobostype == 'controller'
]
if controllerobjs:
controller = controllermodel.deriveController(controllerobjs[0])
else:
controller = None
# assign the derived controller
if controller:
props['controller'] = controller['name']
else:
del props['controller']
return props
def buildRobotDictionary():
"""Builds a python dictionary representation of a Blender robot model for export and inspection.
"""
objectlist = bpy.context.selected_objects
#notifications, faulty_objects = robotupdate.updateModel(bpy.context.selected_objects)
#print(notifications)
robot = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'groups': {},
'chains': {},
'lights': {}
}
#save timestamped version of model
robot["date"] = datetime.now().strftime("%Y%m%d_%H:%M")
root = selectionUtils.getRoot(bpy.context.selected_objects[0])
if root.phobostype != 'link':
raise Exception("Found no 'link' object as root of the robot model.")
else:
if 'modelname' in root:
robot['modelname'] = root["modelname"]
else:
robot['modelname'] = 'unnamed_robot'
# digest all the links to derive link and joint information
print('\nParsing links, joints and motors...')
for obj in bpy.context.selected_objects:
if obj.phobostype == 'link':
link, joint, motor = deriveKinematics(obj)
robot['links'][namingUtils.getObjectName(
obj, phobostype="link"
)] = link # it's important that this is really the object's name
if joint: # joint can be None if link is a root
robot['joints'][joint['name']] = joint
if motor:
robot['motors'][joint['name']] = motor
obj.select = False
# add inertial information to link
print('\n\nParsing inertials...')
for l in robot['links']:
#link = bpy.data.objects[l] NEW NAMING!
link = selectionUtils.getObjectByName(
l)[0] if selectionUtils.getObjectByName(
l) is not None else "ERROR!"
inertials = selectionUtils.getImmediateChildren(link, ['inertial'])
if len(inertials) == 1:
props, parent = deriveDictEntry(inertials[0])
if not (
props is None or parent is None
): # this may be the case if there is inertia information missing
robot['links'][namingUtils.getObjectName(
parent)]['inertial'] = props
inertials[0].select = False
elif len(inertials) > 1:
for i in inertials:
if i.name == 'inertial_' + l:
props, parent = deriveDictEntry(i)
robot['links'][namingUtils.getObjectName(
parent, phobostype="link")]['inertial'] = props
# FIXME: this has to be re-implemented
#if linkinertial == None:
# mass, com, inertia = inertia.fuseInertiaData(inertials)
# parent = inertials[0].parent
# matrix_local = mathutils.Matrix.Translation(mathutils.Vector(com))
# pose = {}
# pose['matrix'] = [list(vector) for vector in list(matrix_local)]
# pose['translation'] = list(matrix_local.to_translation())
# pose['rotation_euler'] = list(matrix_local.to_euler())
# pose['rotation_quaternion'] = list(matrix_local.to_quaternion())
# props = {'mass': mass, 'pose': pose, 'inertia': inertia}
# robot['links'][parent.name]['inertial'] = props
for i in inertials:
i.select = False
# complete link information by parsing visuals and collision objects
print('\n\nParsing visual and collision (approximation) objects...')
capsules_list = []
for obj in bpy.context.selected_objects:
print("Parsing object " + namingUtils.getObjectName(obj))
if obj.phobostype in ['visual', 'collision']:
props, parent = deriveDictEntry(obj)
if all([
key in props for key in ['cylinder', 'sphere1', 'sphere2']
]): # this is the case with simulated capsules
capsules_list.append({
'link':
parent.name,
'name':
props['cylinder']['name'][:-len('_cylinder')],
'radius':
props['cylinder']['geometry']['radius'],
'length':
props['cylinder']['geometry']['length'] +
2 * props['cylinder']['geometry']['radius'],
#'bitmask': props['cylinder']['bitmask']
})
for key in props:
robot['links'][namingUtils.getObjectName(
parent,
phobostype="link")][obj.phobostype][key] = props[key]
else:
robot['links'][namingUtils.getObjectName(
parent, phobostype="link")][obj.phobostype][
namingUtils.getObjectName(
obj, phobostype=obj.phobostype)] = props
obj.select = False
elif obj.phobostype == 'approxsphere':
props, parent = deriveDictEntry(obj)
robot['links'][namingUtils.getObjectName(
parent)]['approxcollision'].append(props)
obj.select = False
robot['capsules'] = capsules_list
# combine collision information for links
for linkname in robot['links']:
link = robot['links'][linkname]
bitmask = 0
for collname in link['collision']:
try:
bitmask = bitmask | link['collision'][collname]['bitmask']
except KeyError:
pass
link['collision_bitmask'] = bitmask
# parse sensors and controllers
print('\n\nParsing sensors and controllers...')
for obj in bpy.context.selected_objects:
if obj.phobostype in ['sensor', 'controller']:
robot[obj.phobostype +
's'][namingUtils.getObjectName(obj)] = deriveDictEntry(obj)
obj.select = False
# parse materials
print('\n\nParsing materials...')
robot['materials'] = collectMaterials(objectlist)
for obj in objectlist:
if obj.phobostype == 'visual' and len(obj.data.materials) > 0:
mat = obj.data.materials[0]
if not namingUtils.getObjectName(mat) in robot['materials']:
robot['materials'][namingUtils.getObjectName(
mat)] = deriveMaterial(
mat) #this should actually never happen
robot['links'][namingUtils.getObjectName(
obj.parent)]['visual'][namingUtils.getObjectName(
obj, phobostype="visual"
)]['material'] = namingUtils.getObjectName(mat)
# gather information on groups of objects
print('\n\nParsing groups...')
for group in bpy.data.groups: # TODO: get rid of the "data" part
if len(group.objects) > 0 and namingUtils.getObjectName(
group) != "RigidBodyWorld":
robot['groups'][namingUtils.getObjectName(
group)] = deriveGroupEntry(group)
# gather information on chains of objects
print('\n\nParsing chains...')
chains = []
for obj in bpy.data.objects:
if obj.phobostype == 'link' and 'endChain' in obj:
chains.extend(deriveChainEntry(obj))
for chain in chains:
robot['chains'][chain['name']] = chain
# gather information on global lights
print('\n\nParsing lights...')
for obj in bpy.context.selected_objects:
if obj.phobostype == 'light':
robot['lights'][namingUtils.getObjectName(obj)] = deriveLight(obj)
robot['poses'] = deriveStoredPoses()
#shorten numbers in dictionary to n decimalPlaces and return it
print('\n\nRounding numbers...')
epsilon = 10**(-bpy.data.worlds[0].decimalPlaces
) # TODO: implement this separately
return generalUtils.epsilonToZero(robot, epsilon,
bpy.data.worlds[0].decimalPlaces)