View Single Post
  #1  
Old 12-07-2012, 09:56 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
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)

Last edited by mhead10; 12-07-2012 at 09:57 AM. Reason: subscribe to thread
Reply With Quote