def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame.
Args:
arm_frame (ArmState)
ref_frame (int): One of ArmState.*
ref_frame_obj (Object): As in Object.msg
Returns:
ArmState: arm_frame (passed in), but modified.
'''
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to itself (nothing to do).
rospy.logdebug(
'No reference frame transformations needed (both ' +
'absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform from object to robot base.
abs_ee_pose = World.transform(
arm_frame.ee_pose,
arm_frame.refFrameObject.name,
'base_link'
)
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr(
'Unhandled reference frame conversion: ' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to object.
rel_ee_pose = World.transform(
arm_frame.ee_pose, 'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform between the same object (nothign to do).
if arm_frame.refFrameObject.name == ref_frame_obj.name:
rospy.logdebug(
'No reference frame transformations needed (same ' +
'object).')
else:
# Transform between two different objects.
rel_ee_pose = World.transform(
arm_frame.ee_pose,
arm_frame.refFrameObject.name,
ref_frame_obj.name
)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr(
'Unhandled reference frame conversion: ' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
def _get_arm_states(self):
'''Returns the current arms states in the right format'''
abs_ee_poses = [Arms.get_ee_state(0),
Arms.get_ee_state(1)]
joint_poses = [Arms.get_joint_state(0),
Arms.get_joint_state(1)]
states = [None, None]
for arm_index in [0, 1]:
nearest_obj = self.world.get_nearest_object(
abs_ee_poses[arm_index])
if (nearest_obj == None):
states[arm_index] = ArmState(ArmState.ROBOT_BASE,
abs_ee_poses[arm_index],
joint_poses[arm_index], Object())
else:
# Relative
rel_ee_pose = World.transform(
abs_ee_poses[arm_index],
'base_link', nearest_obj.name)
states[arm_index] = ArmState(ArmState.OBJECT,
rel_ee_pose,
joint_poses[arm_index], nearest_obj)
return states
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
new_ref = World.get_ref_from_name(new_ref_name)
if (new_ref != ArmState.ROBOT_BASE):
index = ActionStepMarker._ref_names.index(new_ref_name)
new_ref_obj = ActionStepMarker._ref_object_list[index - 1]
else:
new_ref_obj = Object()
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
def __init__(self, pose, index, dimensions, is_recognized):
''' Initialization of objects'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(Object.TABLE_TOP, self.get_name(), pose,
dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name,
'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link',
ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(
arm_frame.ee_pose, arm_frame.refFrameObject.name,
ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
def __init__(self, pose, index, dimensions, is_recognized):
'''
Args:
pose (Pose): Position of bounding box
index (int): For naming object in world (e.g. "thing 0")
dimensions (Vector3): Size of bounding box
is_recognized (bool): Result of object recognition.
'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(
Object.TABLE_TOP, self.get_name(), pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def _get_arm_states(self):
'''Returns the current arms states as a list of two ArmStates.
Returns:
[ArmState]: A list (of length two, one for each arm) of
ArmState objects. Right first, then left.
'''
# TODO(mbforbes): Perhaps this entire method should go in
# the Arms class?
abs_ee_poses = [
Arms.get_ee_state(Side.RIGHT), # (Pose)
Arms.get_ee_state(Side.LEFT)
] # (Pose)
joint_poses = [
Arms.get_joint_positions(Side.RIGHT), # ([float64])
Arms.get_joint_positions(Side.LEFT)
] # ([float64])
states = [None, None]
rel_ee_poses = [None, None]
for arm_index in [Side.RIGHT, Side.LEFT]:
nearest_obj = self.world.get_nearest_object(
abs_ee_poses[arm_index])
if not World.has_objects() or nearest_obj is None:
# Arm state is absolute (relative to robot's base_link).
states[arm_index] = ArmState(
ArmState.ROBOT_BASE, # refFrame (uint8)
abs_ee_poses[arm_index], # ee_pose (Pose)
joint_poses[arm_index], # joint_pose ([float64])
Object() # refFrameObject (Object)
)
else:
# Arm state is relative (to some object in the world).
rel_ee_poses[arm_index] = World.transform(
abs_ee_poses[arm_index], # pose (Pose)
BASE_LINK, # from_frame (str)
nearest_obj.name # to_frame (str)
)
states[arm_index] = ArmState(
ArmState.OBJECT, # refFrame (uint8)
rel_ee_poses[arm_index], # ee_pose (Pose)
joint_poses[arm_index], # joint_pose [float64]
nearest_obj # refFrameObject (Object)
)
return states
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step to
new_ref_name.
Args:
new_ref_name
'''
# Get the id of the new ref (an int).
new_ref = World.get_ref_from_name(new_ref_name)
if new_ref != ArmState.ROBOT_BASE:
index = ActionStepMarker._ref_names.index(new_ref_name)
new_ref_obj = ActionStepMarker._ref_object_list[index - 1]
else:
new_ref_obj = Object()
if self.action_step.type == ActionStep.ARM_TARGET:
# Handle "normal" steps (saved poses).
t = self.action_step.armTarget
if self.arm_index == Side.RIGHT:
t.rArm = World.convert_ref_frame(t.rArm, new_ref, new_ref_obj)
else:
t.lArm = World.convert_ref_frame(t.lArm, new_ref, new_ref_obj)
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Handle trajectory steps.
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
for i in range(len(arm)):
arm_old = arm[i]
arm_new = World.convert_ref_frame(
arm_old, new_ref, new_ref_obj)
arm[i] = arm_new
# Fix up reference frames.
if self.arm_index == Side.RIGHT:
t.rRefFrameObject = new_ref_obj
t.rRefFrame = new_ref
else:
t.lRefFrameObject = new_ref_obj
t.lRefFrame = new_ref
def _find_dominant_ref(self, arm_traj, frame_list):
'''Finds the most dominant reference frame in a continuous
trajectory.
Args:
arm_traj (ArmState[]): List of arm states that form the arm
trajectory.
frame_list ([Object]): List of Object (as defined by
Object.msg), the current reference frames.
Returns:
(int, Object): Tuple of the dominant reference frame's
number (as one of the constants available in ArmState to
be set as ArmState.refFrame) and Object (as in
Object.msg).
'''
# Cycle through all arm states and check their reference frames.
# Whichever one is most frequent becomes the dominant one.
robot_base = Object(name=BASE_LINK)
ref_counts = Counter()
for arm_state in arm_traj:
# We only track objects that
if arm_state.refFrame == ArmState.ROBOT_BASE:
ref_counts[robot_base] += 1
elif arm_state.refFrameObject in frame_list:
ref_counts[arm_state.refFrameObject] += 1
else:
rospy.logwarn('Ignoring object with reference frame name ' +
arm_state.refFrameObject.name +
' because the world does not have this object.')
# Get most common obj.
dominant_ref_obj = ref_counts.most_common(1)[0][0]
# Find the frame number (int) and return with the object.
return World.get_ref_from_name(dominant_ref_obj.name), dominant_ref_obj