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)
