def deriveChainEntry(obj):
"""Derives a phobos dict entry for a kinematic chain ending in the provided object.
:param obj:
:return:
"""
returnchains = []
if 'endChain' in obj:
chainlist = obj['endChain']
for chainName in chainlist:
chainclosed = False
parent = obj
chain = {
'name': chainName,
'start': '',
'end': nUtils.getObjectName(obj),
'elements': []
}
while not chainclosed:
if parent.parent is None: # FIXME: use effectiveParent
log("Unclosed chain, aborting parsing chain " + chainName,
"ERROR", "deriveChainEntry")
chain = None
break
chain['elements'].append(parent.name)
parent = parent.parent # FIXME: use effectiveParent
if 'startChain' in parent:
startchain = parent['startChain']
if chainName in startchain:
chain['start'] = nUtils.getObjectName(parent)
chain['elements'].append(nUtils.getObjectName(parent))
chainclosed = True
if chain is not None:
returnchains.append(chain)
return returnchains
def loadPose(modelname, posename):
"""Load and apply a robot's stored pose.
:param modelname: the model's name
:type modelname: str
:param posename: the name the pose is stored under
:type posename: str
"""
load_file = bUtils.readTextFile(modelname + '::poses')
if load_file == '':
log('No poses stored.', 'ERROR')
return
loadedposes = yaml.load(load_file)
if posename not in loadedposes:
log('No pose with name ' + posename + ' stored for model ' + modelname,
'ERROR')
return
prev_mode = bpy.context.mode
pose = loadedposes[posename]
# apply rotations to all joints defined by the pose
try:
bpy.ops.object.mode_set(mode='POSE')
for obj in sUtils.getObjectsByPhobostypes(['link']):
if nUtils.getObjectName(obj, 'joint') in pose['joints']:
obj.pose.bones['Bone'].rotation_mode = 'XYZ'
obj.pose.bones['Bone'].rotation_euler.y = float(
pose['joints'][nUtils.getObjectName(obj, 'joint')])
except KeyError as error:
log("Could not apply the pose: " + str(error), 'ERROR')
finally:
# restore previous mode
bpy.ops.object.mode_set(mode=prev_mode)
def deriveChainEntry(obj):
"""TODO: Please add pyDoc ASAP.
"""
returnchains = []
if 'endChain' in obj:
chainlist = obj['endChain']
for chainName in chainlist:
chainclosed = False
parent = obj
chain = {'name': chainName, 'start': '', 'end': namingUtils.getObjectName(obj), 'elements': []}
while not chainclosed:
if parent.parent is None:
print('### Error: Unclosed chain, aborting parsing chain', chainName)
chain = None
break
chain['elements'].append(parent.name)
parent = parent.parent
if 'startChain' in parent:
startChain = parent['startChain']
if chainName in startChain:
chain['start'] = namingUtils.getObjectName(parent)
chain['elements'].append(namingUtils.getObjectName(parent))
chainclosed = True
if chain is not None:
returnchains.append(chain)
return returnchains
def deriveChainEntry(obj):
"""TODO: Please add pyDoc ASAP.
"""
returnchains = []
if 'endChain' in obj:
chainlist = obj['endChain']
for chainName in chainlist:
chainclosed = False
parent = obj
chain = {
'name': chainName,
'start': '',
'end': namingUtils.getObjectName(obj),
'elements': []
}
while not chainclosed:
if parent.parent is None:
print('### Error: Unclosed chain, aborting parsing chain',
chainName)
chain = None
break
chain['elements'].append(parent.name)
parent = parent.parent
if 'startChain' in parent:
startChain = parent['startChain']
if chainName in startChain:
chain['start'] = namingUtils.getObjectName(parent)
chain['elements'].append(namingUtils.getObjectName(parent))
chainclosed = True
if chain is not None:
returnchains.append(chain)
return returnchains
def deriveGroupEntry(group):
"""Derives a list of phobos link skeletons for a provided group object.
Args:
group(bpy_types.Group): The blender group to extract the links from.
Returns:
: list
"""
links = []
for obj in group.objects:
if obj.phobostype == 'link':
links.append({'type': 'link', 'name': nUtils.getObjectName(obj)})
else:
log(
"Group "
+ group.name
+ " contains "
+ obj.phobostype
+ ': '
+ nUtils.getObjectName(obj),
"ERROR",
)
return links
def getSubmechanisms(link):
if 'submechanism/name' in link.keys():
submech = {
'type':
link['submechanism/type'],
'contextual_name':
link['submechanism/name'],
'name':
link['submechanism/subtype'] if 'submechanism/subtype' in link
else link['submechanism/type'],
'jointnames_independent': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/independent']
],
'jointnames_spanningtree': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/spanningtree']
],
'jointnames_active': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/active']
],
# TODO: this should work in almost all cases, still a bit of a hack:
'file_path':
'../submechanisms/urdf/' + link['submechanism/name'] + '.urdf'
}
log(' ' + submech['contextual_name'], 'DEBUG')
else:
submech = None
mechanisms = [submech] if submech else []
for c in link.children:
if c.phobostype in ['link', 'interface'] and c in objectlist:
mechanisms.extend(getSubmechanisms(c))
return mechanisms
def execute(self, context):
obj = context.active_object
# rename only if necessary
if self.newname != '' and self.newname != nUtils.getObjectName(obj):
log("Renaming " + obj.phobostype + " '" + nUtils.getObjectName(obj) + "' to '" +
self.newname + "'.", 'INFO')
nUtils.safelyName(obj, self.newname)
elif self.newname == '':
log("Removing custom name from " + obj.phobostype + " '" + obj.name + "'.", 'INFO')
if obj.phobostype + '/name' in obj:
del obj[obj.phobostype + '/name']
# only links have joint names
if obj.phobostype == 'link':
if self.jointname != '':
# only change/add joint/name if it was changed
if 'joint/name' not in obj or (
'joint/name' in obj and self.jointname != obj['joint/name']):
log("Renaming joint of " + obj.phobostype + " '" + nUtils.getObjectName(obj) +
"' to '" + self.jointname + "'.", 'INFO')
obj['joint/name'] = self.jointname
# remove joint/name when empty
elif self.jointname == '':
if 'joint/name' in obj:
log("Removing joint name from " + obj.phobostype + " '" + obj.name + "'.",
'INFO')
del obj['joint/name']
return {'FINISHED'}
def deriveChainEntry(obj):
"""Derives a phobos dict entry for a kinematic chain ending in the provided object.
:param obj:
:return:
"""
returnchains = []
if 'endChain' in obj:
chainlist = obj['endChain']
for chainName in chainlist:
chainclosed = False
parent = obj
chain = {'name': chainName, 'start': '', 'end': nUtils.getObjectName(obj), 'elements': []}
while not chainclosed:
if parent.parent is None: # FIXME: use effectiveParent
log("Unclosed chain, aborting parsing chain " + chainName, "ERROR", "deriveChainEntry")
chain = None
break
chain['elements'].append(parent.name)
parent = parent.parent # FIXME: use effectiveParent
if 'startChain' in parent:
startchain = parent['startChain']
if chainName in startchain:
chain['start'] = nUtils.getObjectName(parent)
chain['elements'].append(nUtils.getObjectName(parent))
chainclosed = True
if chain is not None:
returnchains.append(chain)
return returnchains
def deriveJoint(obj):
if not 'joint/type' in obj.keys():
jt, crot = joints.deriveJointType(obj, adjust=True)
props = initObjectProperties(obj, phobostype='joint', ignoretypes=['link', 'motor'])
props['parent'] = namingUtils.getObjectName(obj.parent)
props['child'] = namingUtils.getObjectName(obj)
axis, minmax = joints.getJointConstraints(obj)
if axis:
props['axis'] = list(axis)
limits = {}
if minmax is not None:
if len(minmax) == 2: # prismatic or revolute joint, TODO: planar etc.
limits['lower'] = minmax[0]
limits['upper'] = minmax[1]
if 'maxvelocity' in props:
limits['velocity'] = props['maxvelocity']
del props['maxvelocity']
if 'maxeffort' in props:
limits['effort'] = props['maxeffort']
del props['maxeffort']
if limits != {}:
props['limits'] = limits
props['state'] = deriveJointState(obj)
#TODO:
# - calibration
# - dynamics
# - mimic
# - safety_controller
return props
def loadPose(modelname, posename):
"""
Load and apply a robot's stored pose.
:param modelname: The model's name.
:type modelname: str.
:param posename: The name the pose is stored under.
:type posename: str.
:return Nothing.
"""
load_file = bUtils.readTextFile(modelname + '::poses')
if load_file == '':
log('No poses stored.', 'ERROR', 'loadPose')
return
poses = yaml.load(load_file)
try:
pose = poses[posename]
prev_mode = bpy.context.mode
bpy.ops.object.mode_set(mode='POSE')
for obj in sUtils.getObjectsByPhobostypes(['link']):
if nUtils.getObjectName(obj, 'joint') in pose['joints']:
obj.pose.bones['Bone'].rotation_mode = 'XYZ'
obj.pose.bones['Bone'].rotation_euler.y = pose['joints'][nUtils.getObjectName(obj, 'joint')]
bpy.ops.object.mode_set(mode=prev_mode)
except KeyError:
log('No pose with name ' + posename + ' stored for model ' + modelname, 'ERROR', "loadPose")
def loadPose(modelname, posename):
"""
Load and apply a robot's stored pose.
:param modelname: The model's name.
:type modelname: str.
:param posename: The name the pose is stored under.
:type posename: str.
:return Nothing.
"""
load_file = bUtils.readTextFile(modelname + '::poses')
if load_file == '':
log('No poses stored.', 'ERROR')
return
poses = yaml.load(load_file)
try:
pose = poses[posename]
prev_mode = bpy.context.mode
bpy.ops.object.mode_set(mode='POSE')
for obj in sUtils.getObjectsByPhobostypes(['link']):
if nUtils.getObjectName(obj, 'joint') in pose['joints']:
obj.pose.bones['Bone'].rotation_mode = 'XYZ'
obj.pose.bones['Bone'].rotation_euler.y = float(
pose['joints'][nUtils.getObjectName(obj, 'joint')])
bpy.ops.object.mode_set(mode=prev_mode)
except KeyError:
log('No pose with name ' + posename + ' stored for model ' + modelname,
'ERROR')
def deriveLinkfromObject(obj, scale=0.2, parent_link=True, parent_objects=False, nameformat=''):
"""Derives a link from an object using its name, transformation and parenting.
Args:
obj(bpy_types.Object): object to derive a link from
scale(float, optional): scale factor for bone size (Default value = 0.2)
parent_link(bool, optional): whether to automate the parenting of the new link or not. (Default value = True)
parent_objects(bool, optional): whether to parent all the objects to the new link or not (Default value = False)
nameformat(str, optional): re-formatting template for obj names (Default value = '')
Returns:
: newly created link
"""
log('Deriving link from ' + nUtils.getObjectName(obj), level="INFO")
try:
nameparts = [p for p in re.split('[^a-zA-Z]', nUtils.getObjectName(obj)) if p != '']
linkname = nameformat.format(*nameparts)
except IndexError:
log('Invalid name format (indices) for naming: ' + nUtils.getObjectName(obj), 'WARNING')
linkname = 'link_' + nUtils.getObjectName(obj)
link = createLink({'scale': scale, 'name': linkname, 'matrix': obj.matrix_world})
# parent link to object's parent
if parent_link:
if obj.parent:
eUtils.parentObjectsTo(link, obj.parent)
# parent children of object to link
if parent_objects:
children = [obj] + sUtils.getImmediateChildren(obj)
eUtils.parentObjectsTo(children, link, clear=True)
return link
def deriveSensor(obj, names=False, objectlist=[], logging=False):
"""This function derives a sensor from a given blender object
Args:
obj(bpy_types.Object): The blender object to derive the sensor from.
names(bool, optional): return the link object name instead of an object link. (Default value = False)
objectlist(list(bpy.types.Object, optional): objectlist to which possible parents are restricted (Default value = [])
logging(bool, optional): whether to write log messages or not (Default value = False)
Returns:
: dict -- phobos representation of the sensor
"""
from phobos.model.models import initObjectProperties
if logging:
log(
"Deriving sensor from object " + nUtils.getObjectName(obj, phobostype='sensor') + ".",
'DEBUG',
)
try:
props = initObjectProperties(obj, phobostype='sensor', ignoretypes=('pose'))
if names:
props['link'] = nUtils.getObjectName(
sUtils.getEffectiveParent(obj, objectlist=objectlist), phobostype='link'
)
else:
props['link'] = sUtils.getEffectiveParent(obj, objectlist=objectlist)
except KeyError:
if logging:
log("Missing data in sensor " + obj.name, "ERROR")
return None
return props
def deriveGroupEntry(group):
links = []
for obj in group.objects:
if obj.phobostype == 'link':
links.append({'type': 'link', 'name': namingUtils.getObjectName(obj)})
else:
print("### Error: group " + namingUtils.getObjectName(group) + " contains " + obj.phobostype + ': ' + namingUtils.getObjectName(obj))
return links
def deriveLinkfromObject(obj,
scale=0.2,
parenting=True,
parentobjects=False,
namepartindices=[],
separator='_',
prefix='link'):
"""Derives a link from an object that defines a joint through its position, orientation and parent-child relationships.
:param obj: The object you want to derive your link from.
:type obj: bpy_types.Object
:param scale: The scale you want to apply to the link.
:type scale: float
:param parenting: Whether you want to automate the parenting of the new link or not.
:type parenting: bool.
:param parentobjects: Whether you want to parent all the objects to the new link or not.
:type parentobjects: bool.
:param namepartindices: Parts of the objects name you want to reuse in the links name.
:type namepartindices: list with two elements.
:param separator: The separator you want to use to separate the links name with. Its '_' per default
:type separator: str
:param prefix: The prefix you want to use for the new links name. Its 'link' per default.
:type prefix: str
"""
print('Deriving link from', namingUtils.getObjectName(obj))
nameparts = namingUtils.getObjectName(obj).split('_')
rotation = obj.matrix_world.to_euler()
if 'invertAxis' in obj and obj['invertAxis'] == 1:
rotation.x += math.pi if rotation.x < 0 else -math.pi
tmpname = namingUtils.getObjectName(obj)
if namepartindices:
try:
tmpname = separator.join([nameparts[p] for p in namepartindices])
except IndexError:
print('Wrong name segment indices given for obj',
namingUtils.getObjectName(obj))
if prefix != '':
tmpname = prefix + separator + tmpname
if tmpname == namingUtils.getObjectName(obj):
obj.name += '*'
link = createLink(scale, obj.matrix_world.to_translation(),
obj.matrix_world.to_euler(), tmpname)
if parenting:
if obj.parent:
selectionUtils.selectObjects([link, obj.parent], True, 1)
if obj.parent.phobostype == 'link':
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else:
bpy.ops.object.parent_set(type='OBJECT')
children = selectionUtils.getImmediateChildren(obj)
if parentobjects:
children.append(obj)
for child in children:
selectionUtils.selectObjects([child], True, 0)
bpy.ops.object.parent_clear(type='CLEAR_KEEP_TRANSFORM')
selectionUtils.selectObjects([child, link], True, 1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
def execute(self, context):
"""
Args:
context:
Returns:
"""
obj = context.active_object
# rename only if necessary
if self.newname != '' and self.newname != nUtils.getObjectName(obj):
log(
"Renaming "
+ obj.phobostype
+ " '"
+ nUtils.getObjectName(obj)
+ "' to '"
+ self.newname
+ "'.",
'INFO',
)
nUtils.safelyName(obj, self.newname)
elif self.newname == '':
log("Removing custom name from " + obj.phobostype + " '" + obj.name + "'.", 'INFO')
if obj.phobostype + '/name' in obj:
del obj[obj.phobostype + '/name']
# only links have joint names
if obj.phobostype == 'link':
if self.jointname != '':
# only change/add joint/name if it was changed
if 'joint/name' not in obj or (
'joint/name' in obj and self.jointname != obj['joint/name']
):
log(
"Renaming joint of "
+ obj.phobostype
+ " '"
+ nUtils.getObjectName(obj)
+ "' to '"
+ self.jointname
+ "'.",
'INFO',
)
obj['joint/name'] = self.jointname
# remove joint/name when empty
elif self.jointname == '':
if 'joint/name' in obj:
log(
"Removing joint name from " + obj.phobostype + " '" + obj.name + "'.",
'INFO',
)
del obj['joint/name']
return {'FINISHED'}
def deriveLinkfromObject(
obj, scale=0.2, parenting=True, parentobjects=False, namepartindices=[], separator="_", prefix="link"
):
"""Derives a link from an object that defines a joint through its position, orientation and parent-child relationships.
:param obj: The object you want to derive your link from.
:type obj: bpy_types.Object
:param scale: The scale you want to apply to the link.
:type scale: float
:param parenting: Whether you want to automate the parenting of the new link or not.
:type parenting: bool.
:param parentobjects: Whether you want to parent all the objects to the new link or not.
:type parentobjects: bool.
:param namepartindices: Parts of the objects name you want to reuse in the links name.
:type namepartindices: list with two elements.
:param separator: The separator you want to use to separate the links name with. Its '_' per default
:type separator: str
:param prefix: The prefix you want to use for the new links name. Its 'link' per default.
:type prefix: str
"""
print("Deriving link from", namingUtils.getObjectName(obj))
nameparts = namingUtils.getObjectName(obj).split("_")
rotation = obj.matrix_world.to_euler()
if "invertAxis" in obj and obj["invertAxis"] == 1:
rotation.x += math.pi if rotation.x < 0 else -math.pi
tmpname = namingUtils.getObjectName(obj)
if namepartindices:
try:
tmpname = separator.join([nameparts[p] for p in namepartindices])
except IndexError:
print("Wrong name segment indices given for obj", namingUtils.getObjectName(obj))
if prefix != "":
tmpname = prefix + separator + tmpname
if tmpname == namingUtils.getObjectName(obj):
obj.name += "*"
link = createLink(scale, obj.matrix_world.to_translation(), obj.matrix_world.to_euler(), tmpname)
if parenting:
if obj.parent:
selectionUtils.selectObjects([link, obj.parent], True, 1)
if obj.parent.phobostype == "link":
bpy.ops.object.parent_set(type="BONE_RELATIVE")
else:
bpy.ops.object.parent_set(type="OBJECT")
children = selectionUtils.getImmediateChildren(obj)
if parentobjects:
children.append(obj)
for child in children:
selectionUtils.selectObjects([child], True, 0)
bpy.ops.object.parent_clear(type="CLEAR_KEEP_TRANSFORM")
selectionUtils.selectObjects([child, link], True, 1)
bpy.ops.object.parent_set(type="BONE_RELATIVE")
def deriveGroupEntry(group):
"""This function gets a blender group and creates a list of phobos link skeletons out of the groups objects.
:param group: The blender group to extract the links from.
:type group: bpy_types.Group
:return: list
"""
links = []
for obj in group.objects:
if obj.phobostype == 'link':
links.append({'type': 'link', 'name': namingUtils.getObjectName(obj)})
else:
print("### Error: group " + namingUtils.getObjectName(group) + " contains " + obj.phobostype + ': ' + namingUtils.getObjectName(obj))
return links
def storePose(robot_name, pose_name):
"""
Store the current pose of all of a robot's selected links.
Existing poses of the same name will be overwritten.
:param robot_name: The robot the pose belongs to.
:type robot_name: str.
:param pose_name: The name the pose will be stored under.
:type pose_name: str.
:return: Nothing.
"""
file_name = 'robot_poses_' + robot_name
load_file = blenderUtils.readTextFile(file_name)
if load_file == '':
poses = {}
else:
poses = yaml.load(load_file)
new_pose = {}
prev_mode = bpy.context.mode
bpy.ops.object.mode_set(mode='POSE')
for root in selectionUtils.getRoots():
if root['modelname'] == robot_name:
links = selectionUtils.getChildren(root)
for link in links:
if link.select and link.phobostype == 'link':
link.pose.bones['Bone'].rotation_mode = 'XYZ'
new_pose[namingUtils.getObjectName(
link,
'joint')] = link.pose.bones['Bone'].rotation_euler.y
bpy.ops.object.mode_set(mode=prev_mode)
poses[pose_name] = new_pose
blenderUtils.updateTextFile(file_name, yaml.dump(poses))
def storePose(root, posename):
"""Stores the current pose of all of a model's selected joints.
Existing poses of the same name will be overwritten.
Args:
root(bpy_types.Object): root of the model the pose belongs to
posename(str): name the pose will be stored under
Returns:
: Nothing.
"""
if root:
filename = nUtils.getModelName(root) + '::poses'
posedict = json.loads(bUtils.readTextFile(filename))
if not posedict:
posedict = {posename: {'name': posename, 'joints': {}}}
else:
posedict[posename] = {'name': posename, 'joints': {}}
links = sUtils.getChildren(root, ('link', ), True, False)
sUtils.selectObjects([root] + links, clear=True, active=0)
bpy.ops.object.mode_set(mode='POSE')
for link in (link for link in links if 'joint/type' in link
and link['joint/type'] not in ['fixed', 'floating']):
link.pose.bones['Bone'].rotation_mode = 'XYZ'
posedict[posename]['joints'][nUtils.getObjectName(
link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y
bpy.ops.object.mode_set(mode='OBJECT')
posedict = gUtils.roundFloatsInDict(
posedict,
ioUtils.getExpSettings().decimalPlaces)
bUtils.updateTextFile(filename, json.dumps(posedict))
else:
log("No model root provided to store the pose for", "ERROR")
def createInertial(obj):
"""Creates an empty inertial object with the same world transform as the corresponding
object and parents it to the correct link.
:param obj: The object you want to copy the world transform from.
:type obj: bpy_types.Object
:return: bpy_types.Object -- the newly created inertia.
"""
if obj.phobostype == 'link':
parent = obj
size = (0.04, 0.04, 0.04)
else:
parent = obj.parent
size = (0.02, 0.02, 0.02)
rotation = obj.matrix_world.to_euler()
center = obj.matrix_world.to_translation()
inertial = blenderUtils.createPrimitive(
'inertial_' + namingUtils.getObjectName(obj, phobostype="link"), 'box',
size, defs.layerTypes["inertial"], 'phobos_inertial', center, rotation)
bpy.ops.object.transform_apply(location=False, rotation=False, scale=True)
inertial.phobostype = 'inertial'
bpy.ops.object.select_all(action="DESELECT")
#utility.selectObjects([inertial], True, 0)
selectionUtils.selectObjects([parent, inertial], True, 0)
#bpy.context.scene.objects.active = parent.pose.bones[0]
bpy.ops.object.parent_set(type='BONE_RELATIVE')
return inertial
def storePose(robot_name, pose_name):
"""
Store the current pose of all of a robot's selected links.
Existing poses of the same name will be overwritten.
:param robot_name: The robot the pose belongs to.
:type robot_name: str.
:param pose_name: The name the pose will be stored under.
:type pose_name: str.
:return: Nothing.
"""
file_name = 'robot_poses_' + robot_name
load_file = blenderUtils.readTextFile(file_name)
if load_file == '':
poses = {}
else:
poses = yaml.load(load_file)
new_pose = {}
prev_mode = bpy.context.mode
bpy.ops.object.mode_set(mode='POSE')
for root in selectionUtils.getRoots():
if root['modelname'] == robot_name:
links = selectionUtils.getChildren(root)
for link in links:
if link.select and link.phobostype == 'link':
link.pose.bones['Bone'].rotation_mode = 'XYZ'
new_pose[namingUtils.getObjectName(link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y
bpy.ops.object.mode_set(mode=prev_mode)
poses[pose_name] = new_pose
blenderUtils.updateTextFile(file_name, yaml.dump(poses))
def deriveDictEntry(obj):
print(namingUtils.getObjectName(obj), end=', ')
props = None
parent = None
try:
if obj.phobostype == 'inertial':
props, parent = deriveInertial(obj)
elif obj.phobostype == 'visual':
props, parent = deriveVisual(obj)
elif obj.phobostype == 'collision':
props, parent = deriveCollision(obj)
elif obj.phobostype == 'approxsphere':
props, parent = deriveApproxsphere(obj)
elif obj.phobostype == 'sensor':
props = deriveSensor(obj)
elif obj.phobostype == 'controller':
props = deriveController(obj)
elif obj.phobostype == 'light':
props, parent = deriveLight(obj)
except KeyError:
print('phobos: A KeyError occurred, likely due to missing information in the model:\n ', sys.exc_info()[0])
if obj.phobostype in ['sensor', 'controller']:
return props
else:
return props, parent
def loadPose(pose_name):
load_file = blenderUtils.readTextFile('robot_poses')
if load_file == '':
log('No poses stored.', 'ERROR')
return
poses = yaml.load(load_file)
if pose_name in poses:
prev_mode = bpy.context.mode
bpy.ops.object.mode_set(mode='POSE')
for obj in selectionUtils.returnObjectList('link'):
if namingUtils.getObjectName(obj, 'joint') in poses[pose_name]:
obj.pose.bones['Bone'].rotation_mode = 'XYZ'
obj.pose.bones['Bone'].rotation_euler.y = poses[pose_name][namingUtils.getObjectName(obj, 'joint')]
bpy.ops.object.mode_set(mode=prev_mode)
else:
log('No pose with name ' + pose_name + ' stored.', 'ERROR')
def deriveGeometry(obj):
if 'geometry/type' in obj:
geometry = {'type': obj['geometry/type']}
gt = obj['geometry/type']
if gt == 'box':
geometry['size'] = list(obj.dimensions)
elif gt == 'cylinder':
geometry['radius'] = obj.dimensions[0]/2
geometry['length'] = obj.dimensions[2]
elif gt == 'sphere':
geometry['radius'] = obj.dimensions[0]/2
elif gt == 'mesh':
filename = obj.data.name
if bpy.data.worlds[0].useObj:
filename += ".obj"
elif bpy.data.worlds[0].useBobj:
filename += ".bobj"
elif bpy.data.worlds[0].useStl:
filename += ".stl"
elif bpy.data.worlds[0].useDae:
filename += ".dae"
else:
filename += ".obj"
geometry['filename'] = os.path.join('meshes', filename)
geometry['scale'] = list(obj.scale)
geometry['size'] = list(obj.dimensions) # this is needed to calculate an approximate inertia
return geometry
else:
warnings.warn("No geometry/type found for object "+namingUtils.getObjectName(obj)+".")
return None
def exportMesh(obj, path, meshtype):
# DOCU add some docstring
objname = nUtils.getObjectName(obj)
tmpobjname = obj.name
# OPT: surely no one will ever name an object like so, better solution?
obj.name = 'tmp_export_666'
tmpobject = bUtils.createPrimitive(objname, 'box', (1.0, 1.0, 1.0))
# copy the mesh here
tmpobject.data = obj.data
outpath = os.path.join(path, obj.data.name + "." + meshtype)
if meshtype == 'obj':
axis_forward = ioUtils.getExpSettings().obj_axis_forward
axis_up = ioUtils.getExpSettings().obj_axis_up
bpy.ops.export_scene.obj(filepath=outpath,
use_selection=True,
use_normals=True,
use_materials=False,
use_mesh_modifiers=True,
axis_forward=axis_forward,
axis_up=axis_up)
elif meshtype == 'stl':
bpy.ops.export_mesh.stl(filepath=outpath,
use_selection=True,
use_mesh_modifiers=True)
elif meshtype == 'dae':
bpy.ops.wm.collada_export(filepath=outpath, selected=True)
bpy.ops.object.select_all(action='DESELECT')
tmpobject.select = True
bpy.ops.object.delete()
obj.name = tmpobjname
def createInertial(obj):
"""Creates an empty inertial object with the same world transform as the corresponding
object and parents it to the correct link.
:param obj: The object you want to copy the world transform from.
:type obj: bpy_types.Object
:return: bpy_types.Object -- the newly created inertia.
"""
if obj.phobostype == 'link':
parent = obj
size = (0.04, 0.04, 0.04)
else:
parent = obj.parent
size = (0.02, 0.02, 0.02)
rotation = obj.matrix_world.to_euler()
center = obj.matrix_world.to_translation()
inertial = blenderUtils.createPrimitive('inertial_' + namingUtils.getObjectName(obj, phobostype="link"), 'box', size,
defs.layerTypes["inertial"], 'phobos_inertial', center, rotation)
bpy.ops.object.transform_apply(location=False, rotation=False, scale=True)
inertial.phobostype = 'inertial'
bpy.ops.object.select_all(action="DESELECT")
#utility.selectObjects([inertial], True, 0)
selectionUtils.selectObjects([parent, inertial], True, 0)
#bpy.context.scene.objects.active = parent.pose.bones[0]
bpy.ops.object.parent_set(type='BONE_RELATIVE')
return inertial
def deriveController(obj):
"""
Args:
obj:
Returns:
"""
import phobos.model.models as models
props = models.initObjectProperties(obj, phobostype='controller')
# return None if no controller is found (there will always be at least a name in the props)
if len(props) < 2:
return None
if not obj.parent or obj.parent.phobostype not in defs.controllabletypes:
log(
("Can not derive controller from {}. " +
"Insufficient requirements from parent object!").format(obj.name),
'ERROR',
)
return None
props['target'] = nUtils.getObjectName(obj.parent)
log(
" Derived controller '{}' for target '{}'.".format(
props['name'], props['target']), 'DEBUG')
return props
def execute(self, context):
location = bpy.context.scene.cursor_location
objects = []
controllers = []
for obj in bpy.context.selected_objects:
if obj.phobostype == "controller":
controllers.append(obj)
else:
objects.append(obj)
if len(controllers) <= 0:
bUtils.createPrimitive("controller", "sphere",
self.controller_scale,
defs.layerTypes["sensor"], "controller",
location)
bpy.context.scene.objects.active.phobostype = "controller"
bpy.context.scene.objects.active.name = "controller"
controllers.append(bpy.context.scene.objects.active)
#empty index list so enable robotupdate of controller
for ctrl in controllers:
for key in ctrl.keys():
if key.find("index") >= 0:
del ctrl[key]
log("Deleting " + str(key) + " in " + ctrl.name, "INFO")
i = 1
for obj in objects:
if obj.phobostype == "link":
ctrl["index" + (str(i) if i >= 10 else "0" +
str(i))] = nUtils.getObjectName(obj)
i += 1
log("Added joints to (new) controller(s).", "INFO")
#for prop in defs.controllerProperties[self.controller_type]:
# for ctrl in controllers:
# ctrl[prop] = defs.controllerProperties[prop]
return {'FINISHED'}
def createInertial(obj):
"""Creates an empty inertial object with the same world transform as the corresponding
object and parents it to the correct link.
:param obj: The object you want to copy the world transform from.
:type obj: bpy_types.Object
:return: bpy_types.Object -- the newly created inertia.
"""
if obj.phobostype == 'link':
parent = obj
size = (0.06, 0.06, 0.06)
else:
parent = obj.parent
size = (0.015, 0.015, 0.015)
rotation = obj.matrix_world.to_euler()
center = obj.matrix_world.to_translation()
inertial = bUtils.createPrimitive(
'inertial_' + nUtils.getObjectName(obj, phobostype="link"), 'box',
size, defs.layerTypes["inertial"], 'phobos_inertial', center, rotation)
bpy.ops.object.transform_apply(location=False, rotation=False, scale=True)
inertial.phobostype = 'inertial'
bpy.ops.object.select_all(action="DESELECT")
sUtils.selectObjects((inertial, parent), clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
return inertial
def deriveLight(obj):
light = initObjectProperties(obj, phobostype='light')
light_data = obj.data
if light_data.use_diffuse:
light['color_diffuse'] = list(light_data.color)
if light_data.use_specular:
light['color_specular'] = copy.copy(light['color_diffuse'])
light['type'] = light_data.type.lower()
if light['type'] == 'SPOT':
light['size'] = light_data.size
light['position'] = list(obj.matrix_local.to_translation())
light['rotation'] = list(obj.matrix_local.to_euler())
try:
light['attenuation_linear'] = float(light_data.linear_attenuation)
except AttributeError:
pass
try:
light['attenuation_quadratic'] = float(light_data.quadratic_attenuation)
except AttributeError:
pass
if light_data.energy:
light['attenuation_constant'] = float(light_data.energy)
if obj.parent is not None:
light['parent'] = namingUtils.getObjectName(obj.parent,phobostype="link")
return light
def exportMesh(obj, path, meshtype):
objname = nUtils.getObjectName(obj)
tmpobjname = obj.name
obj.name = 'tmp_export_666' # surely no one will ever name an object like so
tmpobject = bUtils.createPrimitive(objname, 'box', (1.0, 1.0, 1.0))
tmpobject.data = obj.data # copy the mesh here
outpath = os.path.join(path, obj.data.name + "." + meshtype)
if meshtype == 'obj':
bpy.ops.export_scene.obj(filepath=outpath,
use_selection=True,
use_normals=True,
use_materials=False,
use_mesh_modifiers=True)
elif meshtype == 'stl':
if bpy.app.version[0] * 100 + bpy.app.version[1] >= 277:
bpy.ops.export_mesh.stl(filepath=outpath,
use_selection=True,
use_mesh_modifiers=True)
else:
bpy.ops.export_mesh.stl(filepath=outpath, use_mesh_modifiers=True)
elif meshtype == 'dae':
bpy.ops.wm.collada_export(filepath=outpath, selected=True)
bpy.ops.object.select_all(action='DESELECT')
tmpobject.select = True
bpy.ops.object.delete()
obj.name = tmpobjname
def exportSubmechanisms(model, path):
"""This function exports the submechanisms contained in a robot model.
Args:
model(dict): The robot model to export
path(str): The filepath to export the submechanisms data to
Returns:
"""
log("Phobos Submechanisms export: Creating submechanisms data at " + path, "INFO")
for submechanism in model['submechanisms']:
root = sUtils.getObjectByProperty('submechanism/name', submechanism['contextual_name'])
linkobjs = [root] + root['submechanism/spanningtree']
if 'submechanism/freeloader' in root:
linkobjs += root['submechanism/freeloader']
objects = [
o
for link in linkobjs
for o in link.children
if o.phobostype in ['visual', 'collision', 'inertial']
] + linkobjs
model = deriveModelDictionary(root, root['submechanism/name'], objects)
jointname = nUtils.getObjectName(root, 'joint')
if jointname in model['joints']:
del model['joints'][jointname]
log('Removed joint which is not part of submodel: ' + jointname, 'DEBUG')
exportModel(model, path, ['urdf'])
def initObjectProperties(obj, phobostype=None, ignoretypes=[]):
props = {'name': namingUtils.getObjectName(obj).split(':')[-1]} #allow duplicated names differentiated by types
if not phobostype:
for key, value in obj.items():
props[key] = value
else:
for key, value in obj.items():
if hasattr(value, 'to_list'): # transform Blender id_arrays into lists
value = list(value)
if '/' in key:
if phobostype+'/' in key:
specs = key.split('/')[1:]
if len(specs) == 1:
props[key.replace(phobostype+'/', '')] = value
elif len(specs) == 2:
category, specifier = specs
if '$'+category not in props:
props['$'+category] = {}
props['$'+category][specifier] = value
elif key.count('/') == 1: #ignore two-level specifiers if phobostype is not present
category, specifier = key.split('/')
if category not in ignoretypes:
if '$'+category not in props:
props['$'+category] = {}
props['$'+category][specifier] = value
return props
def calculateSum(objects, numeric_prop):
"""Returns sum of *numeric_prop* in *objects*.
Args:
objects(list(bpy.types.Object): objects to sum up the property for
numeric_prop(str): name of the custom property to sum
Returns:
: float/int -- sum of the values in the property of the objects
"""
numsum = 0
for obj in objects:
try:
numsum += obj[numeric_prop]
except KeyError:
log(
"{0} object {1} does not contain '{2}'".format(
obj.phobostype, obj.name, numeric_prop
),
"WARNING",
)
except TypeError:
import phobos.utils.naming as nUtils
log(
"Could not add this type to the sum: "
+ str(type(obj[numeric_prop]))
+ " @"
+ nUtils.getObjectName(obj),
'WARNING',
)
return numsum
def exportSubmechanisms(model, path):
"""This function exports the submechanisms contained in a robot model.
Args:
model(dict): The robot model to export
path(str): The filepath to export the submechanisms data to
Returns:
"""
log("Phobos Submechanisms export: Creating submechanisms data at " + path,
"INFO")
for submechanism in model['submechanisms']:
root = sUtils.getObjectByProperty('submechanism/name',
submechanism['contextual_name'])
linkobjs = [root] + root['submechanism/spanningtree']
if 'submechanism/freeloader' in root:
linkobjs += root['submechanism/freeloader']
objects = [
o for link in linkobjs for o in link.children
if o.phobostype in ['visual', 'collision', 'inertial']
] + linkobjs
model = deriveModelDictionary(root, root['submechanism/name'], objects)
jointname = nUtils.getObjectName(root, 'joint')
if jointname in model['joints']:
del model['joints'][jointname]
log('Removed joint which is not part of submodel: ' + jointname,
'DEBUG')
exportModel(model, path, ['urdf'])
def execute(self, context):
"""
Args:
context:
Returns:
"""
if self.complete:
roots = set([sUtils.getRoot(obj) for obj in context.selected_objects]) - {None}
objects = set()
for root in roots:
objects = objects | set(sUtils.getChildren(root))
objlist = list(objects)
else:
objlist = [bpy.context.active_object]
for obj in objlist:
try:
entityname = sUtils.getRoot(obj)['entity/name']
except (KeyError, TypeError):
entityname = ''
log(nUtils.getObjectName(obj) + " is not part of a well-defined entity.", "WARNING")
namespace = self.namespace if self.namespace else entityname
nUtils.toggleNamespace(obj, namespace)
return {'FINISHED'}
def storePose(modelname, posename):
"""
Stores the current pose of all of a robot's selected joints.
Existing poses of the same name will be overwritten.
:param modelname: The robot the pose belongs to.
:type modelname: str.
:param posename: The name the pose will be stored under.
:type posename: str.
:return: Nothing.
"""
rootlink = None
for root in sUtils.getRoots():
if root['modelname'] == modelname:
rootlink = root
if rootlink:
filename = modelname + '::poses'
posedict = yaml.load(bUtils.readTextFile(filename))
if not posedict:
posedict = {posename: {'name': posename, 'joints': {}}}
else:
posedict[posename] = {'name': posename, 'joints': {}}
bpy.ops.object.mode_set(mode='POSE')
links = sUtils.getChildren(rootlink, ('link', ), True, False)
for link in (link for link in links if 'joint/type' in link
and link['joint/type'] not in ['fixed', 'floating']):
link.pose.bones['Bone'].rotation_mode = 'XYZ'
posedict[posename]['joints'][nUtils.getObjectName(
link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y
bUtils.updateTextFile(filename,
yaml.dump(posedict, default_flow_style=False))
else:
log("No model root could be found to store the pose for", "ERROR")
def execute(self, context):
startLog(self)
location = bpy.context.scene.cursor_location
objects = []
controllers = []
for obj in bpy.context.selected_objects:
if obj.phobostype == "controller":
controllers.append(obj)
else:
objects.append(obj)
if len(controllers) <= 0:
blenderUtils.createPrimitive("controller", "sphere", self.controller_scale, defs.layerTypes["sensor"], "controller", location)
bpy.context.scene.objects.active.phobostype = "controller"
bpy.context.scene.objects.active.name = "controller"
controllers.append(bpy.context.scene.objects.active)
#empty index list so enable robotupdate of controller
for ctrl in controllers:
for key in ctrl.keys():
if key.find("index") >= 0:
del ctrl[key]
log("Deleting " + str(key) + " in " + ctrl.name, "INFO")
i = 1
for obj in objects:
if obj.phobostype == "link":
ctrl["index"+(str(i) if i >= 10 else "0"+str(i))] = namingUtils.getObjectName(obj)
i += 1
log("Added joints to (new) controller(s).", "INFO")
#for prop in defs.controllerProperties[self.controller_type]:
# for ctrl in controllers:
# ctrl[prop] = defs.controllerProperties[prop]
endLog()
return {'FINISHED'}
def deriveLight(obj):
"""This function derives a light from a given blender object
:param obj: The blender object to derive the light from.
:type obj: bpy_types.Object
:return: tuple
"""
light = initObjectProperties(obj, phobostype='light')
light_data = obj.data
if light_data.use_diffuse:
light['color_diffuse'] = list(light_data.color)
if light_data.use_specular:
light['color_specular'] = copy.copy(light['color_diffuse'])
light['type'] = light_data.type.lower()
if light['type'] == 'SPOT':
light['size'] = light_data.size
pose = deriveObjectPose(obj)
light['position'] = pose['translation']
light['rotation'] = pose['rotation_euler']
try:
light['attenuation_linear'] = float(light_data.linear_attenuation)
except AttributeError:
pass
try:
light['attenuation_quadratic'] = float(light_data.quadratic_attenuation)
except AttributeError:
pass
if light_data.energy:
light['attenuation_constant'] = float(light_data.energy)
light['parent'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
return light
def deriveLight(obj):
"""This function derives a light from a given blender object
:param obj: The blender object to derive the light from.
:type obj: bpy_types.Object
:return: tuple
"""
light = initObjectProperties(obj, phobostype='light')
light_data = obj.data
if light_data.use_diffuse:
light['color_diffuse'] = list(light_data.color)
if light_data.use_specular:
light['color_specular'] = copy.copy(light['color_diffuse'])
light['type'] = light_data.type.lower()
if light['type'] == 'SPOT':
light['size'] = light_data.size
pose = deriveObjectPose(obj)
light['position'] = pose['translation']
light['rotation'] = pose['rotation_euler']
try:
light['attenuation_linear'] = float(light_data.linear_attenuation)
except AttributeError:
# TODO handle this somehow
pass
try:
light['attenuation_quadratic'] = float(
light_data.quadratic_attenuation)
except AttributeError:
pass
if light_data.energy:
light['attenuation_constant'] = float(light_data.energy)
light['parent'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
return light
def deriveMaterial(mat):
material = initObjectProperties(mat, 'material')
material['name'] = mat.name
material['diffuseColor'] = dict(zip(['r', 'g', 'b'], [mat.diffuse_intensity * num for num in list(mat.diffuse_color)]))
material['ambientColor'] = dict(zip(['r', 'g', 'b'], [mat.ambient * mat.diffuse_intensity * num for num in list(mat.diffuse_color)]))
material['specularColor'] = dict(zip(['r', 'g', 'b'], [mat.specular_intensity * num for num in list(mat.specular_color)]))
if mat.emit > 0:
material['emissionColor'] = dict(zip(['r', 'g', 'b'], [mat.emit * mat.specular_intensity * num for num in list(mat.specular_color)]))
material['shininess'] = mat.specular_hardness/2
if mat.use_transparency:
material['transparency'] = 1.0-mat.alpha
try:
material['texturename'] = namingUtils.getObjectName(mat.texture_slots[0].texture.image) # grab the first texture
except (KeyError, AttributeError):
print('None or incomplete texture data for material ' + namingUtils.getObjectName(mat) + '.')
return material
def deriveMaterial(mat):
"""This function takes a blender material and creates a phobos representation from it
:param mat: The blender material to derive a phobos material from
:type mat: bpy.types.Material
:return: dict
"""
material = initObjectProperties(mat, 'material')
material['name'] = mat.name
material['diffuseColor'] = dict(zip(['r', 'g', 'b'],
[mat.diffuse_intensity * num for num in list(mat.diffuse_color)]))
material['ambientColor'] = dict(zip(['r', 'g', 'b'],
[mat.ambient * mat.diffuse_intensity * num for num in list(mat.diffuse_color)]))
material['specularColor'] = dict(zip(['r', 'g', 'b'],
[mat.specular_intensity * num for num in list(mat.specular_color)]))
if mat.emit > 0:
material['emissionColor'] = dict(zip(['r', 'g', 'b'],
[mat.emit * mat.specular_intensity * num for num in list(mat.specular_color)]))
material['shininess'] = mat.specular_hardness/2
if mat.use_transparency:
material['transparency'] = 1.0-mat.alpha
for tex in mat.texture_slots: # there are always 18 slots, regardless of whether they are filled or not
if tex is not None:
try:
if tex.use_map_color_diffuse: # regular diffuse color texture
material['diffuseTexture'] = mat.texture_slots[0].texture.image.filepath.replace('//', '') # grab the first texture
if tex.use_map_normal: # normal map
material['normalTexture'] = mat.texture_slots[0].texture.image.filepath.replace('//', '') # grab the first texture
if tex.use_map_displacement: # displacement map
material['displacementTexture'] = mat.texture_slots[0].texture.image.filepath.replace('//', '') # grab the first texture
except (KeyError, AttributeError):
log("None or incomplete texture data for material " + nUtils.getObjectName(mat, 'material'),
"WARNING", "deriveMaterial")
return material
def storePose(modelname, posename):
"""
Stores the current pose of all of a robot's selected joints.
Existing poses of the same name will be overwritten.
:param modelname: The robot the pose belongs to.
:type modelname: str.
:param posename: The name the pose will be stored under.
:type posename: str.
:return: Nothing.
"""
rootlink = None
for root in sUtils.getRoots():
if root['modelname'] == modelname:
rootlink = root
if rootlink:
filename = modelname + '::poses'
posedict = yaml.load(bUtils.readTextFile(filename))
if not posedict:
posedict = {posename: {'name': posename, 'joints': {}}}
else:
posedict[posename] = {'name': posename, 'joints': {}}
bpy.ops.object.mode_set(mode='POSE')
links = sUtils.getChildren(rootlink, ('link',), True, False)
for link in (link for link in links if 'joint/type' in link
and link['joint/type'] not in ['fixed', 'floating']):
link.pose.bones['Bone'].rotation_mode = 'XYZ'
posedict[posename]['joints'][nUtils.getObjectName(link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y
bUtils.updateTextFile(filename, yaml.dump(posedict, default_flow_style=False))
else:
log("No model root could be found to store the pose for", "ERROR", "storePose")
def deriveGroupEntry(group):
"""Derives a list of phobos link skeletons for a provided group object.
:param group: The blender group to extract the links from.
:type group: bpy_types.Group
:return: list
"""
links = []
for obj in group.objects:
if obj.phobostype == 'link':
links.append({'type': 'link', 'name': nUtils.getObjectName(obj)})
else:
log("Group " + nUtils.getObjectName(group) + " contains " + obj.phobostype
+ ': ' + nUtils.getObjectName(obj), "ERROR", "deriveGroupEntry")
return links
def calculateSum(objects, numeric_prop):
"""Returns sum of *numeric_prop* in *objects*.
Args:
objects(list(bpy.types.Object): objects to sum up the property for
numeric_prop(str): name of the custom property to sum
Returns:
: float/int -- sum of the values in the property of the objects
"""
numsum = 0
for obj in objects:
try:
numsum += obj[numeric_prop]
except KeyError:
log(
"{0} object {1} does not contain '{2}'".format(
obj.phobostype, obj.name, numeric_prop
),
"WARNING",
)
except TypeError:
import phobos.utils.naming as nUtils
log(
"Could not add this type to the sum: "
+ str(type(obj[numeric_prop]))
+ " @"
+ nUtils.getObjectName(obj),
'WARNING',
)
return numsum
def derive_link(linkobj, inertialobjs=None):
"""Derives a link from a blender object and creates its initial phobos data structure.
If inertialobjs are provided, the inertia will be calculated from the specified list instead
of the getInertiaChildren.
The dictionary contains (besides any generic object properties) this information:
*parent*: name of parent object or None
*children*: list of names of the links child links
*object*: bpy.types.Object which represents the link
*pose*: deriveObjectPose of the linkobj
*collision*: empty dictionary
*visual*: empty dictionary
*inertial*: derive_inertia of all child inertials of the link
*approxcollision*: empty dictionary
:param linkobj: blender object to derive the link from.
:type linkobj: bpy.types.Object
:param inertialobjs: override the link inertial objects
:type inertialobjs: list of bpy.types.Object
:return: representation of the link
:rtype: dict
.. seealso deriveObjectPose
.. seealso deriveInertiaChildren
.. seealso derive_inertia
"""
assert linkobj.phobostype == 'link', ("Wrong phobostype: " +
linkobj.phobostype +
" instead of link.")
log("Deriving link from object " + linkobj.name + ".", 'DEBUG')
props = initObjectProperties(linkobj,
phobostype='link',
ignoretypes=linkobjignoretypes - {'link'})
parent = sUtils.getEffectiveParent(linkobj)
props['parent'] = nUtils.getObjectName(parent) if parent else None
props['children'] = [
child.name for child in linkobj.children if child.phobostype == 'link'
]
props['object'] = linkobj
props['pose'] = deriveObjectPose(linkobj)
props['collision'] = {}
props['visual'] = {}
props['inertial'] = {}
props['approxcollision'] = []
if not inertialobjs:
inertialobjs = inertiamodel.getInertiaChildren(linkobj)
log(" Deriving inertial...", 'DEBUG')
# add inertial information to link
if inertialobjs:
props['inertial'] = derive_inertia(inertialobjs)
else:
log(
"No valid inertial data for link " + props['name'] + ". " +
str(len(inertialobjs)) + " inertial objects selected.", 'WARNING')
return props
def deriveController(obj):
"""
Args:
obj:
Returns:
"""
import phobos.model.models as models
props = models.initObjectProperties(obj, phobostype='controller')
# return None if no controller is found (there will always be at least a name in the props)
if len(props) < 2:
return None
if not obj.parent or obj.parent.phobostype not in defs.controllabletypes:
log(
(
"Can not derive controller from {}. "
+ "Insufficient requirements from parent object!"
).format(obj.name),
'ERROR',
)
return None
props['target'] = nUtils.getObjectName(obj.parent)
log(
" Derived controller '{}' for target '{}'.".format(props['name'], props['target']), 'DEBUG'
)
return props
def deriveGeometry(obj):
"""This function derives the geometry from an object.
:param obj: The blender object to derive the geometry from.
:type obj: bpy_types.Object
:return: dict
"""
try:
geometry = {'type': obj['geometry/type']}
gt = obj['geometry/type']
if gt == 'box':
geometry['size'] = list(obj.dimensions)
elif gt == 'cylinder' or gt == 'capsule':
geometry['radius'] = obj.dimensions[0]/2
geometry['length'] = obj.dimensions[2]
elif gt == 'capsule':
geometry['radius'] = obj.dimensions[0]/2
geometry['length'] = obj.dimensions[2] - obj.dimensions[0]
elif gt == 'sphere':
geometry['radius'] = obj.dimensions[0]/2
elif gt == 'mesh':
geometry['filename'] = obj.data.name
geometry['scale'] = list(obj.scale)
# FIXME: is this needed to calculate an approximate inertia
geometry['size'] = list(obj.dimensions)
# any other geometry type, i.e. 'plane'
else:
geometry['size'] = list(obj.dimensions)
return geometry
except KeyError as err:
log("Undefined geometry for object " + nUtils.getObjectName(obj) +
" " + str(err), "ERROR")
return None
def deriveJoint(obj, logging=False, adjust=False, errors=None):
"""Derives a joint from a blender object and creates its initial phobos data structure.
Args:
obj(bpy.types.Object): object to derive the joint from
adjust(bool, optional): TODO (Default value = False)
logging: (Default value = False)
errors: (Default value = None)
Returns:
: dict
"""
joint_type, crot = jointmodel.deriveJointType(obj,
adjust=adjust,
logging=logging)
props = initObjectProperties(obj,
phobostype='joint',
ignoretypes=linkobjignoretypes - {'joint'})
parent = sUtils.getEffectiveParent(obj)
props['parent'] = nUtils.getObjectName(parent)
props['child'] = nUtils.getObjectName(obj)
axis, minmax = jointmodel.getJointConstraints(obj)
if axis:
props['axis'] = list(axis)
limits = {}
if minmax is not None:
# prismatic or revolute joint, TODO: planar etc.
if len(minmax) == 2:
limits['lower'] = minmax[0]
limits['upper'] = minmax[1]
if 'maxvelocity' in props:
limits['velocity'] = props['maxvelocity']
del props['maxvelocity']
if 'maxeffort' in props:
limits['effort'] = props['maxeffort']
del props['maxeffort']
if limits != {}:
props['limits'] = limits
props['pose'] = deriveObjectPose(obj)
# TODO: what about these?
# - calibration
# - dynamics
# - mimic
# - safety_controller
return props
def get_link_information(linkobj):
"""Returns the full link information including joint and motor data from a blender object.
The link information is derived according to :func:`derive_link`.
Args:
linkobj(bpy.types.Object): blender object to derive the link from
Returns:
dict: link representation of the object
"""
props = initObjectProperties(linkobj,
phobostype='link',
ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(linkobj)
props['parent'] = parent.name if parent else None
props['pose'] = deriveObjectPose(linkobj)
props['joint'] = deriveJoint(linkobj, logging=False, adjust=False)
del props['joint']['parent']
# collect collision objs for link
collisionobjs = sUtils.getImmediateChildren(linkobj,
phobostypes=('collision'),
include_hidden=True)
collisiondict = {}
for colobj in collisionobjs:
collisiondict[colobj.name] = colobj
props['collision'] = collisiondict
# collect visual objs for link
visualobjects = sUtils.getImmediateChildren(linkobj,
phobostypes=('visual'),
include_hidden=True)
visualdict = {}
for visualobj in visualobjects:
visualdict[visualobj.name] = visualobj
props["visual"] = visualdict
# collect inertial objects
inertialdict = {
nUtils.getObjectName(obj): obj
for obj in linkobj.children if obj.phobostype == 'inertial'
}
props["inertial"] = inertialdict
# collect sensor objects
sensorobjects = sUtils.getImmediateChildren(linkobj,
phobostypes=('sensor'),
include_hidden=True)
sensordict = {}
for sensorobj in sensorobjects:
sensordict[sensorobj.name] = sensorobj
if sensordict:
props["sensor"] = sensordict
props['approxcollision'] = []
return props
def invoke(self, context, event):
wm = context.window_manager
obj = context.active_object
self.newname = nUtils.getObjectName(obj)
if 'joint/name' in obj:
self.jointname = obj['joint/name']
return wm.invoke_props_dialog(self)
def createInertial(inertialdict,
obj,
size=0.03,
errors=None,
adjust=False,
logging=False):
"""Creates the Blender representation of a given inertial provided a dictionary.
Args:
inertialdict(dict): intertial data
obj:
size: (Default value = 0.03)
errors: (Default value = None)
adjust: (Default value = False)
logging: (Default value = False)
Returns:
: bpy_types.Object -- newly created blender inertial object
"""
if errors and not adjust:
log('Can not create inertial object.', 'ERROR')
try:
origin = mathutils.Vector(inertialdict['pose']['translation'])
except KeyError:
origin = mathutils.Vector()
# create new inertial object
name = nUtils.getUniqueName('inertial_' + nUtils.getObjectName(obj),
bpy.data.objects)
inertialobject = bUtils.createPrimitive(
name,
'box',
(size, ) * 3,
defs.layerTypes["inertial"],
pmaterial='phobos_inertial',
phobostype='inertial',
)
sUtils.selectObjects((inertialobject, ), clear=True, active=0)
bpy.ops.object.transform_apply(scale=True)
# set position according to the parent link
inertialobject.matrix_world = obj.matrix_world
parent = obj
if parent.phobostype != 'link':
parent = sUtils.getEffectiveParent(obj, ignore_selection=True)
eUtils.parentObjectsTo(inertialobject, parent)
# position and parent the inertial object relative to the link
# inertialobject.matrix_local = mathutils.Matrix.Translation(origin)
sUtils.selectObjects((inertialobject, ), clear=True, active=0)
# bpy.ops.object.transform_apply(scale=True)
# add properties to the object
for prop in ('mass', 'inertia'):
inertialobject['inertial/' + prop] = inertialdict[prop]
return inertialobject
def printRotLoc(obj):
print("Location/Rotation for object:", namingUtils.getObjectName(obj))
print("rotation_euler", sep)
print(obj.rotation_euler)
print("rotation_quaternion", sep)
print(obj.rotation_quaternion)
print("Location", sep)
print(obj.location)
print("\n\n")
def deriveMaterial(mat):
"""This function takes a blender material and creates a phobos representation from it
Args:
mat(bpy.types.Material): The blender material to derive a phobos material from
Returns:
dict
"""
material = initObjectProperties(mat, 'material')
material['name'] = mat.name
material['diffuseColor'] = dict(
zip(['r', 'g', 'b'],
[mat.diffuse_intensity * num for num in list(mat.diffuse_color)]))
material['ambientColor'] = dict(
zip(['r', 'g', 'b'], [
mat.ambient * mat.diffuse_intensity * num
for num in list(mat.diffuse_color)
]))
material['specularColor'] = dict(
zip(['r', 'g', 'b'],
[mat.specular_intensity * num
for num in list(mat.specular_color)]))
if mat.emit > 0:
material['emissionColor'] = dict(
zip(['r', 'g', 'b'], [
mat.emit * mat.specular_intensity * num
for num in list(mat.specular_color)
]))
material['shininess'] = mat.specular_hardness / 2
if mat.use_transparency:
material['transparency'] = 1.0 - mat.alpha
# there are always 18 slots, regardless of whether they are filled or not
for tex in mat.texture_slots:
if tex is not None:
try:
# regular diffuse color texture
if tex.use_map_color_diffuse:
# grab the first texture
material['diffuseTexture'] = mat.texture_slots[
0].texture.image.filepath.replace('//', '')
# normal map
if tex.use_map_normal:
# grab the first texture
material['normalTexture'] = mat.texture_slots[
0].texture.image.filepath.replace('//', '')
# displacement map
if tex.use_map_displacement:
# grab the first texture
material['displacementTexture'] = mat.texture_slots[
0].texture.image.filepath.replace('//', '')
except (KeyError, AttributeError):
log(
"None or incomplete texture data for material " +
nUtils.getObjectName(mat, 'material'), "WARNING")
return material
def calculateSum(objects, numeric_prop):
"""Returns sum of *numeric_prop* in *objects*."""
numsum = 0
for obj in objects:
try:
numsum += obj[numeric_prop]
except KeyError:
log("The object '" + namingUtils.getObjectName(obj) + "' has not property '" + numeric_prop + "'")
return numsum
def printMatrices(obj):
print("Transformation Matrices for object:", namingUtils.getObjectName(obj))
print("World", sep)
print(obj.matrix_world)
print("Local", sep)
print(obj.matrix_local)
print("Parent Inverse", sep)
print(obj.matrix_parent_inverse)
print("Basis", sep)
print(obj.matrix_basis)
print("\n\n")
def printBoundBox(obj):
"""This function prints an objects bounding box to the console.
:param obj: The object to print the bounding box for.
:type obj: bpy_types.Object
"""
print("Bounding box for object:", namingUtils.getObjectName(obj))
for vector in obj.bound_box:
for value in vector:
print(value, end="")
print()
