View Single Post
  #1  
Old 05-30-2016, 04:30 AM
M@rcello M@rcello is offline
Member
 
Join Date: May 2016
Posts: 11
Omni Velocity information

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!!
Reply With Quote