def __init__(self, eef_id, world_id, rate=200):
self.eef_previous_pose = [[0, 0, 0], [0, 0, 0, 0]]
self.world_previous_pose = [[0, 0, 0], [0, 0, 0, 0]]
self.eef_pose = [[0, 0, 0], [0, 0, 0, 0]]
self.world_pose = [[0, 0, 0], [0, 0, 0, 0]]
self.lock = RLock()
self.thread = None
self.rate = rate
# Setting up the connection with Matlab
self.bridge = PythonInterface()
# Setting up the connection with Optitrack
print("[Matlab-Optitrack] Setting up the connection...")
self.client = NatNetClient()
print("[Matlab-Optitrack] Attempt to receive a single frame...")
self.client.receive_frame().unpack_data() # Just check once that we receive data from optitrack/NatNet
print("[Matlab-Optitrack] server connected and ready to handle requests!")
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from matlab_bridge.python_interface import PythonInterface
import baxter_pykdl
rospy.init_node('matlab_fk_server')
side = 'left'
kinematics = baxter_pykdl.baxter_kinematics(side)
bridge = PythonInterface()
joints = map(lambda joint: side + '_' + joint,
['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'])
rospy.loginfo("FK server ready to handle FK for {}_arm".format(side))
while not rospy.is_shutdown():
joint_values = bridge.read()
rospy.loginfo(" {}_arm FK request: ".format(side) + str(joint_values))
fk_request = dict(zip(joints, joint_values))
fk_result = kinematics.forward_position_kinematics(fk_request)
position = list(fk_result[:3])
orientation = list(fk_result[3:])
bridge.send(position)
