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 save_arm_pose(self, dummy=None): '''Saves current arm state as an action step''' states = self._get_arm_states() step = ActionStep() step.type = ActionStep.ARM_TARGET step.armTarget = ArmTarget(states[0], states[1], 0.2, 0.2) step.gripperAction = GripperAction( self.arms.get_gripper_state(0), self.arms.get_gripper_state(1)) self.session.save_arm_pose(step, self.world.get_frame_list()) return [RobotSpeech.STEP_RECORDED, GazeGoal.NOD]
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 save_gripper_step(self, arm_index, gripper_state): '''Saves an action step that involves a gripper state change''' if (self.session.n_actions() > 0): if (Interaction._is_programming): states = self._get_arm_states() step = ActionStep() step.type = ActionStep.ARM_TARGET step.armTarget = ArmTarget(states[0], states[1], 0.2, 0.2) actions = [self.arms.get_gripper_state(0), self.arms.get_gripper_state(1)] actions[arm_index] = gripper_state step.gripperAction = GripperAction(actions[0], actions[1]) self.session.add_step_to_action(step, self.world.get_frame_list())
def save_step(self, dummy=None): '''Saves current arm state as an action step''' if (self.session.n_actions() > 0): if (Interaction._is_programming): states = self._get_arm_states() step = ActionStep() step.type = ActionStep.ARM_TARGET step.armTarget = ArmTarget(states[0], states[1], 0.2, 0.2) step.gripperAction = GripperAction( self.arms.get_gripper_state(0), self.arms.get_gripper_state(1)) self.session.add_step_to_action(step, self.world.get_frame_list()) return [RobotSpeech.STEP_RECORDED, GazeGoal.NOD] else: return ['Action ' + str(self.session.current_action_index) + RobotSpeech.ERROR_NOT_IN_EDIT, GazeGoal.SHAKE] else: return [RobotSpeech.ERROR_NO_SKILLS, GazeGoal.SHAKE]
def _copy_action_step(action_step): '''Makes a copy of an action step''' copy = ActionStep() copy.type = int(action_step.type) 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.rRefFrameObject l_obj = action_step.armTrajectory.lRefFrameObject copy.armTrajectory.rRefFrameObject = r_obj copy.armTrajectory.lRefFrameObject = l_obj copy.gripperAction = GripperAction(action_step.gripperAction.rGripper, action_step.gripperAction.lGripper) return copy