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