133 lines
4.0 KiB
Python
133 lines
4.0 KiB
Python
import math
|
||
|
||
from Constant import position_accuracy_command
|
||
from Constant import position_accuracy_action
|
||
from Constant import DebugPosition
|
||
class Position:
|
||
def __init__(self):
|
||
self.X = 0.0
|
||
self.Y = 0.0
|
||
self.Z = 0.0
|
||
self.U = 0.0
|
||
self.V = 0.0
|
||
self.W = 0.0
|
||
self.Axis_0 = 0.0
|
||
self.Axis_1 = 0.0
|
||
self.Axis_2 = 0.0
|
||
self.Axis_3 = 0.0
|
||
self.Axis_4 = 0.0
|
||
self.Axis_5 = 0.0
|
||
#点位类型 1世界坐标(默认）4关节坐标
|
||
self.position_type =1
|
||
|
||
self.a = 0.0
|
||
self.b = 0.0
|
||
self.c = 0.0
|
||
|
||
|
||
def compare(self,position,is_action=False):
|
||
if DebugPosition:
|
||
return True
|
||
#点位类型 1世界坐标(默认）4关节坐标
|
||
if(position.position_type==4):
|
||
return self._compare_joint(position,is_action)
|
||
else:
|
||
return self._compare_world(position,is_action)
|
||
|
||
def _compare_world(self,position,is_action=False):
|
||
"""
|
||
世界坐标比较
|
||
:param position:
|
||
:return:精度内TRUE,否则为FALSE
|
||
"""
|
||
|
||
distance = math.sqrt((self.X - position.X) ** 2 +
|
||
(self.Y - position.Y) ** 2 +
|
||
(self.Z - position.Z) ** 2 )
|
||
if distance<=(position_accuracy_action if is_action else position_accuracy_command):
|
||
return True
|
||
else:
|
||
return False
|
||
|
||
def _compare_joint(self,position,is_action=False):
|
||
"""
|
||
关节坐标比较
|
||
:param position:
|
||
:return:精度内TRUE,否则为FALSE
|
||
"""
|
||
distance = math.sqrt((self.Axis_0 - position.X) ** 2 +
|
||
(self.Axis_1 - position.Y) ** 2 +
|
||
(self.Axis_2 - position.Z) ** 2 +
|
||
(self.Axis_3 - position.U) ** 2 )
|
||
if distance<=(position_accuracy_action if is_action else position_accuracy_command):
|
||
return True
|
||
else:
|
||
return False
|
||
|
||
# def compare(self,position):
|
||
# if self.X-position.X<position_accuracy and \
|
||
# self.Y-position.Y<position_accuracy and \
|
||
# self.Z - position.Z < position_accuracy and \
|
||
# self.U - position.U < position_accuracy and \
|
||
# self.V - position.V < position_accuracy and \
|
||
# self.W - position.W < position_accuracy:
|
||
# return True
|
||
# else:
|
||
# return False
|
||
# pass
|
||
|
||
def is_error_angel_move(self,position,interval):
|
||
if self.X - position.X >= interval or \
|
||
self.Y - position.Y >= interval or \
|
||
self.Z - position.Z >= interval or \
|
||
self.U - position.U >= interval or \
|
||
self.V - position.V >= interval or \
|
||
self.W - position.W >= interval:
|
||
return True
|
||
else:
|
||
return False
|
||
pass
|
||
"""
|
||
摄像头识别位置和角度
|
||
"""
|
||
class Detection_Position(Position):
|
||
def __init__(self):
|
||
super().__init__()
|
||
|
||
def init_position(self,X,Y,Z,U,V,W):
|
||
self.X = X
|
||
self.Y = Y
|
||
self.Z = Z
|
||
self.U = U
|
||
self.V = V
|
||
self.W = W
|
||
|
||
|
||
class Real_Position(Position):
|
||
def __init__(self):
|
||
super().__init__()
|
||
|
||
def init_position(self, X, Y, Z, U, V, W):
|
||
self.X = X
|
||
self.Y = Y
|
||
self.Z = Z
|
||
self.U = U
|
||
self.V = V
|
||
self.W = W
|
||
return self
|
||
|
||
def init_position_joint_and_world(self, X, Y, Z, U, V, W,Axis_0,Axis_1,Axis_2,Axis_3,Axis_4,Axis_5):
|
||
self.init_position(X,Y,Z,U,V,W)
|
||
self.Axis_0 = Axis_0
|
||
self.Axis_1 = Axis_1
|
||
self.Axis_2 = Axis_2
|
||
self.Axis_3 = Axis_3
|
||
self.Axis_4 = Axis_4
|
||
self.Axis_5 = Axis_5
|
||
return self
|
||
|
||
# def init_position(self, position):
|
||
# return self.init_position(position.X,position.Y,position.Z,position.U,position.V,position.W)
|
||
|
||
def to_string(self):
|
||
return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W) |