def ikMove(self, mode='line', pos=_POS, euler=_ORI, phi=_PHI):
"""Publish msg of ik cmd (deg) to manager node."""
roll, pitch, yaw = euler
roll = roll * pi/ 180
pitch = pitch* pi/ 180
yaw = yaw * pi/ 180
self.__is_busy = True
msg = KinematicsPose()
msg.name = 'arm'
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
quaternion = self.euler2quaternion((roll, pitch, yaw))
msg.pose.orientation.x = quaternion[0]
msg.pose.orientation.y = quaternion[1]
msg.pose.orientation.z = quaternion[2]
msg.pose.orientation.w = quaternion[3]
msg.speed = self.__speed
msg.phi = radians(phi)
#rospy.loginfo('Sent:{}'.format(cmd))
if mode == 'line':
self.__line_pub.publish(msg)
elif mode == 'p2p':
self.__p2p_pub.publish(msg)
def ikMove_quat(self, mode, pos, quaternion, phi):
"""Publish msg of ik cmd (deg) to manager node."""
self.__is_busy = True
self.__ik_fail = False
self.__is_singularity = False
msg = KinematicsPose()
msg.name = 'arm'
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.x = quaternion[1]
msg.pose.orientation.y = quaternion[2]
msg.pose.orientation.z = quaternion[3]
msg.pose.orientation.w = quaternion[0]
msg.speed = 400
msg.phi = phi
#rospy.loginfo('Sent:{}'.format(cmd))
if mode == 'line':
self.__line_pub.publish(msg)
elif mode == 'p2p':
self.__p2p_pub.publish(msg)
def ikMove(self,
mode='line',
pos=_POS,
euler=_ORI,
phi=_PHI,
quat=None,
speed=None):
"""Publish msg of ik cmd (deg) to manager node."""
roll, pitch, yaw = euler
roll = radians(roll)
pitch = radians(pitch)
yaw = radians(yaw)
msg = KinematicsPose()
msg.name = 'arm'
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
if quat is not None:
msg.pose.orientation.w = quat[0]
msg.pose.orientation.x = quat[1]
msg.pose.orientation.y = quat[2]
msg.pose.orientation.z = quat[3]
else:
quat = self.euler2quaternion((roll, pitch, yaw))
msg.pose.orientation.x = quat[0]
msg.pose.orientation.y = quat[1]
msg.pose.orientation.z = quat[2]
msg.pose.orientation.w = quat[3]
if speed is not None:
msg.speed = speed
else:
msg.speed = self.speed
msg.phi = radians(phi)
if mode == 'line':
self.__line_pub.publish(self.line_topic, msg)
elif mode == 'p2p':
self.__p2p_pub.publish(self.p2p_topic, msg)