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
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
