def set_in_bin_xform(self,shelf_xform,ux,uy,theta):
"""A utility convenience function for setting up virtual shelves.
Sets self.xform assuming the object is resting in the bin
with its center's translational parameters (ux,uy) in the
range [0,1]x[0,1] and orientation theta about the z axis.
ux goes from left to right and uy goes from front to back."""
global bin_bounds
bmin,bmax = bin_bounds[self.bin_name]
tlocal = [bmin[0]+ux*(bmax[0]-bmin[0]),bmin[1]-self.info.bmin[2],bmax[2]+uy*(bmin[2]-bmax[2])]
Rlocal = so3.from_axis_angle(([0,1,0],theta))
toShelfCoords = (so3.from_axis_angle(([1,0,0],math.pi/2)),[0,0,0])
self.xform = se3.mul(shelf_xform,se3.mul((Rlocal,tlocal),toShelfCoords))
def display(self): if self.startFrame != self.endFrame: idx = int(self.ttotal/0.01) + self.startFrame else: idx = self.startFrame #TODO: plot marker of single frame glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glPointSize(5) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) T = so3.from_axis_angle(([0,0,1], math.pi)) count = 0 for i in range(0, len(self.markers)): marker = self.markers[i] if self.check(marker.name): if marker.isCompleteFrame(idx): count += 1 if count == self.numMarkers: print 'completeIdx: ', idx glColor3f(0,1,1) pos = marker.getPosAtFrame(idx)#so3.apply(T, marker.getPosAtFrame(idx)) glVertex3fv(pos) glEnd() glEnable(GL_DEPTH_TEST)
'bin_E' : ([-0.149,1.32,0],[0.149,1.52,0.42]),
'bin_F' : ([0.158,1.32,0],[0.41,1.52,0.42]),
'bin_G' : ([-0.41,1.09,0],[-0.158,1.29,0.42]),
'bin_H' : ([-0.149,1.09,0],[0.149,1.29,0.42]),
'bin_I' : ([0.158,1.09,0],[0.41,1.29,0.42]),
'bin_J' : ([-0.41,0.825,0],[-0.158,1.06,0.42]),
'bin_K' : ([-0.149,0.825,0],[0.149,1.06,0.42]),
'bin_L' : ([0.158,0.825,0],[0.41,1.06,0.42]),
}
# rotation matrices
# NOTE: XtoZ ~ Zto_Y are flipped?
XtoY = so3.from_axis_angle(([0,0,1],math.pi*0.5))
Xto_Y = so3.from_axis_angle(([0,0,1],-math.pi*0.5))
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
class ItemInfo:
"""Constant information about an object.
Members include:
- name: the identifier for this item
- mass: the item's mass
left_arm_geometry_indices = [15,16,17,18,19,21,22]
right_arm_geometry_indices = [35,36,37,38,39,41,42]
# left_hand_geometry_indices = [54,55,56,57]
left_hand_geometry_indices = [54]
right_hand_geometry_indices = [59,60]
#indices of the left and right arms in the Baxter robot file
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
# gripper
# left_gripper_center_xform = (so3.from_axis_angle(((0,0,1),math.pi*0.5)),[0,0.0,0.11])
right_gripper_center_xform = (so3.from_axis_angle(((0,0,1),math.pi*0.5)),[0,0.0,0.11])
#spatula
left_gripper_center_xform = (so3.from_axis_angle(((0,0,1),math.pi*0.5)),[0.025,0,0.31])
def set_model_gripper_command(robot,limb,command,spatulaPart = None):
"""Given the Baxter RobotModel 'robot' at its current configuration,
this will set the configuration so the gripper on limb 'limb' is
placed at the gripper command values 'command'.
Currently handles rethink parallel jaw gripper commands, range [0] (closed)
to [1] (open).
"""
value = command[0]
if limb=='left':
toShelfCoords = (so3.from_axis_angle(([1,0,0],math.pi/2)),[0,0,0])
self.xform = se3.mul(shelf_xform,se3.mul((Rlocal,tlocal),toShelfCoords))
#set up some items and some potential grasps
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
YZflip_90 = so3.from_axis_angle(([1,0,0],math.pi*0.5))
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,0])
tall_item.grasps.append(x)
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,-0.02]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,-0.02])
tall_item.grasps.append(x)
toShelfCoords = (so3.from_axis_angle(([1,0,0],math.pi/2)),[0,0,0])
self.xform = se3.mul(shelf_xform,se3.mul((Rlocal,tlocal),toShelfCoords))
#set up some items and some potential grasps
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,0.02])))
def load_fake_items():
"""Loads the fake items from the homework assignments"""
global item_info
#set up some items and some potential grasps
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
tall_item.geometryFile = 'box'
XtoY = so3.from_axis_angle(([0,0,1],math.pi*0.5))
Xto_Y = so3.from_axis_angle(([0,0,1],-math.pi*0.5))
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.04])))
small_item = ItemInfo('small_item')
small_item.bmin = [-0.01,-0.04,-0.01]
small_item.bmax = [0.01,0.04,0.01]
small_item.geometryFile = 'box'
small_item.grasps.append(ItemGrasp((so3.identity(),[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((so3.identity(),[0,0.0,0])))
small_item.grasps.append(ItemGrasp((so3.identity(),[0,0.02,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,0.02,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,0.02,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,0.02,0])))
med_item = ItemInfo('med_item')
med_item.bmin = [-0.03,-0.03,-0.03]
med_item.bmax = [0.03,0.03,0.03]
med_item.geometryFile = 'box'
#med_item.grasps.append(ItemGrasp((so3.identity(),[0,0,0])))
med_item.grasps.append(ItemGrasp((so3.mul(XtoY,ZtoY),[0,0,0])))
#med_item.grasps.append(ItemGrasp((Xto_Y,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XYflip,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XtoZ,[0,0,0])))
#med_item.grasps.append(ItemGrasp((Xto_Z,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XZflip,[0,0,0])))
#med_item.grasps.append(ItemGrasp((ZtoY,[0,0,0])))
#med_item.grasps.append(ItemGrasp((Zto_Y,[0,0,0])))
#med_item.grasps.append(ItemGrasp((YZflip,[0,0,0])))
#gripper local rotational axis is x, perturb +/- along that axis
for g in med_item.grasps[:]:
angle = 15.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],angle))),g.grasp_xform[1])))
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
angle = 30.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],angle))),g.grasp_xform[1])))
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
angle = 45.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
#flip 180 about z axis
for g in med_item.grasps[:]:
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],XYflip),g.grasp_xform[1])))
item_info = dict((i.name,i) for i in [tall_item, small_item, med_item])
elif b[4] in 'CFIL':
x = -0.3
#if b[4] == 'H':
# y += 0.05
y += 0.4
if b[4] in 'ABC':
z += 1.50
elif b[4] in 'DEF':
#z += 1.50 - 6*0.0254
z += 1.50 - 9*0.0254
elif b[4] in 'GHI':
z += 1.50 - 18*0.0254
elif b[4] in 'JKL':
z += 1.50 - 29*0.0254
rdowntilt = downtilt*pi / 180
rhighdowntilt = (downtilt+10)*pi/180.0
vps[b+'_center'] = ( so3.mul(so3.from_axis_angle(([-1,0,0],-pi/2-rdowntilt)),
so3.from_axis_angle(([0,0,1],pi))),
[ x, y, z ])
vps[b+'_high'] = ( so3.mul(so3.from_axis_angle(([-1,0,0],-pi/2-rhighdowntilt)),
so3.from_axis_angle(([0,0,1],pi))),
[ x, y, z + 0.08 ])
print b, vps[b+'_center'][1], vps[b+'_high'][1]
json.dump(vps, open('vantage_point_xforms.json', 'w'), indent=4)
