Torsional Spring

[jalvarez]

I want to use a haptic device to control a rigid object through a linear spring and a torsion spring. The position works correctly. However the orientation does not work even the orientation data printed out is correct.

Any suggestions? Here is the code:

import viz
import hd

viz.go()
viz.phys.enable()
viz.phys.setGravity(0,0,0)
viz.phys.setAccuracy(4)
viz.phys.setStepSize(.001)
viz.clearcolor(1,1,1)

cyl = viz.add('cylinder.wrl',pos=[0,2,5])
cyl.collideBox()

marker = viz.add('marker.wrl',pos=[0,2,5])
marker.scale(0.1,0.1,0.1)
hd.marker(marker)
hd.translate(0,2,5)

spring =
cyl.addSpring(viz.LINK_POS|viz.LINK_ORI,linearKd=10,linearKs=1000,angularKd=
10,angularKs=1000)
#spring = cyl.addSpring(viz.LINK_ORI,angularKd=10,angularKs=1000)

spring.setPosition(marker.getPosition())
spring.setQuat(marker.getQuat())


def mytimer(num):
    spring.setPosition(marker.getPosition())
    #print marker.getQuat()
    spring.setQuat(marker.getQuat())


viz.callback(viz.TIMER_EVENT, mytimer)
viz.starttimer( 0, 0, viz.FOREVER )

[farshizzo]

We don't currently have a Phantom Omni here, so I can't test your code. But your angular spring constant seems quite high. Try using the following values to see if they produce better results:

spring = cyl.addSpring(viz.LINK_POS|viz.LINK_ORI,linearKd=10,linearKs=1000,angularKd=1,angularKs=10)

[jalvarez]

I have already played with changing the order of magnitude of the constants back and forth. and it does not really give me control. the position works just fine, but the orientation does not work even if you try to manually input it from the command line.

Is there any other things I should try? or look into?

[farshizzo]

Sorry for the delay, but we just got our Phantom Omni in for testing. I tried it out and have the same problems as you. It seems the problem is related to the fact that the center of the cylinder object is not at the local origin of the object. This is causing problems with the physics engine and creating large angular acceleration values. I created a separate cylinder model that was centered properly and now it works fine. Here is the WRL text for the new cylinder object:

Transform {
	rotation 1 0 0 1.5707963267948966
	children [
		Shape {
			appearance Appearance {
				material Material {
					diffuseColor 0.8353 0.6039 0.898
				}
			}
			geometry Cylinder { radius 0.05 height 0.5 }
		}
	]
}

Save this to a file called pen.wrl and place it in the same directory as the following script:

import viz
import hd

viz.go()
viz.phys.enable()
viz.phys.setGravity(0,0,0)

cyl = viz.add('pen.wrl')
cyl.collideBox([1,1,1])

marker = viz.add('marker.wrl')
marker.scale(0.1,0.1,0.1)
hd.marker(marker)
hd.translate(0,2,5)

spring = cyl.addSpring(viz.LINK_ORI|viz.LINK_POS,linearKd=10,linearKs=100,angularKd=1,angularKs=20)

def UpdateSpring():
	spring.setPosition(hd.get(hd.POSITION))
	spring.setQuat(hd.get(hd.ROTATION))
vizact.ontimer(0,UpdateSpring)

Let me know if this solves your problems.