def __init__(self,
name,
ip=YMC.IP,
port=YMC.PORTS["left"]["server"],
bufsize=YMC.BUFSIZE,
motion_timeout=YMC.MOTION_TIMEOUT,
comm_timeout=YMC.COMM_TIMEOUT,
process_timeout=YMC.PROCESS_TIMEOUT,
from_frame='tool',
to_frame='base',
debug=YMC.DEBUG,
log_pose_histories=False,
log_state_histories=False,
motion_planner=None):
'''Initializes a YuMiArm interface. This interface will communicate with one arm (port) on the YuMi Robot.
This uses a subprocess to handle non-blocking socket communication with the RAPID server.
Parameters
----------
name : string
Name of the arm {'left', 'right'}
ip : string formated ip address, optional
IP of YuMi Robot.
Default uses the one in YuMiConstants
port : int, optional
Port of target arm's server.
Default uses the port for the left arm from YuMiConstants.
bufsize : int, optional
Buffer size for ethernet responses
motion_timeout : float, optional
Timeout for motion commands.
Default from YuMiConstants.MOTION_TIMEOUT
comm_timeout : float, optional
Timeout for non-motion ethernet communication.
Default from YuMiConstants.COMM_TIMEOUT
process_timeout : float, optional
Timeout for ethernet process communication.
Default from YuMiConstants.PROCESS_TIMEOUT
from_frame : string, optional
String name of robot arm frame.
Default to "tool"
to_frame : string, optional
String name of reference for robot frame
Default to "base"
debug : bool, optional
Boolean to indicate whether or not in debug mode. If in debug mode no ethernet communication is attempted. Mock responses will be returned.
Default to YuMiConstants.DEBUG
log_pose_histories : bool, optional
If True, uses yumi_history_logger to log pose histories. Enables usage of flush_pose_histories.
Defaults to False
log_state_histories : bool, optional
If True, uses yumi_history_logger to log state histories. Enables usage of flush_state_histories.
Defaults to False
motion_planner : YuMiMotionPlanner, optional
If given, will use for planning trajectories in joint space.
Defaults to None
'''
self._motion_timeout = motion_timeout
self._comm_timeout = comm_timeout
self._process_timeout = process_timeout
self._ip = ip
self._port = port
self._bufsize = bufsize
self._from_frame = from_frame
self._to_frame = to_frame
self._debug = debug
self._name = name
if log_pose_histories:
self._pose_logger = YuMiMotionLogger()
if log_state_histories:
self._state_logger = YuMiMotionLogger()
self.start()
self._last_sets = {
'zone': None,
'speed': None,
'tool': None,
'gripper_force': None,
'gripper_max_speed': None,
}
self._motion_planner = motion_planner
class YuMiArm:
""" Interface to a single arm of an ABB YuMi robot.
Communicates with the robot over Ethernet.
"""
def __init__(self,
name,
ip=YMC.IP,
port=YMC.PORTS["left"]["server"],
bufsize=YMC.BUFSIZE,
motion_timeout=YMC.MOTION_TIMEOUT,
comm_timeout=YMC.COMM_TIMEOUT,
process_timeout=YMC.PROCESS_TIMEOUT,
from_frame='tool',
to_frame='base',
debug=YMC.DEBUG,
log_pose_histories=False,
log_state_histories=False,
motion_planner=None):
'''Initializes a YuMiArm interface. This interface will communicate with one arm (port) on the YuMi Robot.
This uses a subprocess to handle non-blocking socket communication with the RAPID server.
Parameters
----------
name : string
Name of the arm {'left', 'right'}
ip : string formated ip address, optional
IP of YuMi Robot.
Default uses the one in YuMiConstants
port : int, optional
Port of target arm's server.
Default uses the port for the left arm from YuMiConstants.
bufsize : int, optional
Buffer size for ethernet responses
motion_timeout : float, optional
Timeout for motion commands.
Default from YuMiConstants.MOTION_TIMEOUT
comm_timeout : float, optional
Timeout for non-motion ethernet communication.
Default from YuMiConstants.COMM_TIMEOUT
process_timeout : float, optional
Timeout for ethernet process communication.
Default from YuMiConstants.PROCESS_TIMEOUT
from_frame : string, optional
String name of robot arm frame.
Default to "tool"
to_frame : string, optional
String name of reference for robot frame
Default to "base"
debug : bool, optional
Boolean to indicate whether or not in debug mode. If in debug mode no ethernet communication is attempted. Mock responses will be returned.
Default to YuMiConstants.DEBUG
log_pose_histories : bool, optional
If True, uses yumi_history_logger to log pose histories. Enables usage of flush_pose_histories.
Defaults to False
log_state_histories : bool, optional
If True, uses yumi_history_logger to log state histories. Enables usage of flush_state_histories.
Defaults to False
motion_planner : YuMiMotionPlanner, optional
If given, will use for planning trajectories in joint space.
Defaults to None
'''
self._motion_timeout = motion_timeout
self._comm_timeout = comm_timeout
self._process_timeout = process_timeout
self._ip = ip
self._port = port
self._bufsize = bufsize
self._from_frame = from_frame
self._to_frame = to_frame
self._debug = debug
self._name = name
if log_pose_histories:
self._pose_logger = YuMiMotionLogger()
if log_state_histories:
self._state_logger = YuMiMotionLogger()
self.start()
self._last_sets = {
'zone': None,
'speed': None,
'tool': None,
'gripper_force': None,
'gripper_max_speed': None,
}
self._motion_planner = motion_planner
def reset_settings(self):
'''Reset zone, tool, and speed settings to their last known values. This is used when reconnecting to the RAPID server after a server restart.
'''
# set robot settings
for key, val in self._last_sets.items():
if val is not None:
getattr(self, 'set_{0}'.format(key))(val)
def reset(self):
'''Resets the underlying yumi ethernet process and socket, and resets all the settings.
'''
# empty motion logs
if hasattr(self, '_state_logger'):
self._state_logger.reset_log()
if hasattr(self, '_pose_logger'):
self._pose_logger.reset_log()
# terminate ethernet
try:
self._yumi_ethernet.terminate()
except Exception:
pass
# start ethernet comm
self._create_yumi_ethernet()
self.reset_settings()
def set_motion_planner(self, motion_planner):
'''
Parameters
----------
motion_planner : YuMiMotionPlanner
Sets the current motion planner to the given motion planner.
'''
self._motion_planner = motion_planner
def flush_pose_histories(self, filename):
'''
Parameters
----------
filename : string
Saves the pose history logger data to filename. Empties logger.
'''
self._pose_logger.flush_to_file(filename)
def flush_state_histories(self, filename):
'''
Parameters
----------
filename : string
Saves the state history logger data to filename. Empties logger
'''
self._state_logger.flush_to_file(filename)
def _create_yumi_ethernet(self):
self._req_q = Queue()
self._res_q = Queue()
self._yumi_ethernet = _YuMiEthernet(self._req_q, self._res_q, self._ip,
self._port, self._bufsize,
self._comm_timeout, self._debug)
self._yumi_ethernet.start()
def start(self):
'''Starts subprocess for ethernet communication.
'''
self._create_yumi_ethernet()
def stop(self):
'''Stops subprocess for ethernet communication. Allows program to exit gracefully.
'''
self._req_q.put("stop")
try:
self._yumi_ethernet.terminate()
except Exception:
pass
def __del__(self):
self.stop()
def _request(self, req, wait_for_res, timeout=None):
if timeout is None:
timeout = self._comm_timeout
req_packet = _REQ_PACKET(req, timeout, wait_for_res)
logging.debug('Process req: {0}'.format(req_packet))
self._req_q.put(req_packet)
if wait_for_res:
try:
res = self._res_q.get(block=True,
timeout=self._process_timeout)
except (IOError, Empty):
raise YuMiCommException(
"Request timed out: {0}".format(req_packet))
logging.debug('res: {0}'.format(res))
if res.res_code != YMC.RES_CODES['success']:
raise YuMiControlException(req_packet, res)
return res
@staticmethod
def _construct_req(code_name, body=''):
req = '{0:d} {1}#'.format(YMC.CMD_CODES[code_name], body)
return req
@staticmethod
def _iter_to_str(template, iterable):
result = ''
for val in iterable:
result += template.format(val).rstrip('0').rstrip('.') + ' '
return result
@staticmethod
def _get_pose_body(pose):
if not isinstance(pose, RigidTransform):
raise ValueError('Can only parse RigidTransform objects')
pose = pose.copy()
pose.position = pose.position * METERS_TO_MM
body = '{0}{1}'.format(
YuMiArm._iter_to_str('{:.1f}', pose.position.tolist()),
YuMiArm._iter_to_str('{:.5f}', pose.quaternion.tolist()))
return body
@staticmethod
def from_frame(self):
return self._from_frame
@staticmethod
def to_frame(self):
return self._to_frame
def ping(self, wait_for_res=True):
'''Pings the remote server.
Parameters
----------
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
out : Namedtuple (raw_res, data) from ping command.
Raises
------
YuMiCommException
If communication times out or socket error.
'''
req = YuMiArm._construct_req('ping')
return self._request(req, wait_for_res)
def get_state(self, raw_res=False):
'''Get the current state (joint configuration) of this arm.
Parameters
----------
raw_res : bool, optional
If True, will return raw_res namedtuple instead of YuMiState
Defaults to False
Returns
-------
out :
YuMiState if raw_res is False
_RES(raw_res, state) namedtuple if raw_res is True
Raises
------
YuMiCommException
If communication times out or socket error.
'''
if self._debug:
return YuMiState()
req = YuMiArm._construct_req('get_joints')
res = self._request(req, True)
if res is not None:
state = message_to_state(res.message)
if raw_res:
return _RES(res, state)
else:
return state
def get_pose(self, raw_res=False):
'''Get the current pose of this arm to base frame of the arm.
Parameters
----------
raw_res : bool, optional
If True, will return raw_res namedtuple instead of YuMiState
Defaults to False
Returns
-------
out :
RigidTransform if raw_res is False
_RES(raw_res, pose) namedtuple if raw_res is True
Raises
------
YuMiCommException
If communication times out or socket error.
'''
if self._debug:
return RigidTransform(from_frame=self._from_frame,
to_frame=self._to_frame)
req = YuMiArm._construct_req('get_pose')
res = self._request(req, True)
if res is not None:
pose = message_to_pose(res.message, self._from_frame)
if raw_res:
return _RES(res, pose)
else:
return pose
def is_pose_reachable(self, pose):
'''Check if a given pose is reachable (incurs no kinematic/joint-space limitations and self collisions)
Parameters
----------
pose : RigidTransform
Returns
-------
bool : True if pose is reachable, False otherwise.
Raises
------
YuMiCommException
If communication times out or socket error.
'''
body = YuMiArm._get_pose_body(pose)
req = YuMiArm._construct_req('is_pose_reachable', body)
res = self._request(req, True)
return bool(int(res.message))
def goto_state(self, state, wait_for_res=True):
'''Commands the YuMi to goto the given state (joint angles)
Parameters
----------
state : YuMiState
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') if state logging is not enabled and wait_for_res is False
{
'time': <flaot>,
'state': <YuMistate>,
'res': <namedtuple('_RAW_RES', 'mirror_code res_code message')>
} otherwise. The time field indicates the duration it took for the arm to complete the motion.
Raises
------
YuMiCommException
If communication times out or socket error.
YuMiControlException
If commanded pose triggers any motion errors that are catchable by RAPID sever.
'''
body = YuMiArm._iter_to_str('{:.2f}', state.joints)
req = YuMiArm._construct_req('goto_joints', body)
res = self._request(req, wait_for_res, timeout=self._motion_timeout)
if hasattr(self, '_state_logger') and wait_for_res and res is not None:
if self._debug:
time = -1.
else:
time = float(res.message)
actual_state = self.get_state()
self._state_logger.append_time(time)
self._state_logger.append_expected(state)
self._state_logger.append_actual(actual_state)
return {'time': time, 'state': actual_state, 'res': res}
return res
def _goto_state_sync(self, state, wait_for_res=True):
body = YuMiArm._iter_to_str('{:.2f}', state.joints)
req = YuMiArm._construct_req('goto_joints_sync', body)
return self._request(req, wait_for_res, timeout=self._motion_timeout)
def goto_pose(self, pose, linear=True, relative=False, wait_for_res=True):
'''Commands the YuMi to goto the given pose
Parameters
----------
pose : RigidTransform
linear : bool, optional
If True, will use MoveL in RAPID to ensure linear path. Otherwise use MoveJ in RAPID, which does not ensure linear path.
Defaults to True
relative : bool, optional
If True, will use goto_pose_relative by computing the delta pose from current pose to target pose.
Defaults to False
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') if pose logging is not enabled and wait_for_res is False
{
'time': <flaot>,
'pose': <RigidTransform>,
'res': <namedtuple('_RAW_RES', 'mirror_code res_code message')>
} otherwise. The time field indicates the duration it took for the arm to complete the motion.
Raises
------
YuMiCommException
If communication times out or socket error.
YuMiControlException
If commanded pose triggers any motion errors that are catchable by RAPID sever.
'''
if relative:
cur_pose = self.get_pose()
delta_pose = cur_pose.inverse() * pose
tra = delta_pose.translation
rot = np.rad2deg(delta_pose.euler_angles)
res = self.goto_pose_delta(tra, rot, wait_for_res=wait_for_res)
else:
body = YuMiArm._get_pose_body(pose)
if linear:
cmd = 'goto_pose_linear'
else:
cmd = 'goto_pose'
req = YuMiArm._construct_req(cmd, body)
res = self._request(req,
wait_for_res,
timeout=self._motion_timeout)
if hasattr(self, '_pose_logger') and wait_for_res and res is not None:
if self._debug:
time = -1.
else:
time = float(res.message)
actual_pose = self.get_pose()
self._pose_logger.append_time(time)
self._pose_logger.append_expected(pose)
self._pose_logger.append_actual(actual_pose)
return {'time': time, 'pose': actual_pose, 'res': res}
return res
def _goto_pose_sync(self, pose, wait_for_res=True):
body = YuMiArm._get_pose_body(pose)
req = YuMiArm._construct_req('goto_pose_sync', body)
return self._request(req, wait_for_res, timeout=self._motion_timeout)
def goto_pose_linear_path(self,
pose,
wait_for_res=True,
traj_len=10,
eef_delta=0.01,
jump_thresh=0.0):
""" Go to a pose via the shortest path in joint space """
if self._motion_planner is None:
raise ValueError('Motion planning not enabled')
current_state = self.get_state()
current_pose = self.get_pose().as_frames('gripper', 'world')
traj = self._motion_planner.plan_linear_path(current_state,
current_pose,
pose,
traj_len=traj_len,
eef_delta=eef_delta,
jump_thresh=jump_thresh)
if traj is None:
return
for state in traj:
self.goto_state(state, wait_for_res=wait_for_res)
def goto_pose_shortest_path(self,
pose,
wait_for_res=True,
plan_timeout=0.1):
""" Go to a pose via the shortest path in joint space """
if self._motion_planner is None:
raise ValueError('Motion planning not enabled')
current_state = self.get_state()
current_pose = self.get_pose().as_frames('gripper', 'world')
traj = self._motion_planner.plan_shortest_path(current_state,
current_pose,
pose,
timeout=plan_timeout)
if traj is None:
return
for state in traj:
self.goto_state(state, wait_for_res=wait_for_res)
def goto_pose_delta(self, translation, rotation=None, wait_for_res=True):
'''Goto a target pose by transforming the current pose using the given translation and rotation
Parameters
----------
translation : list-like with length 3
The translation vector (x, y, z) in meters.
rotation : list-like with length 3, optional
The euler angles of given rotation in degrees.
Defaults to 0 degrees - no rotation.
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
YuMiControlException
If commanded pose triggers any motion errors that are catchable by RAPID sever.
'''
translation = [val * METERS_TO_MM for val in translation]
translation_str = YuMiArm._iter_to_str('{:.1f}', translation)
rotation_str = ''
if rotation is not None:
rotation_str = YuMiArm._iter_to_str('{:.5f}', rotation)
body = translation_str + rotation_str
req = YuMiArm._construct_req('goto_pose_delta', body)
return self._request(req, wait_for_res, timeout=self._motion_timeout)
def set_tool(self, pose, wait_for_res=True):
'''Sets the Tool Center Point (TCP) of the arm using the given pose.
Parameters
----------
pose : RigidTransform
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
body = YuMiArm._get_pose_body(pose)
req = YuMiArm._construct_req('set_tool', body)
self._last_sets['tool'] = pose
return self._request(req, wait_for_res)
def set_speed(self, speed_data, wait_for_res=True):
'''Sets the target speed of the arm's movements.
Parameters
----------
speed_data : list-like with length 4
Specifies the speed data that will be used by RAPID when executing motions.
Should be generated using YuMiRobot.get_v
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
body = YuMiArm._iter_to_str('{:.2f}', speed_data)
req = YuMiArm._construct_req('set_speed', body)
self._last_sets['speed'] = speed_data
return self._request(req, wait_for_res)
def set_zone(self, zone_data, wait_for_res=True):
'''Goto a target pose by transforming the current pose using the given translation and rotation
Parameters
----------
speed_data : list-like with length 4
Specifies the speed data that will be used by RAPID when executing motions.
Should be generated using YuMiRobot.get_v
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
pm = zone_data['point_motion']
data = (pm, ) + zone_data['values']
body = YuMiArm._iter_to_str('{:2f}', data)
req = YuMiArm._construct_req('set_zone', body)
self._last_sets['zone'] = zone_data
return self._request(req, wait_for_res)
def move_circular(self, center_pose, target_pose, wait_for_res=True):
'''Goto a target pose by following a circular path around the center_pose
Parameters
----------
center_pose : RigidTransform
Pose for the center of the circle for circula movement.
target_pose : RigidTransform
Target pose
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
YuMiControlException
If commanded pose triggers any motion errors that are catchable by RAPID sever.
'''
body_set_circ_point = YuMiArm._get_pose_body(center_pose)
body_move_by_circ_point = YuMiArm._get_pose_body(target_pose)
req_set_circ_point = YuMiArm._construct_req('set_circ_point',
body_set_circ_point)
req_move_by_circ_point = YuMiArm._construct_req(
'move_by_circ_point', body_move_by_circ_point)
res_set_circ_point = self._request(req_set_circ_point, True)
if res_set_circ_point is None:
logging.error("Set circular point failed. Skipping move circular!")
return None
else:
return self._request(req_move_by_circ_point,
wait_for_res,
timeout=self._motion_timeout)
def buffer_add_single(self, pose, wait_for_res=True):
'''Add single pose to the linear movement buffer in RAPID
Parameters
----------
pose : RigidTransform
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
body = YuMiArm._get_pose_body(pose)
req = YuMiArm._construct_req('buffer_add', body)
return self._request(req, wait_for_res)
def buffer_add_all(self, pose_list, wait_for_res=True):
'''Add a list of poses to the linear movement buffer in RAPID
Parameters
----------
pose_list : list of RigidTransforms
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
ress = [
self.buffer_add_single(pose, wait_for_res) for pose in pose_list
]
return ress
def buffer_clear(self, wait_for_res=True):
'''Clears the linear movement buffer in RAPID
Parameters
----------
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
req = YuMiArm._construct_req('buffer_clear')
return self._request(req, wait_for_res)
def buffer_size(self, raw_res=False):
'''Gets the current linear movement buffer size.
Parameters
----------
wait_for_res : bool, optional
If True, will block main process until response received from RAPID server.
Defaults to True
Returns
-------
None if wait_for_res is False
namedtuple('_RAW_RES', 'mirror_code res_code message') otherwise
Raises
------
YuMiCommException
If communication times out or socket error.
'''
req = YuMiArm._construct_req('buffer_size')
res = self._request(req, True)
if res is not None:
try:
size = int(res.message)
if raw_res:
return _RES(res, size)
else:
return size
except Exception, e:
logging.error(e)