def _set_ref(self, new_ref_name): '''Changes the reference frame of the action step''' new_ref = World.get_ref_from_name(new_ref_name) if (new_ref != ArmState.ROBOT_BASE): index = ActionStepMarker._ref_names.index(new_ref_name) new_ref_obj = ActionStepMarker._ref_object_list[index - 1] else: new_ref_obj = Object() if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: self.action_step.armTarget.rArm = World.convert_ref_frame( self.action_step.armTarget.rArm, new_ref, new_ref_obj) else: self.action_step.armTarget.lArm = World.convert_ref_frame( self.action_step.armTarget.lArm, new_ref, new_ref_obj) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): for i in range(len(self.action_step.armTrajectory.timing)): if self.arm_index == 0: arm_old = self.action_step.armTrajectory.rArm[i] arm_new = World.convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.rArm[i] = arm_new else: arm_old = self.action_step.armTrajectory.lArm[i] arm_new = World.convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.lArm[i] = arm_new if self.arm_index == 0: self.action_step.armTrajectory.rRefFrameObject = new_ref_obj self.action_step.armTrajectory.rRefFrame = new_ref else: self.action_step.armTrajectory.lRefFrameObject = new_ref_obj self.action_step.armTrajectory.lRefFrame = new_ref
def _fix_trajectory_ref(self): '''Makes the reference frame of continuous trajectories uniform''' (r_ref, _), (_, r_ref_obj) = self._find_dominant_ref( Interaction._arm_trajectory.rArm) (l_ref, _), (_, l_ref_obj) = self._find_dominant_ref( Interaction._arm_trajectory.lArm) for i in range(len(Interaction._arm_trajectory.timing)): Interaction._arm_trajectory.rArm[i] = World.convert_ref_frame( Interaction._arm_trajectory.rArm[i], r_ref, r_ref_obj) Interaction._arm_trajectory.lArm[i] = World.convert_ref_frame( Interaction._arm_trajectory.lArm[i], l_ref, l_ref_obj) Interaction._arm_trajectory.rRefFrame = r_ref Interaction._arm_trajectory.rRefFrameObject = r_ref_obj Interaction._arm_trajectory.lRefFrame = l_ref Interaction._arm_trajectory.lRefFrameObject = l_ref_obj