Custom Joystick Rotation

[mhead10]

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:


read in data[],
assign new center to Rx1, complete euler rotation # 1,
assign new center to Rx2, complete euler rotation # 2,
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).


# 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.
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!

[mhead10]

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:
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.

[mhead10]

I solved the matrix multiplication using the following great code by kai.

http://www.syntagmatic.net/matrix-multiplication-in-python/

[mhead10]

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:

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

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_nu m_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!

[Jeff]

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?

[mhead10]

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.


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.

[Jeff]

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:
shaft.center = (1,1,1)
should be:
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:
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())

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.

[mhead10]

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:

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

[mhead10]

I realize this is hard to troubleshoot, but does no response mean you're out of ideas?

[Jeff]

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.