#!/usr/bin/env python3 import rospy
from stdmsgs.msg import Float64MultiArray from sensormsgs.msg import JointState

class ur16estartpose():

def __init__(self):
    rospy.init_node('ur16e_start_pose', anonymous=True)
    self.velocity = Float64MultiArray()
    self.goal_position = [2.3, 0.0, 2.0, 2.0, 1.0, 0.0]
    self.Kp = 1
    pub_joint_velocity = rospy.Publisher("/robot1/joint_group_vel_controller/command", Float64MultiArray, queue_size=10)
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        self.velocity.data = [2.0, 0.0, 2.0, 2.0, 1.0, 0.0]

        while all(item !=0 for item in velocity):
            rospy.Subscriber("/robot1/joint_states", JointState, self.callback)
            self.calculator()
            pub_joint_velocity.publish(velocity)
            rospy.loginfo(velocity)
            rospy.spin()


def callback(self,msg):
    self.joint_pos_msg=JointState()
    self.joint_pos_msg = msg.position
    rospy.loginfo(joint_pos_msg)


def calculator(self):
    e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    e[0] = self.goal_position[0] - self.joint_pos_msg[0] 
    e[1] = self.goal_position[1] - self.joint_pos_msg[1] 
    e[2] = self.goal_position[2] - self.joint_pos_msg[2] 
    e[3] = self.goal_position[3] - self.joint_pos_msg[3]
    e[4] = self.goal_position[4] - self.joint_pos_msg[4]
    e[5] = self.goal_position[5] - self.joint_pos_msg[5]

    self.joint_velocity_msg[0] = self.Kp * e[0]
    self.joint_velocity_msg[1] = self.Kp * e[1]
    self.joint_velocity_msg[2] = self.Kp * e[2]
    self.joint_velocity_msg[3] = self.Kp * e[3]
    self.joint_velocity_msg[4] = self.Kp * e[4]
    self.joint_velocity_msg[5] = self.Kp * e[5]

if name=="main": ur16estartpose()