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 onSpawnSphere(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='SphereAffordanceItem',
Name='sphere',
uuid=newUUID(),
pose=pose)
return self.affordanceManager.newAffordanceFromDescription(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 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 spawnValveAffordance(self):
radius = 0.10
tubeRadius = 0.02
position = [0, 0, 1.2]
rpy = [0, 0, 0]
t_feet_mid = self.footstepPlanner.getFeetMidPoint(self.robotModel)
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t_grasp = self.computeRelativeGraspTransform()
t_grasp.Concatenate(t)
t_stance = self.computeRobotStanceFrame(t_grasp, self.computeRelativeStanceTransform())
t_valve = t_stance.GetInverse()
# This is necessary to get the inversion to actually happen. We don't know why.
t_valve.GetMatrix()
t_valve.Concatenate(t)
t_valve.Concatenate(t_feet_mid)
pose = transformUtils.poseFromTransform(t_valve)
desc = dict(classname='CapsuleRingAffordanceItem', Name='valve', uuid=newUUID(), pose=pose,
Color=[0, 1, 0], Radius=float(radius), Segments=20)
desc['Tube Radius'] = tubeRadius
import affordancepanel
obj = affordancepanel.panel.affordanceFromDescription(desc)
obj.params = dict(radius=radius)
def spawnValveAffordance(self):
radius = 0.10
tubeRadius = 0.02
position = [0, 0, 1.2]
rpy = [0, 0, 0]
t_feet_mid = self.footstepPlanner.getFeetMidPoint(self.robotModel)
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t_grasp = self.computeRelativeGraspTransform()
t_grasp.Concatenate(t)
t_stance = self.computeRobotStanceFrame(
t_grasp, self.computeRelativeStanceTransform())
t_valve = t_stance.GetInverse()
# This is necessary to get the inversion to actually happen. We don't know why.
t_valve.GetMatrix()
t_valve.Concatenate(t)
t_valve.Concatenate(t_feet_mid)
pose = transformUtils.poseFromTransform(t_valve)
desc = dict(classname='CapsuleRingAffordanceItem',
Name='valve',
uuid=newUUID(),
pose=pose,
Color=[0, 1, 0],
Radius=float(radius),
Segments=20)
desc['Tube Radius'] = tubeRadius
import affordancepanel
obj = affordancepanel.panel.affordanceFromDescription(desc)
obj.params = dict(radius=radius)
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 _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 __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 __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 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 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 onSpawnRing(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CapsuleRingAffordanceItem', Name='ring', uuid=newUUID(), pose=pose)
return self.affordanceFromDescription(desc)
def onSpawnBox(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='BoxAffordanceItem', Name='box', uuid=newUUID(), pose=pose)
return self.affordanceFromDescription(desc)
def onSpawnSphere(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='SphereAffordanceItem', Name='sphere', uuid=newUUID(), pose=pose)
return self.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 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 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 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 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 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 createCollisionPlanningScene(self, scene=1, moveRobot=False, loadPerception=False):
if (loadPerception):
#filename = os.path.expanduser('~/drc-testing-data/collision_scene/collision_scene.vtp')
#polyData = ioUtils.readPolyData( filename )
pd = io.readPolyData('/home/mfallon/Desktop/rrt_scene/all.vtp')
vis.showPolyData(pd,'scene')
if (scene == 0):
pose = (array([ 1.20, 0. , 0.8]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.20, 0.5 , 0.4]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-leg1', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,0.05,0.8])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.20, -0.5 , 0.4]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-leg2', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,0.05,0.8])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.05, 0.3 , 0.98]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-object1', uuid=newUUID(), pose=pose, Color=[0.9, 0.9, 0.1], Dimensions=[0.08,0.08,0.24])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.25, 0.1 , 0.98]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-object2', uuid=newUUID(), pose=pose, Color=[0.0, 0.9, 0.0], Dimensions=[0.07,0.07,0.25])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.25, -0.1 , 0.95]), array([ 1., 0., 0., 0.]))
desc = dict(classname='CylinderAffordanceItem', Name='scene0-object3', uuid=newUUID(), pose=pose, Color=[0.0, 0.9, 0.0], Radius=0.035, Length = 0.22)
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.05, -0.2 , 0.95]), array([ 1., 0., 0., 0.]))
desc = dict(classname='CylinderAffordanceItem', Name='scene0-object4', uuid=newUUID(), pose=pose, Color=[0.9, 0.1, 0.1], Radius=0.045, Length = 0.22)
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[0] = 0.67
self.sensorJointController.push()
elif (scene == 1):
pose = (array([-0.69, -1.50, 0.92]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-1.05, -1.10, 0.95]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge1', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.1,0.3,0.05])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.35, -1.10, 0.95]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge2', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.1,0.3,0.05])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.6803156 , -1.1826616 , 1.31299839]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge3', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.14,1.0,0.07])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ -0.7, -1.5 , 1.03]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-object1', uuid=newUUID(), pose=pose, Color=[0.9, 0.9, 0.1], Dimensions=[0.05,0.05,0.14])
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[5] = -1.571
self.sensorJointController.q[0] = -0.75
self.sensorJointController.q[1] = -0.85
self.sensorJointController.push()
elif (scene == 2):
pose = (array([-0.98873106, 1.50393395, 0.91420001]), array([ 0.49752312, 0. , 0. , 0.86745072]))
desc = dict(classname='BoxAffordanceItem', Name='scene2-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.98873106, 1.50393395, 0.57]), array([ 0.49752312, 0. , 0. , 0.86745072]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-object1', uuid=newUUID(), pose=pose, Color=[0.005, 0.005, 0.3], Dimensions=[0.05,0.05,0.14])
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[0] = -0.6
self.sensorJointController.q[1] = 1.1
self.sensorJointController.q[5] = 2.1
self.sensorJointController.push()
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 onSpawnRing(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CapsuleRingAffordanceItem', Name='ring', 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 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 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 onSpawnCylinder(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CylinderAffordanceItem', Name='cylinder', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
