WorldViz User Forum  

Go Back   WorldViz User Forum > Vizard

Reply
 
Thread Tools Rate Thread Display Modes
  #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
  #2  
Old 12-07-2012, 10:40 AM
Jeff Jeff is offline
WorldViz Team Member
 
Join Date: Aug 2008
Posts: 2,459
Can you create a simple example that reproduces the problem and runs directly with Vizard's included resources?
Reply With Quote
  #3  
Old 12-07-2012, 10:48 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
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.
Reply With Quote
  #4  
Old 12-10-2012, 09:00 AM
JimC JimC is offline
Member
 
Join Date: Jun 2009
Posts: 42
Just quickly looking, the reason T12 isn't getting set is probably the error (extra '=') in this line:

Code:
		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)
Reply With Quote
  #5  
Old 12-10-2012, 09:12 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
[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!
Reply With Quote
  #6  
Old 12-11-2012, 11:15 AM
JimC JimC is offline
Member
 
Join Date: Jun 2009
Posts: 42
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.
Reply With Quote
Reply

Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

BB code is On
Smilies are On
[IMG] code is On
HTML code is Off

Forum Jump

Similar Threads
Thread Thread Starter Forum Replies Last Post
Simple Game Design with Vizard 3.0 Apostolos Vizard 1 07-27-2011 01:39 PM
moving object forward backward Zword Vizard 4 01-20-2011 09:21 AM
simple VRPN question mjabon Precision Position Tracker (PPT) 2 08-25-2009 11:12 PM
Simple Head Tracker PHart Vizard 1 05-10-2009 06:35 PM
Kinda simple but I can't find out how... TrashcanPatrol Vizard 1 01-09-2009 12:53 AM


All times are GMT -7. The time now is 05:01 AM.


Powered by vBulletin® Version 3.8.7
Copyright ©2000 - 2019, vBulletin Solutions, Inc.
Copyright 2002-2018 WorldViz LLC