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 )