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