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_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 stop_recording(self, dummy=None):
'''Stops recording continuous motion'''
if (Interaction._is_recording_motion):
Interaction._is_recording_motion = False
traj_step = ActionStep()
traj_step.type = ActionStep.ARM_TRAJECTORY
waited_time = Interaction._arm_trajectory.timing[0]
for i in range(len(Interaction._arm_trajectory.timing)):
Interaction._arm_trajectory.timing[i] -= waited_time
Interaction._arm_trajectory.timing[i] += rospy.Duration(0.1)
'''If motion was relative, record transformed pose'''
traj_step.armTrajectory = ArmTrajectory(
Interaction._arm_trajectory.rArm[:],
Interaction._arm_trajectory.lArm[:],
Interaction._arm_trajectory.timing[:],
Interaction._arm_trajectory.rRefFrame,
Interaction._arm_trajectory.lRefFrame,
Interaction._arm_trajectory.rRefFrameObject,
Interaction._arm_trajectory.lRefFrameObject)
traj_step.gripperAction = GripperAction(
self.arms.get_gripper_state(0),
self.arms.get_gripper_state(1))
self.session.add_step_to_action(traj_step,
self.world.get_frame_list())
Interaction._arm_trajectory = None
Interaction._trajectory_start_time = None
return [RobotSpeech.STOPPED_RECORDING_MOTION + ' ' +
RobotSpeech.STEP_RECORDED, GazeGoal.NOD]
else:
return [RobotSpeech.MOTION_NOT_RECORDING, 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 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 stop_recording(self, dummy=None):
'''Stops recording continuous motion'''
self.busy = True
if (self._demo_state == DemoState.RECORDING_DEMO):
traj_step = ActionStep()
traj_step.type = ActionStep.ARM_TRAJECTORY
waited_time = Interaction._arm_trajectory.timing[0]
for i in range(len(Interaction._arm_trajectory.timing)):
Interaction._arm_trajectory.timing[i] -= waited_time
Interaction._arm_trajectory.timing[i] += rospy.Duration(0.1)
'''If motion was relative, record transformed pose'''
traj_step.armTrajectory = ArmTrajectory(
Interaction._arm_trajectory.rArm[:],
Interaction._arm_trajectory.lArm[:],
Interaction._arm_trajectory.timing[:],
Interaction._arm_trajectory.rRefFrame,
Interaction._arm_trajectory.lRefFrame,
Interaction._arm_trajectory.rRefFrameObject,
Interaction._arm_trajectory.lRefFrameObject)
traj_step.gripperAction = GripperAction(
self.arms.get_gripper_state(0),
self.arms.get_gripper_state(1))
self.session.add_step_to_action(traj_step,
self.world.get_frame_list())
Interaction._arm_trajectory = None
Interaction._trajectory_start_time = None
self.session.save_current_action()
self.freeze_arm(0)
self._demo_state = DemoState.HAS_RECORDED_DEMO
Response.say(RobotSpeech.STOPPED_RECORDING)
Response.perform_gaze_action(GazeGoal.NOD)
else:
Response.say(RobotSpeech.ERROR_NOT_RECORDING)
Response.perform_gaze_action(GazeGoal.SHAKE)
rospy.loginfo('Current state: ' + self._demo_state)
self.busy = False
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]
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):
'''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 _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
