Physics and Haptics

[jalvarez]

I want to use a haptic device to grasp a part, enable collision between multiple parts, and allow the user to feel the force through a Phantom Omni.

First I noticed that manipulating the objects directly interferes with the physics solver, so I tried adding a spring between the part and the marker so that i have position and orientation control through the spring. However when i add the angular Kd and Ks it doesnt give me orientation control. What is the correct way of adding a Torsional spring upon pressing the omni's button?

I have also sat the accuracy and step size of my simulation because the physics were non realistic. However there are still some issues (the parts should not go through one another)

I want to calculate the spring force (which is simply subtracting the
position of Omni marker and the grasped part). However I am not sure how
to render a specific haptic force. I have used constant effect.

Here is my code

thank you in advance for your help



import viz
import math
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.collideMesh()
cyl.enable(viz.COLLIDE_NOTIFY)

#spring = cyl.addSpring(viz.LINK_POS,linearKd=100,linearKs=1 0000, angularKs=100, angularKd=10)

sphere = viz.add('sphere.wrl',pos=[-1,2,5])
sphere.collideSphere()
sphere.disable(viz.DYNAMICS)

hd.add(cyl)
hd.add(sphere)

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)

#add spring effect for collision
ce = hd.ConstantEffect(dir=[0,0,-1],mag=1)

def hdbuttondown(button):

if button == hd.BUTTON1:

objects = hd.get(hd.TOUCH_OBJECTS)
for obj in objects:
#Begin dragging the object
hd.drag(obj)
#Disable the haptic object while dragging it
obj.enablehd(0)
viz.starttimer(0,0,viz.FOREVER)

def hdbuttonup(button):
#Check if button 1 is released
if button == hd.BUTTON1:
for obj in hd.get(hd.DRAG_OBJECTS):
#Re-enable the haptic object
obj.enablehd(1)
#Stop dragging all objects
hd.undrag()

def collision(e):
#on collision render a spring force to the omni
if e.obj1 == cyl:
poscyl=cyl.getPosition()
posMarker=marker.getPosition()
#calculate the force based on position
force=[posMarker[0] - poscyl[0], posMarker[1] - poscyl[1], posMarker[2] - poscyl[2]]

ce.setMagnitude(30*vizmat.Distance(poscyl,posMarke r))

ce.setDirection(force)
ce.start()


def notcollision(e):
#on collision render a spring force to the omni
ce.stop()


viz.callback(viz.COLLIDE_BEGIN_EVENT,collision)
viz.callback(viz.COLLIDE_END_EVENT,notcollision)
hd.callback(hd.BUTTONDOWN_EVENT,hdbuttondown)
hd.callback(hd.BUTTONUP_EVENT,hdbuttonup)