Simple Forward Kinematic Example

[mhead10]

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,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)

[Jeff]

Can you create a simple example that reproduces the problem and runs directly with Vizard's included resources?

[mhead10]

I tried to create an easier example but I was unsuccessful. I am not sure how to replace the DAQ for instance with a keyboard. Can you tell if my syntax where I assign values to the transformations is correct? The only difference between transformations is the theta value. Therefore all transformations should work, or none should work.

[JimC]

Just quickly looking, the reason T12 isn't getting set is probably the error (extra '=') in this line:

		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)

[mhead10]

[QUOTE=JimC;14435]Just quickly looking, the reason T12 isn't getting set is probably the error (extra '=') in this line:

Ahh, JimC, nice find! Unfortunately, this still yields the same results for some reason. I do appreciate your time though!

[JimC]

Oh, this may also not the only problem but I'm pretty sure your loop function is a no-no. You need to define a function that sets the matrices once, the use a timer to have it called repeatedly.