def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
# The interactive marker is shifted forward from the actual
# end-effector pose, so when setting the new pose, we need to shift
# back from where the marker is.
feedback_arm_state = ArmState()
feedback_arm_state.ee_pose = ActionStepMarker._offset_pose(
feedback.pose, -1)
feedback_arm_state.refFrame = ArmState.ROBOT_BASE
self.update_pose(feedback_arm_state)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller.
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Changing visibility of the pose controls.')
self.is_control_visible = not self.is_control_visible
ActionStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
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 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 _copy_arm_state(arm_state):
'''Makes a copy of the arm state'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
## WARNING: the following is not really copying
copy.refFrameObject = arm_state.refFrameObject
return copy
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 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).
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 # refFrameLandmark (Landmark)
)
return states
def testGetAbsoluteMakesCopy(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
pose = world.get_absolute_pose(arm_state)
self.assertEqual(pose.position.x, 0.2)
self.assertEqual(arm_state.refFrame, ArmState.OBJECT)
self.assertEqual(arm_state.ee_pose.position.x, 0.05)
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 testConvertObjToBase(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
abs_arm_state = world.convert_ref_frame(arm_state, ArmState.ROBOT_BASE)
self.assertAlmostEqual(abs_arm_state.ee_pose.position.x, 0.2)
self.assertEqual(abs_arm_state.refFrame, ArmState.ROBOT_BASE)
# Check that the input arg is unchanged
self.assertEqual(arm_state.refFrame, ArmState.OBJECT)
self.assertEqual(arm_state.ee_pose.position.x, 0.05)
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(
arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation), arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
def _copy_arm_state(arm_state):
'''Makes a copy of the arm state'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
## WARNING: the following is not really copying
copy.refFrameObject = arm_state.refFrameObject
return copy
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state (trasnforming
if relative).
Args:
arm_state (ArmState)
Returns:
Pose
'''
if arm_state.refFrame == ArmState.OBJECT:
arm_state_copy = ArmState(
arm_state.refFrame, Pose(
arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameLandmark)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
def _copy_arm_state(arm_state):
'''Returns a copy of arm_state.
Args:
arm_state (ArmState)
Returns:
ArmState
'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
# WARNING: the following is not really copying
copy.refFrameLandmark = arm_state.refFrameLandmark
return copy
def solve_ik_for_arm(arm_index, arm_state, cur_arm_pose=None):
'''Finds an IK solution for a particular arm pose'''
# We need to find IK only if the frame is relative to an object
if (arm_state.refFrame == ArmState.OBJECT):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is relative')
solution = ArmState()
target_pose = World.transform(arm_state.ee_pose,
arm_state.refFrameObject.name,
'base_link')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.ROBOT_BASE):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is absolute')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
arm_state.ee_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for absolute end-effector pose.')
return arm_state, False
else:
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.PREVIOUS_POSE):
#Calculate new arm state offset based on previous arm state
solution = ArmState()
target_ee_pose = Pose(
Point(cur_arm_pose.position.x + arm_state.ee_pose.position.x,
cur_arm_pose.position.y + arm_state.ee_pose.position.y,
cur_arm_pose.position.z + arm_state.ee_pose.position.z),
Quaternion(cur_arm_pose.orientation.x,
cur_arm_pose.orientation.y,
cur_arm_pose.orientation.z,
cur_arm_pose.orientation.w))
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_ee_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = target_ee_pose
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
def solve_ik_for_arm(arm_index, arm_state, z_offset=0.0):
'''Finds an IK solution for a particular arm pose.
Args:
arm_index (int): Side.RIGHT or Side.LEFT
arm_state (ArmState): The arm's state,
z_offset (float, optional): Offset to add to z-values of
pose positions. Defaults to 0.0.
Returns:
(ArmState, bool): Tuple of
the ArmState, which will be an updated ArmState object
with IK solved if an IK solution was found. If IK was
not found, what is returned will depend on whether the
reference frame is relative (ArmState.OBJECT) or
absolute (ArmState.ROBOT_BASE). If the reference frame
is relative and no IK was found, an empty ArmState is
returned. If the reference frame is absolute and no IK
was found, then the original passed arm_state is
returned;
the bool, which is whether an IK solution was
successfully found.
'''
# Ideally we need to find IK only if the frame is relative to an
# object, but users can edit absolute poses in the GUI to make
# them unreachable, so we try IK for absolute poses too.
if arm_state.refFrame == ArmState.OBJECT:
# Arm is relative.
solution = ArmState()
target_pose_state = world.convert_ref_frame(arm_state,
ArmState.ROBOT_BASE)
target_pose = target_pose_state.ee_pose
target_pose.position.z = target_pose.position.z + z_offset
# Try solving IK.
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
# Check whether solution found.
if target_joints is None:
# No solution: RETURN EMPTY ArmState.
rospy.logdebug('No IK for relative end-effector pose.')
return solution, False
else:
# Found a solution; update the solution arm_state and
# return.
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif arm_state.refFrame == ArmState.ROBOT_BASE:
# Arm is absolute.
pos = arm_state.ee_pose.position
target_position = Point(pos.x, pos.y, pos.z + z_offset)
target_pose = Pose(target_position, arm_state.ee_pose.orientation)
# Try solving IK.
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
if target_joints is None:
# No IK found; return the original.
rospy.logdebug('No IK for absolute end-effector pose.')
return arm_state, False
else:
# IK found; fill in solution ArmState and return.
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
def solve_ik_for_arm(arm_index, arm_state, cur_arm_pose=None, z_offset=0.0):
'''Finds an IK solution for a particular arm pose'''
# We need to find IK only if the frame is relative to an object
if (arm_state.refFrame == ArmState.OBJECT):
solution = ArmState()
target_pose = World.transform(arm_state.ee_pose,
arm_state.refFrameObject.name, 'base_link')
target_pose.position.z = target_pose.position.z + z_offset
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.ROBOT_BASE):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is absolute')
pos = arm_state.ee_pose.position
target_position = Point(pos.x, pos.y, pos.z + z_offset)
target_pose = Pose(target_position, arm_state.ee_pose.orientation)
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for absolute end-effector pose.')
return arm_state, False
else:
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.PREVIOUS_POSE):
#Calculate new arm state offset based on previous arm state
solution = ArmState()
target_ee_pose = Pose(Point(
cur_arm_pose.position.x + arm_state.ee_pose.position.x,
cur_arm_pose.position.y + arm_state.ee_pose.position.y,
cur_arm_pose.position.z + arm_state.ee_pose.position.z),
Quaternion(cur_arm_pose.orientation.x,
cur_arm_pose.orientation.y,
cur_arm_pose.orientation.z,
cur_arm_pose.orientation.w))
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_ee_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = target_ee_pose
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
