| mhead10 |
12-07-2012 08:56 AM |
Simple Forward Kinematic Example
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:
Code:
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!
Code:
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,splitFaces=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),buf_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.radians(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.radians(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.radians(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.radians(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.radians(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.radians(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.radians(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.radians(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)
|
