def _save_gripper_step(self, arm_index, gripper_state):
'''Saves an action step that involves a gripper state change.
Args:
arm_index (int): Side.RIGHT or Side.LEFT
gripper_state (int): GripperState.OPEN or
GripperState.CLOSED
'''
if self.session.n_actions() > 0:
states = self._get_arm_states()
step = ActionStep()
step.type = ActionStep.ARM_TARGET
step.armTarget = ArmTarget(
states[Side.RIGHT], # rArm (ArmSTate)
states[Side.LEFT], # lArm (ArmState)
DEFAULT_VELOCITY, # rArmVelocity (float64)
DEFAULT_VELOCITY # lArmVelocity (float 64)
)
new_gripper_states = [
self.arms.get_gripper_state(Side.RIGHT),
self.arms.get_gripper_state(Side.LEFT)
]
new_gripper_states[arm_index] = gripper_state
step.gripperAction = GripperAction(
GripperState(new_gripper_states[Side.RIGHT]),
GripperState(new_gripper_states[Side.LEFT]))
self.session.add_step_to_action(step, self._world.get_frame_list())
def _save_step(self, __=None):
'''Saves current arm state as an action step.
Args:
__ (Landmark): unused, default: None
Returns:
[str, int]: a speech response and a GazeGoal.* constant
'''
if self.session.n_actions() > 0:
states = self._get_arm_states()
step = ActionStep()
step.type = ActionStep.ARM_TARGET
step.armTarget = ArmTarget(
states[Side.RIGHT], # rArm (ArmState)
states[Side.LEFT], # lArm (ArmState)
DEFAULT_VELOCITY, # rArmVelocity (float64)
DEFAULT_VELOCITY # lArmVelocity (float64)
)
step.gripperAction = GripperAction(
GripperState(self.arms.get_gripper_state(Side.RIGHT)),
GripperState(self.arms.get_gripper_state(Side.LEFT)))
self.session.add_step_to_action(step, self._world.get_frame_list())
return [RobotSpeech.STEP_RECORDED, GazeGoal.NOD]
else:
return [RobotSpeech.ERROR_NO_SKILLS, GazeGoal.SHAKE]
def _copy_action_step(action_step):
'''Returns a copy of action_step.
Args:
action_step (ActionStep)
Returns:
ActionStep
'''
copy = ActionStep()
copy.type = int(action_step.type)
# Only the arm target is filled if this is an arm target, and
# vice versa with a trajectory.
if copy.type == ActionStep.ARM_TARGET:
copy.armTarget = ArmTarget()
copy.armTarget.rArmVelocity = float(
action_step.armTarget.rArmVelocity)
copy.armTarget.lArmVelocity = float(
action_step.armTarget.lArmVelocity)
copy.armTarget.rArm = ProgrammedAction._copy_arm_state(
action_step.armTarget.rArm)
copy.armTarget.lArm = ProgrammedAction._copy_arm_state(
action_step.armTarget.lArm)
elif copy.type == ActionStep.ARM_TRAJECTORY:
copy.armTrajectory = ArmTrajectory()
copy.armTrajectory.timing = action_step.armTrajectory.timing[:]
for j in range(len(action_step.armTrajectory.timing)):
copy.armTrajectory.rArm.append(
ProgrammedAction._copy_arm_state(
action_step.armTrajectory.rArm[j]))
copy.armTrajectory.lArm.append(
ProgrammedAction._copy_arm_state(
action_step.armTrajectory.lArm[j]))
copy.armTrajectory.rRefFrame = int(
action_step.armTrajectory.rRefFrame)
copy.armTrajectory.lRefFrame = int(
action_step.armTrajectory.lRefFrame)
# WARNING: the following is not really copying
r_obj = action_step.armTrajectory.rRefFrameLandmark
l_obj = action_step.armTrajectory.lRefFrameLandmark
copy.armTrajectory.rRefFrameLandmark = r_obj
copy.armTrajectory.lRefFrameLandmark = l_obj
# NOTE(mbforbes): Conditions currently aren't copied (though
# they also currently don't do anything, so I'm leaving for
# now.)
# Both arm targets and trajectories have a gripper action.
copy.gripperAction = GripperAction(
GripperState(action_step.gripperAction.rGripper.state),
GripperState(action_step.gripperAction.lGripper.state))
return copy
def _stop_recording(self, __=None):
'''Stops recording continuous motion.
Args:
__ (Landmark): unused, default: None
Returns:
[str, int]: a speech response and a GazeGoal.* constant
'''
if self._is_recording_motion:
self._is_recording_motion = False
traj_step = ActionStep()
traj_step.type = ActionStep.ARM_TRAJECTORY
waited_time = self._arm_trajectory.timing[0]
for i in range(len(self._arm_trajectory.timing)):
self._arm_trajectory.timing[i] -= waited_time
self._arm_trajectory.timing[i] += rospy.Duration(0.1)
self._fix_trajectory_ref()
# Note that [:] is a shallow copy, copying references to
# each element into a new list.
traj_step.armTrajectory = ArmTrajectory(
self._arm_trajectory.rArm[:], # (ArmState[])
self._arm_trajectory.lArm[:], # (ArmState[])
self._arm_trajectory.timing[:], # (duration[])
self._arm_trajectory.rRefFrame, # (uint8)
self._arm_trajectory.lRefFrame, # (uint8)
self._arm_trajectory.rRefFrameLandmark, # (Landmark)
self._arm_trajectory.lRefFrameLandmark # (Landmark)
)
traj_step.gripperAction = GripperAction(
GripperState(self.arms.get_gripper_state(Side.RIGHT)),
GripperState(self.arms.get_gripper_state(Side.LEFT)))
self.session.add_step_to_action(traj_step,
self._world.get_frame_list())
self._arm_trajectory = None
self._trajectory_start_time = None
return [
RobotSpeech.STOPPED_RECORDING_MOTION + ' ' +
RobotSpeech.STEP_RECORDED, GazeGoal.NOD
]
else:
return [RobotSpeech.MOTION_NOT_RECORDING, GazeGoal.SHAKE]