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 message_to_state(message):
tokens = message.split()
try:
if len(tokens) != YuMiState.NUM_JOINTS:
raise Exception("Invalid format for states! Got: \n{0}".format(message))
state_vals = [float(token) for token in tokens]
state = YuMiState(state_vals)
return state
except Exception, e:
logging.error(e)
def plan_linear_path(self,
start_state,
start_pose,
goal_pose,
traj_len=10,
eef_delta=0.01,
jump_thresh=0.0):
"""
Plans a linear trajectory between the start and goal pose from the initial joint configuration. The poses should be specified in the frame of reference of the end effector tip. Start state must correspond to the start pose - right now this is up to the user.
Parameters
----------
start_state : YuMiState
initial state of the arm
start_pose : RigidTransform
initial pose of end effector tip
goal_pose : RigidTransform
goal pose of end effector tip
traj_len : int
number of waypoints to use in planning
eef_delta : float
distance for interpolation of the final planner path in cartesian space
jump_thresh : float
the threshold for jumping between poses
Returns
-------
traj : YuMiTrajectory
the planned trajectory to execute
"""
# check valid state types
if not isinstance(start_state, YuMiState):
raise ValueError('Start state must be specified')
# check valid pose types
if not isinstance(start_pose, RigidTransform) or not isinstance(
goal_pose, RigidTransform):
raise ValueError(
'Start and goal poses must be specified as RigidTransformations'
)
# check valid frames
if start_pose.from_frame != 'gripper' or goal_pose.from_frame != 'gripper' or (
start_pose.to_frame != 'world'
and start_pose.to_frame != 'base'
) or (goal_pose.to_frame != 'world' and goal_pose.to_frame != 'base'):
raise ValueError(
'Start and goal poses must be from frame \'gripper\' to frame \{\'world\', \'base\'\}'
)
# set start state of planner
start_state_msg = moveit_msgs.msg.RobotState()
start_state_msg.joint_state.name = [
'yumi_joint_%d_r' % i for i in range(1, 8)
]
if self._arm == 'left_arm':
start_state_msg.joint_state.name = [
'yumi_joint_%d_l' % i for i in range(1, 8)
]
start_state_msg.joint_state.position = start_state.in_radians
self._planning_group.set_start_state(start_state_msg)
# convert start and end pose to the planner's reference frame
start_pose_hand = start_pose * ymc.T_GRIPPER_HAND
goal_pose_hand = goal_pose * ymc.T_GRIPPER_HAND
# get waypoints
pose_traj = start_pose_hand.linear_trajectory_to(
goal_pose_hand, traj_len)
waypoints = [t.pose_msg for t in pose_traj]
# plan plath
plan, fraction = self._planning_group.compute_cartesian_path(
waypoints, eef_delta, jump_thresh)
if fraction >= 0.0 and fraction < 1.0:
logging.warning('Failed to plan full path.')
return None
if fraction < 0.0:
logging.warning('Error while planning path.')
return None
# convert from moveit joint traj in radians
joint_names = plan.joint_trajectory.joint_names
joint_traj = []
for t in plan.joint_trajectory.points:
joints_and_names = zip(joint_names, t.positions)
joints_and_names.sort(key=lambda x: x[0])
joint_traj.append(
YuMiState([np.rad2deg(x[1]) for x in joints_and_names]))
return YuMiTrajectory(joint_traj)
def plan_shortest_path(self,
start_state,
start_pose,
goal_pose,
timeout=0.1):
"""
Plans the shortest path in joint space between the start and goal pose from the initial joint configuration. The poses should be specified in the frame of reference of the end effector tip. Start state must correspond to the start pose - right now this is up to the user.
Parameters
----------
start_state : YuMiState
initial state of the arm
start_pose : RigidTransform
initial pose of end effector tip
goal_pose : RigidTransform
goal pose of end effector tip
timeout : float
planner timeout (shorthand for setting new planning time then planning)
Returns
-------
traj : YuMiTrajectory
the planned trajectory to execute
"""
# check valid state types
if not isinstance(start_state, YuMiState):
raise ValueError('Start state must be specified')
# check valid pose types
if not isinstance(start_pose, RigidTransform) or not isinstance(
goal_pose, RigidTransform):
raise ValueError(
'Start and goal poses must be specified as RigidTransformations'
)
# check valid frames
if start_pose.from_frame != 'gripper' or goal_pose.from_frame != 'gripper' or (
start_pose.to_frame != 'world'
and start_pose.to_frame != 'base'
) or (goal_pose.to_frame != 'world' and goal_pose.to_frame != 'base'):
raise ValueError(
'Start and goal poses must be from frame \'gripper\' to frame \{\'world\', \'base\'\}'
)
# set planning time
self.planning_time = timeout
# set start state of planner
start_state_msg = moveit_msgs.msg.RobotState()
start_state_msg.joint_state.name = [
'yumi_joint_%d_r' % i for i in range(1, 8)
]
if self._arm == 'left_arm':
start_state_msg.joint_state.name = [
'yumi_joint_%d_l' % i for i in range(1, 8)
]
start_state_msg.joint_state.position = start_state.in_radians
self._planning_group.set_start_state(start_state_msg)
# convert start and end pose to the planner's reference frame
start_pose_hand = start_pose * ymc.T_GRIPPER_HAND
goal_pose_hand = goal_pose * ymc.T_GRIPPER_HAND
# plan plath
self._planning_group.set_pose_target(goal_pose_hand.pose_msg)
plan = self._planning_group.plan()
if len(plan.joint_trajectory.points) == 0:
logging.warning('Failed to plan path.')
return None
# convert from moveit joint traj in radians
joint_names = plan.joint_trajectory.joint_names
joint_traj = []
for t in plan.joint_trajectory.points:
joints_and_names = zip(joint_names, t.positions)
joints_and_names.sort(key=lambda x: x[0])
joint_traj.append(
YuMiState([np.rad2deg(x[1]) for x in joints_and_names]))
return YuMiTrajectory(joint_traj)
class YuMiConstants:
IP = '192.168.125.1'
PORTS = {
"left": {
"server": 5000,
"states": 5010,
"poses": 5012,
"torques": 5014
},
"right": {
"server": 5001,
"states": 5011,
"poses": 5013,
"torques": 5015
},
}
BUFSIZE = 4096
MOTION_TIMEOUT = 8
COMM_TIMEOUT = 5
PROCESS_TIMEOUT = 10
PROCESS_SLEEP_TIME = 0.01
GRASP_COUNTER_PATH = '/home/autolab/Public/alan/grasp_counts'
# used to rate limit real-time YuMi controls (in seconds)
COMM_PERIOD = 0.04
DEBUG = False
LOGGING_LEVEL = logging.DEBUG
# reset mechanism
RESET_RIGHT_COMM = '/dev/ttyACM0'
RESET_BAUDRATE = 115200
CMD_CODES = {
'ping': 0,
'goto_pose_linear': 1,
'goto_joints': 2,
'get_pose': 3,
'get_joints': 4,
'goto_pose': 5,
'set_tool': 6,
'set_speed': 8,
'set_zone': 9,
'goto_pose_sync': 11,
'goto_joints_sync': 12,
'goto_pose_delta': 13,
'close_gripper': 20,
'open_gripper': 21,
'calibrate_gripper': 22,
'set_gripper_max_speed': 23,
'set_gripper_force': 24,
'move_gripper': 25,
'get_gripper_width': 26,
'set_circ_point': 35,
'move_by_circ_point': 36,
'buffer_add': 30,
'buffer_clear': 31,
'buffer_size': 32,
'buffer_move': 33,
'is_pose_reachable': 40,
'is_joints_reachable': 41,
'close_connection': 99,
'reset_home': 100,
}
RES_CODES = {'failure': 0, 'success': 1}
SUB_CODES = {'pose': 0, 'state': 1}
MOVEIT_PLANNER_IDS = {
'SBL': 'SBLkConfigDefault',
'EST': 'ESTkConfigDefault',
'LBKPIECE': 'LBKPIECEkConfigDefault',
'BKPIECE': 'BKPIECEkConfigDefault',
'KPIECE': 'KPIECEkConfigDefault',
'RRT': 'RRTkConfigDefault',
'RRTConnect': 'RRTConnectkConfigDefault',
'RRTstar': 'RRTstarkConfigDefault',
'TRRT': 'TRRTkConfigDefault',
'PRM': 'PRMkConfigDefault',
'PRMstar': 'PRMstarkConfigDefault'
}
MOVEIT_PLANNING_REFERENCE_FRAME = 'yumi_body'
ROS_TIMEOUT = 10
T_GRIPPER_HAND = RigidTransform(translation=[0, 0, -0.157],
from_frame='gripper',
to_frame='gripper')
TCP_ABB_GRIPPER = RigidTransform(translation=[0, 0, 0.136])
TCP_ABB_GRASP_GRIPPER = RigidTransform(translation=[0, 0, 0.136 - 0.0065])
TCP_LONG_GRIPPER = RigidTransform(
translation=[0, 0, (136 - 56 + 88 - 12) / 1000.])
TCP_DEFAULT_GRIPPER = RigidTransform(
translation=[0, 0, (136 - 56 + 88 - 12) / 1000.])
L_HOME_STATE = YuMiState([0, -130, 30, 0, 40, 0, 135])
L_HOME_POSE = RigidTransform(
translation=[0.123, 0.147, 0.124],
rotation=[0.06551, 0.84892, -0.11147, 0.51246])
R_HOME_STATE = YuMiState([0, -130, 30, 0, 40, 0, -135])
R_HOME_POSE = RigidTransform(
translation=[-0.0101, -0.1816, 0.19775],
rotation=[-0.52426, 0.06481, -0.84133, -0.11456])
R_FORWARD_STATE = YuMiState(
[9.66, -133.36, 34.69, -13.19, 28.85, 28.81, -110.18])
R_FORWARD_POSE = RigidTransform(
translation=[0.07058, -0.26519, 0.19775],
rotation=[-0.52425, 0.0648, -0.84133, -0.11454])
R_AWAY_POSE = RigidTransform(translation=[0.15, -0.4, 0.22],
rotation=[0.38572, 0.39318, 0.60945, 0.57027])
L_AWAY_POSE = RigidTransform(
translation=[0.30, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
AXIS_ALIGNED_STATES = {
'inwards': {
'right': YuMiState([75, -90, 0, -30, 90, -10, -45]),
'left': YuMiState([-75, -90, 0, 30, 90, 10, 45])
},
'forwards': {
'right':
YuMiState([36.42, -117.3, 35.59, 50.42, 46.19, 66.02, -100.28]),
'left':
YuMiState([-36.42, -117.3, 35.59, -50.42, 46.19, 113.98, 100.28])
},
'downwards': {
'right':
YuMiState([-60.0, -110.0, 35.0, 116.0, 100.0, -90.0, 30.0]),
'left':
YuMiState([60.0, -110.0, 35.0, -116.0, 100.0, -90.0, -30.0])
}
}
L_THINKING_POSES = [
RigidTransform(translation=[0.32, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.30, 0.10, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.28, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.30, 0.14, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
]
R_READY_STATE = YuMiState(
[51.16, -99.4, 21.57, -107.19, 84.11, 94.61, -36.00])
L_READY_STATE = YuMiState(
[-51.16, -99.4, 21.57, 107.19, 84.11, 94.61, 36.00])
L_PREGRASP_POSE = RigidTransform(
translation=[0.30, 0.20, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
L_KINEMATIC_AVOIDANCE_POSE = RigidTransform(
translation=[0.45, -0.05, 0.15], rotation=[0, 0, 1, 0])
L_RAISED_STATE = YuMiState(
[5.5, -99.46, 20.52, -21.03, 67, -22.31, 110.11])
L_RAISED_POSE = RigidTransform(
translation=[-0.0073, 0.39902, 0.31828],
rotation=[0.54882, 0.07398, 0.82585, -0.10630])
L_FORWARD_STATE = YuMiState(
[-21.71, -142.45, 43.82, 31.14, 10.43, -40.67, 106.63])
L_FORWARD_POSE = RigidTransform(
translation=[0.13885, 0.21543, 0.19774],
rotation=[0.55491, 0.07064, 0.82274, -0.1009])
L_PREDROP_POSE = RigidTransform(
translation=[0.55, 0.0, 0.20],
rotation=[0.09815, -0.20528, 0.97156, -0.06565])
L_PACKAGE_DROP_POSES = [
RigidTransform(translation=[0.33, 0.42, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.33, 0.33, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.23, 0.33, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.23, 0.42, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790])
]
L_REJECT_DROP_POSES = [
RigidTransform(translation=[0.60, 0.42, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.60, 0.35, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.54, 0.35, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.54, 0.42, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790])
]
BOX_CLOSE_SEQUENCE = [
#('L', RigidTransform(translation=[0.071, 0.385, 0.118], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.213, 0.385, 0.118], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.213, 0.385, 0.156], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.323, 0.385, 0.156], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.323, 0.385, 0.144], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('R', [0, 0.300, 0]),
('R', [0.100, 0, 0]),
('R',
YuMiState([111.30, -65.37, -29.93, -50.53, 65.41, -94.59, -68.84])),
('R', YuMiState([147.13, -76.28, -58.98, -13.85, 54.95, 54.95,
-96.95])),
('R', [0, 0, 0.070]),
('R', [-0.074, 0, 0]),
('R', [0, -0.012, 0]),
('R', [0, 0, -0.040]),
('R', YuMiState([132.32, -56.98, -8.13, -72.42, 36.44, 135.16,
-99.36])),
('R',
YuMiState([146.72, -57.09, -9.44, -101.74, 69.26, 155.98, -98.41])),
('R',
YuMiState([168.36, -64.12, -23.60, -118.0, 97.50, 163.97, -102.95])),
('R',
YuMiState([168.37, -59.60, 2.63, -146.15, 95.29, 199.63, -129.70])),
('R',
YuMiState([168.37, -82.24, -10.81, -196.49, 115.92, 199.63,
-148.06])),
('L',
RigidTransform(translation=[0.323, 0.385, 0.203],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.203, 0.478, 0.203],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.203, 0.478, 0.092],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.478, 0.092],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.478, 0.183],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.359, 0.183],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.359, 0.149],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.425, 0.359, 0.149],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.425, 0.359, 0.245],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.265, 0.445, 0.245],
rotation=[0.06744, 0.84050, -0.11086, 0.52604]))
]
"""
