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)