Example #1
0
    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))
Example #2
0
	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)
Example #3
0
    '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
Example #4
0
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':
Example #5
0
        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)
Example #6
0
        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])))
Example #7
0
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)