def update_pose(self, new_arm_state):
'''Changes the pose of the action step to that given by new_arm_state.
new_arm_state may specify the pose in the base frame or in the frame of
a landmark. However, it will be transformed into the frame of this
action step.
Args:
new_pose (Pose)
'''
if self.action_step.type == ActionStep.ARM_TARGET:
t = self.action_step.armTarget
current_arm_state = t.rArm if self.arm_index == Side.RIGHT else t.lArm
updated_arm_state = world.convert_ref_frame(
new_arm_state, current_arm_state.refFrame,
current_arm_state.refFrameLandmark)
# Note that this will be overwritten later when computing IK
updated_arm_state.joint_pose = current_arm_state.joint_pose[:]
if self.arm_index == Side.RIGHT:
t.rArm = updated_arm_state
else:
t.lArm = updated_arm_state
self.update_viz()
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
rospy.logwarn('Modification of whole trajectory segments is not ' +
'implemented.')
def _fix_trajectory_ref(self):
'''Makes the reference frame of continuous trajectories
uniform.
This means finding the dominant reference frame of the
trajectory (for right and left arms separately), and then
altering all steps in the trajctory to be relative to the same
reference frame (again, separate for right and left arms).
'''
# First, get objects from the world.
frame_list = self._world.get_frame_list()
# Next, find the dominant reference frame (e.g. robot base,
# an object).
t = self._arm_trajectory
r_ref_n, r_ref_obj = self._find_dominant_ref(t.rArm, frame_list)
l_ref_n, l_ref_obj = self._find_dominant_ref(t.lArm, frame_list)
# Next, alter all trajectory steps (ArmState's) so that they use
# the dominant reference frame as their reference frame.
for i in range(len(self._arm_trajectory.timing)):
t.rArm[i] = world.convert_ref_frame(
self._arm_trajectory.rArm[i], # arm_frame (ArmState)
r_ref_n, # ref_frame (int)
r_ref_obj # ref_frame_obj (Objet)
)
t.lArm[i] = world.convert_ref_frame(
self._arm_trajectory.lArm[i], # arm_frame (ArmState)
l_ref_n, # ref_frame (int)
l_ref_obj # ref_frame_obj (Objet)
)
# Save the dominant ref. frame no./name in the trajectory for
# reference.
t.rRefFrame = r_ref_n
t.lRefFrame = l_ref_n
t.rRefFrameLandmark = r_ref_obj
t.lRefFrameLandmark = l_ref_obj
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 _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 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