def __init__(self,world,name="My GL Widget Program"):
GLRealtimeProgram.__init__(self,name)
self.world = world
self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
self.drivePos = [0,0,0]
self.driveRot = so3.identity()
self.driveArm = 'l'
self.useRotation = True
self.localMotion = True
self.increment = 0.05
self.incrementang = 0.1
self.trace_sensed = {'l':[],
'r':[]}
self.baseCommand = [0,0,0]
self.increment = 0.01
self.incrementang = 0.02
print "Moving to neutral configuration"
q = motion.robot.left_limb.commandedPosition()
q[1] = -1.0;
q[3] = 2.0;
q[5] = 1.0;
motion.robot.left_mq.appendLinearRamp(q)
q = motion.robot.right_limb.commandedPosition()
q[1] = -1.0;
q[3] = 2.0;
q[5] = 1.0;
motion.robot.right_mq.setRamp(q)
motion.robot.left_ee.setOffset([0,0,0.1])
motion.robot.right_ee.setOffset([0,0,0.1])
self.state = 'move to start'
def spawn_objects(world):
"""For all ground_truth_items, spawns RigidObjects in the world
according to their sizes / mass properties"""
print "Initializing world objects"
obj = world.makeRigidObject('object1')
bmin = [0,0,0]
bmax = [0.08,0.08,0.3]
mass = 0.001
m = obj.getMass()
m.setMass(mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
c.kFriction = 10
c.kRestitution = 0.1;
c.kStiffness = 100
c.kDamping = 10
obj.setContactParameters(c)
load_item_geometry(bmin,bmax,obj.geometry())
obj.setTransform(so3.identity(),[0,-0.2,0.155])
obj.geometry().setCollisionMargin(0)
return obj
def __init__(self, world, name="My GL Widget Program"):
GLRealtimeProgram.__init__(self, name)
self.world = world
self.world.robot(0).setConfig(
motion.robot.getKlamptCommandedPosition())
self.drivePos = [0, 0, 0]
self.driveRot = so3.identity()
self.driveArm = 'l'
self.useRotation = True
self.localMotion = True
self.increment = 0.05
self.incrementang = 0.1
self.trace_sensed = {'l': [], 'r': []}
self.baseCommand = [0, 0, 0]
self.increment = 0.01
self.incrementang = 0.02
print "Moving to neutral configuration"
q = motion.robot.left_limb.commandedPosition()
q[1] = -1.0
q[3] = 2.0
q[5] = 1.0
motion.robot.left_mq.appendLinearRamp(q)
q = motion.robot.right_limb.commandedPosition()
q[1] = -1.0
q[3] = 2.0
q[5] = 1.0
motion.robot.enableCollisionChecking(True)
motion.robot.right_mq.setRamp(q)
motion.robot.left_ee.setOffset([0, 0, 0.1])
motion.robot.right_ee.setOffset([0, 0, 0.1])
self.state = 'move to start'
def test(self):
#testing
xform = self.robot.link(6).getTransform()
R_wrist = so3.identity()
t_wrist = xform[1]
# Setup ik objectives for both arms
goal = ik.objective(self.robot.link(6),R=R_wrist,t=t_wrist)
q = self.get_ik_solutions(goal)
return q
def localizeOrderBin(self):
order_bin_xform = (so3.identity(),[0.57,0,-0.87])
resource.setDirectory("resources/")
return resource.get("calibrated_order_bin.xform",type="RigidTransform",default=order_bin_xform,doedit=False)
T = numpy.array([[ 1, 0, 0, 0.57 ],
[ 0, 1, 0, 0 ],
[ 0, 0, 1, -0.87 ],
[ 0, 0, 0, 1 ]])
return (list(T[:3, :3].T.flat), list(T[:3, 3].flat))
def keyboardfunc(self,c,x,y):
c = c.lower()
if c=='z':
self.simulate = not self.simulate
print "Simulating:",self.simulate
elif c=='`':
print "Testing:"
# self.simworld.rigidObject(0).geometry().transform(so3.identity(), [0,0,0.21])
print self.simworld.rigidObject(0).getTransform()
# self.simworld.rigidObject(0).setTransform(so3.identity(), [0,0,1])
self.simworld.rigidObject(0).setTransform(so3.identity(), [0,0,0.21])
print self.simworld.rigidObject(0).getTransform()
else:
self.command_queue.put(c)
if c=='q':
self.picking_thread.join()
exit(0)
glutPostRedisplay()
def __init__(self,world,name="My GL Widget Program"):
GLRealtimeProgram.__init__(self,name)
self.world = world
self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
self.drivePos = [0,0,0]
self.driveRot = so3.identity()
self.driveArm = 'l'
self.useRotation = True
self.localMotion = True
self.increment = 0.05
self.incrementang = 0.1
self.trace_sensed = {'l':[],
'r':[]}
self.baseCommand = [0,0,0]
self.increment = 0.01
self.incrementang = 0.02
LEFT_CONFIG = None#[-1.145883647241211, -0.6657476611816406, 0.794602047216797, 1.0722525695068361, -0.5284563808227539, -1.5719468105895997, 2.829811055218506]
RIGHT_CONFIG = None#[0.8624806970031739, -0.7213544646789551, -0.700262228869629, 0.4651796733947754, 0.585213669909668, 1.9167089922729494, -0.20555342534179688]
print "Moving to neutral configuration"
q = motion.robot.left_limb.commandedPosition()
if(not LEFT_CONFIG is None ):
print len(LEFT_CONFIG)
if len(LEFT_CONFIG) < 8:
for i in range(len(LEFT_CONFIG)):
q[i] = LEFT_CONFIG[i]
motion.robot.left_mq.appendLinearRamp(q)
q = motion.robot.right_limb.commandedPosition()
if(not RIGHT_CONFIG is None) :
print len(RIGHT_CONFIG)
if len(RIGHT_CONFIG) < 8:
for i in range(len(RIGHT_CONFIG)):
q[i] = RIGHT_CONFIG[i]
motion.robot.right_mq.setRamp(q)
motion.robot.left_ee.setOffset([0,0,0.1])
motion.robot.right_ee.setOffset([0,0,0.1])
self.state = 'move to start'
def load_item_geometry(bmin,bmax,geometry_ptr = None):
"""Loads the geometry of the given item and returns it. If geometry_ptr
is provided, then it is assumed to be a Geometry3D object and the object
geometry is loaded into it."""
if geometry_ptr == None:
geometry_ptr = Geometry3D()
# fn = model_dir + "cylinder.tri"
fn = model_dir + "cube.tri"
# fn = model_dir + "/objects/teapot/pots.tri"
# bmin = [0,0,0]
# bmax = [1.2,1.5,1.25]
# fn = model_dir + "items/rollodex_mesh_collection_jumbo_pencil_cup/textured_meshes/optimized_poisson_textured_mesh.ply"
if not geometry_ptr.loadFile(fn):
print "Error loading cube file",fn
exit(1)
center = vectorops.mul(vectorops.add(bmin,bmax),0.5)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
geometry_ptr.transform(so3.mul(scale,so3.rotation([0,0,1],math.pi/180*0)),translate)
geometry_ptr.setCurrentTransform(so3.identity(),[0,0,0])
return geometry_ptr
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]
def collides(point):
test_geom.setCurrentTransform(so3.identity(), point)
return test_geom.withinDistance(shelf_model.geometry(), 0.01)
def display(self):
if self.knowledge_base:
# update the world
if self.knowledge_base.shelf_xform:
# load the shelf once a transform is available
if not self.shelf:
self.shelf = self.world.loadRigidObject("klampt_models/north_shelf/shelf_with_bins.obj")
logger.info("spawned shelf model")
self.shelf.setTransform(*self.knowledge_base.shelf_xform)
if self.knowledge_base.order_bin_xform:
# load the order bin once a transform is available
if not self.order_bin:
self.order_bin = self.world.loadRigidObject("klampt_models/apc_bin/apc_bin.obj")
logger.info("spawned order bin model")
self.order_bin.setTransform(*self.knowledge_base.order_bin_xform)
# spawn/update objects
for (name, xform) in self.knowledge_base.object_xforms.items():
# load the object once a transform is availale
if name not in self.objects:
body = self.world.loadRigidObject("klampt_models/items/{0}/{0}.obj".format(name))
logger.info("spawned {} model".format(name))
# load the point cloud
if name in self.knowledge_base.object_clouds:
self.n += 1
display_list = glGenLists(self.n)
# compile the display list
glNewList(display_list, GL_COMPILE)
glDisable(GL_LIGHTING)
glBegin(GL_POINTS)
points = self.knowledge_base.object_clouds[name]
for point in points:
if len(point) == 2:
xyz = point[0]
rgb = point[1]
else:
xyz = point[:3]
if len(point) == 4:
rgb = point[3]
elif len(point) > 3:
rgb = point[3:6]
else:
rgb = None
if rgb is not None:
glColor3f(*map(lambda x: x / 255.0, rgb))
else:
glColor3f(*colors[i % len(colors)])
glVertex3f(*xyz[:3])
glEnd()
glEndList()
logging.debug("compiled {} points for {}".format(len(points), name))
else:
display_list = None
self.objects[name] = {"body": body, "display_list": display_list}
# self.objects[name]['body'].setTransform(*xform)
# delete objects
for (name, props) in self.objects.items():
if name not in self.knowledge_base.object_xforms:
# remove the object
# XXX: cannot actually delete object... so move it far away... very far away
props["body"].setTransform(so3.identity(), [1e3, 1e3, 1e3])
del self.objects[name]
# update the robot state
if self.robot_state:
try:
self.robot.setConfig(self.robot_state.sensed_config)
except TypeError as e:
logger.error("error visualizing config: {}".format(self.robot_state.sensed_config))
logger.error(traceback.format_exc())
sys.exit(-1)
self.world.drawGL()
# draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
if self.robot_state:
qi = self.robot.getConfig()
self.robot.setConfig(self.robot_state.commanded_config)
self.robot.drawGL(False)
self.robot.setConfig(qi)
glDisable(GL_BLEND)
# draw the point clouds
glPointSize(2)
for (name, props) in self.objects.items():
if "display_list" in props:
glCallList(props["display_list"])
# draw gripper link transforms
for name in ["left_gripper", "right_gripper"]:
gldraw.xform_widget(self.robot.getLink(name).getTransform(), 0.1, 0.01)
# draw axis-aligned axes for reference
gldraw.xform_widget((so3.identity(), [0, 0, 1]), 0.1, 0.01)
# draw local origins of objects
if self.knowledge_base:
for xform in self.knowledge_base.object_xforms.values():
gldraw.xform_widget(xform, 0.1, 0.01)
def localizeSpecificObject(self, bin, object):
SHELF_DIMS_PATH = os.path.join('kb', 'shelf_dims.json')
SHELF_CLOUD_PATH = os.path.join('perception', 'segmentation', 'models', 'shelf_thin_10000.npy')
# acquire the camera image
address = { 'left': '192.168.0.103', 'right': '192.168.0.104' }[self.knowledge_base.active_limb]
#address = { 'left': '192.168.0.103', 'right': '192.168.0.103' }[self.knowledge_base.active_limb]
camera = RemoteCamera(address, xform=self.camera_xform)
camera.read()
camera.close()
# load the shelf subtraction data
bin_bounds = json.load(open(SHELF_DIMS_PATH))[bin]
# load and transform the shelf point cloud
shelf_cloud = numpy.load(open(SHELF_CLOUD_PATH))
shelf_cloud = shelf_cloud.dot(numpy.array(self.knowledge_base.shelf_xform[0]).reshape((3,3))) + self.knowledge_base.shelf_xform[1]
# load and transform the shelf model
world = WorldModel()
shelf = world.loadRigidObject(os.path.join('klampt_models', 'north_shelf', 'shelf_with_bins.obj'))
shelf.setTransform(*self.knowledge_base.shelf_xform)
# clean up the point cloud
(clean_mask, clean_cloud) = camera.clean()
_, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000)
object_mask = numpy.zeros(camera.cloud.shape[:2], dtype=numpy.bool)
object_mask[clean_mask] = clean_object_mask
#clean_cloud_aligned_color = map(lambda x: x[0] + [ x[1] ], zip(clean_cloud_aligned[clean_object_mask].tolist(), camera.colorize()[clean_mask][clean_object_mask].tolist()))
#debug_cloud([ clean_cloud_aligned_color ], [ self.base_xform, self.camera_xform ])
#debug_cloud([ shelf_cloud, clean_cloud, clean_cloud_aligned ])
#pcd.write(clean_cloud_aligned_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
(target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object)
bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else []
# perform special handling
if target_object == 'rollodex_mesh_collection_jumbo_pencil_cup':
logger.info('running specialized rollodex cups detection')
return self._localizeRollodexCups(world, camera.color, clean_cloud_aligned, camera.depth_uv, clean_mask, object_mask)
# dilate the object mask
# from scipy.misc import imsave
from scipy.ndimage.morphology import binary_dilation
object_mask_dilated = binary_dilation(object_mask, iterations=2)
# imsave('/tmp/test.png', object_mask)
# label connected components
(object_labeled, label_count) = distance_label(camera.cloud, object_mask_dilated, threshold=0.01)
logger.info('found {} connected components'.format(label_count))
#KH: added to help debugging without changing order bin contents
if object not in bin_contents:
logger.warning("Warning, trying to detect object "+object+" not in bin contents, adding a temporary entry")
bin_contents = bin_contents + [object]
bin_contents_with_shelf = bin_contents + [ 'shelf' ]
if not label_count:
logger.warning('found no connected components')
mask = object_mask
bin_contents = self.knowledge_base.bin_contents[self.knowledge_base.target_bin]
confusion_row = dict(zip(bin_contents, [0]*len(bin_contents)))
else:
object_blobs = sorted([ object_labeled == (l+1) for l in range(label_count) ], key=lambda b: -numpy.count_nonzero(b))
#for (i, blob) in enumerate(object_blobs):
# logger.debug('blob {}: {} points'.format(i, numpy.count_nonzero(blob)))
# filter out blobs with fewer than 1000 points
min_point_count = 1000
object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= min_point_count, object_blobs)
#debug_cloud([ clean_cloud_aligned[(object_mask & blob_mask)[clean_mask]] for blob_mask in object_blobs ], [ self.base_xform, self.camera_xform ], wait=False)
known_object_count = len(bin_contents)
if known_object_count == 0:
# return all large blobs
n = len(object_blobs)
# take only the largest n blobs
blob_mask = reduce(numpy.bitwise_or, object_blobs[:n])
mask = object_mask & blob_mask
logger.warning('no objects in bin... -> returning all blobs')
else:
# load the matching statistics
match_stats = json.load(open(os.path.join('perception', 'segmentation', 'match_stats.json')))
# load the object hue histograms
known_histograms = {}
for obj in bin_contents_with_shelf:
#array = numpy.load(os.path.join('perception', 'hue_array', '{}.npz'.format(obj)))['arr_0']
#known_histograms[obj] = numpy.histogram(array, bins=range(0,181), density=True)[0].reshape(-1,1).astype(numpy.float32)
known_histograms[obj] = numpy.load(os.path.join('perception', 'uv_hist2', '{}.npz'.format(obj)))['arr_0']
# compute the color validity mask
color_valid_mask = ~invalid_mask(camera.depth_uv)
#KH addition 5/23: test to see whether blobs should be broken up
if len(object_blobs)==1 and len(known_histograms) > 2:
logger.info("Trying KMeans segmentation to break up large blob")
blob_mask = reduce(numpy.bitwise_or, object_blobs)
mask = object_mask & blob_mask & color_valid_mask
labels,k,score = xyzrgb_segment_kmeans(camera.cloud[mask],camera.colorize()[mask],known_histograms.values())
labels = numpy.array(labels)
object_blobs = []
for i in xrange(len(known_histograms)):
blank = numpy.zeros_like(object_mask,dtype=numpy.bool)
blank[mask] = (labels==i)
object_blobs.append(blank)
# score each blob for each object in the bin
blob_scores = []
blob_accepts = []
blob_rejects = []
matched_blobs = {}
confusion_matrix = []
for (i, blob) in enumerate(object_blobs):
# compute the blob histogram
blob_uv = [ rgb2yuv(*rgb)[1:3] for rgb in camera.colorize()[object_mask & blob & color_valid_mask] ]
blob_histogram = make_uv_hist(blob_uv)
# compare the blob to each possible object
scores = dict([ (obj, 2 * numpy.minimum(blob_histogram, histogram).sum() - numpy.maximum(blob_histogram, histogram).sum()) for (obj, histogram) in known_histograms.items() ])
logger.debug('blob {}:'.format(i))
blob_scores.append(scores)
logger.debug(' scores: {}'.format([ '{}={}'.format(*x) for x in scores.items() ]))
# apply the cutoff to each score and associate with positive confidence
accepts = dict([ (obj, match_stats[obj]['ppv']) for obj in bin_contents_with_shelf if scores[obj] >= match_stats[obj]['cutoff'] ])
blob_accepts.append(accepts)
logger.debug(' accepts: {}'.format([ '{}={}'.format(*x) for x in accepts.items() ]))
# apply the cutoff to each score and associate with negative confidence
rejects = dict([ (obj, match_stats[obj]['npv']) for obj in bin_contents_with_shelf if scores[obj] < match_stats[obj]['cutoff'] ])
blob_rejects.append(rejects)
logger.debug(' rejects: {}'.format([ '{}={}'.format(*x) for x in rejects.items() ]))
# populate the confusion matrix
shelf_probability = 0.1
S = lambda o: match_stats[o]['specificity']
iS = lambda o: 1 - S(o)
total_probability = sum(map(S, accepts)) + sum(map(iS, rejects)) + shelf_probability
confusion_matrix.append([ [ iS(o), S(o) ][ o in accepts ] / total_probability for o in bin_contents ])
# resolve multiple assignments
if len(accepts) > 1:
# choose the object with the highest matched confidence
best_match_object = sorted(accepts.items(), key=lambda a: -a[1])[0][0]
# remove all other objects
for (obj, confidence) in accepts.items():
if obj != best_match_object:
del accepts[obj]
logger.warn('blob {} multiple accept ignored: {}={}'.format(i, obj, confidence))
if len(accepts) == 1:
# a single match so record it
matched_blobs.setdefault(accepts.keys()[0], []).append(i)
logger.info('blob {} assigned to {} by single match'.format(i, accepts.keys()[0]))
confusion_matrix = numpy.array(confusion_matrix)
logger.debug('confusion matrix:\n{}'.format(numpy.array(confusion_matrix)))
# assign blobs by least threshold difference until all objects are assigned (excluding shelf) or all blobs are assigned
while len([ k for k in matched_blobs.keys() if k != 'shelf']) < len(bin_contents) and len(sum(matched_blobs.values(), [])) < len(object_blobs):
best_match_objects = []
for (i, scores) in enumerate(blob_scores):
# only consider unmatched blobs
if i not in sum(matched_blobs.values(), []):
threshold_differences = [ [obj, match_stats[obj]['cutoff'] - score] for (obj, score) in scores.items() ]
best_match_objects.append([ i ] + sorted(threshold_differences, key=lambda d: d[1])[0])
# choose the least threshold difference across all blobs and objects
best_blob, best_object, _ = sorted(best_match_objects, key=lambda b: b[2])[0]
matched_blobs.setdefault(best_object, []).append(best_blob)
logger.warn('blob {} assigned to {} by least threshold difference'.format(best_blob, best_object))
# report unmatched blobs and objects (excluding shelf)
for obj in bin_contents:
if obj not in matched_blobs:
logger.warn('no blobs matched {}'.format(obj))
for i in range(len(object_blobs)):
if i not in sum(matched_blobs.values(), []):
logger.warn('blob {} is unassigned'.format(i))
for obj in bin_contents_with_shelf:
if obj in matched_blobs:
print obj
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[obj] ], numpy.zeros_like(object_mask))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# check that the target object was found
if target_object not in matched_blobs:
logger.error('no blobs assigned to target object')
return None
# return blob for the selected object
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[target_object] ], numpy.zeros_like(object_mask))
# return the confusion matrix rows for the selected object
confusion_rows = confusion_matrix[matched_blobs[target_object], :]
confusion_row = dict([ (obj, confusion_rows[:, i].mean()) for (i, obj) in enumerate(bin_contents) ])
logger.info('confusion row for {}: {}'.format(target_object, confusion_row))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
#xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
xform = se3.identity()
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# downsample the cloud to about 1000 points
ratio = int(len(object_cloud) / 1000)
if ratio > 1:
object_cloud_color = object_cloud_color[::ratio]
pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
return (xform, object_cloud_color, confusion_row)
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model "+fn)
coordinates.setWorldModel(world)
#add the world to the visualizer
visualization.add("world",world)
#add the coordinate Manager to the visualizer
visualization.add("coordinates",coordinates.manager())
#test a point
pt = [2,5,1]
visualization.add("some point",pt)
#test a rigid transform
visualization.add("some blinking transform",[so3.identity(),[1,3,0.5]])
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks()-1)
#point constraint
#obj = ik.objective(link,local=[[0,0,0]],world=[pt])
#hinge constraint
obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
visualization.add("ik objective",obj)
print "Starting visualization..."
#run the visualizer in a separate thread
visualization.show()
iteration = 0
while visualization.shown():
'widgetTransform': se3.identity(),
'rotationScale': 1,
'positionScale': 3
}
deviceState = []
#device +z is gripper -z
#device +x is gripper -y
#device +y is gripper -x
deviceToWidgetTransform = ([0, -1, 0, -1, 0, 0, 0, 0, -1], [0, 0, 0])
deviceToBaxterBaseTransform = ([0, -1, 0, 0, 0, 1, -1, 0, 0], [0, 0, 0])
endEffectorIndices = [25, 45]
#widget coordinates in local frame of end effector
endEffectorLocalTransforms = [(so3.identity(), (0, 0, 0.08)),
(so3.identity(), (0, 0, 0.08))]
#TendEffector*localCoordinates = widget
def toggleMode(dstate):
if dstate['mode'] == 'normal':
dstate['mode'] = 'scaling'
elif dstate['mode'] == 'scaling':
dstate['mode'] = 'gripper'
else:
dstate['mode'] = 'normal'
class HapticCartesianTaskService(Service):
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])
# Question 4: set NO_SIMULATION_COLLISIONS = 0
NO_SIMULATION_COLLISIONS = 1
#Turn this on to help fast prototyping of later stages
FAKE_SIMULATION = 0
SKIP_PATH_PLANNING = 0
# The path of the klampt_models directory
model_dir = "../klampt_models/"
# global variable for baxter's resting configuration
# baxter_rest_config = [0.0]*54 # no need to declare as this because we load from file in main()
global baxter_rest_config
# The transformation of the order bin
# = identity rotation and translation in x
order_bin_xform = (so3.identity(),[0.5,0,0])
# the local bounding box of the order bin
order_bin_bounds = ([-0.2,-0.4,0],[0.2,0.4,0.7])
# Order list. Can be parsed from JSON input
global orderList
orderList = ['med_item','tall_item']
# a list of actual items -- this is only used for the fake perception module, and your
# code should not use these items directly
def init_ground_truth():
global ground_truth_items
ground_truth_items = [apc.ItemInBin(apc.tall_item,'bin_C'),
apc.ItemInBin(apc.small_item,'bin_A'),
apc.ItemInBin(apc.med_item,'bin_E')]
model_dir = "../klampt_models/"
#indices of the left and right cameras in the Baxter robot file
left_camera_link_name = 'left_hand_camera'
right_camera_link_name = 'right_hand_camera'
#indices of the left and right grippers in the Baxter robot file
left_gripper_link_name = 'left_gripper'
right_gripper_link_name = 'right_gripper'
#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']
#local transformations (rotation, translation pairs) of the grasp center
left_gripper_center_xform = (so3.identity(),[0,-0.04,0.1])
right_gripper_center_xform = (so3.identity(),[0,-0.04,0.1])
#resting configuration
baxter_rest_config = [0.0]*54
#the transformation of the order bin
order_bin_xform = (so3.identity(),[0.5,0,0])
#the local bounding box of the order bin
order_bin_bounds = ([-0.2,-0.4,0],[0.2,0.4,0.7])
class KnowledgeBase:
"""A structure containing the robot's dynamic knowledge about the world.
Members:
- bin_contents: a map from bin names to lists of known items in
the bin. Items are given by apc.ItemInBin objects.
CHENYU_GO_PATH = MAT_PATH + "chenyugo.txt" CHENYU_DONE_PATH = MAT_PATH + "chenyudone.txt" ARDUINO_SERIAL_PORT = "/dev/ttyACM0" ROS_DEPTH_TOPIC = "/realsense/pc" PICK_JSON_PATH = REPO_ROOT + '/group3/planning/apc_pick_task.json' # Indices in Baxter robot file link LEFT_CAMERA_LINK_NAME = 'left_hand_camera' RIGHT_CAMERA_LINK_NAME = 'right_hand_camera' LEFT_GRIPPER_LINK_NAME = 'left_gripper' RIGHT_GRIPPER_LINK_NAME = 'right_gripper' 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'] # Local transform from right_wrist to vacuum point VACUUM_POINT_XFORM = (so3.identity(), [.14, 0.0, .43]) # Local transform from right_lower_forearm to F200 RIGHT_F200_CAMERA_CALIBRATED_XFORM = ([0.03041006181767996, 0.016744860249637956, -0.9993972372362588, 0.990578526979957, 0.13306496892712172, 0.032371220715998364, 0.13352681388569373, -0.9909658539524185, -0.012540585069730208], [0.13, -0.12000000000000001, -0.02999999999999997]) # Transform of shelf relative to world SHELF_MODEL_XFORM = [1.43,-.25,-.03] SHELF_MODEL_XFORM_CALIBRATED = ([0.009999333346666592, -0.9999000033332889, 0.00999983333416667, 0.9999500004166664, 0.009999833334166692, 7.049050118954173e-18, -9.99966667111252e-05, 0.009999333346666538, 0.9999500004166657], [1.4299999999999995, -0.05500000000000002, 3.469446951953614e-18]) # Distances (meters) GRASP_MOVE_DISTANCE = .07 COM_ADJUSTMENT = [.03, 0.0, GRASP_MOVE_DISTANCE] COM_Y_ADJUSTMENT = 0.0 MEAN_OBJ_HEIGHT = .05
)
# for (i, blob) in enumerate(object_blobs):
# logger.info('blob {}: {} points'.format(i, numpy.count_nonzero(blob)))
# filter out blobs with fewer than 1000 points
object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= 1000, object_blobs)
# take only the largest blobs
if object_blobs:
blob_mask = reduce(numpy.bitwise_or, object_blobs)
mask = object_mask & blob_mask
else:
mask = numpy.zeros_like(object_mask)
object_cloud = clean_cloud_aligned[mask[clean_mask]]
xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
object_cloud_color = map(
lambda x: x[0] + [x[1]], zip(object_cloud.tolist(), uvtexture(color, depth_uv)[mask].tolist())
)
try:
pcd.write(object_cloud_color, os.path.join(base_path, "{}_{}_segmented.pcd".format(obj, n)))
except IndexError:
print "error writing PCD"
pass
numpy.save(os.path.join(base_path, "{}_{}_mask.npy".format(obj, n)), mask)
imsave(os.path.join(base_path, "{}_{}_mask.png".format(obj, n)), mask)
imsave(os.path.join(base_path, "{}_{}_color.png".format(obj, n)), color)
color2 = uvtexture(color, depth_uv)
color2[~mask] = [0, 0, 0]
imsave(os.path.join(base_path, "{}_{}_segmented.png".format(obj, n)), color2)
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model " + fn)
coordinates.setWorldModel(world)
#add the world to the visualizer
visualization.add("world", world)
#add the coordinate Manager to the visualizer
visualization.add("coordinates", coordinates.manager())
#test a point
pt = [2, 5, 1]
visualization.add("some point", pt)
#test a rigid transform
visualization.add("some blinking transform", [so3.identity(), [1, 3, 0.5]])
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks() - 1)
#point constraint
#obj = ik.objective(link,local=[[0,0,0]],world=[pt])
#hinge constraint
obj = ik.objective(link,
local=[[0, 0, 0], [0, 0, 0.1]],
world=[pt, [pt[0], pt[1], pt[2] + 0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
visualization.add("ik objective", obj)
print "Starting visualization..."
#run the visualizer in a separate thread
visualization.show()