WorldViz User Forum  

Go Back   WorldViz User Forum > Vizard

Reply
 
Thread Tools Rate Thread Display Modes
  #1  
Old 10-12-2012, 09:00 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
Custom Joystick Rotation

Hello,

I'm trying to find the best way to simulate my grasper (like a tweezer- referred to as "babcock" in my code), which is driven by a custom joystick (I'm including an example picture (attachment # 1) of my joystick (left) and grasper (like a tweezer, on the right) for clarity).

I'm trying to rotate my grasper around three different axes (Rx1, Rx2, Rx3). Therefore, I'm currently thinking the best way to accomplish this is by:
  1. read in data[],
  2. assign new center to Rx1, complete euler rotation # 1,
  3. assign new center to Rx2, complete euler rotation # 2,
  4. assign new center to Rx3, complete euler rotation # 3.


However, when I assign a new center from the original grasper origin, 1 unit in the z-direction away (to obtain Rx1), the center remains at the original origin (as seen in attachment # 3).

Code:
	# move babcock up and forward a bit (1.25, 2) from World Coordinate System
	babcock.setPosition([0,1.25, 2+ data[2]],viz.ABS_GLOBAL)	 # data[2] = TRANSLATION potentiometer
	babcock_axes = vizshape.addAxes(parent = babcock, pos=[0,0,0],scale=[.05,.05,.05])
	X2 = viz.addText3D('X2',pos=[0.1,0,0],color=viz.RED,scale=[0.05,0.05,0.05],parent=babcock)
	Y2 = viz.addText3D('Y2',pos=[0,0.1,0],color=viz.GREEN,scale=[0.05,0.05,0.05],align=viz.ALIGN_CENTER_BASE,parent=babcock)
	Z2 = viz.addText3D('Z2',pos=[0,0,0.1],color=viz.BLUE,scale=[0.05,0.05,0.05],align=viz.ALIGN_CENTER_BASE,parent=babcock)
	
	# center of rotation is 3.5 from original (LCS)
	babcock.center(0, 0, 1) #change center of rotation to Z0 + 1 (DOESN'T WORK?)
	babcock_axes2 = vizshape.addAxes(parent = babcock, pos=[0,0,0], scale = [.05,.05,.05]) #CHANGE local axis to babcock (0,0,1) for Rx1 Rotation
	X3 = viz.addText3D('X3',pos=[0.1,0,0],color=viz.RED,scale=[0.1,0.1,0.1],parent=babcock)
	Y3 = viz.addText3D('Y3',pos=[0,0.1,0],color=viz.GREEN,scale=[0.1,0.1,0.1],align=viz.ALIGN_CENTER_BASE,parent=babcock)
	Z3 = viz.addText3D('Z3',pos=[0,0,0.1],color=viz.BLUE,scale=[0.1,0.1,0.1],align=viz.ALIGN_CENTER_BASE,parent=babcock)
	# why is the new babcock.center over the previous still? attached pic # 3
Furthermore, when I use the spin feature, the new center seems to work correctly.
Code:
	babcock.addAction( vizact.spin(1,0,0,90)) #for testing purposes only
Any suggestions if there is a better method using transformation matrices to accomplish this task? I would use linking, but my virtual grasper is only comprised of the tweezer (babcock) (it does not have the other links as does the joystick in the picture).

Lastly, (a less important problem currently) it seems like my scaling functions are in meters, but the numbers chosen for position are in units of inches for some reason. Is this correct? For example, if I enter a unit of 1 for the new center position distance, it definitely doesn't look like 1m, rather it's more like 1 inch. (as a frame of reference, I put a 1 inch cube (second attachment) in my VR environment, and all the sizes seem correct)

Thanks in advance!
Attached Thumbnails
Click image for larger version

Name:	transformation.JPG
Views:	435
Size:	46.1 KB
ID:	536   Click image for larger version

Name:	paint.png
Views:	427
Size:	25.7 KB
ID:	537   Click image for larger version

Name:	lcs.png
Views:	425
Size:	105.6 KB
ID:	538  
Reply With Quote
  #2  
Old 10-15-2012, 11:30 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
I think I should be able to find a solution by matrix multiplication. However, I'm having some trouble with syntax.

Can someone tell me the correct way to multiply 4x4 and 4x1 matrices?

For instance:
Code:
import vizmat
import numpy
import math

A = vizmat.Transform()
B = vizmat.Transform()

A.set(0.866,-0.5,0,0,0.5,0.866,0,0,0,0,1,0,0,0,0,1)
B=[[0],[0],[2],[0]]
#B.set(0,0,0,0,2,0,0,0,0,0,0,0,0,0,0,0)
C = A*B

print 'C = ', C
Doing this yields C = None.

Therefore, if I uncomment B.set, and comment out B, C = the correct answer of ([[-1],[1.732],[0],[0]]), but then I would need to add another step of extracting just the first column from the matrix.
Reply With Quote
  #3  
Old 10-16-2012, 07:41 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
I solved the matrix multiplication using the following great code by kai.

http://www.syntagmatic.net/matrix-mu...ion-in-python/
Reply With Quote
  #4  
Old 11-02-2012, 08:49 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
Defining & Using Multiple Centers (of Rotation)

HELLO!

I'm still trying to find out how to set multiple centers (three) for my custom joystick.

Separately, my three joystick centers (and rotations/translations) work perfectly. However, when I try and put all three of the centers (and rotations/translation) into one main function, I get three separate joysticks or the last of the centers overwrites the first two.

Can someone please tell me how I can have three successive centers (and rotation/translation functions) for my joystick?

The pseudo code would look something like:
Code:
def Kinematics_1():
	setCenter_1
	setEuler_1
	setPosition_1

def Kinematics_2():
	setCenter_2
	setEuler_2
	setPosition_2
	
def Kinematics_3():
	setCenter_3
	setEuler_3
	setPosition_3
	
def read_DAQ(): #read in data from DAQ
	data = pdata
	
def director_loop():
	read_DAQ()
	Kinematics_1()
	Kinematics_2()
	Kinematics_3()

viztask.schedule(director_loop())
My actual current code (summarized):
Code:
shaft = viz.add('shaft.dae', pos = (0,5,-5))
babcock_1 = viz.addChild('origin.dae', parent = shaft)
babcock_2 = viz.addChild('origin.dae', parent = shaft)

scale = 40 #degrees (360/9)
trans_scale = 5 #translation pot (pot #1)

def read_DAQ(): # used obtain DAQ values
	CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
		float64(-1),
		DAQmx_Val_GroupByChannel,pdata.ctypes.data,max_num_samples,ctypes.byref(read),None))
	return(pdata)
	
def Kinematics_1():	
	shaft.center(0,0,1.738)
	#POT 0- BASE ROTATIONS
	shaft.setEuler(180+scale*pdata[0],0,0)				
	
	#POT 1- TRANSLATION#
	shaft.setPosition(0,0,pdata[1]*trans_scale-3) 
	
	#pot 2: grasper open/clock
	
	babcock_1.setEuler(160-pdata[2]*scale,0,0)			
	babcock_2.setEuler(-160+pdata[2]*scale,0,180)	
	
	#POT 3: JOYSTICK TIP ROTATION
	#POT 4: FIRST U-JOINT ROTATION
	shaft.setEuler(180+scale*pdata[0],(pdata[4]*scale)+180,-(pdata[3]*scale)+90) #CORRECT(3), CORRECT(4)
	
def Kinematics_2():
	shaft.setCenter(0,-1.6125,1.738) 
	#POT 5- 1st U-Joint, 2nd Rotation
	#POT 6- 2nd U-Joing, 1st Rotation
	shaft.setEuler(0,(-pdata[5]*scale)-180,pdata[6]*scale) #reversed pot glue position 180 deg 
	
def Kinematics_3():
	shaft.setCenter(0,-3.227/2,1.738)
	shaft.setEuler(0,0,pdata[7]*scale+40)
	
def read_DAQ(): # used obtain DAQ values
	CHK(nidaq.DAQmxReadAnalogF64(taskHandle,1,
		float64(-1),
		DAQmx_Val_GroupByChannel,pdata.ctypes.pdata,max_num_samples,ctypes.byref(read),None))
	return(pdata)

def director_function_loop():
	while True:
		value0 = yield viztask.waitDirector(read_DAQ) 
		value1 = yield viztask.waitDirector(Kinematics_1)
		value2 = yield viztask.waitDirector(Kinematics_2)
		value3 = yield viztask.waitDirector(Kinematics_3)
viztask.schedule(director_function_loop())
Thanks!
Reply With Quote
  #5  
Old 11-02-2012, 02:50 PM
Jeff Jeff is offline
WorldViz Team Member
 
Join Date: Aug 2008
Posts: 2,453
Setting the center should change the rotation point but will not affect the position of the object. You should be able to modify the center as many times as you want during the course of the script. You can use the node.getCenter command to verify the center is where you expect it to be.

It's a bit difficult to understand the problem without having an example to run. Can you create an example using Vizard's included resources that reproduces the issue and can be run directly?
Reply With Quote
  #6  
Old 11-03-2012, 11:43 AM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
Jeff, thanks for the reply. I've created a simple example below for your review. I've assigned three different centers to rotate about in my Kinematic function.

Code:
import viztask
import viz
import vizshape
import vizcam
import vizact
viz.go()

viz.clearcolor(0.5,0.7,1)

#Change navigation style to pivot
cam = vizcam.PivotNavigate(center=[0,0,0], distance = 25) 
cam.rotateUp(45)
cam.rotateRight(0)

#Add grid
grid = vizshape.addGrid()
grid.color(viz.GRAY)
grid.collidePlane()   # Make collideable plane

#Add an object.
shaft = vizshape.addCylinder(height = 5, radius = .25, slices = 15)

r = 5
def Kinematics_1(rotation1):	
	shaft.center =(1,1,1)
	vizact.whilekeydown('a',shaft.setEuler,[0,vizact.elapsed(r),0],viz.REL_LOCAL)
		
	shaft.center = (2,2,2)
	vizact.whilekeydown('s',shaft.setEuler,[0,vizact.elapsed(-r),0],viz.REL_LOCAL)
	
	shaft.center = (3,3,3)
	vizact.whilekeydown('d',shaft.setEuler,[vizact.elapsed(-r),0,0],viz.REL_LOCAL)
	vizact.whilekeydown('f',shaft.setEuler,[vizact.elapsed(r),0,0],viz.REL_PARENT)

def Main_Loop(rot):
	while True:
		print 'shaft center = ', shaft.getCenter()
		yield Kinematics_1(r)
		print ''
viztask.schedule(Main_Loop(r))
The center, in this example, actually remains at (0,0,0) and does not get assigned to (1,1,1), (2,2,2), or (3,3,3). However, the keyboard rotations all work correctly. Does this make sense?

Furthermore, the 'f' key would not rotate with viz.REL_LOCAL for some reason, but does for viz.REL_PARENT (even though there is no parent present).

Thanks for your help. Hopefully I'm missing something silly.
Reply With Quote
  #7  
Old 11-06-2012, 02:16 PM
Jeff Jeff is offline
WorldViz Team Member
 
Join Date: Aug 2008
Posts: 2,453
Quote:
The center, in this example, actually remains at (0,0,0) and does not get assigned to (1,1,1), (2,2,2), or (3,3,3). However, the keyboard rotations all work correctly. Does this make sense?
The center is not changing because there is some incorrect syntax. The line:
Code:
shaft.center = (1,1,1)
should be:
Code:
shaft.center(1,1,1)
If you make that change throughout the function then it will print the center to be (3,3,3) because that's the last center command called before Kinematics_1 returns. Also, it does not appear you are using the task function properly. If the main task function yields to a subtask, the subtask should have a yield command somewhere within it. The following is an example that yields within the subtask for a key event and sets a new rotation point based on that:
Code:
import viztask
import viz
import vizshape
import vizcam
import vizact
viz.go()

viz.clearcolor(0.5,0.7,1)

#Change navigation style to pivot
cam = vizcam.PivotNavigate(center=[0,0,0], distance = 25) 
cam.rotateUp(45)
cam.rotateRight(0)

#Add grid
grid = vizshape.addGrid()
grid.color(viz.GRAY)
grid.collidePlane()   # Make collideable plane

#Add an object.
shaft = vizshape.addCylinder(height = 5, radius = .25, slices = 15)
shaft.runAction(vizact.spin(0,0,1,30))

def Kinematics_1():	
	
	d = yield viztask.waitKeyDown(('a','s','d','f'))
	if d.key == 'a':
		shaft.setCenter(0.5,0,0)
	elif d.key == 's':
		shaft.setCenter(1,0,0)
	elif d.key == 'd':
		shaft.setCenter(2,0,0)
	elif d.key == 'f':
		shaft.setCenter(0,0,0)

def Main_Loop():
	while True:
		print 'shaft center = ', shaft.getCenter()
		yield Kinematics_1()
		print ''
viztask.schedule(Main_Loop())
Quote:
Furthermore, the 'f' key would not rotate with viz.REL_LOCAL for some reason, but does for viz.REL_PARENT (even though there is no parent present).
The shaft object is a child of the world so viz.REL_PARENT in this case refers to world coordinates.
Reply With Quote
  #8  
Old 11-08-2012, 01:26 PM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
Jeff,

I'm getting closer, but still not quite there. I can get the centers assigned correctly, but my simulation displays my tool in what looks like 3 different places at the same time still (attached).

I think the centers, eulers, and positions are not getting assigned quite quickly enough, therefore, there's a small lag between each.

Is there a better way to do what I'm trying to accomplish (get the shaft to rotate/position with three separate centers in one visual piece)? Altering the subtasks with yields (setCenters) hasn't changed anything.

Thanks again for your help!

Updated Code:
Code:
def Kinematics_1():	
	scale = 40 #degrees (360/9)
	trans_scale = 1 #translation pot (pot #1)
	x = 180
	
	shaft.center(0,0,1.738)
	#POT 0- BASE ROTATIONS#
	yield shaft.setEuler(180+scale*pdata[0],0,0)				

	#POT 1- TRANSLATION#
	yield shaft.setPosition(0,0,pdata[1]*trans_scale-3, viz.ABS_PARENT) 
	
	#pot 2: grasper open/clock
	if pdata[2]*scale > x: 	#graspers are not touching
		#print 'graspers NOT touching'
		babcock_1.setEuler(x-pdata[2]*scale,0,0,viz.ABS_PARENT)
		babcock_2.setEuler(-x+pdata[2]*scale,0,180,viz.ABS_PARENT)
	else: 					#graspers are touching
		#print 'graspers touching'
		babcock_1.setEuler(x/scale,0,0,viz.ABS_PARENT)
		babcock_2.setEuler((x/scale),0,180,viz.ABS_PARENT)	
	
	#POT 3: JOYSTICK TIP ROTATION
	#POT 4: FIRST U-JOINT ROTATION
	yield shaft.setEuler(180+scale*pdata[0],(pdata[4]*scale)+180,-(pdata[3]*scale)+90) 
		
#def Kinematics_2():
	#alter center point of rotation
	shaft.setCenter(0,-1.6125,1.738)
	#POT 5- 1st U-Joint, 2nd Rotation
	#POT 6- 2nd U-Joing, 1st Rotation
	yield shaft.setEuler(0,(-pdata[5]*scale)-180,pdata[6]*scale) 
	
#def Kinematics_3():
	scale = 40
	#alter center point of rotation
	shaft.setCenter(0,-3.227,1.738)
	yield shaft.setEuler(0,0,pdata[7]*scale+40)

def Main_Loop():
	while True:
		yield Kinematics_1()

viztask.schedule(Main_Loop())
Attached Thumbnails
Click image for larger version

Name:	help.png
Views:	416
Size:	40.5 KB
ID:	545  
Reply With Quote
  #9  
Old 11-25-2012, 03:21 PM
mhead10 mhead10 is offline
Member
 
Join Date: Mar 2012
Posts: 40
I realize this is hard to troubleshoot, but does no response mean you're out of ideas?
Reply With Quote
  #10  
Old 11-26-2012, 12:06 PM
Jeff Jeff is offline
WorldViz Team Member
 
Join Date: Aug 2008
Posts: 2,453
Sorry for the delay responding. As I mentioned before it's best to post simple example code that shows the problem clearly and that can be run directly.

It sounds like your questions are project specific. You may be interested in our closed support forum where you can get more assistance with that. For more information please contact sales@worldviz.com.
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
joystick position nmohandes Vizard 2 01-16-2012 10:03 AM
writing joystick position to a data file Saz Vizard 3 12-17-2008 05:18 AM
Joystick Navigation Vinicius Lima Vizard 7 10-23-2007 10:42 AM
Facetracking and Immersion Joystick Vygreif Vizard 1 01-25-2006 10:56 AM
animating custom faces: in search of "open_mouth" morphs vr_boyko Vizard 1 09-16-2004 10:30 AM


All times are GMT -7. The time now is 05:43 PM.


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