PDA

View Full Version : Simple Forward Kinematic Example


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)

Jeff
12-07-2012, 09:40 AM
Can you create a simple example that reproduces the problem and runs directly with Vizard's included resources?

mhead10
12-07-2012, 09:48 AM
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
12-10-2012, 08:00 AM
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.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)

mhead10
12-10-2012, 08:12 AM
[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
12-11-2012, 10:15 AM
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.