Hi, do you know if there is the chance to get the velocity information from the Omni.
The problem is that i have attacched a ball to the omni, and i am able to control the pose of the ball with it.
What i want to do is to use velocity like trigger to send forces from the device to the operator.
This is my code:
Code:
import viz
import hd
import vizinfo
import time
import math
import cmath
from datetime import datetime
viz.go()
ball=viz.add('white_ball.wrl')
viz.MainView.setPosition([0,-0.5,-5])
hd.marker(ball)
start_box=viz.add('box.wrl')
start_box.setPosition([2.5,-0.7,3])
start_box.color([0,256,0])
start_box.setScale([0.5,0.5,0.5])
start_box.collideBox()
finish_box=viz.add('box.wrl')
finish_box.setPosition([-2.5,-0.7,3])
finish_box.setScale([0.5,0.5,0.5])
finish_box.collideBox()
pos_goal=[2.35,-0.4,0]
#print pos_new[0],pos_new[1],pos_new[2]
vizinfo.add('To start reach the green zone and then reach the red one')
error=[0,0,0]
vel=[0,0,0]
Kp=([0.05,0,0],[0,0.05,0],[0,0,0.05])
pos_hidden=[0,0,0]
def print_pos():
t1 = current_milli_time()
delay=t1-t0
print delay
if (delay>3000):
for i in range(0,3):
pos=hd.get(hd.POSITION)
error[i]=-pos_goal[i]+pos_hidden[i]
vel[i]=-Kp[i][i]*error[i]
pos_hidden[i]=pos_hidden[i]+vel[i]
ce = hd.ConstantEffect(dir=[pos_hidden[0]-pos[0],pos_hidden[1]-pos[1],0],mag=0.2)
ce.trigger(0.2)
# print pos_goal[0],pos_goal[1],pos_goal[2]
# print error[0],error[1],error[2]
# print vel[0],vel[1],vel[2]
# print pos_hidden[0],pos_hidden[1],pos_hidden[2]
current_milli_time = lambda: int(round(time.time() * 1000))
def start():
global t0
t0 = current_milli_time()
vizact.ontimer(0.1,print_pos)
vizact.onkeydown(' ',start)
Thanks in advance!!