def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, "submodels"):
if len(link.submodels) > 0:
print model.name + " - This is an articulated object - SKIPPING!"
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = "" # model.name
color = col.color[0:3] if col.color is not None else [
0.8, 0.8, 0.8
]
if len(link.name) > 0: # and link.name != model.name:
name += link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += "-" + col.name
if col.geometry_type == "mesh":
print "Mesh geometry is unsupported - SKIPPING!"
if col.geometry_type == "image":
print "image geometry is unsupported - SKIPPING!"
if col.geometry_type == "height_map":
print "Height map geometry is unsupported - SKIPPING!"
if col.geometry_type == "plane":
print "Plane geometry is unsupported - SKIPPING!"
if col.geometry_type == "sphere":
print "Sphere geometry is unsupported - SKIPPING!"
if col.geometry_type == "box":
desc = dict(
classname="BoxAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Dimensions=map(
float, col.geometry_data["size"].split(" ")),
)
self.affordanceManager.newAffordanceFromDescription(
desc)
if col.geometry_type == "cylinder":
desc = dict(
classname="CylinderAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Radius=float(col.geometry_data["radius"]),
Length=float(col.geometry_data["length"]),
)
self.affordanceManager.newAffordanceFromDescription(
desc)
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, 'submodels'):
if len(link.submodels) > 0:
print model.name + ' - This is an articulated object - SKIPPING!'
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
color = col.color[0:3] if col.color is not None else [
0.8, 0.8, 0.8
]
if len(link.name) > 0 and link.name != model.name:
name += '-' + link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += '-' + col.name
if col.geometry_type == 'mesh':
print 'Mesh geometry is unsupported - SKIPPING!'
if col.geometry_type == 'image':
print 'image geometry is unsupported - SKIPPING!'
if col.geometry_type == 'height_map':
print 'Height map geometry is unsupported - SKIPPING!'
if col.geometry_type == 'plane':
print 'Plane geometry is unsupported - SKIPPING!'
if col.geometry_type == 'sphere':
print 'Sphere geometry is unsupported - SKIPPING!'
if col.geometry_type == 'box':
desc = dict(classname='BoxAffordanceItem',
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Dimensions=map(
float,
col.geometry_data['size'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(
desc)
if col.geometry_type == 'cylinder':
desc = dict(classname='CylinderAffordanceItem',
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Radius=float(col.geometry_data['radius']),
Length=float(col.geometry_data['length']))
self.affordanceManager.newAffordanceFromDescription(
desc)
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, "submodels"):
if len(link.submodels) > 0:
print model.name + " - This is an articulated object - SKIPPING!"
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
if len(link.name) > 0 and link.name != model.name:
name += "-" + link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += "-" + col.name
if col.geometry_type == "mesh":
print "Mesh geometry is unsupported - SKIPPING!"
if col.geometry_type == "image":
print "image geometry is unsupported - SKIPPING!"
if col.geometry_type == "height_map":
print "Height map geometry is unsupported - SKIPPING!"
if col.geometry_type == "plane":
print "Plane geometry is unsupported - SKIPPING!"
if col.geometry_type == "sphere":
print "Sphere geometry is unsupported - SKIPPING!"
if col.geometry_type == "box":
desc = dict(
classname="BoxAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=[0.8, 0.8, 0.8],
Dimensions=map(float, col.geometry_data["size"].split(" ")),
)
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type == "cylinder":
desc = dict(
classname="CylinderAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=[0.8, 0.8, 0.8],
Radius=float(col.geometry_data["radius"]),
Length=float(col.geometry_data["length"]),
)
self.affordanceManager.newAffordanceFromDescription(desc)
def newSphere():
desc = dict(classname='SphereAffordanceItem',
Name='test sphere',
Radius=0.2,
uuid=newUUID(),
pose=((0.5, 0.0, 1.0), (1, 0, 0, 0)))
return affordanceFromDescription(desc)
def newBox():
desc = dict(classname='BoxAffordanceItem',
Name='test box',
Dimensions=[0.5, 0.2, 0.1],
uuid=newUUID(),
pose=((0.5, 0.0, 1.0), (1, 0, 0, 0)))
return affordanceFromDescription(desc)
def onSpawnBox(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='BoxAffordanceItem',
Name='box',
uuid=newUUID(),
pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnSphere(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname="SphereAffordanceItem",
Name="sphere",
uuid=newUUID(),
pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def sendEchoResponse(self, requestId=None):
if requestId is None:
requestId = newUUID()
msg = self._newCommandMessage('echo_response',
requestId=requestId,
descriptions=self.collection)
lcmUtils.publish(self.channel, msg)
def newCapsule():
desc = dict(classname='CapsuleAffordanceItem',
Name='test capsule',
Radius=0.05,
Length=0.5,
uuid=newUUID(),
pose=((0.5, 0.0, 1.0), (1, 0, 0, 0)))
return affordanceFromDescription(desc)
def onSpawnRing(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(
classname="CapsuleRingAffordanceItem",
Name="ring",
uuid=newUUID(),
pose=pose,
)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnCylinder(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(
classname="CylinderAffordanceItem",
Name="cylinder",
uuid=newUUID(),
pose=pose,
)
return self.affordanceManager.newAffordanceFromDescription(desc)
def __init__(self, channel):
self.collection = OrderedDict()
self.collectionId = newUUID()
self.sentCommands = set()
self.sentRequest = None
self.channel = channel
self.callbacks = callbacks.CallbackRegistry(
[self.DESCRIPTION_UPDATED_SIGNAL, self.DESCRIPTION_REMOVED_SIGNAL])
self.sub = None
self._modified()
def _newCommandMessage(self, commandName, **commandArgs):
commandId = newUUID()
self.sentCommands.add(commandId)
commandArgs['commandId'] = commandId
commandArgs['collectionId'] = self.collectionId
commandArgs['command'] = commandName
msg = lcmdrc.affordance_collection_t()
msg.name = numpyjsoncoder.encode(commandArgs)
msg.utime = getUtime()
return msg
def __init__(self, name, polyData, view):
PolyDataItem.__init__(self, name, polyData, view)
self.params = {}
self.addProperty('uuid', newUUID(), attributes=om.PropertyAttributes(hidden=True))
self.addProperty('Collision Enabled', True)
self.addProperty('Origin', [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], attributes=om.PropertyAttributes(hidden=True))
self.addProperty('Camera Texture Enabled', False)
self.properties.setPropertyIndex('Collision Enabled', 0)
self.setProperty('Icon', om.Icons.Hammer)
def _newCommandMessage(self, commandName, **commandArgs):
commandId = newUUID()
self.sentCommands.add(commandId)
commandArgs['commandId'] = commandId
commandArgs['collectionId'] = self.collectionId
commandArgs['command'] = commandName
msg = lcmbotcore.system_status_t()
msg.value = numpyjsoncoder.encode(commandArgs)
msg.utime = getUtime()
return msg
def __init__(self, name, polyData, view):
PolyDataItem.__init__(self, name, polyData, view)
self.params = {}
self.addProperty('uuid', newUUID(), attributes=om.PropertyAttributes(hidden=True))
self.addProperty('Collision Enabled', True)
self.addProperty('Origin', [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], attributes=om.PropertyAttributes(hidden=True))
self.addProperty('Camera Texture Enabled', False)
self.properties.setPropertyIndex('Collision Enabled', 0)
self.setProperty('Icon', om.Icons.Hammer)
def _newCommandMessage(self, commandName, **commandArgs):
commandId = newUUID()
self.sentCommands.add(commandId)
commandArgs['commandId'] = commandId
commandArgs['collectionId'] = self.collectionId
commandArgs['command'] = commandName
msg = lcmbotcore.system_status_t()
msg.value = numpyjsoncoder.encode(commandArgs)
msg.utime = getUtime()
return msg
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, 'submodels'):
if len(link.submodels)>0:
print model.name+' - This is an articulated object - SKIPPING!'
break
for col in link.collisions:
t1=transformUtils.getTransformFromNumpy(model.pose)
t2=transformUtils.getTransformFromNumpy(link.pose)
t3=transformUtils.getTransformFromNumpy(col.pose)
t=t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
color = col.color[0:3] if col.color is not None else [0.8,0.8,0.8]
if len(link.name)>0 and link.name != model.name:
name+='-'+link.name
if len(col.name)>0 and len(link.collisions)>1:
name+='-'+col.name
if col.geometry_type=='mesh':
desc = dict(classname='MeshAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Filename=col.geometry_data['uri'], Scale=map(float, col.geometry_data['scale'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type=='image':
print 'image geometry is unsupported - SKIPPING!'
if col.geometry_type=='height_map':
print 'Height map geometry is unsupported - SKIPPING!'
if col.geometry_type=='plane':
print 'Plane geometry is unsupported - SKIPPING!'
if col.geometry_type=='sphere':
print 'Sphere geometry is unsupported - SKIPPING!'
if col.geometry_type=='box':
desc = dict(classname='BoxAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Dimensions=map(float, col.geometry_data['size'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type=='cylinder':
desc = dict(classname='CylinderAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Radius=float(col.geometry_data['radius']), Length = float(col.geometry_data['length']))
self.affordanceManager.newAffordanceFromDescription(desc)
def __init__(self, channel):
self.collection = OrderedDict()
self.collectionId = newUUID()
self.sentCommands = set()
self.sentRequest = None
self.channel = channel
self.callbacks = callbacks.CallbackRegistry([self.DESCRIPTION_UPDATED_SIGNAL,
self.DESCRIPTION_REMOVED_SIGNAL])
self.sub = lcmUtils.addSubscriber(self.channel, messageClass=lcmdrc.affordance_collection_t, callback=self._onCommandMessage)
self.sub.setNotifyAllMessagesEnabled(True)
self._modified()
def newMesh():
d = DebugData()
d.addArrow((0, 0, 0), (0, 0, 0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem',
Name='test mesh',
Filename=meshId,
uuid=newUUID(),
pose=((0.5, 0.0, 1.0), (1, 0, 0, 0)))
return affordanceFromDescription(desc)
def __init__(self, channel):
self.collection = OrderedDict()
self.collectionId = newUUID()
self.sentCommands = set()
self.sentRequest = None
self.channel = channel
self.callbacks = callbacks.CallbackRegistry([self.DESCRIPTION_UPDATED_SIGNAL,
self.DESCRIPTION_REMOVED_SIGNAL])
self.sub = None
self._modified()
if USE_LCM:
self.sub = lcmUtils.addSubscriber(self.channel, messageClass=lcmdrc.affordance_collection_t, callback=self._onCommandMessage)
self.sub.setNotifyAllMessagesEnabled(True)
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0, 0, 0), (0, 0, 0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem',
Name='mesh',
Filename=meshId,
uuid=newUUID(),
pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def newCapsuleRing():
desc = dict(classname='CapsuleRingAffordanceItem', Name='test capsule ring', uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
def newCapsule():
desc = dict(classname='CapsuleAffordanceItem', Name='test capsule', Radius=0.05, Length=0.5, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
def newSphere():
desc = dict(classname='SphereAffordanceItem', Name='test sphere', Radius=0.2, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
def newBox():
desc = dict(classname='BoxAffordanceItem', Name='test box', Dimensions=[0.5, 0.2, 0.1], uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
def onSpawnCylinder(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CylinderAffordanceItem', Name='cylinder', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnRing(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CapsuleRingAffordanceItem', Name='ring', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def add(self, polyData, publish=True):
meshId = newUUID()
self.meshes[meshId] = polyData
if publish and self.collection:
self._publishMesh(meshId)
return meshId
def sendEchoRequest(self):
self.sentRequest = newUUID()
msg = self._newCommandMessage('echo_request', requestId=self.sentRequest)
lcmUtils.publish(self.channel, msg)
def add(self, polyData, publish=True):
meshId = newUUID()
self.meshes[meshId] = polyData
if publish:
self.collection.updateDescription(dict(uuid=meshId, data=geometryencoder.encodePolyData(polyData)), notify=False)
return meshId
def newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
def sendEchoResponse(self, requestId=None):
if requestId is None:
requestId = newUUID()
msg = self._newCommandMessage('echo_response', requestId=requestId, descriptions=self.collection)
lcmUtils.publish(self.channel, msg)
def newCapsuleRing():
desc = dict(classname='CapsuleRingAffordanceItem',
Name='test capsule ring',
uuid=newUUID(),
pose=((0.5, 0.0, 1.0), (1, 0, 0, 0)))
return affordanceFromDescription(desc)
def sendEchoRequest(self):
self.sentRequest = newUUID()
msg = self._newCommandMessage('echo_request',
requestId=self.sentRequest)
lcmUtils.publish(self.channel, msg)
def __init__(self):
self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
desc = dict(classname='MeshAffordanceItem', Name='polaris',
Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
self.originFrame = self.pointcloudAffordance.getChildFrame()
self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]),
np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why
# t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304]))
t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]),
np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602]))
self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]),
np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636]))
self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]),
np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412]))
self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance)
# pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location
# pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location
pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation
desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose,
Color=[1, 0, 0], Radius=float(0.18), Segments=20)
self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])]
desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0])
self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
# t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]),
# np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996]))
# t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]),
# np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626]))
t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]),
np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963]))
self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]),
np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721]))
self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance)
# t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
# np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
# t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]),
# np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ]))
# t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]),
# np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581]))
# self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]),
np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633]))
self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]),
np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963]))
self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]),
np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01,
8.85015882e-01]))
self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def newAffordanceFromDescription(self, desc):
if 'uuid' not in desc:
desc['uuid'] = newUUID()
self.collection.updateDescription(desc)
return self.getAffordanceById(desc['uuid'])
def newAffordanceFromDescription(self, desc):
if 'uuid' not in desc:
desc['uuid'] = newUUID()
self.collection.updateDescription(desc)
return self.getAffordanceById(desc['uuid'])
def add(self, polyData, publish=True):
meshId = newUUID()
self.meshes[meshId] = polyData
if publish and self.collection:
self._publishMesh(meshId)
return meshId
