mhead10
11-01-2012, 04:52 PM
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!
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_RA DIUS, 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)
"""
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!
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_RA DIUS, 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'
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'
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)
"""