joint_names = ['base', 'j2', 'j3', 'j4', 'target']
ids = [0, 1, 2, 3, 4]
#Tracking class
print("Starting streaming client now...")
streamingClient = NatNetClient(server_ip,
multicastAddress,
verbose=print_trak_data)
NatNet = NatNetFuncs()
streamingClient.newFrameListener = NatNet.receiveNewFrame
streamingClient.rigidBodyListListener = NatNet.receiveRigidBodyFrameList
prev_frame = 0
time.sleep(0.5)
streamingClient.run()
time.sleep(3)
track_data = data(joint_names, ids)
track_data.parse_data(
NatNet.joint_data,
NatNet.frame) #updates the frame and data that is being used
#debug values
print_cartesian = True
#Control code
print("Arming motors now...")
motors.arm()
time.sleep(2)
error_cum = 100 #initialize break error to a large value
zero_position = deepcopy(motors.motor_pos)
motor_command = deepcopy(zero_position)
#---------------------------------------#---------------------------------------
save_data = False
#Tracking class
print("Starting streaming client now...")
streamingClient = NatNetClient(server_ip,
multicastAddress,
verbose=print_trak_data)
NatNet = NatNetFuncs()
streamingClient.newFrameListener = NatNet.receiveNewFrame
streamingClient.rigidBodyListListener = NatNet.receiveRigidBodyFrameList
prev_frame = 0
time.sleep(0.5)
streamingClient.run()
time.sleep(0.5)
track_data = data(optitrack_joint_names, ids)
track_data.parse_data(
NatNet.joint_data,
NatNet.frame) #updates the frame and data that is being used
old_frame = track_data.frame
#---------------------------------------------
#helper functions
#---------------------------------------------
def calculate_sine(dt, count, hz, amplitude):
return np.sin(count / dt * (2 * pi) * hz) * amplitude
#---------------------------------------------