add slider control script for pro 600.

CMakeLists.txt

@@ -59,6 +59,7 @@ catkin_install_python(PROGRAMS
   scripts/following_marker.py
   scripts/follow_and_pump.py
   scripts/simple_gui.py
+  scripts/slider_600.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

config/mycobot_600.rviz

@@ -163,7 +163,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.58317447
+      Distance: 2.0593462
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -178,10 +178,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.440398335
+      Pitch: 0.660398126
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.430389732
+      Yaw: 3.10539246
     Saved: ~
 Window Geometry:
   Displays:

scripts/slider_600.py

@@ -0,0 +1,290 @@
+#!/usr/bin/python
+
+from socket import *
+import math
+import sys
+import time
+from multiprocessing import Lock
+
+import rospy
+from sensor_msgs.msg import JointState
+
+global mc
+mutex = Lock()
+
+
+class ElephantRobot(object):
+    def __init__(self,host,port):
+        # setup connection
+        self.BUFFSIZE = 2048
+        self.ADDR = (host, port)
+        self.tcp_client = socket(AF_INET, SOCK_STREAM)
+
+    def start_client(self):
+        try:
+            self.tcp_client.connect(self.ADDR)
+            return ''
+        except error, e:
+            return e
+
+    def stop_client(self):
+        self.tcp_client.close()
+
+    def send_command(self,command):
+        with mutex:
+            self.tcp_client.send(command.encode())
+            recv_data = self.tcp_client.recv(self.BUFFSIZE).decode()
+            res_str = str(recv_data)
+            print 'recv = ' + res_str
+            res_arr = res_str.split(":")
+            if len(res_arr) == 2:
+                return res_arr[1]
+            else:
+                return ''
+
+    def string_to_coords(self, data):
+        data = data.replace('[','')
+        data = data.replace(']','')
+        data_arr = data.split(",")
+        if len(data_arr) == 6:
+            try:
+                coords_1 = float(data_arr[0])
+                coords_2 = float(data_arr[1])
+                coords_3 = float(data_arr[2])
+                coords_4 = float(data_arr[3])
+                coords_5 = float(data_arr[4])
+                coords_6 = float(data_arr[5])
+                coords = [coords_1, coords_2, coords_3,
+                        coords_4, coords_5, coords_6]
+                return coords
+            except:
+                return invalid_coords()
+        return invalid_coords()
+
+    def string_to_double(self, data):
+        try:
+            val = float(data)
+            return val
+        except:
+            return -9999.99
+
+    def string_to_int(self, data):
+        try:
+            val = int(data)
+            return val
+        except:
+            return -9999
+
+    def invalid_coords(self):
+        coords = [-1, -2, -3, -4, -1, -1]
+        return coords
+
+    def get_angles(self):
+        command = 'get_angles()\n'
+        res = self.send_command(command)
+        return self.string_to_coords(res)
+
+    def get_coords(self):
+        command = 'get_coords()\n'
+        res = self.send_command(command)
+        return self.string_to_coords(res)
+
+    def get_speed(self):
+        command = 'get_speed()\n'
+        res = self.send_command(command)
+        return self.string_to_double(res)
+
+    def power_on(self):
+        command = 'power_on()\n'
+        res = self.send_command(command)
+        return True
+
+    def power_off(self):
+        command = 'power_off()\n'
+        res = self.send_command(command)
+        return True
+
+    def check_running(self):
+        command = 'check_running()\n'
+        res = self.send_command(command)
+        return res == '1'
+
+    def state_check(self):
+        command = 'state_check()\n'
+        res = self.send_command(command)
+        return res == '1'
+
+    def program_open(self, file_path):
+        command = 'program_open(' + file_path + ")\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def program_run(self, start_line):
+        command = 'program_run(' + str(start_line) + ")\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def read_next_error(self):
+        command = 'read_next_error()\n'
+        res = self.send_command(command)
+        return res
+
+    def write_coords(self,coords,speed):
+        command = "set_coords("
+        for item in coords:
+            command += str(item) + ','
+        command += str(speed) + ')\n'
+        self.send_command(command)
+
+    def write_coord(self, axis, value, speed):
+        coords = self.get_coords()
+        if coords != self.invalid_coords():
+            coords[axis] = value
+            self.write_coords(coords,speed)
+
+    def write_angles(self,angles,speed):
+        command = "set_angles("
+        for item in angles:
+            command += str(item) + ','
+        command += str(speed) + ')\n'
+        self.send_command(command)
+
+    def write_angle(self, joint, value, speed):
+        angles = self.get_angles()
+        if angles != self.invalid_coords():
+            angles[joint] = value
+            self.write_angles(angles,speed)
+
+    def set_speed(self, percentage):
+        command = "set_speed(" + str(percentage) + ")\n"
+        self.send_command(command)
+
+    def set_carte_torque_limit(self, axis_str, value):
+        command = "set_torque_limit(" + axis_str + "," + str(value) + ")\n"
+        self.send_command(command)
+
+    def set_upside_down(self, up_down):
+        up = "1"
+        if up_down:
+            up = "0"
+        command = "set_upside_down(" + up + ")\n"
+        self.send_command(command)
+
+    def set_payload(self, payload):
+        command = "set_speed(" + str(payload) + ")\n"
+        self.send_command(command)
+
+    def state_on(self):
+        command = "state_on()\n"
+        self.send_command(command)
+
+    def state_off(self):
+        command = "state_off()\n"
+        self.send_command(command)
+
+    def task_stop(self):
+        command = "task_stop()\n"
+        self.send_command(command)
+
+    def jog_angle(self, joint_str, direction, speed):
+        command = "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
+        self.send_command(command)
+
+    def jog_coord(self, axis_str, direction, speed):
+        command = "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
+        self.send_command(command)
+
+    def get_digital_in(self, pin_number):
+        command = "get_digital_in(" + str(pin_number) + ")\n"
+        self.send_command(command)
+
+    def get_digital_out(self, pin_number):
+        command = "get_digital_out(" + str(pin_number) + ")\n"
+        self.send_command(command)
+
+    def set_digital_out(self, pin_number, pin_signal):
+        command = "set_digital_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
+        self.send_command(command)
+
+    def set_analog_out(self, pin_number, pin_signal):
+        command = "set_analog_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
+        self.send_command(command)
+
+    def get_acceleration(self):
+        command = "get_acceleration()\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def set_acceleration(self, acceleration):
+        command = "set_acceleration(" + str(acceleration) + ")\n"
+        self.send_command(command)
+
+    def command_wait_done(self):
+        command = "wait_command_done()\n"
+        self.send_command(command)
+
+    def wait(self, seconds):
+        command = "wait(" + str(seconds) + ")\n"
+        self.send_command(command)
+
+    def assign_variable(self, var_name, var_value):
+        command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ')\n'
+        self.send_command(command)
+
+    def get_variable(self, var_name):
+        command = 'get_variable("' + str(var_name) + '")\n'
+        return self.send_command(command)
+
+
+
+old_list = []
+
+def callback(data):
+    global old_list
+    #rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+    print('position', data.position)
+    data_list = []
+    for index, value in enumerate(data.position):
+        value = value * 180 / math.pi
+        data_list.append(value)
+    print('data', data_list)
+
+    if not old_list:
+        old_list = data_list
+        mc.write_angles(data_list, 1999)
+    elif old_list != data_list:
+        old_list = data_list
+        if (mc.check_running()):
+            mc.task_stop()
+            time.sleep(0.05)
+        
+        mc.write_angles(data_list, 1999)
+
+
+def listener():
+    global mc
+    rospy.init_node('control_slider', anonymous=True)
+
+    ip = rospy.get_param('~ip', '192.168.10.169')
+    print(ip)
+    mc = ElephantRobot(ip, 5001)
+    # START CLIENT
+    res = mc.start_client()
+    if res != '':
+        print res
+        sys.exit(1)
+        print ep.wait(5)
+    print mc.get_angles()
+    print mc.get_coords()
+    mc.set_speed(30)
+    print mc.get_speed()
+
+    rospy.Subscriber("joint_states", JointState, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+    print('sping ...')
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    listener()

urdf/600_urdf/mycobot_600_urdf.urdf

@@ -142,16 +142,16 @@
 
 
   <joint name="joint3_to_joint2" type="revolute">
-    <axis xyz="0 0 1"/>
+    <axis xyz="0 0 -1"/>
     <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
     <parent link="link1"/>
     <child link="link2"/>
-    <origin xyz= "0 0  0" rpy = "0 -1.57080  1.57080"/>  
+    <origin xyz= "0 0  0" rpy = "1.5708 0 0"/>  
   </joint>
 
 
   <joint name="joint4_to_joint3" type="revolute">
-    <axis xyz=" 0 0 1"/>
+    <axis xyz=" 0 0 -1"/>
     <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
     <parent link="link2"/>
     <child link="link3"/>
@@ -159,11 +159,11 @@
   </joint>
 
   <joint name="joint5_to_joint4" type="revolute">
-    <axis xyz=" 0 0 1"/>
+    <axis xyz=" 0 0 -1"/>
     <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
     <parent link="link3"/>
     <child link="link4"/>
-    <origin xyz= "0.25 0  0.1091" rpy = "0 0 1.57080"/>  
+    <origin xyz= "0.25 0  0.1091" rpy = "0 0 0"/>  
   </joint>
 
   <joint name="joint6_to_joint5" type="revolute">