def test_whenSettingPositionThenPositionIsSetWithSaidData(self): self.referentialConverter = ReferentialConverter( self.ANOTHER_POINT, self.ANOTHER_ORIENTATION) thePoint = self.ANOTHER_POINT theOrientation = float(self.ANOTHER_ORIENTATION % 360) / 180 positionX = self.referentialConverter.positionRobotInWorldX positionY = self.referentialConverter.positionRobotInWorldY orientation = self.referentialConverter.orientation self.assertEqual(thePoint.__getitem__(0), positionX) self.assertEqual(thePoint.__getitem__(1), positionY) self.assertEqual(theOrientation, orientation)
def __adjustOrientation(self, pointToMove): degreeAngle = (np.arctan( (pointToMove[1] / pointToMove[0])) / np.pi) * 180 angleToRotate = degreeAngle % 90 if angleToRotate > 45: angleToRotate = angleToRotate - 90 print angleToRotate point = (0, 0) referentialConverter = ReferentialConverter(point, angleToRotate) print "adjusting angle", -angleToRotate self.rotate(-angleToRotate) pointAdjusted = referentialConverter.convertWorldToRobot(pointToMove) print "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ BOBA FETT", pointAdjusted[ 0], pointAdjusted[1] return pointAdjusted
def setUp(self): self.referentialConverter = ReferentialConverter( self.DEFAULT_POINT, self.DEFAULT_ORIENTATION)