init

CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(myCobotROS) 
+add_compile_options(-std=c++11)
+
+## Find catkin and any catkin packages
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg serial)
+
+## Generate added messages and services
+generate_messages(DEPENDENCIES std_msgs)
+
+## Declare a catkin package
+catkin_package()
+
+## Build talker and listener
+include_directories(include ${catkin_INCLUDE_DIRS}) 
+
+catkin_install_python(PROGRAMS 
+  scripts/display.py 
+  scripts/control_slider.py 
+  scripts/control_marker.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+
+

LICENSE

@@ -0,0 +1,25 @@
+BSD 2-Clause License
+
+Copyright (c) 2020, Elephant Robotics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,96 @@
+# myCobotROS
+
+<!-- This is the mycobot ROS package designed by Zhang Lijun([lijun.zhang@elephantrobotics.com]()) -->
+
+## 1. Installation
+
+### 1.1 Pre-Requriements
+
+For using this package, the [pymycobot]() library should be installed first.
+
+### 1.2 Package Download and Install
+
+Install ros package in your src folder of your Catkin workspace.
+
+```bash
+$ cd ~/catkin_ws/src
+$ git clone https://github.com/elephantrobotics/myCobotROS.git
+$ cd ~/catkin_ws
+$ catkin_make
+```
+
+## 2. Package Modules
+
+### 2.1 Nodes
+
+- `display` is a display node. When the node is running, When the node is running, the model of ROS will show the movement of mycobot synchronously.
+- `control_slider` is the node which slider bar control.
+- `control_marker` is the node which use interactive marker control.
+
+### 2.2 Topics
+
+- `joint_states` - control mycobot status.
+
+  ```
+  Message_type: std_msgs/JointState
+  Data: position[float, float, float, float, float, float]
+  ```
+
+## 3. Visualization in RViz
+
+### 3.1 Functions
+
+- Visualization -- display.launch: This function will display robot arm movement in realtime when you manually move mycobot.
+
+- Control -- control.launch: This function will allow you use slider bar to control movement of the robot arm. 
+
+### 3.2 Lanuch and Run
+
+-**Step 1**: In one terminal, open the core.
+
+```bash
+rocore #open another tab
+```
+
+-**Step 2**: Launch
+
+a) For display or marker control, in second terminal, run:
+
+```bash
+roslaunch myCobotROS display.launch
+```
+
+b) For slider bar control, in second terminal, run:
+
+```
+roslaunch myCobotROS control.launch
+```
+
+-**Step 3**: Open rviz to view robot
+
+```bash
+rosrun rviz rviz
+```
+
+-**Step 4**: Run python script
+
+a) For display
+
+```bash
+rosrun myCobotRos display.py
+```
+
+b) For slider bar.
+
+```bash
+rosrun myCobotRos control_slider.py
+```
+
+c) For marker control
+
+```bash
+rosrun myCobots control_marker.py
+```
+
+
+

launch/control.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="model" default="$(find beginner_tutorials)/urdf/mycobot_urdf.urdf"/>
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 
+		<param name="use_gui" value="true"/>	
+	</node>
+
+</launch>

launch/mycobot.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="model" default="$(find beginner_tutorials)/urdf/mycobot_urdf.urdf"/>
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+	<!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 
+		<param name="use_gui" value="true"/>	
+	</node>  -->
+
+</launch>

package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>myCobotROS</name>
+  <version>0.1.0</version>
+  <description>The myCobotROS package</description>
+
+  <maintainer email="lijun.zhang@elephantrobotics.com">ZhangLijun</maintainer>
+  <license>BSD</license>
+  <url type="website"></url>
+  <author email="lijun.zhang@elephantrobotics.com">ZhangLijun</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scripts/.gitignore

@@ -0,0 +1,138 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/

scripts/control_marker.py

@@ -0,0 +1,227 @@
+#!/usr/bin/env python3
+# from std_msgs.msg import String
+import time
+
+import rospy
+from interactive_markers.interactive_marker_server import *
+from interactive_markers.menu_handler import *
+from visualization_msgs.msg import *
+from geometry_msgs.msg import Point, Quaternion
+
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+
+from pythonAPI.mycobot import MyCobot
+
+
+server = None
+menu_handler = MenuHandler()
+
+# center of the body
+center_x_changed = 0
+center_y_changed = 0
+center_z_changed = 0
+
+# marker box 
+def processFeedback( feedback ):#
+    global center_x_changed, center_y_changed, center_z_changed
+    current_x = feedback.pose.position.x
+    current_y = feedback.pose.position.y
+    current_z = feedback.pose.position.z
+
+    _x = feedback.pose.orientation.x
+    _y = feedback.pose.orientation.y
+    _z = feedback.pose.orientation.z
+
+    center_x_changed = current_x
+    center_y_changed = current_y
+    center_z_changed = current_z
+    print(center_x_changed, center_y_changed, center_z_changed,
+         _x, _y, _z)
+
+    coords = [
+                current_y * 1000, 
+                current_x * -1000, 
+                current_z * 1000, 
+                _x * 100,
+                _y * 100,
+                _z * 100,
+             ]
+    speed = 80
+    mode = 0
+    mycobot.send_coords(coords, speed, mode)
+
+    server.applyChanges()
+
+def makeBox( msg ):#
+    marker = Marker()
+
+    marker.type = Marker.SPHERE
+    marker.scale.x = msg.scale * 0.1
+    marker.scale.y = msg.scale * 0.1
+    marker.scale.z = msg.scale * 0.1
+    marker.color.r = 0.5
+    marker.color.g = 0.5
+    marker.color.b = 0.5
+    marker.color.a = 0.1
+
+    return marker
+
+def makeBoxControl( msg ):#
+    control =  InteractiveMarkerControl()
+    control.always_visible = True
+    control.markers.append( makeBox(msg) )
+    msg.controls.append( control )
+    return control
+
+def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof = False):#
+    int_marker = InteractiveMarker()
+    int_marker.header.frame_id = "/joint1"
+    int_marker.pose.position = position	# Defined the position of the marker
+    int_marker.pose.orientation = orientation
+    int_marker.scale = 0.1
+
+    int_marker.name = "simple_6dof"
+    int_marker.description = "mycobot_controller"
+
+    # insert a box
+    makeBoxControl(int_marker)
+    int_marker.controls[0].interaction_mode = interaction_mode
+
+    if show_6dof: 
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 1
+        control.orientation.y = 0
+        control.orientation.z = 0
+        control.name = "rotate_x"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 1
+        control.orientation.y = 0
+        control.orientation.z = 0
+        control.name = "move_x"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+      
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 1
+        control.orientation.z = 0
+        control.name = "rotate_z"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 1
+        control.orientation.z = 0
+        control.name = "move_z"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 0
+        control.orientation.z = 1
+        control.name = "rotate_y"
+        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+
+        control = InteractiveMarkerControl()
+        control.orientation.w = 1
+        control.orientation.x = 0
+        control.orientation.y = 0
+        control.orientation.z = 1
+        control.name = "move_y"
+        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        if fixed:
+            control.orientation_mode = InteractiveMarkerControl.FIXED
+        int_marker.controls.append(control)
+
+    server.insert(int_marker, processFeedback)
+    menu_handler.apply( server, int_marker.name )
+
+# marker box finish
+
+def listener():
+    global server
+    rospy.init_node('control_marker', anonymous=True)
+
+    coords = mycobot.get_coords()
+    # print(coords)
+
+    # crate a timer to update the pushlished transforms
+    server = InteractiveMarkerServer('mycobot_controller')
+    menu_handler.insert('First Entry', callback=processFeedback)
+    menu_handler.insert('Second Entry', callback=processFeedback)
+
+    # initial position
+    position = Point(coords[1] / -1000, coords[0] / 1000, coords[2] / 1000)
+    # orientation = Quaternion(coords[4] / 100, coords[3] / 100, coords[5] / 100, 1)
+    orientation = Quaternion(0, 0, 0, 1)
+    make6DofMarker(True, InteractiveMarkerControl.NONE, 
+                   position, orientation, True)
+    server.applyChanges()
+
+    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+    rate = rospy.Rate(30) # 10hz
+
+    # pub joint state
+    joint_state_send = JointState()
+    joint_state_send.header = Header()
+
+    joint_state_send.name = [
+                            'joint2_to_joint1', 
+                            'joint3_to_joint2', 
+                            'joint4_to_joint3', 
+                            'joint5_to_joint4', 
+                            'joint6_to_joint5', 
+                            'joint6output_to_joint6'
+                            ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    marker_ = Marker()
+    marker_.header.frame_id = '/joint1'
+    marker_.ns = 'my_namespace'
+
+    while not rospy.is_shutdown():
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        angles = mycobot.get_angles_of_radian()
+        data_list = []
+        for index, value in enumerate(angles):
+            if index != 2:
+                value *= -1
+            data_list.append(value)
+
+        rospy.loginfo(data_list)
+        
+        joint_state_send.position = data_list
+
+        pub.publish(joint_state_send)
+
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    mycobot = MyCobot()
+    listener()

scripts/control_slider.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# from std_msgs.msg import String
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+
+from pythonAPI.mycobot import MyCobot
+
+
+def callback(data):
+    rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+    # print(data.position)
+    data_list = []
+    for index, value in enumerate(data.position):
+        if index != 2:
+            value *= -1
+        data_list.append(value)
+
+    mc.send_angles_by_radian(data_list, 80)
+    # time.sleep(0.5)
+    
+def listener():
+    rospy.init_node('control_slider', anonymous=True)
+
+    rospy.Subscriber("joint_states", JointState, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+    rospy.spin()
+
+if __name__ == '__main__':
+    mc = MyCobot()
+    listener()

scripts/display.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+# license removed for brevity
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+
+from pythonAPI.mycobot import MyCobot
+
+
+def talker():
+    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+    pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
+    rospy.init_node('display', anonymous=True)
+    rate = rospy.Rate(30) # 30hz
+
+    # pub joint state
+    joint_state_send = JointState()
+    joint_state_send.header = Header()
+
+    joint_state_send.name = [
+                            'joint2_to_joint1', 
+                            'joint3_to_joint2', 
+                            'joint4_to_joint3', 
+                            'joint5_to_joint4', 
+                            'joint6_to_joint5', 
+                            'joint6output_to_joint6'
+                            ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    marker_ = Marker()
+    marker_.header.frame_id = '/joint1'
+    marker_.ns = 'my_namespace'
+
+    while not rospy.is_shutdown():
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        angles = mycobot.get_angles_of_radian()
+        data_list = []
+        for index, value in enumerate(angles):
+            if index != 2:
+                value *= -1
+            data_list.append(value)
+
+        joint_state_send.position = data_list
+
+        pub.publish(joint_state_send)
+
+        coords = mycobot.get_coords()
+        rospy.loginfo('{}'.format(coords))
+
+        #marker 
+        marker_.header.stamp = rospy.Time.now()
+        marker_.type = marker_.SPHERE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        #marker position initial 
+        marker_.pose.position.x =  coords[1] / 1000 * -1
+        marker_.pose.position.y =  coords[0] / 1000 
+        marker_.pose.position.z =  coords[2] / 1000
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+if __name__ == '__main__':
+    mycobot = MyCobot()
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
+

scripts/pythonAPI/.gitignore

@@ -0,0 +1,138 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/

scripts/pythonAPI/README.md

@@ -0,0 +1,2 @@
+# ros-python-api
+

scripts/pythonAPI/common.py

@@ -0,0 +1,13 @@
+import enum
+
+class Angle(enum.Enum):
+    J1 = '00'
+    J2 = '01'
+    J3 = '02'
+    J4 = '03'
+    J5 = '04'
+    J6 = '05'
+
+
+class Coords(enum.Enum):
+    ...

scripts/pythonAPI/mycobot.py

@@ -0,0 +1,333 @@
+import sys
+sys.path.append('.')
+import time, subprocess, serial, struct
+
+class MyCobot():
+    '''MyCobot Python API
+
+    Possessed function:
+        power_on()              : 
+        power_off()             :
+        get_angles()            :
+        get_angles_of_radian()  :
+        send_angle()            :
+        send_angles()           :
+        send_angles_by_radian() :
+        set_color()             :
+        get_coords()            :
+        send_coords()           :
+        is_moving()             :
+        pause()                 :
+        resume()                :
+        stop()                  :
+        is_paused()             :
+        get_speed()             :
+        set_speed()             :
+    '''
+
+    def __init__(self):
+        _prot = subprocess.run(['echo -n /dev/ttyUSB*'], 
+                                    stdout=subprocess.PIPE, 
+                                    shell=True).stdout.decode('utf-8')
+        _boudrate = '115200'
+        _timeout = 0.1
+
+        for _ in range(5):
+            try:
+                self.serial_port = serial.Serial(_prot, _boudrate, timeout=_timeout)
+                break
+            except Exception as e:
+                print(e)
+                time.sleep(5)
+                continue
+        else:
+            print('Connect prot failed, eixt.')
+            exit(0)
+
+    def power_on(self):
+        self._write('fe fe 02 10 fa')
+
+    def power_off(self):
+        self._write('fe fe 02 11 fa')
+
+    def get_angles(self):
+        '''Get all angle return a list
+
+        Return:
+            data_list (list):
+
+        '''
+        command = 'fefe0220fa'
+        self._write(command)
+        if self.serial_port.inWaiting() > 0:
+            data = self._read()
+            data_list = self._parse_data(data, 'get_angles')
+            return data_list
+        else:
+            return []
+
+    def get_angles_of_radian(self):
+        '''Get all angle return a list
+
+        Return:
+            data_list (list):
+
+        '''
+        command = 'fefe0220fa'
+        self._write(command)
+        if self.serial_port.inWaiting() > 0:
+            data = self._read()
+            data_list = self._parse_data(data, 'get_angles_of_radian')
+            return data_list
+        else:
+            return []
+
+    def send_angle(self, id, degree, speed):
+        '''Send one angle
+
+        Args:
+            id (common.Angle):
+            degree (int):
+            speed (int): 0 ~100
+
+        '''
+        _hex = self._angle_to_hex(degree)
+        command = 'fefe0621{}{}{}fa'.format(id, _hex, hex(speed)[2:])
+        # print(command)  
+        self._write(command)
+
+    def send_angles(self, degrees, speed):
+        '''Send all angles
+
+        Args:
+            degrees (list): example [0, 0, 0, 0, 0, 0]
+            speed (int): 0 ~ 100
+
+        '''
+        if len(degrees) != 6:
+            print('The lenght of degrees is not right')
+            return
+        command = 'fefe0f22'
+        speed = self._complement_zero(hex(speed)[2:], digit=2)
+        for degree in degrees:
+            _hex = self._angle_to_hex(degree)
+            # print(_hex)
+            command += _hex
+        command += '{}fa'.format(speed)
+        # print(command)
+        self._write(command)
+
+    def send_angles_by_radian(self, radians, speed):
+        '''Send all angles
+
+        Args:
+            degrees (list): example [0, 0, 0, 0, 0, 0]
+            speed (int): 0 ~ 100
+
+        '''
+        if len(radians) != 6:
+            print('The lenght of degrees is not right')
+            return
+        command = 'fefe0f22'
+        speed = self._complement_zero(hex(speed)[2:], digit=2)
+        for radian in radians:
+            # print(radian)
+            _hex = self._angle_to_hex(radian, is_degree=False)
+            # print(_hex)
+            command += _hex
+        command += '{}fa'.format(speed)
+        # print(command)
+        self._write(command)
+
+    def get_coords(self):
+        '''Get all coords.
+
+        Return:
+            data_list (list): [x, y, z, rx, ry, rz] (mm)
+
+        '''
+        command = 'fe fe 02 23 fa'
+        self._write(command)
+        if self.serial_port.inWaiting() > 0:
+            data = self._read()
+            data_list = self._parse_data(data, 'get_coords')
+            return data_list
+        else:
+            return []
+
+    def send_coord(self):
+        ...
+
+    def send_coords(self, coords, speed, mode):
+        if len(coords) != 6:
+            print('The lenght of coords is not right')
+            return
+        command = 'fefe1025 '
+        speed = hex(speed)[2:]
+        speed = self._complement_zero(speed, digit=2)
+        mode = self._complement_zero(hex(mode)[2:], digit=2)
+        for coord in coords:
+            _hex = self._coord_to_hex(coord)
+
+            command += (_hex + ' ')
+
+        command += '{}{}fa'.format(speed, mode)
+        # print(command)
+        self._write(command)
+
+    def is_servo_enable(self):
+        ...
+
+    def is_all_servo_enable(self):
+        ...
+
+    def set_color(self, rgb):
+        '''Set the light color
+
+        Args:
+            rgs (str): example 'ff0000'
+
+        '''
+        command = 'fe fe 05 6a {} fa'.format(rgb)
+        # print(command)
+        self._write(command)
+
+    def is_moving(self):
+        command = 'fe fe 02 2b fa'
+        self._write(command)
+        data = self._read(2)
+        # print(data)
+        if not data:
+            return True
+        flag = int(data.hex(), 16)
+        if flag:
+            return True
+        else:
+            return False
+
+    def pause(self):
+        self._write('fe fe 02 26 fa')
+    
+    def resume(self):
+        self._write('fe fe 02 28 fa')
+
+    def stop(self):
+        self._write('fe fe 02 29 fa')
+
+    def is_paused(self):
+        self._write('fe fe 02 27 fa')
+        data = self._read()
+        flag = int(data.hex(), 16)
+        return False if flag else True
+    
+    def is_in_position(self, coords):
+        if len(coords) != 6:
+            print('The lenght of coords is not right')
+            return
+        command = 'fe fe 0d 2a '
+        for coord in coords:
+            _hex = self._coord_to_hex(coord)
+
+            command += (_hex + ' ')
+
+        command += 'fa'
+        print(command)
+        self._write(command)
+        data = self._read()
+        flag = int(data.hex(), 16)
+        return False if flag else True
+
+    def get_speed(self):
+        self._write('fe fe 02 40 fa')
+        data = self._read()
+        if data:
+            return int(data.hex(), 16)
+
+    def set_speed(self, speed):
+        '''Set speed value
+
+        Args:
+            speed (int): 0 - 100
+        '''
+        speed = int(speed)
+        if not 0 <= speed <= 100:
+            raise Exception('speed value out of range (0 ~ 100)')
+        _hex = str(hex(speed))[2:]
+        self._write('fe fe 03 41 {} fa'.format(_hex))
+
+    def _parse_data(self, data, name):
+        data_list = []
+        data = data.hex()
+        # print(data)
+        if name == 'get_angles':
+            data = data[-26:-2]
+            for i in range(6):
+                _hex = data[i * 4: (i * 4) + 4]
+                degree = self._hex_to_degree(_hex)
+                data_list.append(degree)
+
+        elif name == 'get_coords':
+            data = data[-26:-2]
+            for i in range(6):
+                _hex = data[i * 4: (i * 4) + 4]
+                _coord = self._hex_to_int(_hex) / 10
+                data_list.append(_coord)
+
+        elif name == 'get_angles_of_radian':
+            data = data[-26:-2]
+            for i in range(6):
+                _hex = data[i * 4: (i * 4) + 4]
+                _radian = self._hex_to_int(_hex) / 1000
+                data_list.append(_radian)
+
+        return (data_list)
+
+    def _hex_to_degree(self, _hex: str):
+        _int = self._hex_to_int(_hex)
+        return  _int * 18 / 314
+    
+    def _hex_to_int(self, _hex: str):
+        _int = int(_hex, 16)
+        if _int > 0x8000:
+            _int -= 0x10000
+        return _int
+
+    def _angle_to_hex(self, _degree: float, is_degree=True):
+        if is_degree:
+            radian = (_degree * (3140 / 180))
+        else:
+            radian = _degree * 1000
+        radian = int(radian)
+        if radian < 0:
+            radian += 0x10000
+        radian = round(radian)
+        s = str(hex(radian))[2:] 
+        s = self._complement_zero(s)
+        return  s
+
+    def _coord_to_hex(self, coord):
+        coord *= 10
+        coord = int(coord)
+        if coord < 0:
+            coord += 0x10000
+        s = str(hex(coord))[2:]
+        s = self._complement_zero(s)
+        return s
+    
+    def _complement_zero(self, s, digit=4):
+        s_len = len(s)
+        if s_len == digit:
+            return s
+        need_len = digit - s_len
+        s = ''.join(['0' for _ in range(need_len)] + [s])
+        return  s
+
+    def _write(self, data: str):
+        # print(data)
+        data = bytes.fromhex(data)
+        self.serial_port.write(data)
+        time.sleep(0.05)
+
+    def _read(self, size: int=1024):
+        data = self.serial_port.read(size)
+        return data

urdf/mycobot_urdf.urdf

@@ -0,0 +1,158 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_ros/urdf/joint1.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+	<material name = "white">
+		<color rgba = "0.1 0.1 0.9 1"/>
+	</material>
+    </visual>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_ros/urdf/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  <material name = "white">
+    <color rgba = "0.4 0.4 0.4 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_ros/urdf/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  <material name = "white">
+    <color rgba = "0.4 0.4 0.4 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_ros/urdf/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  <material name = "white">
+    <color rgba = "0.4 0.4 0.4 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_ros/urdf/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  <material name = "white">
+    <color rgba = "0.4 0.4 0.4 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_ros/urdf/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  <material name = "white">
+    <color rgba = "0.4 0.4 0.4 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+
+        <cylinder length="0.005" radius="0.02"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 1.57075 0 0"/>
+  <material name = "white">
+    <color rgba = "0.9 0.9 0.9 1"/>
+  </material>
+    </visual>
+  </link>
+
+
+
+    <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.0706" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="1 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0.03256 0  0.05976" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz="1 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= " 0.00272 0 0.1104   " rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz="1 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.00164 0 0.096" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0.0288 0 0.02976" rpy = "0 0 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 1  0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0.03792 " rpy = "0 0 0"/>  
+  </joint>
+
+
+
+</robot>
+