How to use the klampt.model.collide.self_collision_iter function in Klampt

To help you get started, we’ve selected a few Klampt examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github krishauser / IROS2016ManipulationChallenge / main.py View on Github external
numConflicts = [0]*len(objects)
	for (i,j) in collide.self_collision_iter([o.geometry() for o in objects]):
		numConflicts[i] += 1
		numConflicts[j] += 1
	for (i,j) in collide.group_collision_iter([o.geometry() for o in objects],[o.geometry() for o in fixed_objects]):
		numConflicts[i] += 1
	removed = []
	while max(numConflicts) > 0:
		amax = max((c,i) for (i,c) in enumerate(numConflicts))[1]
		cmax = numConflicts[amax]
		print "Unable to find conflict-free configuration, removing object",objects[amax].getName(),"with",cmax,"conflicts"
		removed.append(amax)

		#revise # of conflicts -- this could be faster, but whatever...
		numConflicts = [0]*len(objects)
		for (i,j) in collide.self_collision_iter([o.geometry() for o in objects]):
			if i in removed or j in removed:
				continue
			numConflicts[i] += 1
			numConflicts[j] += 1
		for (i,j) in collide.group_collision_iter([o.geometry() for o in objects],[o.geometry() for o in fixed_objects]):
			if i in removed:
				continue
			numConflicts[i] += 1
	removeIDs = [objects[i].index for i in removed]
	for i in sorted(removeIDs)[::-1]:
		world.remove(world.rigidObject(i))
	raw_input("Press enter to continue")
github krishauser / IROS2016ManipulationChallenge / main.py View on Github external
def xy_jiggle(world,objects,fixed_objects,bmin,bmax,iters,randomize = True):
	"""Jiggles the objects' x-y positions within the range bmin - bmax, and randomizes orientation about the z
	axis until the objects are collision free.  A list of fixed objects (fixed_objects) may be given as well.

	Objects for which collision-free resolutions are not found after iters steps will be
	deleted from the world.
	"""
	if randomize:
		for obj in objects:
			xy_randomize(obj,bmin,bmax)
	inner_iters = 10
	while iters > 0:
		numConflicts = [0]*len(objects)
		for (i,j) in collide.self_collision_iter([o.geometry() for o in objects]):
			numConflicts[i] += 1
			numConflicts[j] += 1
		for (i,j) in collide.group_collision_iter([o.geometry() for o in objects],[o.geometry() for o in fixed_objects]):
			numConflicts[i] += 1
		
		amax = max((c,i) for (i,c) in enumerate(numConflicts))[1]
		cmax = numConflicts[amax]
		if cmax == 0:
			#conflict free
			return
		print cmax,"conflicts with object",objects[amax].getName()
		other_geoms = [o.geometry() for o in objects[:amax]+objects[amax+1:]+fixed_objects]
		for it in xrange(inner_iters):
			xy_randomize(objects[amax],bmin,bmax)
			nc = sum([1 for p in collide.group_collision_iter([objects[amax].geometry()],other_geoms)])
			if nc < cmax: