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)
#!/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)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from matlab_bridge.python_interface import PythonInterface
from baxter_commander import ArmCommander
from baxter_commander.persistence import dicttostate, statetodict
rospy.init_node('matlab_joint_commander')
side = 'left'
simulation = True
bridge = PythonInterface()
joints = map(lambda joint: side + '_' + joint,
['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'])
rospy.loginfo(
"Starting the commander... Don't forget to roslaunch baxter_commander commander.launch if it does not end"
)
commander = ArmCommander(side)
rospy.loginfo("{} commander started!".format(side))
rospy.loginfo(
"Matlab joint commander server ready to receive commands for {}_arm {}".
format(side, 'in simulation only' if simulation else 'to the real robot'))
while not rospy.is_shutdown():
joint_values = bridge.read()
rospy.loginfo("{}_arm command received: ".format(side) + str(joint_values))
target = {'name': joints, 'position': list(joint_values)}
commander.move_to_controlled(dicttostate(target), display_only=simulation)
#current_angles = statetodict(commander.get_current_state())
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from matlab_bridge.python_interface import PythonInterface
from baxter_commander import ArmCommander
from baxter_commander.persistence import dicttostate, statetodict
rospy.init_node('matlab_joint_commander')
side = 'left'
simulation = True
bridge = PythonInterface()
joints = map(lambda joint: side+'_'+joint, ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'])
rospy.loginfo("Starting the commander... Don't forget to roslaunch baxter_commander commander.launch if it does not end")
commander = ArmCommander(side)
rospy.loginfo("{} commander started!".format(side))
rospy.loginfo("Matlab joint commander server ready to receive commands for {}_arm {}".format(side, 'in simulation only' if simulation else 'to the real robot'))
while not rospy.is_shutdown():
joint_values = bridge.read()
rospy.loginfo("{}_arm command received: ".format(side) + str(joint_values))
target = {'name': joints, 'position': list(joint_values)}
commander.move_to_controlled(dicttostate(target), display_only=simulation)
#current_angles = statetodict(commander.get_current_state())
#rospy.loginfo('Target reached! Sending back current angles {}'.format(str(current_angles)))
endpoint = list(commander.endpoint_pose()[0])
bridge.send(endpoint)
class MatlabOptitrackServer(object):
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!")
def _threaded_update(self):
while True:
data = self.client.receive_frame().unpack_data()
with self.lock:
self.eef_previous_pose = self.eef_pose
self.world_previous_pose = self.world_pose
self.eef_pose = self.get_pose(eef_id, data)
self.world_pose = self.get_pose(world_id, data)
# No sleep needed, data availability is blocking
@staticmethod
def get_pose(id, data):
"""
Returns the pose of Rigid body id from data, None if not found
"""
for rb in data['rb']:
if rb['id'] == id:
return rb['position'], rb['orientation']
return None
def is_world_visible(self):
return not (allclose(self.world_previous_pose[0], self.world_pose[0]) and\
allclose(self.world_previous_pose[1], self.world_pose[1]))
def is_eef_visible(self):
return not (allclose(self.eef_previous_pose[0], self.eef_pose[0]) and\
allclose(self.eef_previous_pose[1], self.eef_pose[1]))
def start(self):
self.thread = Thread(target=self._threaded_update)
self.thread.setDaemon(True)
self.thread.start()
while True:
self.bridge.read()
print("[Matlab-Optitrack] Received request for object {} with respect to {}".format(eef_id, world_id))
relative_pose_eef = [[0, 0, 0], [0, 0, 0, 0]]
with self.lock:
if self.eef_pose is None or self.world_pose is None:
print("[Matlab-Optitrack] ERROR End effector is {}, World is {}, sending a vector of zeros".format('unknown' if self.eef_pose is None else 'known',
'unknown' if self.world_pose is None else 'known'))
elif not self.is_eef_visible() or not self.is_world_visible():
print("[Matlab-Optitrack] Objects are known but not visible, sending a vector of zeros")
else:
relative_pose_eef = multiply_transform(inverse_transform(self.world_pose), self.eef_pose)
print("[Matlab-Optitrack] Sending relative pose {}".format(str(relative_pose_eef)))
self.bridge.send(relative_pose_eef)