I have a grasper which opens and closes. When the grasper is nearby a torus, and is closed, I have linked the grasper with the torus.
However, when I open the grasper, and I remove the link (by link.remove() and link=None), the grasper is visually still linked to the torus in Vizard.
I have a print statement telling me the grasper is no longer linked (link = None), so I don't understand what the problem could be.
Furthermore, I have a separate grouping of commented out code, using vizact and the spacebar, which performs the above task flawlessly.
Any help would be appreciated.
Thanks!
Code:
import viz
import ctypes
import numpy
import viztask
import time
import threading
import math
import vizshape
import vizcam
import vizact
viz.phys.enable()
viz.collision(viz.ON)
viz.go()
#Add grid
grid = vizshape.addGrid()
grid.color(viz.GRAY)
grid.collidePlane() # Make collideable plane
data = numpy.zeros((max_num_samples,),dtype=numpy.float64)
# read in initial value from DAQ NI-6008
CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
float64(-1),
DAQmx_Val_GroupByChannel,data.ctypes.data,max_num_samples,ctypes.byref(read),None))
#Add an object.
shaft = viz.add('shaft.dae', pos = (0,5,-5))
babcock_1 = viz.addChild('origin.dae', parent = shaft)
babcock_2 = viz.addChild('origin.dae', parent = shaft)
pic_base = viz.addTexture('silver.jpg') #silver cylindar
pic_torus = viz.addTexture('green.jpg') #green rings
#creation of pegs
cylinder1=vizshape.addCylinder(height=2, radius=.25, topRadius=None, bottomRadius=None, axis=vizshape.AXIS_Y, slices=20, bottom=True,top=True, pos = (-1,1,2.5), texture = pic_base)
cylinder1.collideCapsule(radius = .25, length = 1,bounce = 0, friction = 1)
joint1 = viz.phys.addFixedJoint(cylinder1,grid)
# creation of rings-modified from farshizzo
RADIUS = 0.5
TUBE_RADIUS = 0.1
d = 2 # density
f = 10 #friction
h = .01 #hardness
torus1 = vizshape.addTorus(radius=RADIUS,tubeRadius=TUBE_RADIUS, pos = (-1,0,2.5), texture = pic_torus)
for deg in range(0,360,20):
x = math.sin(viz.radians(deg)) * RADIUS
z = math.cos(viz.radians(deg)) * RADIUS
torus1.collideSphere(radius=TUBE_RADIUS-.08,pos=(x,0,z), bounce = 0, friction = f, density = d, hardness = h)
link = None #The handle to the link object
def read_DAQ(): # used obtain DAQ values
global link
scale = 40 #degrees (360/9)
trans_scale = 10 #translation pot (pot #1)
CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
float64(-1),
DAQmx_Val_GroupByChannel,data.ctypes.data,max_num_samples,ctypes.byref(read),None))
#Grasper Movement (opening/closing)
if data[2]*scale > x: #graspers are not touching #data[2] is my grasper reading from a DAQ
babcock_1.setEuler(x-data[2]*scale,0,0,viz.ABS_GLOBAL)
babcock_2.setEuler(-x+data[2]*scale,0,180,viz.ABS_GLOBAL)
#print 'greater'
print
else: #graspers are touching
babcock_1.setEuler(x/scale,0,0,viz.ABS_GLOBAL)
babcock_2.setEuler((x/scale),0,180,viz.ABS_GLOBAL)
#print 'less'
print
if data[2]*scale < x:#GRASPER CLOSED
if (spx >= (t1px - 0.20) and spx <= (t1px + 0.20)): #and grasper's within bounds
print 'within range so; linked'
link = viz.grab(shaft, torus1) #link shaft to torus1
else: #grasper is closed, but out of bounds
if link != None:#previously linked
link.remove()
link = None
print 'grasper is closed, out of bounds, and was not previously linked'
else: #GRASPER OPEN
if link != None:
link.remove()
link = None
##########################################################################################################################################
# director function used to call the DAQ reading (difference) and WAITS for its return
def director_function_loop():
while True:
value = yield viztask.waitDirector(read_DAQ) #director function!
viztask.schedule(director_function_loop())
"""#the following code works perfectly with the space bar
#Grab or let go of the ring
def toggleLink():
global link
print link
if link:
#If link exits, stop grabbing
link.remove()
link = None
else:
#If no link, grab the torus with the shaft(parent + grasper tip (children))
link = viz.grab(shaft, torus1)
vizact.onkeydown(' ',toggleLink)
"""