def __call__(self,controller): sim = self.sim xform = self.base_xform #turn this to false to turn off print statements ReflexController.verbose = True ReflexController.__call__(self,controller) #controller state machine #print "State:",state t_lift = 2 lift_traj_duration = 0.5 if self.state == 'idle': if sim.getTime() > 0.5: desired = se3.mul((so3.identity(),[0,0,-0.10]),xform) send_moving_base_xform_linear(controller,desired[0],desired[1],lift_traj_duration) self.state = 'lowering' elif self.state == 'lowering': if sim.getTime() > 1: #this is needed to stop at the current position in case there's some residual velocity controller.setPIDCommand(controller.getCommandedConfig(),[0.0]*len(controller.getCommandedConfig())) #the controller sends a command to the hand: f1,f2,f3,preshape self.hand.setCommand([0.2,0.2,0.2,0]) self.state = 'closing' elif self.state == 'closing': if sim.getTime() > t_lift: self.base_xform = get_moving_base_xform(self.sim.controller(0).model()) self.state = 'raising' elif self.state == 'raising': #the controller sends a command to the base after 1 s to lift the object t_traj = min(1, max(0, (sim.getTime() - t_lift) / lift_traj_duration)) desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform) send_moving_base_xform_PID(controller, desired[0], desired[1]) if sim.getTime() > (t_lift + lift_traj_duration): self.state = 'raised'
def controlfunc(controller):
"""Place your code here... for a more sophisticated controller you could also create a class where the control loop goes in the __call__ method."""
if not is_soft_hand:
#print the contact sensors... you can safely take this out if you don't want to use it
try:
f1_contact = [s.getMeasurements()[0] for s in f1_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f1_distal_takktile_sensors]
f2_contact = [s.getMeasurements()[0] for s in f2_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f2_distal_takktile_sensors]
f3_contact = [s.getMeasurements()[0] for s in f3_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f3_distal_takktile_sensors]
print "Contact sensors"
print " finger 1:",[int(v) for v in f1_contact]
print " finger 2:",[int(v) for v in f2_contact]
print " finger 3:",[int(v) for v in f3_contact]
except:
pass
if sim.getTime() < 0.05:
if is_soft_hand:
hand.setCommand([0.8])
else:
#the controller sends a command to the hand: f1,f2,f3,preshape
hand.setCommand([0.2,0.2,0.2,0])
t_lift = 1
lift_traj_duration = 0.5
if sim.getTime() > t_lift:
#the controller sends a command to the base after 1 s to lift the object
t_traj = min(1, max(0, (sim.getTime() - t_lift) / lift_traj_duration))
desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform)
send_moving_base_xform_PID(controller, desired[0], desired[1])
#need to manually call the hand emulator
hand.process({},dt)
def setTransform(self,name,R=so3.identity(),t=[0]*3,matrix=None):
"""Sets the transform of the target object. If matrix is given, it's a 16-element
array giving the 4x4 homogeneous transform matrix, in row-major format. Otherwise,
R and t are the 9-element klampt.so3 rotation and 3-element translation."""
if matrix != None:
self._do_rpc({'type':'set_transform','object':name,'matrix':matrix})
else:
self._do_rpc({'type':'set_transform','object':name,'matrix':[R[0],R[3],R[6],t[0],R[1],R[4],R[7],t[1],R[2],R[5],R[8],t[2],0,0,0,1]})
def controlfunc(controller):
"""Place your code here... for a more sophisticated controller you could also create a class where the control loop goes in the __call__ method."""
#print the contact sensors... you can safely take this out if you don't want to use it
try:
f1_contact = [s.getMeasurements()[0] for s in f1_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f1_distal_takktile_sensors]
f2_contact = [s.getMeasurements()[0] for s in f2_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f2_distal_takktile_sensors]
f3_contact = [s.getMeasurements()[0] for s in f3_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f3_distal_takktile_sensors]
print "Contact sensors"
print " finger 1:",[int(v) for v in f1_contact]
print " finger 2:",[int(v) for v in f2_contact]
print " finger 3:",[int(v) for v in f3_contact]
except:
pass
if sim.getTime() < 0.05:
#the controller sends a command to the hand: f1,f2,f3,preshape
hand.setCommand([0.2,0.2,0.2,0])
if sim.getTime() > 1:
#the controller sends a command to the base after 1 s to lift the object
desired = se3.mul((so3.identity(),[0,0,0.10]),xform)
send_moving_base_xform_linear(controller,desired[0],desired[1],0.5)
#need to manually call the hand emulator
hand.process({},dt)
for i in range(num_in_firstbin):
dataset=random.choice(objects.keys())
index = random.randint(0,len(objects[dataset])-1)
objname = objects[dataset][index]
firstbin.append((dataset,objname))
for i in range(int(sys.argv[2])-num_in_firstbin):
dataset=random.choice(objects.keys())
index = random.randint(0,len(objects[dataset])-1)
objname = objects[dataset][index]
shelved.append((dataset,objname))
for objectset,objectname in shelved:
object=make_object(objectset,objectname,world)
object.setTransform(*se3.mul((so3.identity(),[shelf_x_shift,shelf_offset,shelf_height+increment/2*0.15]),object.getTransform()))
rigid_objects.append(object)
increment+=1
for objectset,objectname in firstbin:
object=make_object(objectset,objectname,world)
object.setTransform(*se3.mul((so3.identity(),[firstbin_x_shift,firstbin_offset,firstbin_height+increment_firstbin/2*0.22]),object.getTransform()))
firstbin_objects.append(object)
increment_firstbin+=1
if len(rigid_objects)>0:
xy_jiggle(world,rigid_objects,[shelf],[shelf_x_shift-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[shelf_x_shift+0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100)
if len(firstbin_objects)>0:
xy_jiggle(world,firstbin_objects,[shelf],[firstbin_x_shift-0.5*firstbin_dims[0],-0.5*firstbin_dims[1]+firstbin_offset],[firstbin_x_shift+0.5*firstbin_dims[0],0.5*firstbin_dims[1]+firstbin_offset],100)
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,
visualize=False,verbose=0):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Side effect: the positions of objects in world are modified
Returns:
(tuple): (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:",objects_bound)
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = _get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in range(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
if verbose:
print("Sampled position of",object.getName(),object.getTransform()[1])
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
if verbose:
print("Failed to place object",object.getName())
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
if verbose:
print("Beginning to simulate")
#start letting everything fall
for firstfall in range(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
def find_target(self):
self.target_prep_pos = (so3.identity(), [0, 10, -1])
count = 0
# print self.waiting_list
for idx in self.waiting_list:
# print 'i',idx
bb = self.sim.world.rigidObject(idx).geometry().getBB()
#decide the hand facing and finger configuration
z_limit = 0.65
ratio = 1.0
if bb[1][2] > z_limit:
hand_facing = 'face_toward_shelf'
count += 1
else:
hand_facing = 'face_down'
count += 1
if bb[1][2] < 0.54 and self.hand_facing != 'sweep':
hand_facing = 'sweep'
if hand_facing == 'face_toward_shelf':
max_limit = forward_max
min_limit = forward_min
face = face_toward_shelf
hand_mode = 'power'
elif hand_facing == 'face_down':
max_limit = face_down_max
min_limit = face_down_min
x_diff = bb[1][0] - bb[0][0]
y_diff = bb[1][1] - bb[0][1]
if y_diff > x_diff * ratio:
hand_mode = 'power'
face = face_down
elif x_diff > y_diff * ratio:
hand_mode = 'power'
face = face_down_rot
else:
hand_mode = 'precision'
face = face_down
else:
face = sweep_pos
hand_mode = 'power'
if hand_facing == 'face_down':
target_prep_pos = (face,
vectorops.add(
vectorops.div(
vectorops.add(bb[1], bb[0]), 2),
[0, 0, 0.5]))
top_bb = bb[1][2]
target_pos = (face,
vectorops.div(vectorops.add(bb[1], bb[0]), 2))
target_pos[1][2] = top_bb + 0.06
for i in range(3):
target_prep_pos[1][i] = max(
min(target_prep_pos[1][i], max_limit[i]), min_limit[i])
target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]),
min_limit[i])
elif hand_facing == 'face_toward_shelf':
target_prep_pos = (face,
vectorops.add(
vectorops.div(
vectorops.add(bb[1], bb[0]), 2),
[0, 0, 0.02]))
back_bb = bb[0][1]
target_pos = (face,
vectorops.div(vectorops.add(bb[1], bb[0]), 2))
target_prep_pos[1][1] = back_bb - 0.07
target_pos[1][1] = back_bb - 0.04
for i in range(3):
target_prep_pos[1][i] = max(
min(target_prep_pos[1][i], max_limit[i]), min_limit[i])
target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]),
min_limit[i])
else:
target_prep_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.55,
0.66])
y_len = bb[1][1] - bb[0][1]
target_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.92 - y_len,
0.66])
if count > 0 and hand_facing == 'sweep':
continue
if target_prep_pos[1][
1] + self.num_pick[idx] * 0.01 < self.target_prep_pos[1][
1] or self.hand_facing == 'sweep':
self.hand_facing = hand_facing
self.hand_mode = hand_mode
self.target_prep_pos = target_prep_pos
self.target_pos = target_pos
self.target_id = idx
self.num_pick[self.target_id] += 1
def modification_template(world):
"""Tests a variety of miscellaneous vis functions"""
vis.add("world",world)
robot = world.robot(0)
vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque
import random
for i in range(10):
#set some links to random colors
randlink = random.randint(0,robot.numLinks()-1)
color = (random.random(),random.random(),random.random())
vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color)
#test the on-screen text display
vis.addText("text2","Here's some red text")
vis.setColor("text2",1,0,0)
vis.addText("text3","Here's bigger text")
vis.setAttribute("text3","size",24)
vis.addText("text4","Transform status")
vis.addText("textbottom","Text anchored to bottom of screen",(20,-30))
#test a point
pt = [2,5,1]
vis.add("some point",pt)
#test a rigid transform
vis.add("some blinking transform",[so3.identity(),[1,3,0.5]])
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#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)
vis.add("ik objective",obj)
#enable plotting
vis.addPlot('plot')
vis.addPlotItem('plot','some point')
vis.setPlotDuration('plot',10.0)
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Manual animation visualization test")
class MyCallback:
def __init__(self):
self.iteration = 0
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
callback = MyCallback()
if not MULTITHREADED:
vis.loop(callback=callback,setup=vis.show)
else:
vis.show()
while vis.shown():
callback()
time.sleep(0.01)
#use this to remove a plot
vis.remove("plot")
vis.kill()
print('y:', so3.apply(default[0], [0, 1, 0]))
print('z:', so3.apply(default[0], [0, 0, 1]))
print('offset:', default[1])
circle_points = []
npts = 50
times = []
for i in range(npts + 1):
angle = math.radians(360 * i / npts)
circle_points.append(
se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default))
times.append(i * 20 / npts)
circle_traj = SE3Trajectory(times, circle_points)
circle_traj.milestones[-1] = circle_traj.milestones[0]
circle_smooth_traj = SE3HermiteTrajectory()
circle_smooth_traj.makeSpline(circle_traj, loop=True)
R0 = so3.identity()
R1 = so3.rotation([0, 0, 1], math.pi / 2)
dR0 = [0.0] * 9
#TODO: for some reason the interpolation doesn't work very well...
#vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1))
#for m in circle_smooth_traj.milestones:
# T = m[:12]
# vT = m[12:]
# print("Orientation velocity",vT[:9])
#print("SMOOTH")
#for R in circle_smooth_traj.discretize_se3(0.1).milestones:
# print(so3.apply(R,[0,1,0]))
vis.add("xform", se3.identity())
vis.animate("xform", circle_smooth_traj)
def testMultiRobot():
#Create a world with two robots -- this will be the simulation world
w = WorldModel()
w.readFile("../../data/tx90scenario0.xml")
w.readFile("../../data/robots/jaco.rob")
r1 = w.robot(0)
r2 = w.robot(1)
#Create a world with a unified robot -- this will be the controller's model of the robot
w2 = w.copy()
w2.robot(0).mount(-1, w2.robot(1), so3.identity(), [1, 0, 0.5])
w2.remove(w2.robot(1))
whole_robot_model = w2.robot(0)
robot_1_indices = list(range(r1.numLinks()))
robot_2_indices = list(range(r1.numLinks(), r1.numLinks() + r2.numLinks()))
#update the base transform of robot 2
T0 = r2.link(0).getParentTransform()
r2.link(0).setParentTransform(T0[0], vectorops.add(T0[1], [1, 0, 0.5]))
r2.setConfig(r2.getConfig())
#Note: don't pass sim as the second argument to SimXControlInterface; we will need to simulate ourselves
sim = Simulator(w)
sim_controller1 = RobotInterfaceCompleter(
SimFullControlInterface(sim.controller(0)))
sim_controller2 = RobotInterfaceCompleter(
SimFullControlInterface(sim.controller(1)))
whole_robot_controller = MultiRobotInterface()
whole_robot_controller.addPart("Robot 1", sim_controller1,
whole_robot_model, robot_1_indices)
whole_robot_controller.addPart("Robot 2", sim_controller2,
whole_robot_model, robot_2_indices)
print("Num total DOFs", whole_robot_controller.numDOFs())
print("Parts:")
for k, v in whole_robot_controller.parts().items():
print(" ", k, ":", v)
print("Control rate", whole_robot_controller.controlRate())
print(
whole_robot_controller.getPartInterface("Robot 1").__class__.__name__)
print(
whole_robot_controller.getPartInterface("Robot 2").__class__.__name__)
if not whole_robot_controller.initialize():
raise RuntimeError("Failed to initialize")
visplugin1 = RobotInterfacetoVis(
whole_robot_controller.getPartInterface("Robot 1"), 0)
visplugin1.text_x = 10
visplugin1.tag = ''
visplugin2 = RobotInterfacetoVis(
whole_robot_controller.getPartInterface("Robot 2"), 1)
visplugin2.text_x = 200
visplugin2.tag = 'a'
vis.add("world", w)
#vis.add("world",w2)
#vis.edit(("world",whole_robot_model))
vis.add("qdes",
sim_controller1.configToKlampt(sim_controller1.sensedPosition()),
color=[1, 0, 0, 0.5],
robot=0)
vis.add("qdes2",
sim_controller2.configToKlampt(sim_controller2.sensedPosition()),
color=[1, 1, 0, 0.5],
robot=1)
vis.show()
dt = 1.0 / whole_robot_controller.controlRate()
while vis.shown():
t0 = time.time()
vis.lock()
whole_robot_controller.startStep()
#send commands here
clock = whole_robot_controller.clock()
if clock > 0.5 and clock < 2.5:
velocity = [0] * whole_robot_controller.numDOFs()
velocity[2] = -0.1
velocity[10] = 0.3
whole_robot_controller.setVelocity(velocity, None)
elif clock >= 2.5 and clock < 2.75:
velocity = [0] * whole_robot_controller.numDOFs()
whole_robot_controller.setVelocity(velocity)
elif clock > 2.75 and clock < 2.80:
tgt = [0] * sim_controller1.numDOFs()
tgt[2] = 1.0
whole_robot_controller.getPartInterface("Robot 1").moveToPosition(
tgt)
visplugin1.update()
visplugin2.update()
whole_robot_controller.endStep()
#update the simulator
sim.simulate(dt)
#update the visualization world
sim.updateWorld()
vis.add("qdes",
sim_controller1.configToKlampt(
sim_controller1.sensedPosition()),
color=[1, 0, 0, 0.5],
robot=0)
vis.add("qdes2",
sim_controller2.configToKlampt(
sim_controller2.sensedPosition()),
color=[1, 1, 0, 0.5],
robot=1)
#whole_robot_model.setConfig(r1.getConfig()+r2.getConfig())
vis.unlock()
t1 = time.time()
telapsed = t1 - t0
time.sleep(max(dt - telapsed, 0))
vis.clear()
a = Geometry3D()
if not a.loadFile(sys.argv[1]):
print("Error loading",sys.argv[1])
exit()
else:
a = sphere(0.4,center=(0,0,0),type='TriangleMesh')
a = Geometry3D(a)
w = WorldModel()
w.makeRigidObject("a")
w.rigidObject(0).geometry().set(a)
#a_pc = a.convert("PointCloud",0.05)
bb = a.getBBTight()
a_pc = a.convert("PointCloud")
a_pc.setCurrentTransform(so3.identity(),[1.25*(bb[1][0]-bb[0][0]),0,0])
value = None
cmap = None
feature = None
lighting = None
vis.add("A",a)
vis.add("B",a_pc)
def convert(value_new,cmap_new,feature_new=None,lighting_new=None):
global value,cmap,feature,lighting,w
if value_new is None or value_new == value:
if cmap_new is None or cmap == cmap_new:
if feature_new is None or feature == feature_new:
if lighting_new is None or feature == lighting_new:
return
#add the coordinate Manager to the visualizer
#vis.add("coordinates",coordinates.manager())
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
#do this if you want to test the robot configuration auto-fitting
#vis.add("text1","Using a random configuration")
#setRandomSeed(int(time.time()))
#world.robot(0).randomizeConfig()
#test a point
pt = [2, 5, 1]
vis.add("some point", pt)
#test a rigid transform
vis.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)
vis.add("ik objective", obj)
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test the on-screen text display
vis.addText("text2", "Here's some red text")
def makeHapticCartesianPoseMsg(self):
deviceState = self.haptic_client.deviceState
if len(deviceState)==0:
print "No device state"
return None
transforms = [self.serviceThread.eeGetter_left.get(),self.serviceThread.eeGetter_right.get()]
update = False
commanding_arm=[0,0]
for i,dstate in enumerate(deviceState):
if dstate.buttonDown[0]:
commanding_arm[i]=1
if self.startTransforms[i] == None:
self.startTransforms[i] = transforms[i]
#RESET THE HAPTIC FORCE FEEDBACK CENTER
#self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=dstate.position,device=i)
#UPDATE THE HAPTIC FORCE FEEDBACK CENTER FROM ROBOT EE POSITION
#need to invert world coordinates to widget coordinates to haptic device coordinates
#widget and world translations are the same
dx = vectorops.sub(transforms[i][1],self.startTransforms[i][1]);
dxwidget = vectorops.div(dx,hapticArmTranslationScaling)
R,device_center = widgetToDeviceTransform(so3.identity(),vectorops.add(dxwidget,dstate.widgetInitialTransform[0][1]))
#print "Device position error",vectorops.sub(dstate.position,device_center)
if vectorops.distance(dstate.position,device_center) > 0.03:
c = vectorops.madd(device_center,vectorops.unit(vectorops.sub(dstate.position,device_center)),0.025)
self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=c,device=i)
else:
#deactivate
self.serviceThread.hapticupdater.activateSpring(kP=0,kD=0,device=i)
if dstate.newupdate:
update = True
else:
if self.startTransforms[i] != None:
self.serviceThread.hapticupdater.deactivateHapticFeedback(device=i)
self.startTransforms[i] = None
dstate.newupdate = False
if update:
msg = {}
msg['type'] = 'CartesianPose'
T1 = self.getDesiredCartesianPose('left',0)
R1,t1=T1
T2 = self.getDesiredCartesianPose('right',1)
R2,t2=T2
transforms = [(R1,t1),(R2,t2)]
if commanding_arm[0] and commanding_arm[1]:
msg['limb'] = 'both'
msg['position'] = t1+t2
msg['rotation'] = R1+R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
elif commanding_arm[0] ==0:
msg['limb'] = 'right'
msg['position'] = t2
msg['rotation'] = R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
msg['limb'] = 'left'
msg['position'] = t1
msg['rotation'] = R1
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
return None
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,visualize=False):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Arguments:
- world: a WorldModel
- container: the container RigidObject / Terrain in world over which objects should be spawned. Assumed axis-aligned and with an open top.
- objects: a list of RigidObjects in the world, at arbitrary locations. They are placed in order.
- container_wall_thickness: a margin subtracted from the container's outer dimensions into which the objects are spawned.
- randomize_orientation: if True, the orientation of the objects are completely randomized. If 'z', only the z orientation is randomized.
If False or None, the orientation is unchanged
Side effect: the positions of objects in world are modified
Return value (world,sim):
- world: the original world
- sim: the Simulator instance at the state used to obtain the stable placement of the objects.
Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy()
to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
print "Spawn area",spawn_area
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in xrange(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = get_bound(objects[:index])
print "Existing objects bound:",objects_bound
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in xrange(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
print "Sampled position of",object.getName(),object.getTransform()[1]
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in xrange(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
print "Terminated lowering at",lower,"cm lower"
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
print "Failed to place object",object.getName()
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
print "Beginning to simulate"
#start letting everything fall
for firstfall in xrange(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
if res is not None:
fn, obj = res
else:
exit(1)
print "Cloning object for second object"
obj2 = obj.clone()
if obj is None:
exit(1)
print "Input object has type", obj.type(), "with", obj.numElements(
), "elements"
geom1 = PenetrationDepthGeometry(obj, gridres, pcres)
geom2 = PenetrationDepthGeometry(obj2, gridres, pcres)
geom2.setTransform((so3.identity(), [1.2, 0, 0]))
vis.add("obj1", geom1.grid)
vis.add("obj2", geom2.pc)
vis.setColor("obj1", 1, 1, 0, 0.5)
vis.setColor("obj2", 0, 1, 1, 0.5)
vis.setAttribute("obj2", "size", 3)
vis.addPlot("timing")
vis.add("transform", se3.identity())
vis.edit("transform")
vis.show()
oldcps = []
while vis.shown():
vis.lock()
t0 = time.time()
def __init__(self, fn):
## Creates a world and loads all the items on the command line
self.world = WorldModel()
self.robotSystemName = 'O'
for f in fn:
print(f)
res = self.world.readFile(f)
if not res:
raise RuntimeError("Unable to load model " + fn)
self.showVis = False
coordinates.setWorldModel(self.world)
vis.lock()
bW.getDoubleRoomWindow(self.world, 8, 8, 1)
vis.unlock()
## Add the world to the visualizer
vis.add("world", self.world)
vp = vis.getViewport()
vp.w, vp.h = 1800, 800
vis.setViewport(vp)
self.robots = []
self.n = self.world.numRobots()
for i in range(self.n):
self.robots.append(
sphero6DoF(self.world.robot(i),
self.world.robot(i).getName()))
self.eps = 0.000001
self.sj = [[0, 0, 0], [0.2, 0, 0]]
self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0],
[0.070534, -0.097082, 0], [0.057063, -0.018541, 0],
[0.11413, 0.037082, 0], [0.035267, 0.048541, 0],
[0, 0.12, 0], [-0.035267, 0.048541, 0],
[-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]]
self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0],
[0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0],
[0.6, 0, 0], [0.8, 0, 0]]
self.sj = self.sjL
self.xB = [-4, 4]
self.yB = [-4, 4]
self.zB = [0.02, 1]
self.rad = 0.04
self.currConfig = [0, 0, 1, 1, 0]
self.scMin = 1
self.scXMin = 1
self.scYMin = 2
self.sumDist = 0
if self.n > 1:
minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1]))
minSijX = math.fabs(self.sj[0][0] - self.sj[1][0])
minSijY = math.fabs(self.sj[0][1] - self.sj[1][1])
for i in range(self.n):
for j in range(self.n):
if i != j:
dist = vectorops.norm(
vectorops.sub(self.sj[i], self.sj[j]))
if dist < minSij:
minSij = dist
dist = math.fabs(self.sj[i][0] - self.sj[j][0])
if dist < minSijX:
minSijX = dist
dist = math.fabs(self.sj[i][1] - self.sj[j][1])
if dist < minSijY:
minSijY = dist
for i in range(self.n):
self.sumDist += vectorops.norm(self.sj[i])
if minSij > self.eps:
self.scMin = 2 * math.sqrt(2) * self.rad / minSij
if minSijX > self.eps:
self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX
if minSijY > self.eps:
self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY
self.scMax = max(2, self.scMin)
self.scB = [self.scMin, 2 * self.scMin]
print('Minimum scale')
print(self.scMin)
vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]])
vis.setAttribute(self.robotSystemName, "size", 12)
vis.edit(self.robotSystemName)
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 32)
vis.edit("WCS")
self.collisionChecker = collide.WorldCollider(self.world)
if self.showVis:
## Display the world coordinate system
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom",
"WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
vis.show()
simTime = 60
startTime = time.time()
oldTime = startTime
self.setConfig(0, 0, 1, 1, 0)
q = self.robots[0].getConfig()
if self.showVis:
q2f = ['{0:.2f}'.format(elem) for elem in q]
strng = "Robot configuration: " + str(q2f)
vis.addText("textConfig", strng)
colFlag = self.checkCollision()
print(colFlag)
if self.showVis:
time.sleep(10)
w = WorldModel()
if not w.readFile("myworld.xml"):
raise RuntimeError("Couldn't read the world file")
shelf = make_shelf(w,*shelf_dims)
shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height))
obj = w.makeRigidObject("point_cloud_object") #Making Box
obj.geometry().loadFile(KLAMPT_Demo+"/data/objects/apc/genuine_joe_stir_sticks.pcd")
#set up a "reasonable" inertial parameter estimate for a 200g object
m = obj.getMass()
m.estimate(obj.geometry(),0.200)
obj.setMass(m)
#we'll move the box slightly forward so the robot can reach it
obj.setTransform(so3.identity(),[shelf_offset_x-0.05,shelf_offset_y-0.3,shelf_height+0.01])
vis.add("world",w)
vis.add("ghost",w.robot(0).getConfig(),color=(0,1,0,0.5))
vis.edit("ghost")
from klampt import Simulator
sim = Simulator(w)
def setup():
vis.show()
def callback():
sim.controller(0).setPIDCommand(vis.getItemConfig("ghost"),[0]*w.robot(0).numLinks())
sim.simulate(0.01)
sim.updateWorld()
def control_func(controller):
"""
Place your code here... for a more sophisticated controller you could also create a class
where the control loop goes in the __call__ method.
"""
global t_lift
global lift_trajectory_duration
global syn_closed
global xform
if global_vars.is_adaptive_running:
# Integrating the velocities
(success, syn_comm, palm_comm, syn_vel_comm,
palm_vel_comm) = integrate_velocities(controller, sim, dt)
# print 'The integration of velocity -> success = ', success
if success:
if DEBUG:
print 'The commanded position of the hand encoder (in memory) is ', syn_comm
print 'The commanded position of the palm is \n', palm_comm
print 'The commanded velocity of the palm is \n', palm_vel_comm
hand.setCommandVel([syn_comm], syn_vel_comm)
mv_el.send_moving_base_xform_PID_vel(controller, palm_comm[0],
palm_comm[1],
palm_vel_comm)
# mv_el.send_moving_base_xform_PID(controller, palm_comm[0], palm_comm[1])
else:
print 'Finished the ADAPTIVE GRASPING NOW! LIFTING!!!'
if not global_vars.got_pres_pose: # this will be set to false after getting xform
t_lift = sim.getTime()
# Wait and close the hand then lift
syn_now = controller.getSensedConfig()[34]
print 'syn_now is ', syn_now
# In this way even if hand reopens after getting lift traj, the lifting won't be compromised
if syn_now < global_vars.syn_closed and not global_vars.got_pres_pose:
# Closing hand completely
hand.setCommand([1.0])
# Wait for remaining time
rem_time = controller.remainingTime()
time.sleep(rem_time)
t_lift = sim.getTime()
print 'Closing hand completely!!!'
else:
print 'Hand is closed sufficiently! Sending the lift pose now!!!'
# Getting the pose only once and sending lift trajectory
if not global_vars.got_pres_pose:
xform = mv_el.get_moving_base_xform(
sim.controller(0).model()) # for later lifting
global_vars.got_pres_pose = True
print 'xform is ', xform
# the controller sends command to the base
t_trajectory = min(
1,
max(0,
(sim.getTime() - t_lift) / lift_trajectory_duration))
desired = se3.mul(
(so3.identity(), [0, 0, 0.10 * t_trajectory]), xform)
mv_el.send_moving_base_xform_PID(controller, desired[0],
desired[1])
# need to manually call the hand emulator
hand.process({}, dt)
if DEBUG:
print "The simulation time is " + str(sim.getTime())
vp = vis.getViewport()
vp.w, vp.h = 1200, 800
vis.setViewport(vp)
## Create robot object. Change the class to the desired robot.
## Also, make sure the robot class corresponds to the robot in simpleWorld.xml file
#robot = kobuki(world.robot(0), vis)
#robot.setAltitude(0.01)
robot = turtlebot(world.robot(0), "turtle", vis)
robot.setAltitude(0.12)
#robot = sphero6DoF(world.robot(0), "sphero", vis)
## Display the world coordinate system
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 24)
#print "Visualization items:"
#vis.listItems(indent=2)
#vis.autoFitCamera()
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
collisionFlag = False
collisionChecker = collide.WorldCollider(world)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue",
print(" COM surface-only %.3f %.3f %.3f" % tuple(m.getCom()))
H = m.getInertia()
print(" Inertia surface-only %.3f %.3f %.3f" % (H[0], H[4], H[8]))
m.estimate(g, 1.0, 0.0)
H = m.getInertia()
print(" COM volume-only %.3f %.3f %.3f" % tuple(m.getCom()))
print(" Inertia volume-only %.3f %.3f %.3f" % (H[0], H[4], H[8]))
a = atypes['GeometricPrimitive']
b = btypes['GeometricPrimitive']
vis.add("A", a)
vis.add("B", b)
vis.setColor("A", 1, 0, 0, 0.5)
vis.setColor("B", 0, 1, 0, 0.5)
Ta = se3.identity()
Tb = [so3.identity(), [1, 0, 0]]
vis.add("Ta", Ta)
vis.add("Tb", Tb)
vis.edit("Ta")
vis.edit("Tb")
ray = ([-3, 0, 0], [1, 0, 0])
vis.add("ray",
Trajectory([0, 1], [ray[0], vectorops.madd(ray[0], ray[1], 20)]),
color=[1, 0.5, 0, 1])
vis.add("hitpt", [0, 0, 0], color=[1, 0, 1, 1])
def convert(geom, type, label):
global a, b, atypes, btypes, Ta, Tb
if label == 'A':
def se3translation(v):
return (so3.identity(), v)
def testCompleter():
w = WorldModel()
w.readFile("../../data/tx90scenario0.xml")
r = w.robot(0)
sim = Simulator(w)
#TODO: CHANGE ME
#controller = RobotInterfaceCompleter(KinematicSimControlInterface(r))
#controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim))
#controller = RobotInterfaceCompleter(SimMoveToControlInterface(sim.controller(0),sim))
#controller = RobotInterfaceCompleter(SimVelocityControlInterface(sim.controller(0),sim))
controller = RobotInterfaceCompleter(
SimFullControlInterface(sim.controller(0), sim))
testProperties = ['controlRate', 'parts', 'sensors', 'numDOFs', 'indices']
testFuncs = [
'clock', 'status', 'isMoving', 'sensedPosition', 'sensedVelocity',
'sensedTorque', 'commandedPosition', 'commandedVelocity',
'commandedTorque', 'destinationPosition', 'destinationVelocity',
'destinationTime'
]
if not controller.initialize():
raise RuntimeError("There was some problem initializing controller " +
str(controller))
#start logger
testFile = open('controllertest_results.csv', 'w', newline='')
testWriter = csv.writer(testFile)
testWriter.writerow(testProperties)
results = []
for prop in testProperties:
try:
results.append(str(getattr(controller, prop)()))
except Exception as e:
results.append('Error ' + str(e))
testWriter.writerow(results)
testWriter.writerow(['emulatorControlMode', 'baseControlMode'] + testFuncs)
if controller.numDOFs() != r.numDrivers():
raise RuntimeError("Invalid DOFs")
if controller.klamptModel() is None:
raise RuntimeError("Can't get Klampt model")
q = r.getConfig()[1:]
q2 = [x for x in q]
q2[2] -= 1.0
q2[3] -= 1.0
controller.setToolCoordinates([0, 0, 0])
#TODO: CHANGE ME
"""
#testing a single movement
moves = [(1.0,lambda: controller.setPiecewiseLinear([1],[q[:2]+[q[2]+1.0]+q[3:]]))]
"""
#testing general movements with interruption
moves = [
(0.5, lambda: controller.setVelocity([0] * 2 + [1.0] + [0] *
(len(q) - 3), 1.0)),
(1.0, lambda: controller.setPiecewiseLinear(
[1], [q[:2] + [q[2] + 1.0] + q[3:]])),
(3.0, lambda: controller.setPiecewiseCubic([1], [q], [[0] * len(q)])),
(3.5, lambda: controller.moveToPosition(q[:2] + [q[2] - 1.0] + q[3:])),
(5.0, lambda: controller.moveToPosition(q, 0.1)),
(5.5, lambda: controller.moveToPosition(q2, 1.0)),
(8.0, lambda: controller.moveToCartesianPosition(
(so3.identity(), [0.5, 0, 1.0]))),
(10.0, lambda: controller.setCartesianVelocity([0, 0, 0.2], 3.0)),
(11.0, lambda: controller.moveToCartesianPosition(
(so3.identity(), [0.5, 0, 1.0])))
]
"""
#testing interrupted cartesian velocity movements
moves = [(0.5,lambda: controller.moveToCartesianPosition((so3.identity(),[0.5,0,1.0]))),
(2.0,lambda: controller.setCartesianVelocity([0,0,0.1],5.0)) ,
#(3.0,lambda: controller.moveToPosition(q,1))
(3.0,lambda: controller.moveToCartesianPosition((so3.identity(),[0.5,0,1.0])))
]
"""
"""
#testing cartesian velocity movements
moves = [(0.5,lambda: controller.moveToCartesianPosition((so3.identity(),[0.5,0,1.0]))),
(2.0,lambda: controller.setCartesianVelocity([0,0,0.1],5.0))
]
"""
visplugin = RobotInterfacetoVis(controller)
#visplugin.tag = ''
endTime = 13.0
lastClock = 0
dt = 1.0 / controller.controlRate()
vis.add("world", w)
vis.show()
while controller.status() == 'ok' and vis.shown(
): #no error handling done here...
t0 = time.time()
vis.lock()
controller.startStep()
clock = controller.clock()
if (clock % 1.0) <= dt:
controller.print_status()
for (trigger, callback) in moves:
if clock > trigger and lastClock <= trigger:
print("Calling trigger", trigger)
callback()
lastClock = clock
visplugin.update()
if controller.clock() > endTime:
vis.unlock()
break
controller.endStep()
#log results to disk
results = [
controller._emulatorControlMode, controller._baseControlMode
]
for func in testFuncs:
try:
results.append(str(getattr(controller, func)()))
except Exception as e:
results.append('Error ' + str(e))
testWriter.writerow(results)
if isinstance(controller._base, KinematicSimControlInterface):
r.setConfig(
controller.configToKlampt(controller.commandedPosition()))
else:
sim.updateWorld()
#give visualization some chance to update
vis.unlock()
t1 = time.time()
telapsed = t1 - t0
time.sleep(max(dt - telapsed, 0))
if vis.shown():
print("STATUS CHANGED TO", controller.status())
print("FINAL CLOCK", controller.clock())
controller.print_status()
vis.show(False)
vis.clear()
testFile.close()
