def getObjPos(ObjName):
client = natnet.NatClient(client_ip = '127.0.0.1', data_port = 1511, comm_port = 1510)
obj = client.rigid_bodies[ObjName]
if obj.tracking_valid:
return obj.position
else:
return None
def getObjPos(ObjName):
client = natnet.NatClient(client_ip='127.0.0.1',
data_port=1511,
comm_port=1510)
obj = client.rigid_bodies[ObjName]
if obj.seen:
x = obj.position[0]
y = -obj.position[1]
z = -obj.position[2]
objPos = [x, y, z] # make a line
return objPos
else:
return None
def __init__(self):
''' ROS node for natnet client'''
self.read_parameters()
try:
self.client = natnet.NatClient(client_ip=self.client_ip,
server_ip=self.server_ip,
data_port=self.data_port,
comm_port=self.comm_port,
read_rate=self.read_rate)
except:
rospy.logerr(
"Error at connecting to the server. Please check the ip address."
)
exit()
def get_rigid_body(rigid_body):
try:
import natnetclient as natnet
client = natnet.NatClient()
try:
led = client.rigid_bodies[rigid_body]
except KeyError:
raise KeyError("No Motive Rigid Body detected named '{}'.".format(rigid_body))
if led.position is None:
raise IOError("Motive is not sending rigid body positions")
except ConnectionResetError:
raise ConnectionResetError("Cannot detect Tracking Client. Is your tracking system sending data?")
return led
def test_total_tracking():
myarduino = vrl.Arduino.from_experiment_type(port='COM9', baudrate=250000, experiment_type='Total')
mystim = vrl.Stimulus(position=(0, 0), color=(1, 1, 1))
mystim.mesh.point_size = 5
mystim.mesh.scale.xyz = .03, .2, 1
client = natnet.NatClient()
led = client.rigid_bodies['LED']
trials = random.randint(5, 16)
myexp = vrl.TotalExperiment(arduino=myarduino,
stim=mystim,
on_width=[.01, .3],
rigid_body=led,
trials=trials,
screen_ind=1,
fullscreen=True)
myexp.run()
assert trials == myexp.data.values[-2]
assert len(myexp.data.values) == 5 * 500 * trials # each trial, 500 packets, each packet 5 elements
pyglet.options['debug_gl'] = False
from pyglet.window import key
import ratcave as rc
import _transformations as trans
import numpy as np
import natnetclient
import socket
import pickle
import pdb
RIGID_BODY_NAME = 'Rat'
ADJUSTMENT_STEP_SIZE = .001
# Connect to Motive and get tracked object
try:
tracker = natnetclient.NatClient()
except socket.error as excpt:
raise IOError("NatNet Server not detected. Confirm that Motive is streaming data to localhost!")
try:
rbody = tracker.rigid_bodies[RIGID_BODY_NAME]
assert bool(rbody), "No rigid body position found. Make sure rigid bodies are streaming from Motive (off in Motive now by default)"
except KeyError:
raise KeyError("Rigid Body {} not detected. Add it in Motive, so we can track it!".format(RIGID_BODY_NAME))
print(tracker.rigid_bodies)
# Load up Projector
with open('projector_data.pickle') as f:
projdict = pickle.load(f)
import natnetclient as natnet
client = natnet.NatClient(client_ip='192.168.1.101',
data_port=1511,
comm_port=1510)
hand = client.rigid_bodies[
'Rigid Body1'] # Assuming a Motive Rigid Body is available that you named "Hand"
print(hand.position)
print(hand.rotation)
hand_markers = hand.markers # returns a list of markers, each with their own properties
print(hand_markers[0].position)
dest='body_name',
action='store',
type=str,
default="Plane",
help=
'The body name of the optitrack rigid body, if not inputted, will send first key'
)
parser.add_argument('-v',
"--verbose",
action='store_true',
help='Turns on verbose messages.')
args = parser.parse_args()
# Connect to Motive
try:
client = natnet.NatClient(client_ip=args.natnet_ip,
data_port=1511,
comm_port=1510)
except Exception as e:
print("Could not connect to NatNet computer at IP " + args.natnet_ip)
print(e)
sys.exit(0)
# Connect to the drone
# vehicle = dronekit.connect(args.mav_link, wait_ready=False, rate=200, heartbeat_timeout=0)
#print client.rigid_bodies
# embed()
## Create the ros node
rospy.init_node('CameraNode', anonymous=True)
rate = rospy.Rate(100) # run the loop at these many Hz
pub = rospy.Publisher('CameraStream', Camera, queue_size=10)
# pub_plane = rospy.Publisher('CameraStream_Plane', Camera, queue_size = 10)
# pub_pole = rospy.Publisher('CameraStream_Pole', Camera, queue_size = 10)
import pyglet
pyglet.options['debug_gl'] = False
pyglet.options['debug_gl_trace'] = False
import ratcave as rc
import natnetclient as natnet
from Stimulus.stimulus import Stimulus
# initiate a natnet object
client = natnet.NatClient()
LED = client.rigid_bodies['LED']
# create a window and project it on the display of your choice
platform = pyglet.window.get_platform()
display = platform.get_default_display()
screen = display.get_screens()[1]
mywin = pyglet.window.Window(fullscreen=True, screen=screen, vsync=False)
fr = pyglet.window.FPSDisplay(mywin)
# initialize a stim object
plane = rc.WavefrontReader(rc.resources.obj_primitives).get_mesh(
'Plane', drawmode=rc.POINTS)
plane.point_size = 4.
plane.position.xyz = 0, 0, -3
plane.rotation.x = 0
plane.scale.xyz = .2
# plane.uniforms['diffuse'] = [0., 0., 0.]
@mywin.event
def on_draw():
mywin.clear()
import pyglet
pyglet.options['debug_gl'] = False
pyglet.options['debug_gl_trace'] = False
import ratcave as rc
import natnetclient as natnet
# initiate a natnet object
client = natnet.NatClient() # read_rate=1000
LED = client.rigid_bodies['LED']
# create a window and project it on the display of your choice
platform = pyglet.window.get_platform()
display = platform.get_default_display()
screen = display.get_screens()[1]
mywin = pyglet.window.Window(fullscreen=True, screen=screen, vsync=False)
fr = pyglet.window.FPSDisplay(mywin)
# initialize a stim object
plane = rc.WavefrontReader(rc.resources.obj_primitives).get_mesh(
'Plane', drawmode=rc.POINTS)
plane.point_size = 10.
plane.position.xyz = 0, 0, -3
plane.rotation.x = 0
plane.scale.xyz = .2
# plane.uniforms['diffuse'] = [0., 0., 0.]
@mywin.event
def on_draw():
mywin.clear()
def master_loop(q_controls, q_gains, q_ref, q_data):
global ref
# Initialize natnet client
client = natnet.NatClient(client_ip='127.0.0.1',
data_port=1511,
comm_port=1510)
uav_tracker = client.rigid_bodies['UAV']
# Initialize PCtoRC
#uav_controller = pctorc.UAVController()
#Initialize ARDrone
uav_controller = ardrone_wrapper.ARDroneController()
uav_controller.navdata_ready.wait()
uav_controller.trim()
# Initialize ERG
dic = spio.loadmat("./ss_decoupled_final_mat_only.mat")
ss = (np.matrix(dic["A"]), np.matrix(dic["B"]), np.matrix(dic["C"]),
np.matrix(dic["D"]))
#P = np.matrix(np.load("./erg/ss_v1_P.npy"))
P = np.matrix(spio.loadmat("./ss_decoupled_final_P_optimal.mat")["P"])
# Create constraints
delta = 0.4
zeta = 0.5
c1 = erg.WallConstraint(np.array([[1, 1, 0]]).T, 1, delta, zeta)
c2 = erg.WallConstraint(np.array([[-1, -1, 0]]).T, 1, delta, zeta)
c3 = erg.WallConstraint(np.array([[1, -1, 0]]).T, 1, delta, zeta)
c4 = erg.WallConstraint(np.array([[-1, 1, 0]]).T, 1, delta, zeta)
c5 = erg.SphereConstraint(np.array([[0.5, 0.5, -1.2]]).T, 0.1, delta, zeta)
c6 = erg.CylinderConstraint(np.array([[0.5, 0.5]]).T, 0.1, 0.4, 0.5)
uav_erg = erg.ERG(ss, P, [c1, c6], 2, 0.01)
ref_mod = np.copy(ref)
# Initialize variables
is_watching = True
is_recording = False
measurements = np.zeros([0, 18])
#Main loop
iframe = client.iFrame
timestamp = client.timestamp
while is_watching:
# Get natnet data
client.get_data()
# Get GUI controls
while True:
try:
c = q_controls.get_nowait()
if c == GuiControls.ON_OFF:
if uav_controller.state.fly_mask:
uav_controller.landing = True
else:
uav_controller.takingoff = True
elif c == GuiControls.EMERGENCY:
pass
elif c == GuiControls.CONTROL:
uav_controller.controlling = not uav_controller.controlling
elif c == GuiControls.RECORD:
is_recording = not is_recording
if not is_recording:
np.save(
time.strftime("recordings/%Y-%m-%d-%H%M.npy",
time.localtime()), measurements)
measurements = np.zeros([0, 18])
elif c == GuiControls.QUIT:
uav_controller.controlling = False
is_watching = False
time.sleep(1)
except:
break
# Update gains and reference
try:
kpxy, kpz, kdxy, kdz = q_gains.get_nowait()
control.Kp_pos = np.array([[kpxy, kpxy, kpz]]).T
control.Kd_pos = np.array([[kdxy, kdxy, kdz]]).T
except:
pass
try:
r = q_ref.get_nowait()
ref = np.matrix(r).T
except:
pass
iframe_old = iframe
iframe = client.iFrame
"""if iframe == iframe_old:
continue"""
# Update time step
timestamp_old = timestamp
timestamp = client.timestamp
pc_time = time.time()
dt = timestamp - timestamp_old
# Get measurements
p_meas = np.matrix(uav_tracker.position).T
q_meas = np.matrix(uav_tracker.quaternion).T
r_meas = np.matrix(quat.q_to_euler(uav_tracker.quaternion, 'xyzw')).T
# Filter
p_est, v_est = kalman.update(p_meas, dt)
# ERG
x = np.concatenate((p_meas, v_est), axis=0)
ref_mod[0:3] = uav_erg.compute_reference(x, ref[0:3], ref_mod[0:3], dt)
ref_mod[3] = ref[3] # forward psi ref
# Control
Tc, phic, thetac, psic = control.outerloop(p_est, v_est, r_meas[0],
ref_mod)
# Send command to UAV
uav_controller.command = uav_controller.normalize_command(
np.array([phic, thetac, Tc, psic]))
uav_controller.send_control()
# Update recordings
meas = np.concatenate([[[iframe, pc_time, timestamp]], p_meas.T,
q_meas.T, ref.T, ref_mod.T],
axis=1)
q_data.put(meas)
if is_recording:
measurements = np.append(measurements, meas, axis=0)
time.sleep(0.002)
# Finalize recording
if is_recording:
np.save(
time.strftime("recordings/%Y-%m-%d-%H%M.npy", time.localtime()),
measurements)
uav_controller._close()
def client():
return natnet.NatClient()
