def get_current_jaw(self, unit='rad'):
"""
:param unit: 'rad' or 'deg'
:return: Numpy.float64
"""
jaw = np.float64(self.__position_jaw_current)
if unit == "deg":
jaw = U.rad_to_deg(self.__position_jaw_current)
return jaw
def get_current_joint(self, unit='rad'):
"""
:param unit: 'rad' or 'deg'
:return: List
"""
joint = self.__position_joint_current
print joint
if unit == 'deg':
joint = U.rad_to_deg(self.__position_joint_current)
joint[2] = self.__position_joint_current[2]
return joint
def get_current_pose(self,
unit='rad'): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
raise NotImplementedError
pos, rot = self.PyKDLFrame_to_NumpyArray(
self.__position_cartesian_current)
if unit == 'deg':
rot = U.rad_to_deg(rot)
return pos, rot
def get_current_jaw_and_wait(self,
unit='rad'
): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
self.__get_jaw_event.clear()
# the position is originally not received
self.__get_jaw = False
# recursively call this function until the position is received
self.__get_jaw_event.wait(20) # 1 minute at most
if self.__get_jaw:
jaw = np.float64(self.__position_jaw_current)
if unit == "deg":
jaw = U.rad_to_deg(self.__position_jaw_current)
return jaw
else:
return []
def get_current_pose_and_wait(self,
unit='rad'
): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
raise NotImplementedError
self.__get_position_event.clear()
# the position is originally not received
# self.__get_position = False
# recursively call this function until the position is received
# self.__get_position_event.wait(20) # 1 minute at most
if self.__get_position_event.wait(20): # 1 minute at most
pos, rot = self.PyKDLFrame_to_NumpyArray(
self.__position_cartesian_current)
if unit == 'deg':
rot = U.rad_to_deg(rot)
return pos, rot
else:
return [], []