#!/usr/bin/env python import numpy as npfrom geometry_msgs.msg import Pose, Point, Quaternion, Twistfrom tf.transformations import quaternion_from_euler #orientation x,y,z,w#x=i[3]#y=i[4]#z=i[5]#w=i[6] g=np.array([i[3],i[4],i[5],i[6]]) #position x,y,