def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Landmark()):
'''Transforms an arm frame to a new ref. frame.
Args:
arm_frame (ArmState)
ref_frame (int): One of ArmState.*
ref_frame_obj (Landmark): As in Landmark.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.refFrameLandmark.name,
'base_link'
)
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameLandmark = Landmark()
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.refFrameLandmark = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform between the same object (nothign to do).
if arm_frame.refFrameLandmark.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.refFrameLandmark.name,
ref_frame_obj.name
)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameLandmark = ref_frame_obj
else:
rospy.logerr(
'Unhandled reference frame conversion: ' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
def testConvertFromObjToAnotherObj(self):
arm_state = ArmState()
arm_state.ee_pose.position.x = 0.05
arm_state.ee_pose.position.y = 0
arm_state.ee_pose.position.z = 0
arm_state.ee_pose.orientation.w = 1
arm_state.refFrame = ArmState.OBJECT
arm_state.refFrameLandmark.name = 'landmark1'
arm_state.refFrameLandmark.pose.position.x = 0.15
arm_state.refFrameLandmark.pose.position.y = 0
arm_state.refFrameLandmark.pose.position.z = 0
arm_state.refFrameLandmark.pose.orientation.w = 1
landmark = Landmark()
landmark.name = 'landmark2'
landmark.pose.position.x = 0.2
landmark.pose.position.y = 0.1
landmark.pose.position.z = 0
landmark.pose.orientation.w = 1
rel_arm_state = world.convert_ref_frame(arm_state, ArmState.OBJECT,
landmark)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.x, 0)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.y, -0.1)
# Test copy behavior for passed-in landmark
rel_arm_state.refFrameLandmark.name = 'modified'
self.assertNotEqual(rel_arm_state.refFrameLandmark.name, landmark.name)
def __init__(self, name, pose, dimensions, db_id):
"""
Constructs a WorldLandmark object.
Use bounding_box() or cloud_box() instead.
Args:
name (str): Name of this landmark
pose (Pose): Pose of the landmark, in base frame
dimensions (Vector3): Size of bounding box
db_id (str): The MongoDB id if this is a cloud box
"""
self._name = name
self._pose = pose
self._dimensions = dimensions
self._db_id = db_id
if self.is_bounding_box():
self.object = Landmark(type=Landmark.TABLE_TOP,
name=name,
pose=pose,
dimensions=dimensions,
db_id='')
elif self.is_cloud_box():
self.object = Landmark(type=Landmark.CLOUD_BOX,
name=name,
pose=pose,
dimensions=dimensions,
db_id=db_id)
self.int_marker = None
self.is_removed = False
# TODO(jstn): Move this outside.
self.menu_handler = MenuHandler()
self.menu_handler.insert('Remove from scene', callback=self.remove)
def testConvertFromBaseToObj(self):
arm_state = ArmState()
arm_state.ee_pose.position.x = 0.2
arm_state.ee_pose.position.y = 0
arm_state.ee_pose.position.z = 0
arm_state.ee_pose.orientation.w = 1
arm_state.refFrame = ArmState.ROBOT_BASE
landmark = Landmark()
landmark.name = 'landmark1'
landmark.pose.position.x = 0.15
landmark.pose.position.y = 0
landmark.pose.position.z = 0
landmark.pose.orientation.w = 1
rel_arm_state = world.convert_ref_frame(arm_state, ArmState.OBJECT,
landmark)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.x, 0.05)
self.assertEqual(rel_arm_state.refFrame, ArmState.OBJECT)
self.assertEqual(rel_arm_state.refFrameLandmark.pose.position.x, 0.15)
def make_ref(self, parent):
"""Creates the reference frame for the side of an object.
Args:
parent (Marker)
Returns:
ref (Landmark)
"""
ref = Landmark()
ref.type = 1
ref.pose.position.x = parent.pose.position.x
ref.pose.position.y = parent.pose.position.y
ref.pose.position.z = parent.pose.position.z
ref.pose.orientation.x = parent.pose.orientation.x
ref.pose.orientation.y = parent.pose.orientation.y
ref.pose.orientation.z = parent.pose.orientation.z
ref.pose.orientation.w = parent.pose.orientation.w
ref.name = parent.ns + " Ref"
ref.dimensions.x = parent.scale.x
ref.dimensions.y = parent.scale.y
ref.dimensions.z = parent.scale.z
return ref
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_state(Side.RIGHT), # ([float64])
Arms.get_joint_state(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 self._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])
Landmark() # refFrameLandmark (Landmark)
)
else:
# Arm state is relative (to some object in the world).
arm_frame = ArmState()
arm_frame.ee_pose = abs_ee_poses[arm_index]
arm_frame.refFrame = ArmState.ROBOT_BASE
rel_arm_frame = world.convert_ref_frame(
arm_frame, ArmState.OBJECT, nearest_obj)
rel_ee_poses[arm_index] = rel_arm_frame.ee_pose
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 # refFrameLandmark (Landmark)
)
return states
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 = Landmark(
Landmark.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 _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 = Landmark()
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.rRefFrameLandmark = new_ref_obj
t.rRefFrame = new_ref
else:
t.lRefFrameLandmark = 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 ([Landmark]): List of Landmark (as defined by
Landmark.msg), the current reference frames.
Returns:
(int, Landmark): Tuple of the dominant reference frame's
number (as one of the constants available in ArmState to
be set as ArmState.refFrame) and Landmark (as in
Landmark.msg).
'''
# Cycle through all arm states and check their reference frames.
# Whichever one is most frequent becomes the dominant one.
robot_base = Landmark(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.refFrameLandmark in frame_list:
ref_counts[arm_state.refFrameLandmark] += 1
else:
rospy.logwarn('Ignoring object with reference frame name ' +
arm_state.refFrameLandmark.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
def _execute_action(self, __=None, preregistered_landmarks=[]):
'''Starts the execution of the current action.
This saves the action before starting it.
Args:
__ (Landmark): unused, default: None
preregistered_landmarks: A list of Landmarks.
Landmarks in this list are expected to be custom landmarks
with a name. We will skip searching for these landmarks.
Other landmarks that are part of the action that are not
pre-registered must be searched for using
custom_landmark_finder.
Returns:
[str, int]: a speech response and a GazeGoal.* constant
'''
# We must *have* a current action.
if self.session.n_actions <= 0:
return [RobotSpeech.ERROR_NO_SKILLS, GazeGoal.SHAKE]
# We must have also recorded steps (/poses/frames) in it.
if self.session.n_frames() < 1:
return [
RobotSpeech.EXECUTION_ERROR_NOPOSES + ' ' +
str(self.session.current_action_id), GazeGoal.SHAKE
]
# Save current action to disk (rosbag)
# TODO(jstn): Not sure if this works or is necessary anymore.
self.session.save_current_action()
current_action = self.session.get_current_action()
rospy.loginfo('Executing action {}'.format(current_action.name))
self._world.clear_all_objects()
# Check if we need to find tabletop objects in this action.
if current_action.is_tabletop_object_required():
# Find tabletop objects
if self._world.update_object_pose():
self._world.update()
else:
# An object is required, but we didn't get it.
return [RobotSpeech.OBJECT_NOT_DETECTED, GazeGoal.SHAKE]
custom_landmarks = current_action.custom_landmarks()
rospy.loginfo('Custom landmarks: {}'.format(', '.join(
[l.name for l in custom_landmarks])))
prereg_str = ', '.join([
'{} ({})'.format(l.name, l.db_id) for l in preregistered_landmarks
])
rospy.loginfo('Pre-registered landmarks: {}'.format(prereg_str))
if len(custom_landmarks) > 0:
preregistered = {}
for landmark in preregistered_landmarks:
preregistered[landmark.name] = landmark
registered_landmarks = {} # Maps db_ids to Landmarks
for landmark in custom_landmarks:
# Just in case custom_landmarks() returns duplicates
if landmark.name in registered_landmarks:
continue
if landmark.name in preregistered:
registered_landmarks[landmark.name] = preregistered[
landmark.name]
continue
# Move head to look at where the custom landmark was at
# demonstration time -- we assume that it is roughly in the
# same location at run time.
# Assumes that the landmark pose is given in the base frame.
#Response.force_look_at_point(landmark.pose.position)
if not Response.gaze_client.wait_for_result(
rospy.Duration(10)):
rospy.logwarn('Took longer than 10 seconds to look down.')
rospy.sleep(2)
rospy.loginfo('Searching for landmark: {}, ID: {}'.format(
landmark.name, landmark.db_id))
matches = self._custom_landmark_finder.find(
None, landmark.name)
if matches is None or len(matches) == 0:
rospy.logwarn('Could not find landmark: {}'.format(
landmark.name))
return [RobotSpeech.OBJECT_NOT_DETECTED, GazeGoal.SHAKE]
rospy.logwarn('Picking best of {} matches'.format(
len(matches)))
best_match = None
for match in matches:
if best_match is None or match.error < best_match.error:
best_match = match
registered_landmark = Landmark(type=Landmark.CLOUD_BOX,
name=landmark.name,
pose=best_match.pose,
dimensions=landmark.dimensions,
db_id=landmark.db_id)
registered_landmarks[landmark.db_id] = registered_landmark
for db_id, landmark in registered_landmarks.items():
world_landmark = WorldLandmark(landmark.name, landmark.pose,
landmark.dimensions,
landmark.db_id)
self._world.add_landmark(world_landmark)
if current_action.is_tabletop_object_required(
) or len(custom_landmarks) > 0:
current_action.update_objects(self._world.get_frame_list())
self.arms.start_execution(current_action, EXECUTION_Z_OFFSET)
# Reply: starting execution.
return [
RobotSpeech.START_EXECUTION + ' ' +
str(self.session.current_action_id), None
]
def convert_ref_frame(arm_state, ref_frame, ref_frame_obj=Landmark()):
"""Transforms an arm frame to a new ref. frame.
Args:
arm_state (ArmState): The arm state to transform
ref_frame (int): One of ArmState.*, the desired reference frame to
transform into.
ref_frame_obj (Landmark): The landmark to transform relative to.
Returns:
ArmState: A copy of arm_state, but transformed.
"""
output_state = copy.deepcopy(arm_state)
ref_frame_obj_copy = copy.deepcopy(ref_frame_obj)
if ref_frame == ArmState.ROBOT_BASE:
if arm_state.refFrame == ArmState.ROBOT_BASE:
pass # Nothing to do
elif arm_state.refFrame == ArmState.OBJECT:
# Transform from object to robot base.
ee_in_obj = get_matrix_from_pose(arm_state.ee_pose)
obj_pose = arm_state.refFrameLandmark.pose # In base frame
obj_to_base = get_matrix_from_pose(obj_pose)
abs_ee_pose = get_pose_from_transform(
np.dot(obj_to_base, ee_in_obj))
output_state.ee_pose = abs_ee_pose
output_state.refFrame = ArmState.ROBOT_BASE
output_state.refFrameLandmark = Landmark()
else:
rospy.logerr(
'Unhandled reference frame conversion: {} to {}'.format(
arm_state.refFrame, ref_frame))
elif ref_frame == ArmState.OBJECT:
if arm_state.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to provided object.
arm_in_base = get_matrix_from_pose(arm_state.ee_pose)
base_to_obj = np.linalg.inv(
get_matrix_from_pose(ref_frame_obj.pose))
rel_ee_pose = get_pose_from_transform(
np.dot(base_to_obj, arm_in_base))
output_state.ee_pose = rel_ee_pose
output_state.refFrame = ArmState.OBJECT
output_state.refFrameLandmark = ref_frame_obj_copy
elif arm_state.refFrame == ArmState.OBJECT:
if arm_state.refFrameLandmark.name == ref_frame_obj.name:
pass # Nothing to do
else:
# Transform from arm state's object to provided object.
ee_in_source_obj = get_matrix_from_pose(arm_state.ee_pose)
source_obj_to_base = get_matrix_from_pose(
arm_state.refFrameLandmark.pose)
base_to_target_obj = np.linalg.inv(
get_matrix_from_pose(ref_frame_obj.pose))
rel_ee_pose = get_pose_from_transform(
np.dot(np.dot(base_to_target_obj, source_obj_to_base),
ee_in_source_obj))
output_state.ee_pose = rel_ee_pose
output_state.refFrame = ArmState.OBJECT
output_state.refFrameLandmark = ref_frame_obj_copy
else:
rospy.logerr(
'Unhandled reference frame conversion: {} to {}'.format(
arm_state.refFrame, ref_frame))
return output_state