mhead10
12-07-2012, 08:56 AM
Hello,
I am trying to create a simple forward kinematics simulation using a DAQ USB 6008 connected to potentiometers. Each potentiometer signifies a revolute degree of freedom of a link like the following example http://demonstrations.wolfram.com/ForwardKinematics/. However, my example is only interested in the end effector position/orientation (I don't care about seeing the other links).
My problem seems to be that Vizard will only calculate the first transform (frame {0} to frame {1}), but will not do any further computations. For example, here are my printed out results:
T01 [ -0.974240, 0.225514, 0.000000, 0.000000
-0.225514, -0.974240, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
theta2 153.228278124
math.cos(math.radians(theta2)) -0.892808238702
T12 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T23 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T34 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T04 [ -0.974240, 0.225514, 0.000000, 0.000000
-0.225514, -0.974240, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
Only the first transform values are being evaluated and assigned to T04 (T04 = T01*T12*T23*T34). I am unsure of why T12 is the identity matrix, because as can be seen, theta2 has a value. Therefore, T12 should not be the identity matrix.
Below is my code. I hope someone can share something insightful!
Thanks!
import viz
import ctypes
import numpy
import viztask
import time
import threading
import math
import vizshape
import vizcam
import vizact
import vizinfo
viz.phys.enable()
viz.collision(viz.ON)
viz.go()
viz.clearcolor(0.5,0.7,1)
#Change navigation style to pivot
cam = vizcam.PivotNavigate(center=[0,0,5], distance = 15)
cam.rotateUp(-60)
cam.rotateRight(180)
#Add grid
#creation of base
#grid = vizshape.addBox(size=(10,1,8),right=True,left=True ,top=True,bottom=True,front=True,back=True,splitFa ces=False, pos = (0,-.5,0))
#base_grid = vizshape.addGrid(pos = (0,0,0))
grid = vizshape.addGrid(pos = (0,0,0), axis = vizshape.AXIS_Z)
grid.collidePlane() # Make collideable plane
#cookbook for accessing DAQ
nidaq = ctypes.windll.nicaiu # load the DLL
int32 = ctypes.c_long
uInt32 = ctypes.c_ulong
uInt64 = ctypes.c_ulonglong
float64 = ctypes.c_double
TaskHandle = uInt32
read = int32()
DAQmx_Val_Cfg_Default = int32(-1) #Default (differential)
DAQmx_Val_RSE=int32(10083) #RSE
DAQmx_Val_Volts = 10348
DAQmx_Val_Rising = 10280
DAQmx_Val_FiniteSamps = 10178
DAQmx_Val_ContSamps = 10123
DAQmx_Val_GroupByChannel = 0
DAQmx_Val_GroupByScanNumber= 1 #interleaved
taskHandle = TaskHandle(0)
max_num_samples = 8
pdata = numpy.zeros((max_num_samples,),dtype=numpy.float64 )
initial_data = numpy.zeros((max_num_samples,),dtype=numpy.float64 ) #probably won't need
link = None #The handle to the link object
scale = 36 #360deg/10volts #SCALING FACTOR!
#add shaft and graspers
base = vizmat.Transform() #Identity Matrix
shaft1 = vizmat.Transform() #Identity Matrix
shaft2 = vizmat.Transform() #Identity Matrix
end_eff = vizmat.Transform()#Identity Matrix
#base = viz.add('base.dae', pos = [-5,0,-5])
#shaft1 = base.add('shaft.dae', pos = [4.5,1,4.5], euler = [90,0,90])
#shaft2 = shaft1.add('shaft.dae', pos = [5,0,0], euler = [0,0,0])
end_eff = viz.add('END-EFF.dae', pos = [0,0,5], euler = [0,0,0])
def CHK(err):
if err < 0:
buf_size = 1000
buf = ctypes.create_string_buffer('\000' * buf_size)
nidaq.DAQmxGetErrorString(err,ctypes.byref(buf),bu f_size)
raise RuntimeError('nidaq call failed with error %d: %s'%(err,repr(buf.value)))
################################################## ################################################## #############
CHK(nidaq.DAQmxCreateTask("",ctypes.byref(taskHandle)))
CHK(nidaq.DAQmxCreateAIVoltageChan(taskHandle,"Dev2/ai0:7","",DAQmx_Val_RSE,float64(-10),float64(10),DAQmx_Val_Volts,None))
CHK(nidaq.DAQmxCfgSampClkTiming(taskHandle,"", #source terminal of clock
float64(9), # S/s/N (samples/sec/channel = sampling rate)
DAQmx_Val_Rising,DAQmx_Val_ContSamps,
uInt64(max_num_samples))); #(samples/sec/channel) #ContSamps
CHK(nidaq.DAQmxStartTask(taskHandle))
def Main_Loop():
while True:
pdata = numpy.zeros((max_num_samples,),dtype=numpy.float64 )
#print shaft.getEuler()
#print shaft.getPosition()
# beginning of cookbook
# http://www.scipy.org/Cookbook/Data_Acquisition_with_NIDAQmx
yield (CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
float64(1), #-1 = max read rated
DAQmx_Val_GroupByChannel,pdata.ctypes.data,max_num _samples,ctypes.byref(read),None)))
#def Kinematics():
T01 = vizmat.Transform()
T12 = vizmat.Transform()
T23 = vizmat.Transform()
T34 = vizmat.Transform()
T04 = vizmat.Transform()
################################################## ################################
#DH PARAMETERS-MEAT/POTATOES
################################################## ################################
L = 5 # LINK LENGTH (inches)
d_scale = 1 #TRANSLATION SCALE CAN BE ADJUSTED
# i = 1
a1 = 0
alpha1 = 0
d1 = 0
theta1 = pdata[0]*scale
# i = 2
a2 = 0
alpha2 = 90
d2 = 0
theta2 = pdata[7]*scale
# i = 3
a3 = L
alpha3 = 0
d3 = L
theta3 = pdata[6]*scale
# i = 4
a4 = L
alpha4 = 0
d4 = L
theta4 = pdata[3]*scale
################################################## ################################
print
print pdata
#base {0}-->{1}
T01.set(math.cos(math.radians(theta1)),-math.sin(math.radians(theta1)),0,a1,(math.sin(math .radians(theta1))*math.cos(math.radians(alpha1))), (math.cos(math.radians(theta1))*math.cos(math.radi ans(alpha1))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta1))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta1))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T01.transpose()
print 'T01', T01
#SHAFT1 {1}-->{2}
print 'theta2', theta2
print 'math.cos(math.radians(theta2))', math.cos(math.radians(theta2))
T12.set=(math.cos(math.radians(theta2)),-math.sin(math.radians(theta2)),0,a1,(math.sin(math .radians(theta2))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta2))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta2))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta2))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T12.transpose()
print 'T12', T12
#SHAFT2 {2}-->{3}
T23.set(math.cos(math.radians(theta3)),-math.sin(math.radians(theta3)),0,a1,(math.sin(math .radians(theta3))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta3))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta3))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta3))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T23.transpose()
print 'T23', T23
#End_Effector {3}-->{4}
T34.set(math.cos(math.radians(theta4)),-math.sin(math.radians(theta4)),0,a1,(math.sin(math .radians(theta4))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta4))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta4))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta4))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T34.transpose()
print 'T34', T34
T04 = T01*T12*T23*T34#T34*T23*T12*T01
matrix = end_eff.setMatrix(T04)
#print 'pdata', pdata
print 'T04', end_eff.getMatrix()
#print 'euler', end_eff.getEuler()
#print 'position', end_eff.getPosition()
viztask.schedule(Main_Loop)
I am trying to create a simple forward kinematics simulation using a DAQ USB 6008 connected to potentiometers. Each potentiometer signifies a revolute degree of freedom of a link like the following example http://demonstrations.wolfram.com/ForwardKinematics/. However, my example is only interested in the end effector position/orientation (I don't care about seeing the other links).
My problem seems to be that Vizard will only calculate the first transform (frame {0} to frame {1}), but will not do any further computations. For example, here are my printed out results:
T01 [ -0.974240, 0.225514, 0.000000, 0.000000
-0.225514, -0.974240, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
theta2 153.228278124
math.cos(math.radians(theta2)) -0.892808238702
T12 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T23 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T34 [ 1.000000, 0.000000, 0.000000, 0.000000
0.000000, 1.000000, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
T04 [ -0.974240, 0.225514, 0.000000, 0.000000
-0.225514, -0.974240, 0.000000, 0.000000
0.000000, 0.000000, 1.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000 ]
Only the first transform values are being evaluated and assigned to T04 (T04 = T01*T12*T23*T34). I am unsure of why T12 is the identity matrix, because as can be seen, theta2 has a value. Therefore, T12 should not be the identity matrix.
Below is my code. I hope someone can share something insightful!
Thanks!
import viz
import ctypes
import numpy
import viztask
import time
import threading
import math
import vizshape
import vizcam
import vizact
import vizinfo
viz.phys.enable()
viz.collision(viz.ON)
viz.go()
viz.clearcolor(0.5,0.7,1)
#Change navigation style to pivot
cam = vizcam.PivotNavigate(center=[0,0,5], distance = 15)
cam.rotateUp(-60)
cam.rotateRight(180)
#Add grid
#creation of base
#grid = vizshape.addBox(size=(10,1,8),right=True,left=True ,top=True,bottom=True,front=True,back=True,splitFa ces=False, pos = (0,-.5,0))
#base_grid = vizshape.addGrid(pos = (0,0,0))
grid = vizshape.addGrid(pos = (0,0,0), axis = vizshape.AXIS_Z)
grid.collidePlane() # Make collideable plane
#cookbook for accessing DAQ
nidaq = ctypes.windll.nicaiu # load the DLL
int32 = ctypes.c_long
uInt32 = ctypes.c_ulong
uInt64 = ctypes.c_ulonglong
float64 = ctypes.c_double
TaskHandle = uInt32
read = int32()
DAQmx_Val_Cfg_Default = int32(-1) #Default (differential)
DAQmx_Val_RSE=int32(10083) #RSE
DAQmx_Val_Volts = 10348
DAQmx_Val_Rising = 10280
DAQmx_Val_FiniteSamps = 10178
DAQmx_Val_ContSamps = 10123
DAQmx_Val_GroupByChannel = 0
DAQmx_Val_GroupByScanNumber= 1 #interleaved
taskHandle = TaskHandle(0)
max_num_samples = 8
pdata = numpy.zeros((max_num_samples,),dtype=numpy.float64 )
initial_data = numpy.zeros((max_num_samples,),dtype=numpy.float64 ) #probably won't need
link = None #The handle to the link object
scale = 36 #360deg/10volts #SCALING FACTOR!
#add shaft and graspers
base = vizmat.Transform() #Identity Matrix
shaft1 = vizmat.Transform() #Identity Matrix
shaft2 = vizmat.Transform() #Identity Matrix
end_eff = vizmat.Transform()#Identity Matrix
#base = viz.add('base.dae', pos = [-5,0,-5])
#shaft1 = base.add('shaft.dae', pos = [4.5,1,4.5], euler = [90,0,90])
#shaft2 = shaft1.add('shaft.dae', pos = [5,0,0], euler = [0,0,0])
end_eff = viz.add('END-EFF.dae', pos = [0,0,5], euler = [0,0,0])
def CHK(err):
if err < 0:
buf_size = 1000
buf = ctypes.create_string_buffer('\000' * buf_size)
nidaq.DAQmxGetErrorString(err,ctypes.byref(buf),bu f_size)
raise RuntimeError('nidaq call failed with error %d: %s'%(err,repr(buf.value)))
################################################## ################################################## #############
CHK(nidaq.DAQmxCreateTask("",ctypes.byref(taskHandle)))
CHK(nidaq.DAQmxCreateAIVoltageChan(taskHandle,"Dev2/ai0:7","",DAQmx_Val_RSE,float64(-10),float64(10),DAQmx_Val_Volts,None))
CHK(nidaq.DAQmxCfgSampClkTiming(taskHandle,"", #source terminal of clock
float64(9), # S/s/N (samples/sec/channel = sampling rate)
DAQmx_Val_Rising,DAQmx_Val_ContSamps,
uInt64(max_num_samples))); #(samples/sec/channel) #ContSamps
CHK(nidaq.DAQmxStartTask(taskHandle))
def Main_Loop():
while True:
pdata = numpy.zeros((max_num_samples,),dtype=numpy.float64 )
#print shaft.getEuler()
#print shaft.getPosition()
# beginning of cookbook
# http://www.scipy.org/Cookbook/Data_Acquisition_with_NIDAQmx
yield (CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
float64(1), #-1 = max read rated
DAQmx_Val_GroupByChannel,pdata.ctypes.data,max_num _samples,ctypes.byref(read),None)))
#def Kinematics():
T01 = vizmat.Transform()
T12 = vizmat.Transform()
T23 = vizmat.Transform()
T34 = vizmat.Transform()
T04 = vizmat.Transform()
################################################## ################################
#DH PARAMETERS-MEAT/POTATOES
################################################## ################################
L = 5 # LINK LENGTH (inches)
d_scale = 1 #TRANSLATION SCALE CAN BE ADJUSTED
# i = 1
a1 = 0
alpha1 = 0
d1 = 0
theta1 = pdata[0]*scale
# i = 2
a2 = 0
alpha2 = 90
d2 = 0
theta2 = pdata[7]*scale
# i = 3
a3 = L
alpha3 = 0
d3 = L
theta3 = pdata[6]*scale
# i = 4
a4 = L
alpha4 = 0
d4 = L
theta4 = pdata[3]*scale
################################################## ################################
print pdata
#base {0}-->{1}
T01.set(math.cos(math.radians(theta1)),-math.sin(math.radians(theta1)),0,a1,(math.sin(math .radians(theta1))*math.cos(math.radians(alpha1))), (math.cos(math.radians(theta1))*math.cos(math.radi ans(alpha1))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta1))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta1))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T01.transpose()
print 'T01', T01
#SHAFT1 {1}-->{2}
print 'theta2', theta2
print 'math.cos(math.radians(theta2))', math.cos(math.radians(theta2))
T12.set=(math.cos(math.radians(theta2)),-math.sin(math.radians(theta2)),0,a1,(math.sin(math .radians(theta2))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta2))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta2))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta2))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T12.transpose()
print 'T12', T12
#SHAFT2 {2}-->{3}
T23.set(math.cos(math.radians(theta3)),-math.sin(math.radians(theta3)),0,a1,(math.sin(math .radians(theta3))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta3))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta3))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta3))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T23.transpose()
print 'T23', T23
#End_Effector {3}-->{4}
T34.set(math.cos(math.radians(theta4)),-math.sin(math.radians(theta4)),0,a1,(math.sin(math .radians(theta4))*math.cos(math.radians(alpha1)),( math.cos(math.radians(theta4))*math.cos(math.radia ns(alpha1)))),-math.sin(math.radians(alpha1)),(-math.sin(math.radians(alpha1))*d1),(math.sin(math. radians(theta4))*math.sin(math.radians(alpha1))),( math.cos(math.radians(theta4))*math.sin(math.radia ns(alpha1))),math.cos(math.radians(alpha1)),(math. cos(math.radians(alpha1))*d1),0,0,0,1)
T34.transpose()
print 'T34', T34
T04 = T01*T12*T23*T34#T34*T23*T12*T01
matrix = end_eff.setMatrix(T04)
#print 'pdata', pdata
print 'T04', end_eff.getMatrix()
#print 'euler', end_eff.getEuler()
#print 'position', end_eff.getPosition()
viztask.schedule(Main_Loop)