Merge pull request #119 from elephantrobotics/320-aikit

320 aikit-ros
2026-07-05 19:47:04 +00:00 · 2023-06-14 18:30:13 +08:00 · 2023-06-14 18:30:13 +08:00 · 823c77808c
commit 823c77808c
parent eed6f6fe7e 10b2cfff0d
57 changed files with 26026 additions and 19 deletions
--- a/mycobot_320/mycobot_320_communication/launch/communication_topic_pi.launch
+++ b/mycobot_320/mycobot_320_communication/launch/communication_topic_pi.launch
@ -1,10 +1,10 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="port" default="/dev/ttyAMA0" />
 	<arg name="baud" default="115200" />

 	<!-- Open communication service --><!-- 开启通讯服务 -->
-	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">
+	<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_topics_pi.py" output="screen">
 		<param name="port" type="string" value="$(arg port)" />
 		<param name="baud" type="int" value="$(arg baud)" />
    	</node>
--- a/mycobot_320/mycobot_320_communication/scripts/mycobot_services.py
+++ b/mycobot_320/mycobot_320_communication/scripts/mycobot_services.py
@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 import time
 import rospy
--- a/mycobot_320/mycobot_320_communication/scripts/mycobot_topics.py
+++ b/mycobot_320/mycobot_320_communication/scripts/mycobot_topics.py
@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 import time
 import os
--- a/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_jsnn.py
+++ b/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_jsnn.py
--- a/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_pi.py
+++ b/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_pi.py
@ -1,10 +1,11 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 import time
 import os
 import sys
 import signal
 import threading
+import fcntl

 import rospy

@ -19,9 +20,45 @@ from mycobot_communication.msg import (



-from pymycobot import MyCobotSocket
+from pymycobot import MyCobot


+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+    
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+        print('pid waiting for lock:%d'% pid)
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
 class Watcher:
    """this class solves two problems with multithreaded
    programs in Python, (1) a signal might be delivered
@ -74,11 +111,10 @@ class MycobotTopics(object):
        rospy.init_node("mycobot_topics_pi")
        rospy.loginfo("start ...")
        # problem
-        port = rospy.get_param("~port", "/dev/ttyUSB0")
+        port = rospy.get_param("~port", "/dev/ttyAMA0")
        baud = rospy.get_param("~baud", 115200)
        rospy.loginfo("%s,%s" % (port, baud))
-        self.mc = MyCobotSocket(port, baud) # port
-        self.mc.connect()   #pi
+        self.mc = MyCobot(port, baud) # port
        self.lock = threading.Lock()

    def start(self):
@ -115,9 +151,12 @@ class MycobotTopics(object):
        pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
        ma = MycobotAngles()
        while not rospy.is_shutdown():
-            self.lock.acquire()
-            angles = self.mc.get_angles()
-            self.lock.release()
+            if self.mc:
+                # self.lock.acquire()
+                lock = acquire("/tmp/mycobot_lock")
+                angles = self.mc.get_angles()
+                release(lock)
+                # self.lock.release()
            if angles:
                ma.joint_1 = angles[0]
                ma.joint_2 = angles[1]
@ -135,9 +174,12 @@ class MycobotTopics(object):
        ma = MycobotCoords()

        while not rospy.is_shutdown():
-            self.lock.acquire()
-            coords = self.mc.get_coords()
-            self.lock.release()
+            # self.lock.acquire()
+            if self.mc:
+                lock = acquire("/tmp/mycobot_lock")
+                coords = self.mc.get_coords()
+                # self.lock.release()
+                release(lock)
            if coords:
                ma.x = coords[0]
                ma.y = coords[1]
@ -161,7 +203,10 @@ class MycobotTopics(object):
                data.joint_6,
            ]
            sp = int(data.speed)
-            self.mc.send_angles(angles, sp)
+            if self.mc:
+                lock = acquire("/tmp/mycobot_lock")
+                self.mc.send_angles(angles, sp)
+                release(lock)

        sub = rospy.Subscriber(
            "mycobot/angles_goal", MycobotSetAngles, callback=callback
@ -173,7 +218,10 @@ class MycobotTopics(object):
            angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
            sp = int(data.speed)
            model = int(data.model)
-            self.mc.send_coords(angles, sp, model)
+            if self.mc:
+                lock = acquire("/tmp/mycobot_lock")
+                self.mc.send_coords(angles, sp, model)
+                release(lock)

        sub = rospy.Subscriber(
            "mycobot/coords_goal", MycobotSetCoords, callback=callback
@ -185,9 +233,15 @@ class MycobotTopics(object):
        """订阅夹爪状态"""
        def callback(data):
            if data.Status:
-                self.mc.set_gripper_state(0, 80)
+                if self.mc:
+                    lock = acquire("/tmp/mycobot_lock")
+                    self.mc.set_gripper_state(0, 80)
+                    release(lock)
            else:
-                self.mc.set_gripper_state(1, 80)
+                if self.mc:
+                    lock = acquire("/tmp/mycobot_lock")
+                    self.mc.set_gripper_state(1, 80)
+                    release(lock)

        sub = rospy.Subscriber(
            "mycobot/gripper_status", MycobotGripperStatus, callback=callback
--- a/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_seeed.py
+++ b/mycobot_320/mycobot_320_communication/scripts/mycobot_topics_seeed.py
--- a/mycobot_ai/aikit_320_pi/CMakeLists.txt
+++ b/mycobot_ai/aikit_320_pi/CMakeLists.txt
@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(aikit_320_pi)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  new_mycobot_320
+  new_mycobot_320_pi
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES mycobot_ai
+#  CATKIN_DEPENDS mycobot_280
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/mycobot_ai.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/mycobot_ai_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mycobot_ai.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/mycobot_ai/aikit_320_pi/config/ai_mycobot_pi.rviz
+++ b/mycobot_ai/aikit_320_pi/config/ai_mycobot_pi.rviz
@ -0,0 +1,226 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /Marker1
+      Splitter Ratio: 0.5
+    Tree Height: 579
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        env:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6_flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        env:
+          Value: true
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+        joint6:
+          Value: true
+        joint6_flange:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        joint1:
+          env:
+            {}
+          joint2:
+            joint3:
+              joint4:
+                joint5:
+                  joint6:
+                    joint6_flange:
+                      {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: cube
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: joint1
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.0433008670806885
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.07064759731292725
+        Y: -0.0814988762140274
+        Z: 0.10758385062217712
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3703983426094055
+      Target Frame: <Fixed Frame>
+      Yaw: 0.555390477180481
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 876
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ce000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c2000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000400000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
--- a/mycobot_ai/aikit_320_pi/config/aikit_320_pi.rviz
+++ b/mycobot_ai/aikit_320_pi/config/aikit_320_pi.rviz
@ -0,0 +1,229 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /TF1/Tree1
+        - /Marker1
+      Splitter Ratio: 0.5
+    Tree Height: 603
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        env:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        env:
+          Value: true
+        link1:
+          Value: true
+        link2:
+          Value: true
+        link3:
+          Value: true
+        link4:
+          Value: true
+        link5:
+          Value: true
+        link6:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.20000000298023224
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base:
+          env:
+            {}
+          link1:
+            link2:
+              link3:
+                link4:
+                  link5:
+                    link6:
+                      {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: cube
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.4485745429992676
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.834796667098999
+      Target Frame: <Fixed Frame>
+      Yaw: 0.6185896992683411
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 900
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b3000002e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
--- a/mycobot_ai/aikit_320_pi/launch/test.launch
+++ b/mycobot_ai/aikit_320_pi/launch/test.launch
@ -0,0 +1,16 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/new_320_pi/new_mycobot_vision_v2.urdf"/>
+	<arg name="rvizconfig" default="$(find aikit_320_pi)/config/aikit_320_pi.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
--- a/mycobot_ai/aikit_320_pi/launch/vision_pi.launch
+++ b/mycobot_ai/aikit_320_pi/launch/vision_pi.launch
@ -0,0 +1,28 @@
+<launch>
+<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="115200" />
+
+	<!-- <arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/> -->
+	<arg name="model" default="$(find mycobot_description)/urdf/new_320_pi/new_mycobot_vision_v2.urdf"/>
+	<arg name="rvizconfig" default="$(find aikit_320_pi)/config/aikit_320_pi.rviz" />
+	<!-- <arg name="gui" default="false" /> -->
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+
+	<!-- mycobot-topics -->
+    <include file="$(find mycobot_320_communication)/launch/communication_topic_pi.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+
+    <!-- listen and pub the real angles -->
+	<node name="real_listener" pkg="new_mycobot_320_pi" type="mycobot_320_listen_real_of_topic.py" />
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
--- a/mycobot_ai/aikit_320_pi/local_photo/goal5.jpeg
+++ b/mycobot_ai/aikit_320_pi/local_photo/goal5.jpeg
--- a/mycobot_ai/aikit_320_pi/local_photo/img/goal01.jpeg
+++ b/mycobot_ai/aikit_320_pi/local_photo/img/goal01.jpeg
--- a/mycobot_ai/aikit_320_pi/local_photo/img/goal03.jpeg
+++ b/mycobot_ai/aikit_320_pi/local_photo/img/goal03.jpeg
--- a/mycobot_ai/aikit_320_pi/local_photo/img/goal2.jpeg
+++ b/mycobot_ai/aikit_320_pi/local_photo/img/goal2.jpeg
--- a/mycobot_ai/aikit_320_pi/local_photo/takephoto.jpeg
+++ b/mycobot_ai/aikit_320_pi/local_photo/takephoto.jpeg
--- a/mycobot_ai/aikit_320_pi/package.xml
+++ b/mycobot_ai/aikit_320_pi/package.xml
@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>aikit_320_pi</name>
+  <version>0.0.0</version>
+  <description>The aikit_320_pi package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="wang@todo.todo">wang</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mycobot_ai</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>new_mycobot_320</build_depend>
+  <build_export_depend>new_mycobot_320</build_export_depend>
+  <exec_depend>new_mycobot_320</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/mycobot_ai/aikit_320_pi/prof.calltree
+++ b/mycobot_ai/aikit_320_pi/prof.calltree
--- a/mycobot_ai/aikit_320_pi/prof.out
+++ b/mycobot_ai/aikit_320_pi/prof.out
--- a/mycobot_ai/aikit_320_pi/res/A/goal1.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal1.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal2.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal2.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal3.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal3.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal4.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal4.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal5.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal5.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal6.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal6.jpeg
--- a/mycobot_ai/aikit_320_pi/res/A/goal7.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/A/goal7.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal13.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal13.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal14.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal14.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal15.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal15.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal4.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal4.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal5.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal5.jpeg
--- a/mycobot_ai/aikit_320_pi/res/B/goal6.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/B/goal6.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal1.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal1.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal2.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal2.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal3.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal3.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal4.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal4.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal5.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal5.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal6.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal6.jpeg
--- a/mycobot_ai/aikit_320_pi/res/C/goal7.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/C/goal7.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal1.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal1.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal2.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal2.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal3.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal3.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal4.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal4.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal5.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal5.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal6.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal6.jpeg
--- a/mycobot_ai/aikit_320_pi/res/D/goal7.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/D/goal7.jpeg
--- a/mycobot_ai/aikit_320_pi/res/takephoto.jpeg
+++ b/mycobot_ai/aikit_320_pi/res/takephoto.jpeg
--- a/mycobot_ai/aikit_320_pi/scripts/OpenVideo.py
+++ b/mycobot_ai/aikit_320_pi/scripts/OpenVideo.py
@ -0,0 +1,13 @@
+import cv2
+
+
+cap  = cv2.VideoCapture(0)
+cap.set(3,640)
+cap.set(4,480)
+
+while cv2.waitKey(1)<0:
+    ret, frame = cap.read()
+    if not ret:
+        break
+    cv2.imshow('', frame)
+    
--- a/mycobot_ai/aikit_320_pi/scripts/add_img.py
+++ b/mycobot_ai/aikit_320_pi/scripts/add_img.py
@ -0,0 +1,127 @@
+# coding:utf-8
+from fileinput import filename
+import os, cv2, sys
+
+
+def take_photo():
+    # 提醒用户操作字典
+    print("************************************************")
+    print("*  热键(请在摄像头的窗口使用):                   *")
+    print("*  hotkey(please use it in the camera window): *")
+    print("*  z: 拍摄图片(take picture)                    *")
+    print("*  q: 退出(quit)                               *")
+    print("************************************************")
+
+    # 创建/使用local_photo文件夹
+    class_name = "res"
+    if (os.path.exists("res")):
+        pass
+    else:
+        os.mkdir(class_name)
+
+    # 设置特定值
+
+    index = 'takephoto'
+    cap = cv2.VideoCapture(0)
+    cap.set(3,640)
+    cap.set(4, 480)
+
+    while True:
+        # 读入每一帧
+        ret, frame = cap.read()
+
+        cv2.imshow("capture", frame)
+
+        # 存储
+        input = cv2.waitKey(1) & 0xFF
+        # 拍照
+        if input == ord('z'):
+            cv2.imwrite(
+                "%s/%s.jpeg" % (class_name, index),
+                cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA))
+            break
+
+        # 退出
+        if input == ord('q'):
+
+            # 关闭窗口
+            cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
+
+
+def cut_photo():
+    
+    path = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_320_pi'    # pi
+
+    path_red = path + '/res/A'
+    for i, j, k in os.walk(path_red):
+        file_len_red = len(k)
+
+    path_gray = path + '/res/B'
+    for i, j, k in os.walk(path_gray):
+        file_len_gray = len(k)
+
+    path_green = path + '/res/C'
+    for i, j, k in os.walk(path_green):
+        file_len_green = len(k)
+
+    path_blue = path + '/res/D'
+    for i, j, k in os.walk(path_blue):
+        file_len_blue = len(k)
+    print("请截取要识别的部分")
+    print("Please intercept the part to be identified")
+
+    cut = cv2.imread(r"res/takephoto.jpeg")
+
+    cv2.imshow('original', cut)
+
+    # 选择ROI
+    roi = cv2.selectROI(windowName="original",
+                        img=cut,
+                        showCrosshair=False,
+                        fromCenter=False)
+    x, y, w, h = roi
+    print(roi)
+
+    msg = """\
+    Image save location:
+        1 - 保存至A分拣区文件夹 Save to A folder 
+        2 - 保存至B分拣区文件夹 Save to B folder 
+        3 - 保存至C分拣区文件夹 Save to C folder 
+        4 - 保存至D分拣区文件夹 Save to D folder
+        """
+    print(msg)
+    kw = int(input("请输入保存图片文件夹数字编号(Please enter the number of the folder to save the picture):"))
+    # print(kw)
+
+    # 显示ROI并保存图片
+    if roi != (0, 0, 0, 0):
+        
+        crop = cut[y:y + h, x:x + w]
+        # cv2.imshow('crop', crop)
+        # 选择D区文件夹
+        if kw == 1:
+            cv2.imwrite(path + '/res/A/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
+            print('Saved')
+        # 选择B区文件夹
+        elif kw == 2:
+            cv2.imwrite(path + '/res/B/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
+            print('Saved')
+        # 选择C区文件夹
+        elif kw == 3:
+            cv2.imwrite(path + '/res/C/goal{}.jpeg'.format(str(file_len_green+1)),crop)
+            print('Saved')
+        # 选择A区文件夹
+        elif kw == 4:
+            cv2.imwrite(path + '/res/D/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
+            print('Saved')
+
+    # 退出
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    take_photo()
+    cut_photo()
--- a/mycobot_ai/aikit_320_pi/scripts/aikit_color.py
+++ b/mycobot_ai/aikit_320_pi/scripts/aikit_color.py
@ -0,0 +1,490 @@
+#!/usr/bin/env python3
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+from pymycobot.mycobot import MyCobot
+
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"
+# Adaptive seeed
+
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x = 256, camera_y = 0):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # declare mycobot320
+        self.mc = None
+
+        # 移动角度
+        self.move_angles = [
+            [0.61, 45.87, -92.37, -41.3, 89.56, 9.58],  # init the point
+            [18.8, -7.91, -54.49, -23.02, -0.79, -14.76],  # point to grab
+            [17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [28.9, -226, 246, -171.13, -3.94, -92.37],  # D Sorting area
+            [253.3, -216.1, 257, -163.12, -6.12, -95.27],  # C Sorting area
+            [241.8, 219.5, 270.6, -168.47, 10.42, -76.84],  # A Sorting area
+            [37.8, 233, 251.4, -170.6, -6.75, 88.53],  # B Sorting area
+        ]
+        
+        # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        self.raspi = False
+    
+        # choose place to set cube 选择放置立方体的地方
+        self.color = 0
+        # parameters to calculate camera clipping parameters 计算相机裁剪参数的参数
+        self.x1 = self.x2 = self.y1 = self.y2 = 0
+        # set cache of real coord 设置真实坐标的缓存
+        self.cache_x = self.cache_y = 0
+        # set color HSV
+        self.HSV = {
+            "yellow": [np.array([11, 85, 70]), np.array([59, 255, 245])],
+            # "yellow": [np.array([22, 93, 0]), np.array([45, 255, 245])],
+            "red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
+            "green": [np.array([35, 43, 35]), np.array([90, 255, 255])],
+            "blue": [np.array([78, 43, 46]), np.array([110, 255, 255])],
+            "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
+        }
+        
+        # use to calculate coord between cube and mycobot320
+        # 用于计算立方体和 mycobot 之间的坐标
+        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+        # The coordinates of the grab center point relative to the mycobot320
+        # 抓取中心点相对于 mycobot 的坐标
+        self.camera_x, self.camera_y = camera_x, camera_y
+        # The coordinates of the cube relative to the mycobot320
+        # 立方体相对于 mycobot 的坐标
+        self.c_x, self.c_y = 0, 0
+        # The ratio of pixels to actual values
+        # 像素与实际值的比值
+        self.ratio = 0
+        
+        # Get ArUco marker dict that can be detected.
+        # 获取可以检测到的 ArUco 标记字典。
+        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        # Get ArUco marker params. 获取 ArUco 标记参数
+        self.aruco_params = cv2.aruco.DetectorParameters_create()
+
+        # init a node and a publisher
+        rospy.init_node("marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+        # init a Marker
+        self.marker = Marker()
+        self.marker.header.frame_id = "base"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+        
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+        
+    # publish marker
+    def pub_marker(self, x, y, z=0.03):
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = x 
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        self.marker.color.g = self.color
+        self.pub.publish(self.marker)
+        
+    # pump_control pi
+    def gpio_status(self, flag):
+        if flag:
+            """start the suction pump"""
+            self.mc.set_basic_output(1, 0)
+            self.mc.set_basic_output(2, 1)
+        else:
+            """stop suction pump"""
+            self.mc.set_basic_output(1, 1)
+            self.mc.set_basic_output(2, 0)
+            time.sleep(1)
+            self.mc.set_basic_output(2, 1)
+    
+    # 开启吸泵 m5
+    def pump_on(self):
+        """Start the suction pump"""
+        self.mc.set_basic_output(1, 0)
+        self.mc.set_basic_output(2, 1)
+
+    def pump_off(self):
+        """stop suction pump m5"""
+        self.mc.set_basic_output(1, 1)
+        self.mc.set_basic_output(2, 0)
+        time.sleep(1)
+        self.mc.set_basic_output(2, 1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        # send Angle to move mycobot320
+        print(color)
+        print('x,y:', round(x, 2), round(y, 2))
+        self.mc.send_angles(self.move_angles[2], 50)
+        time.sleep(3)
+
+        # send coordinates to move mycobot
+        self.mc.send_coords([x, y, 250, -173.84, -0.14, -74.37], 100, 1)
+        time.sleep(2.5)
+
+        self.mc.send_coords([x, y, 150, -173.84, -0.14, -74.37], 100, 1)
+        time.sleep(3)
+
+        # open pump
+        self.pump_on()
+        time.sleep(1.5)
+
+        tmp = []
+        while True:
+            if not tmp: 
+                tmp = self.mc.get_angles()    
+            else:
+                break
+        time.sleep(0.5)
+        
+        # print(tmp)
+        self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+        time.sleep(3)
+        
+        self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
+        
+        self.mc.send_coords(self.move_coords[color], 100, 1)
+        
+        self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0)
+        time.sleep(3)
+       
+        # close pump
+        self.pump_off()
+        time.sleep(6.5)
+        
+        if color == 1:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0-0.02
+            )
+        elif color == 0:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0
+            )
+
+        self.mc.send_angles(self.move_angles[0], 25)
+        time.sleep(4.5)
+
+    # decide whether grab cube 决定是否抓取立方体
+    def decide_move(self, x, y, color):
+        print(x, y, self.cache_x, self.cache_y)
+        # detect the cube status move or run 检测立方体状态移动或运行
+        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
+            self.cache_x, self.cache_y = x, y
+            return
+        else:
+            self.cache_x = self.cache_y = 0
+            # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+            self.move(x, y, color)
+
+    # init mycobot320
+    def run(self):
+        if "dev" in self.robot_raspi:
+            self.mc = MyCobot(self.robot_raspi, 115200)
+        self.pump_off()
+        self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 89.56, 9.58], 20)
+        time.sleep(2.5)
+
+    # draw aruco
+    def draw_marker(self, img, x, y):
+        # draw rectangle on img 在 img 上绘制矩形
+        cv2.rectangle( 
+            img,
+            (x - 20, y - 20),
+            (x + 20, y + 20),
+            (0, 255, 0),
+            thickness=2,
+            lineType=cv2.FONT_HERSHEY_COMPLEX,
+        )
+        # add text on rectangle
+        cv2.putText(img, "({},{})".format(x, y), (x, y),
+                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
+
+    # get points of two aruco 获得两个 aruco 的点位
+    def get_calculate_params(self, img):
+        # Convert the image to a gray image 将图像转换为灰度图像
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        # Detect ArUco marker.
+        corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+            gray, self.aruco_dict, parameters=self.aruco_params
+        )
+
+        """
+        Two Arucos must be present in the picture and in the same order.
+        There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+        Determine the center of the aruco by the four corners of the aruco.
+        """
+        if len(corners) > 0:
+            if ids is not None:
+                if len(corners) <= 1 or ids[0] == 1:
+                    return None
+                x1 = x2 = y1 = y2 = 0
+                point_11, point_21, point_31, point_41 = corners[0][0]
+                x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
+                    (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
+                point_1, point_2, point_3, point_4 = corners[1][0]
+                x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
+                    (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
+                
+                return x1, x2, y1, y2
+        return None
+
+    # set camera clipping parameters 设置相机裁剪参数 
+    def set_cut_params(self, x1, y1, x2, y2):
+        self.x1 = int(x1)
+        self.y1 = int(y1)
+        self.x2 = int(x2)
+        self.y2 = int(y2)
+
+    # set parameters to calculate the coords between cube and mycobot320
+    # 设置参数以计算立方体和 mycobot 之间的坐标
+    def set_params(self, c_x, c_y, ratio):
+        self.c_x = c_x
+        self.c_y = c_y
+        self.ratio = 220.0/ratio
+
+    # calculate the coords between cube and mycobot320
+    # 计算立方体和 mycobot 之间的坐标
+    def get_position(self, x, y):
+        return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
+
+    """
+    Calibrate the camera according to the calibration parameters.
+    Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+    If two ARuco values have been calculated, clip the video.
+    """
+    def transform_frame(self, frame):
+        # enlarge the image by 1.5 times
+        fx = 1.5
+        fy = 1.5
+        frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
+                           interpolation=cv2.INTER_CUBIC)
+        if self.x1 != self.x2:
+            # the cutting ratio here is adjusted according to the actual situation
+            frame = frame[int(self.y2*0.78):int(self.y1*1.1),
+                          int(self.x1*0.86):int(self.x2*1.08)]
+        return frame
+
+    # detect cube color
+    def color_detect(self, img):
+        # set the arrangement of color'HSV
+        x = y = 0
+        for mycolor, item in self.HSV.items():
+            # print("mycolor:",mycolor)
+            redLower = np.array(item[0])
+            redUpper = np.array(item[1])
+
+            # transfrom the img to model of gray 将图像转换为灰度模型
+            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+            # print("hsv",hsv)
+
+            # wipe off all color expect color in range 擦掉所有颜色期望范围内的颜色
+            mask = cv2.inRange(hsv, item[0], item[1])
+
+            # a etching operation on a picture to remove edge roughness
+            # 对图片进行蚀刻操作以去除边缘粗糙度
+            erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
+
+            # the image for expansion operation, its role is to deepen the color depth in the picture
+            # 用于扩展操作的图像，其作用是加深图片中的颜色深度
+            dilation = cv2.dilate(erosion, np.ones(
+                (1, 1), np.uint8), iterations=2)
+
+            # adds pixels to the image 向图像添加像素
+            target = cv2.bitwise_and(img, img, mask=dilation)
+
+            # the filtered image is transformed into a binary image and placed in binary
+            # 将过滤后的图像转换为二值图像并放入二值
+            ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
+
+            # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
+            # 获取图像的轮廓坐标，其中contours为坐标值，这里只检测轮廓
+            contours, hierarchy = cv2.findContours(
+                dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+            if len(contours) > 0:
+                # do something about misidentification
+                boxes = [
+                    box
+                    for box in [cv2.boundingRect(c) for c in contours]
+                    if min(img.shape[0], img.shape[1]) / 10
+                    < min(box[2], box[3])
+                    < min(img.shape[0], img.shape[1]) / 1
+                ]
+                if boxes:
+                    for box in boxes:
+                        x, y, w, h = box
+                    # find the largest object that fits the requirements 找到符合要求的最大对象
+                    c = max(contours, key=cv2.contourArea)
+                    # get the lower left and upper right points of the positioning object
+                    # 获取定位对象的左下和右上点
+                    x, y, w, h = cv2.boundingRect(c)
+                    # locate the target by drawing rectangle 通过绘制矩形来定位目标
+                    cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
+                    # calculate the rectangle center 计算矩形中心
+                    x, y = (x*2+w)/2, (y*2+h)/2
+                    # calculate the real coordinates of mycobot320 relative to the target
+                    #  计算 mycobot 相对于目标的真实坐标
+                    
+                    if mycolor  == "yellow":
+                        
+                        self.color = 3
+                        break
+
+                    elif mycolor == "red":
+                        self.color = 0
+                        break
+
+                    elif mycolor == "cyan":
+                        self.color = 2
+                        break
+
+                    elif mycolor == "blue":
+                        self.color =2
+                        break
+                    elif mycolor == "green":
+                        self.color = 1
+                        break
+        
+        # 判断是否正常识别
+        if abs(x) + abs(y) > 0:
+            return x, y
+        else:
+            return None
+
+if __name__ == "__main__":
+
+    # open the camera
+    cap_num = 0
+    cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+ 
+    if not cap.isOpened():
+        cap.open()
+    # init a class of Object_detect
+    detect = Object_detect()
+    # init mycobot320
+    detect.run()
+
+    _init_ = 20  
+    init_num = 0
+    nparams = 0
+    num = 0
+    real_sx = real_sy = 0
+    while cv2.waitKey(1) < 0:
+       # read camera
+        _, frame = cap.read()
+        # deal img
+        frame = detect.transform_frame(frame)
+        if _init_ > 0:
+            _init_ -= 1
+            continue
+
+        # calculate the parameters of camera clipping 计算相机裁剪的参数
+        if init_num < 20:
+            if detect.get_calculate_params(frame) is None:
+                cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                init_num += 1
+                continue
+        elif init_num == 20:
+            detect.set_cut_params(
+                (detect.sum_x1)/20.0,
+                (detect.sum_y1)/20.0,
+                (detect.sum_x2)/20.0,
+                (detect.sum_y2)/20.0,
+            )
+            detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
+            init_num += 1
+            continue
+
+        # calculate params of the coords between cube and mycobot320 计算立方体和 mycobot 之间坐标的参数
+        if nparams < 10:
+            if detect.get_calculate_params(frame) is None:
+                cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                nparams += 1
+                continue
+        elif nparams == 10:
+            nparams += 1
+            # calculate and set params of calculating real coord between cube and mycobot320
+            # 计算和设置计算立方体和mycobot之间真实坐标的参数
+            detect.set_params(
+                (detect.sum_x1+detect.sum_x2)/20.0,
+                (detect.sum_y1+detect.sum_y2)/20.0,
+                abs(detect.sum_x1-detect.sum_x2)/10.0 +
+                abs(detect.sum_y1-detect.sum_y2)/10.0
+            )
+            print("ok")
+            continue
+
+        # get detect result 获取检测结果
+        detect_result = detect.color_detect(frame)
+        if detect_result is None:
+            cv2.imshow("figure", frame)
+            continue
+        else:
+            x, y = detect_result
+            # calculate real coord between cube and mycobot320 计算立方体和 mycobot 之间的真实坐标
+            real_x, real_y = detect.get_position(x, y)
+            # print('real_x',round(real_x, 3),round(real_y, 3))
+            if num == 20:
+                detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
+                detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
+                num = real_sx = real_sy = 0
+
+            else:
+                num += 1
+                real_sy += real_y
+                real_sx += real_x
+
+        cv2.imshow("figure", frame)
+
+        # close the window
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
--- a/mycobot_ai/aikit_320_pi/scripts/aikit_encode.py
+++ b/mycobot_ai/aikit_320_pi/scripts/aikit_encode.py
@ -0,0 +1,243 @@
+#encoding: UTF-8
+import cv2
+import numpy as np
+from pymycobot.mycobot import MyCobot
+import time
+import os
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+
+# y轴偏移量
+pump_y = -55
+# x轴偏移量
+pump_x = 15
+
+class Detect_marker(Movement):
+    def __init__(self):
+        # set cache of real coord
+        self.cache_x = self.cache_y = 0
+        
+        # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        
+        # Creating a Camera Object
+        cap_num = 0
+        self.cap = cv2.VideoCapture(cap_num)
+        self.cap.set(3, 640)
+        self.cap.set(4, 480)
+        
+        # Determine the placement point of the QR code
+        self.color = 0
+        
+        # Get ArUco marker dict that can be detected.
+        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        # Get ArUco marker params.
+        self.aruco_params = cv2.aruco.DetectorParameters_create()
+        # 摄像头的内参矩阵
+        self.camera_matrix = np.array([
+            [781.33379113, 0., 347.53500524],
+            [0., 783.79074192, 246.67627253],
+            [0., 0., 1.]])
+
+        # 摄像头的畸变系数
+        self.dist_coeffs = np.array(([[3.41360787e-01, -2.52114260e+00, -1.28012469e-03,  6.70503562e-03,
+             2.57018000e+00]]))
+    
+        # init a node and a publisher
+        rospy.init_node("encode_marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+
+        self.marker = Marker()
+        self.marker.header.frame_id = "base"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1
+        self.marker.color.r = 0.3
+        self.marker.color.g = 0.3
+        self.marker.color.b = 0.3
+
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+        
+        
+    
+    # 控制吸泵      
+    def pub_pump(self, flag):
+        if flag:
+            """start the suction pump"""
+            self.mc.set_basic_output(1, 0)
+            self.mc.set_basic_output(2, 1)
+        else:
+            """stop suction pump"""
+            self.mc.set_basic_output(1, 1)
+            self.mc.set_basic_output(2, 0)
+            time.sleep(1)
+            self.mc.set_basic_output(2, 1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        
+        print(color)
+        
+        angles = [
+            [0.61, 45.87, -92.37, -41.3, 89.56, 9.58],  # init to point
+            [18.8, -7.91, -54.49, -23.02, 89.56, -14.76],
+            [17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
+        ]
+
+        coords = [
+            [145.0, -65.5, 280.1, 178.99, 7.67, -179.9],  # 初始化点 init point
+            [253.8, 236.8, 224.6, -170, 6.87, -77.91],  # A分拣区 A sorting area
+            [35.9, 235.4, 211.8, -169.33, -9.27, 88.3],  # B分拣区  B sorting area
+            [266.5, -219.7, 209.3, -170, -3.64, -94.62],  # C分拣区 C sorting area
+            [32, -228.3, 201.6, -168.07, -7.17, -92.56],  # D分拣区 D sorting area
+        ]
+        print('real_x, real_y:', round(coords[0][0] + x, 2), round(coords[0][1] + y, 2))
+        
+        # publish marker
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = (coords[0][0]-x)/1000.0
+        self.marker.pose.position.y = (coords[0][1]-y)/1000.0
+        self.pub.publish(self.marker)
+
+        # send coordinates to move mycobot
+        self.mc.send_angles(angles[2], 50)
+        time.sleep(3)
+        
+        self.mc.send_coords([coords[0][0] + x, coords[0][1] + y, 240, 178.99, -3.78, -62.9], 100, 1)
+        time.sleep(2)
+        self.mc.send_coords([coords[0][0] + x, coords[0][1] + y, 100.5, 178.99, -3.78, -62.9], 100, 1)
+        time.sleep(2.5)
+        
+        # open pump
+        if "dev" in self.robot_raspi:
+            self.pub_pump(True)
+        time.sleep(1.5)
+        
+        tmp = []
+        while True:
+            if not tmp: 
+                tmp = self.mc.get_angles()    
+            else:
+                break
+        time.sleep(0.5)
+        
+        # print(tmp)
+        self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+        time.sleep(3)
+        
+        self.mc.send_coords(coords[color], 100, 1) # coords[1] 为A分拣区，coords[2] 为B分拣区, coords[3] 为C分拣区，coords[4] 为D分拣区
+        time.sleep(6.5)
+        
+        # close pump
+        if "dev" in self.robot_raspi:
+            self.pub_pump(False)  
+        time.sleep(6.5)
+        
+        # publish marker
+        time.sleep(1)
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = coords[1][0]/1000.0
+        self.marker.pose.position.y = coords[1][1]/1000.0
+        self.pub.publish(self.marker)
+        
+        self.mc.send_angles(angles[0], 25)
+        time.sleep(4.5)
+
+    # decide whether grab cube
+    def decide_move(self, x, y, color):
+
+        print(x,y)
+        # detect the cube status move or run
+        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
+            self.cache_x, self.cache_y = x, y
+            return
+        else:
+            self.cache_x = self.cache_y = 0
+            # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+            self.move(x + 105, y + 130, color)
+
+    # init mycobot
+    def init_mycobot(self):
+        if "dev" in self.robot_raspi:
+            self.mc = MyCobot(self.robot_raspi, 115200)
+        self.pub_pump(False)
+        self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 89.56, 9.58], 20)
+        time.sleep(2.5)
+        
+
+
+    def run(self):
+        global pump_y, pump_x
+        self.init_mycobot()
+        print('ok')
+        num = sum_x = sum_y = 0 
+        while cv2.waitKey(1) < 0:
+            success, img = self.cap.read()
+            if not success:
+                print("It seems that the image cannot be acquired correctly.")
+                break
+
+            # transfrom the img to model of gray
+            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+            # Detect ArUco marker.
+            corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+                gray, self.aruco_dict, parameters=self.aruco_params
+            )
+
+            # Determine the placement point of the QR code
+            if ids == np.array([[1]]):
+                self.color = 1
+            elif ids == np.array([[2]]):
+                self.color = 2
+            elif ids == np.array([[3]]):
+                self.color = 3
+            elif ids == np.array([[4]]):
+                self.color = 4
+
+            if len(corners) > 0:
+                if ids is not None:
+                    # get informations of aruco
+                    ret = cv2.aruco.estimatePoseSingleMarkers(
+                        corners, 0.03, self.camera_matrix, self.dist_coeffs
+                    )
+                    # rvec:rotation offset,tvec:translation deviator
+                    (rvec, tvec) = (ret[0], ret[1])
+                    (rvec - tvec).any()
+                    xyz = tvec[0, 0, :]
+                    # calculate the coordinates of the aruco relative to the pump
+                    xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]*1000+pump_x, 2), round(xyz[2]*1000, 2)]
+
+                    # cv2.putText(img, str(xyz[:2]), (0, 64), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
+                    for i in range(rvec.shape[0]):
+			# draw the aruco on img
+                        cv2.aruco.drawDetectedMarkers(img, corners)
+                        
+                        if num < 40 :
+                            sum_x += xyz[1]
+                            sum_y += xyz[0]
+                            num += 1
+                        elif num ==40 :
+                            self.decide_move(sum_x/40.0, sum_y/40.0, self.color)
+                            num = sum_x = sum_y = 0
+
+            cv2.imshow("encode_image", img)
+
+if __name__ == "__main__":
+    detect = Detect_marker()
+    detect.run()
+
--- a/mycobot_ai/aikit_320_pi/scripts/aikit_img.py
+++ b/mycobot_ai/aikit_320_pi/scripts/aikit_img.py
@ -0,0 +1,601 @@
+from multiprocessing import Process, Pipe
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+
+from pymycobot.mycobot import MyCobot
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"  # Adaptive seeed
+
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x = 263, camera_y = 0):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+
+        # declare mycobot 280pi
+        self.mc = None
+        # 移动角度
+        self.move_angles = [
+            [0.61, 45.87, -92.37, -41.3, 89.56, 9.58],  # init the point
+            [18.8, -7.91, -54.49, -23.02, 89.56, -14.76],  # point to grab
+            [17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [32, -228.3, 201.6, -168.07, -7.17, -92.56],  # D Sorting area
+            [266.5, -219.7, 209.3, -170, -3.64, -94.62],  # C Sorting area
+            [253.8, 236.8, 224.6, -170, 6.87, -77.91],  # A Sorting area
+            [35.9, 235.4, 211.8, -169.33, -9.27, 88.3],  # B Sorting area
+        ]
+        
+        # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        
+        # choose place to set cube
+        self.color = 0
+        # parameters to calculate camera clipping parameters
+        self.x1 = self.x2 = self.y1 = self.y2 = 0
+        # set cache of real coord
+        self.cache_x = self.cache_y = 0
+
+        # use to calculate coord between cube and mycobot
+        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+        # The coordinates of the grab center point relative to the mycobot
+        self.camera_x, self.camera_y = camera_x, camera_y
+        # The coordinates of the cube relative to the mycobot
+        self.c_x, self.c_y = 0, 0
+        # The ratio of pixels to actual values
+        self.ratio = 0
+        # Get ArUco marker dict that can be detected.
+        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        # Get ArUco marker params.
+        self.aruco_params = cv2.aruco.DetectorParameters_create()
+        
+        # init a Marker
+        rospy.init_node("marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+        # init a Marker
+        self.marker = Marker()
+        self.marker.header.frame_id = "base"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+
+        self.cache_x = self.cache_y = 0
+
+    # publish marker
+    def pub_marker(self, x, y, z=0.03):
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = x
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        self.marker.color.g = self.color
+        self.pub.publish(self.marker)
+        
+    # pump_control pi
+    def gpio_status(self, flag):
+        if flag:
+            """start the suction pump"""
+            self.mc.set_basic_output(1, 0)
+            self.mc.set_basic_output(2, 1)
+        else:
+            """stop suction pump"""
+            self.mc.set_basic_output(1, 1)
+            self.mc.set_basic_output(2, 0)
+            time.sleep(1)
+            self.mc.set_basic_output(2, 1)
+
+    def pump_on(self):
+        """Start the suction pump"""
+        self.mc.set_basic_output(1, 0)
+        self.mc.set_basic_output(2, 1)
+
+    def pump_off(self):
+        """stop suction pump m5"""
+        self.mc.set_basic_output(1, 1)
+        self.mc.set_basic_output(2, 0)
+        time.sleep(1)
+        self.mc.set_basic_output(2, 1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        print(color)
+        print('x,y:', round(x, 2), round(y, 2))
+        # send Angle to move mycobot320
+        self.mc.send_angles(self.move_angles[2], 50)
+        time.sleep(3)
+
+        # send coordinates to move mycobot
+        self.mc.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
+        time.sleep(2.5)
+        self.mc.send_coords([x, y, 100, -173.84, -0.14, -74.37], 100, 1)  #
+        time.sleep(3)
+
+        # open pump
+        self.gpio_status(True)
+        time.sleep(1.5)
+
+        tmp = []
+        while True:
+            if not tmp: 
+                tmp = self.mc.get_angles()    
+            else:
+                break
+        time.sleep(0.5)
+
+         # print(tmp)
+        self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+        time.sleep(3)
+
+
+
+        self.mc.send_coords(self.move_coords[color], 100, 1)
+        self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0,
+                        self.move_coords[color][2]/1000.0)
+        time.sleep(6.5)
+
+        # close pump
+        self.gpio_status(False)
+        time.sleep(6.5)
+
+        self.mc.send_angles(self.move_angles[0], 50)
+        time.sleep(4.5)
+
+    # decide whether grab cube
+    def decide_move(self, x, y, color):
+        print(x, y, self.cache_x, self.cache_y)
+        # detect the cube status move or run
+        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
+            self.cache_x, self.cache_y = x, y
+            return
+        else:
+            self.cache_x = self.cache_y = 0
+            # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+       
+            self.move(x, y, color)
+      
+
+    # init mycobot
+    def run(self):
+     
+        if "dev" in self.robot_raspi:
+            self.mc = MyCobot(self.robot_raspi, 115200)
+        self.gpio_status(False)
+        self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 89.56, 9.58], 20)
+        time.sleep(2.5)
+
+
+    # draw aruco
+    def draw_marker(self, img, x, y):
+        # draw rectangle on img
+        cv2.rectangle(
+            img,
+            (x - 20, y - 20),
+            (x + 20, y + 20),
+            (0, 255, 0),
+            thickness=2,
+            lineType=cv2.FONT_HERSHEY_COMPLEX,
+        )
+        # add text on rectangle
+        cv2.putText(
+            img,
+            "({},{})".format(x, y),
+            (x, y),
+            cv2.FONT_HERSHEY_COMPLEX_SMALL,
+            1,
+            (243, 0, 0),
+            2,
+        )
+
+    # get points of two aruco
+    def get_calculate_params(self, img):
+        # Convert the image to a gray image
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        # Detect ArUco marker.
+        corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+            gray, self.aruco_dict, parameters=self.aruco_params)
+
+        """
+        Two Arucos must be present in the picture and in the same order.
+        There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+        Determine the center of the aruco by the four corners of the aruco.
+        """
+        if len(corners) > 0:
+            if ids is not None:
+                if len(corners) <= 1 or ids[0] == 1:
+                    return None
+                x1 = x2 = y1 = y2 = 0
+                point_11, point_21, point_31, point_41 = corners[0][0]
+                x1, y1 = int(
+                    (point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
+                    4.0), int(
+                        (point_11[1] + point_21[1] + point_31[1] + point_41[1])
+                        / 4.0)
+                point_1, point_2, point_3, point_4 = corners[1][0]
+                x2, y2 = int(
+                    (point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
+                    4.0), int(
+                        (point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
+                        4.0)
+                return x1, x2, y1, y2
+        return None
+
+    # set camera clipping parameters
+    def set_cut_params(self, x1, y1, x2, y2):
+        self.x1 = int(x1)
+        self.y1 = int(y1)
+        self.x2 = int(x2)
+        self.y2 = int(y2)
+        print(self.x1, self.y1, self.x2, self.y2)
+
+    # set parameters to calculate the coords between cube and mycobot
+    def set_params(self, c_x, c_y, ratio):
+        self.c_x = c_x
+        self.c_y = c_y
+        self.ratio = 220.0 / ratio
+
+    # calculate the coords between cube and mycobot
+    def get_position(self, x, y):
+        return ((y - self.c_y) * self.ratio +
+                self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
+
+    """
+    Calibrate the camera according to the calibration parameters.
+    Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+    If two ARuco values have been calculated, clip the video.
+    """
+
+    def transform_frame(self, frame):
+        # enlarge the image by 1.5 times
+        fx = 1.5
+        fy = 1.5
+        frame = cv2.resize(frame, (0, 0),
+                           fx=fx,
+                           fy=fy,
+                           interpolation=cv2.INTER_CUBIC)
+        if self.x1 != self.x2:
+            # the cutting ratio here is adjusted according to the actual situation
+            frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
+                          int(self.x1 * 0.7):int(self.x2 * 1.15)]
+        return frame
+
+    # according the class_id to get object name
+    def id_class_name(self, class_id):
+        for key, value in self.labels.items():
+            if class_id == int(key):
+                return value
+
+    # detect object
+    def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
+        i = 0
+        MIN_MATCH_COUNT = 5
+        # sift = cv2.xfeatures2d.SIFT_create()
+
+        # find the keypoints and descriptors with SIFT
+        # kp = []
+        # des = []
+        kp = kp_list
+        des = desc_list
+
+        kp2, des2 = kp_img, desc_img
+
+        # FLANN parameters
+        FLANN_INDEX_KDTREE = 0
+        index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
+        search_params = dict(checks=50)  # or pass empty dictionary
+        flann = cv2.FlannBasedMatcher(index_params, search_params)
+
+        x, y = 0, 0
+        try:
+            for i in range(len(des)):
+                matches = flann.knnMatch(des[i], des2, k=2)
+                # store all the good matches as per Lowe's ratio test.  根据Lowe比率测试存储所有良好匹配项。
+                good = []
+                for m, n in matches:
+                    if m.distance < 0.7 * n.distance:
+                        good.append(m)
+
+                # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
+                if len(good) > MIN_MATCH_COUNT:
+
+                    # extract corresponding point pairs from matching 从匹配中提取出对应点对
+                    # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
+                    src_pts = np.float32([kp[i][m.queryIdx].pt
+                                          for m in good]).reshape(-1, 1, 2)
+                    dst_pts = np.float32([kp2[m.trainIdx].pt
+                                          for m in good]).reshape(-1, 1, 2)
+
+                    # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
+                    M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
+                                                 5.0)
+                    matchesMask = mask.ravel().tolist()
+                    # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
+                    h, w, d = goal[i].shape
+                    pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
+                                      [w - 1, 0]]).reshape(-1, 1, 2)
+                    dst = cv2.perspectiveTransform(pts, M)
+                    coord = (dst[0][0] + dst[1][0] + dst[2][0] +
+                              dst[3][0]) / 4.0
+                    connection.send((DRAW_COORDS, coord))
+                    # cv2.putText(img, "{}".format(coord), (50, 60),
+                    #         fontFace=None, fontScale=1,
+                    #         color=(0, 255, 0), lineType=1)
+                    print(format(dst[0][0][0]))
+                    x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
+                         dst[3][0][0]) / 4.0
+                    y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
+                         dst[3][0][1]) / 4.0
+
+                    # bound box  绘制边框
+                    # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
+                    connection.send((DRAW_RECT, dst))
+                    # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
+        except Exception as e:
+            pass
+
+        if x + y > 0:
+            return x, y
+        else:
+            return None
+
+# The path to save the image folder
+def parse_folder(folder):
+    restore = []
+    path = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_320_pi/' + folder    # pi
+   
+    for i, j, k in os.walk(path):
+        for l in k:
+            restore.append(cv2.imread(folder + '/{}'.format(l)))
+    # print(restore)
+    return restore
+
+def compute_keypoints_and_descriptors(sift, images_lists):
+    kp_list = []
+    desc_list = []
+    for images in images_lists:
+        kp_tmp = []
+        desc_tmp = []
+        for img in images:
+            kp, desc = sift.detectAndCompute(img, None)
+            kp_tmp.append(kp)
+            desc_tmp.append(desc)
+        kp_list.append(kp_tmp)
+        desc_list.append(desc_tmp)
+
+    return kp_list, desc_list
+
+GET_FRAME = 1
+STOP_PROCESSING = 2
+DRAW_COORDS = 3
+DRAW_RECT = 4
+CLEAR_DRAW = 5
+CROP_FRAME = 6
+
+def get_frame(connection):
+    connection.send(GET_FRAME)
+    frame = connection.recv()
+    return frame
+
+def process_transform_frame(frame, x1, y1, x2, y2):
+    # enlarge the image by 1.5 times
+    fx = 1.5
+    fy = 1.5
+    frame = cv2.resize(frame, (0, 0),
+                        fx=fx,
+                        fy=fy,
+                        interpolation=cv2.INTER_CUBIC)
+    if x1 != x2:
+        # the cutting ratio here is adjusted according to the actual situation
+       frame = frame[int(y2 * 0.7):int(y1 * 1.15),
+                       int(x1 * 0.7):int(x2 * 1.15)]
+    return frame
+
+def process_display_frame(connection):
+    cap_num = 0
+    coord = None
+    dst = None
+    x1 = 0
+    y1 = 0
+    x2 = 0
+    y2 = 0
+    cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+    # cap = cv2.VideoCapture(cap_num, cv2.CAP_DSHOW)
+    if not cap.isOpened():
+        cap.open()
+    while cv2.waitKey(1) < 0:
+        _, frame = cap.read()
+        frame = process_transform_frame(frame, x1, y1, x2, y2)
+        if connection.poll():
+            request = connection.recv()
+            if request == GET_FRAME:
+                connection.send(frame)
+            elif request == CLEAR_DRAW:
+                coord = None
+                dst = None
+            elif type(request) is tuple:
+                if request[0] == DRAW_COORDS:
+                    coord = request[1]
+                elif request[0] == DRAW_RECT:
+                    dst = request[1]
+                elif request[0] == CROP_FRAME:
+                    x1 = request[1]
+                    y1 = request[2]
+                    x2 = request[3]
+                    y2 = request[4]
+
+        if not coord is None:
+            cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
+                    fontScale=1, color=(0, 255, 0), lineType=1)
+        if not dst is None:
+            frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
+        cv2.imshow("figure", frame)
+        time.sleep(0.04)
+    connection.send(STOP_PROCESSING)
+
+def run():
+    parent_conn, child_conn = Pipe()
+    child = Process(target = process_display_frame, args=(child_conn,))
+    child.start()
+
+    res_queue = [[], [], [], []]
+    res_queue[0] = parse_folder('res/D')
+    res_queue[1] = parse_folder('res/C')
+    res_queue[2] = parse_folder('res/A')
+    res_queue[3] = parse_folder('res/B')
+
+    sift = cv2.xfeatures2d.SIFT_create()
+    # sift = cv2.SIFT_create()
+    kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
+
+    # init a class of Object_detect
+    detect = Object_detect()
+
+    # init mycobot
+    detect.run()
+
+    # _init_ = 20  #
+    init_num = 0
+    nparams = 0
+    # num = 0
+    # real_sx = real_sy = 0
+    while True:
+        start_time = time.time()
+        if parent_conn.poll():
+            data = parent_conn.recv()
+            if data == STOP_PROCESSING:
+                break
+        # read camera
+        frame = get_frame(parent_conn)
+        # deal img
+        #frame = detect.transform_frame(frame)
+
+        # if _init_ > 0:
+        #     _init_ -= 1
+        #     continue
+        # calculate the parameters of camera clipping
+        if init_num < 20:
+            if detect.get_calculate_params(frame) is None:
+                # cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                init_num += 1
+                continue
+        elif init_num == 20:
+            detect.set_cut_params(
+                (detect.sum_x1) / 20.0,
+                (detect.sum_y1) / 20.0,
+                (detect.sum_x2) / 20.0,
+                (detect.sum_y2) / 20.0,
+            )
+            parent_conn.send((CROP_FRAME,
+                (detect.sum_x1) / 20.0,
+                (detect.sum_y1) / 20.0,
+                (detect.sum_x2) / 20.0,
+                (detect.sum_y2) / 20.0))
+            detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
+            init_num += 1
+            continue
+
+        # calculate params of the coords between cube and mycobot
+        if nparams < 10:
+            if detect.get_calculate_params(frame) is None:
+                # cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                nparams += 1
+                print ("ok")
+                continue
+        elif nparams == 10:
+            nparams += 1
+            # calculate and set params of calculating real coord between cube and mycobot
+            detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
+                              (detect.sum_y1 + detect.sum_y2) / 20.0,
+                              abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
+                              abs(detect.sum_y1 - detect.sum_y2) / 10.0)
+            print("ok")
+            continue
+
+        # get detect result
+        kp_img, desc_img = sift.detectAndCompute(frame, None)
+        frame = get_frame(parent_conn)
+        for i, v in enumerate(res_queue):
+            # HACK: to update frame every time
+            detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
+            if detect_result:
+                x, y = detect_result
+                # calculate real coord between cube and mycobot
+                real_x, real_y = detect.get_position(x, y)
+                detect.color = i
+                detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
+                detect.decide_move(real_x, real_y, detect.color)
+                # if num == 5:
+                #     detect.color = i
+                #     detect.pub_marker(real_sx / 5.0 / 1000.0,
+                #                       real_sy / 5.0 / 1000.0)
+                #     detect.decide_move(real_sx / 5.0, real_sy / 5.0,
+                #                        detect.color)
+                #     num = real_sx = real_sy = 0
+                # else:
+                #     num += 1
+                #     real_sy += real_y
+                #     real_sx += real_x
+                parent_conn.send(CLEAR_DRAW)
+
+        # cv2.imshow("figure", frame)
+        time.sleep(0.05)
+        end_time = time.time()
+        # print("loop_time = ", end_time - start_time)
+
+        # close the window
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            # cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
+
+    child.join()
+    
+
+if __name__ == "__main__":
+    run()
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()
--- a/mycobot_ai/aikit_320_pi/scripts/aikit_shape.py
+++ b/mycobot_ai/aikit_320_pi/scripts/aikit_shape.py
@ -0,0 +1,482 @@
+import cv2
+import numpy as np
+import time
+import os,sys
+import math
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+from pymycobot.mycobot import MyCobot
+
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"
+# Adaptive seeed
+
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x = 260, camera_y = 0):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # declare mycobot320
+        self.mc = None
+
+        # 移动角度
+        self.move_angles = [
+            [0.61, 45.87, -92.37, -41.3, 89.56, 9.58],  # init the point
+            [18.8, -7.91, -54.49, -23.02, 89.56, -14.76],  # point to grab
+            [17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [32, -228.3, 201.6, -168.07, -7.17, -92.56],  # D Sorting area
+            [266.5, -219.7, 209.3, -170, -3.64, -94.62],  # C Sorting area
+            [253.8, 236.8, 224.6, -170, 6.87, -77.91],  # A Sorting area
+            [35.9, 235.4, 211.8, -169.33, -9.27, 88.3],  # B Sorting area
+        ]
+        
+        # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        
+        # choose place to set cube
+        self.color = 0
+        # parameters to calculate camera clipping parameters
+        self.x1 = self.x2 = self.y1 = self.y2 = 0
+        # set cache of real coord
+        self.cache_x = self.cache_y = 0
+
+        # use to calculate coord between cube and mycobot
+        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+        # The coordinates of the grab center point relative to the mycobot
+        self.camera_x, self.camera_y = camera_x, camera_y
+        # The coordinates of the cube relative to the mycobot
+        self.c_x, self.c_y = 0, 0
+        # The ratio of pixels to actual values
+        self.ratio = 0
+        # Get ArUco marker dict that can be detected.
+        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        # Get ArUco marker params.
+        self.aruco_params = cv2.aruco.DetectorParameters_create()
+        
+        # 初始化背景减法器
+        self.mog =cv2.bgsegm.createBackgroundSubtractorMOG() 
+        
+        # init a node and a publisher
+        rospy.init_node("marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+        # init a Marker
+        self.marker = Marker()
+        self.marker.header.frame_id = "base"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+        
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+        
+    # publish marker
+    def pub_marker(self, x, y, z=0.03):
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = x 
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        self.marker.color.g = self.color
+        self.pub.publish(self.marker)
+    
+    # pump_control pi
+    def gpio_status(self, flag):
+        if flag:
+            """start the suction pump"""
+            self.mc.set_basic_output(1, 0)
+            self.mc.set_basic_output(2, 1)
+        else:
+            """stop suction pump"""
+            self.mc.set_basic_output(1, 1)
+            self.mc.set_basic_output(2, 0)
+            time.sleep(1)
+            self.mc.set_basic_output(2, 1)
+
+    def pump_on(self):
+        """Start the suction pump"""
+        self.mc.set_basic_output(1, 0)
+        self.mc.set_basic_output(2, 1)
+
+    def pump_off(self):
+        """stop suction pump m5"""
+        self.mc.set_basic_output(1, 1)
+        self.mc.set_basic_output(2, 0)
+        time.sleep(1)
+        self.mc.set_basic_output(2, 1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        # send Angle to move mycobot320
+        print(color)
+        print('x, y:', round(x, 2), round(y, 2))
+        # send Angle to move mycobot320
+        self.mc.send_angles(self.move_angles[2], 50)
+        time.sleep(3)
+
+        # send coordinates to move mycobot
+        self.mc.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
+        time.sleep(2.5)
+        self.mc.send_coords([x, y, 100, -173.84, -0.14, -74.37], 100, 1)  #
+        time.sleep(3)
+
+        # open pump
+        self.gpio_status(True)
+        time.sleep(1.5)
+
+        tmp = []
+        while True:
+            if not tmp:
+                tmp = self.mc.get_angles()
+            else:
+                break
+        time.sleep(0.5)
+        
+        # print(tmp)
+        self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+        time.sleep(3)
+        self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
+        
+        self.mc.send_coords(self.move_coords[color], 100, 1)
+        
+        self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0)
+ 
+        time.sleep(6.5)
+       
+        # close pump
+        self.gpio_status(False)
+        time.sleep(6.5)
+        
+        if color == 1:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0-0.02
+            )
+        elif color == 0:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0
+            )
+
+        self.mc.send_angles(self.move_angles[0], 25)
+        time.sleep(4.5)
+
+    # decide whether grab cube
+    def decide_move(self, x, y, color):
+        print(x, y, self.cache_x, self.cache_y)
+        # detect the cube status move or run
+        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
+            self.cache_x, self.cache_y = x, y
+            return
+        else:
+            self.cache_x = self.cache_y = 0
+            # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+            self.move(x, y, color)
+
+    # init mycobot320
+    def run(self):
+     
+        if "dev" in self.robot_raspi:
+            self.mc = MyCobot(self.robot_raspi, 115200)
+        self.gpio_status(False)
+        self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 89.56, 9.58], 20)
+        time.sleep(2.5)
+
+    # draw aruco
+    def draw_marker(self, img, x, y):
+        # draw rectangle on img
+        cv2.rectangle( 
+            img,
+            (x - 20, y - 20),
+            (x + 20, y + 20),
+            (0, 255, 0),
+            thickness=2,
+            lineType=cv2.FONT_HERSHEY_COMPLEX,
+        )
+        # add text on rectangle
+        cv2.putText(img, "({},{})".format(x, y), (x, y),
+                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
+
+    # get points of two aruco
+    def get_calculate_params(self, img):
+        # Convert the image to a gray image
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        # Detect ArUco marker.
+        corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+            gray, self.aruco_dict, parameters=self.aruco_params
+        )
+
+        """
+        Two Arucos must be present in the picture and in the same order.
+        There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+        Determine the center of the aruco by the four corners of the aruco.
+        """
+        if len(corners) > 0:
+            if ids is not None:
+                if len(corners) <= 1 or ids[0] == 1:
+                    return None
+                x1 = x2 = y1 = y2 = 0
+                point_11, point_21, point_31, point_41 = corners[0][0]
+                x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
+                    (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
+                point_1, point_2, point_3, point_4 = corners[1][0]
+                x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
+                    (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
+                return x1, x2, y1, y2
+        return None
+
+    # set camera clipping parameters
+    def set_cut_params(self, x1, y1, x2, y2):
+        self.x1 = int(x1)
+        self.y1 = int(y1)
+        self.x2 = int(x2)
+        self.y2 = int(y2)
+        print(self.x1, self.y1, self.x2, self.y2)
+
+    # set parameters to calculate the coords between cube and mycobot320
+    def set_params(self, c_x, c_y, ratio):
+        self.c_x = c_x
+        self.c_y = c_y
+        self.ratio = 220.0/ratio
+
+    # calculate the coords between cube and mycobot320
+    def get_position(self, x, y):
+        return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
+
+    """
+    Calibrate the camera according to the calibration parameters.
+    Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+    If two ARuco values have been calculated, clip the video.
+    """
+    def transform_frame(self, frame):
+        # enlarge the image by 1.5 times
+        fx = 1.5
+        fy = 1.5
+        frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
+                           interpolation=cv2.INTER_CUBIC)
+        if self.x1 != self.x2:
+            # the cutting ratio here is adjusted according to the actual situation
+            frame = frame[int(self.y2*0.78):int(self.y1*1.1),
+                          int(self.x1*0.86):int(self.x2*1.08)]
+        return frame
+    
+    # 检测物体的形状
+    def shape_detect(self,img):
+        x = 0
+        y = 0
+        Alpha = 65.6
+        Gamma=-8191.5
+        cal = cv2.addWeighted(img, Alpha,img, 0, Gamma)
+        gray = cv2.cvtColor(cal, cv2.COLOR_BGR2GRAY)
+    
+        # 转换为灰度图片
+        #ray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
+
+        # a etching operation on a picture to remove edge roughness
+        erosion = cv2.erode(gray, np.ones((2, 2), np.uint8), iterations=2)
+
+        # the image for expansion operation, its role is to deepen the color depth in the picture
+        dilation = cv2.dilate(erosion, np.ones(
+            (1, 1), np.uint8), iterations=2)
+
+
+        # 设定灰度图的阈值 175, 255
+        _, threshold = cv2.threshold(dilation, 175, 255, cv2.THRESH_BINARY)
+        # 边缘检测
+        edges = cv2.Canny(threshold,50,100)
+        # 检测物体边框
+        contours,_ = cv2.findContours(
+            edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+        
+
+
+        if len(contours)>0:
+            for cnt in contours:
+                # if 6000>cv2.contourArea(cnt) and cv2.contourArea(cnt)>4500:
+                if cv2.contourArea(cnt)>5500:
+                    objectType = None
+                    peri = cv2.arcLength(cnt,True)
+                    approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
+                    objCor = len(approx)
+                    x, y, w, h = cv2.boundingRect(approx)
+
+                    boxes = [
+                        box
+                        for box in [cv2.boundingRect(c) for c in contours]
+                        if min(img.shape[0], img.shape[1]) / 10
+                        < min(box[2], box[3])
+                        < min(img.shape[0], img.shape[1]) / 1
+                    ]
+                    if boxes:
+                        for box in boxes:
+                            x, y, w, h = box
+                        # find the largest object that fits the requirements
+                        c = max(contours, key=cv2.contourArea)
+                        rect = cv2.minAreaRect(c)
+                        box = cv2.boxPoints(rect)
+                        box = np.int0(box)
+                        cv2.drawContours(img, [box], 0, (153, 153, 0), 2)
+                        x = int(rect[0][0])
+                        y = int(rect[0][1])
+
+                    if objCor==3:
+                        objectType = "Triangle(三角形)"
+                        cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
+                        self.color = 3
+                    elif objCor==4:
+                        box = cv2.boxPoints(rect)
+                        box = np.int0(box)
+                        _W = math.sqrt(math.pow((box[0][0] - box[1][0]), 2) + math.pow((box[0][1] - box[1][1]), 2))
+                        _H = math.sqrt(math.pow((box[0][0] - box[3][0]), 2) + math.pow((box[0][1] - box[3][1]), 2))
+                        aspRatio = _W/float(_H)
+                        if 0.98 < aspRatio < 1.03:
+                            objectType = "Square(正方形)"
+                            cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
+                            self.color=1
+                        else:
+                            objectType = "Rectangle(长方形)"
+                            cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
+                            self.color=2
+                    elif objCor>=8:
+                        objectType = "Circle(圆形)"
+                        self.color=0
+                        cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
+                    else:
+                        print('not recognized!')
+                    print(objectType)
+
+        if abs(x) + abs(y) > 0:
+            return x, y
+        else:
+            return None
+
+        
+if __name__ == "__main__":
+
+    # open the camera
+    cap_num = 0
+    cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+    # cap = cv2.VideoCapture(cap_num)
+    cap.set(3, 640)
+    cap.set(4, 480)
+    if not cap.isOpened():
+        cap.open()
+    # init a class of Object_detect
+    detect = Object_detect()
+    # init mycobot320
+    detect.run()
+
+    _init_ = 20  
+    init_num = 0
+    nparams = 0
+    num = 0
+    real_sx = real_sy = 0
+    while cv2.waitKey(1) < 0:
+       # read camera
+        _, frame = cap.read()
+        # deal img
+        frame = detect.transform_frame(frame)
+        if _init_ > 0:
+            _init_ -= 1
+            continue
+
+        # calculate the parameters of camera clipping
+        if init_num < 20:
+            if detect.get_calculate_params(frame) is None:
+                cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                init_num += 1
+                continue
+        elif init_num == 20:
+            detect.set_cut_params(
+                (detect.sum_x1)/20.0,
+                (detect.sum_y1)/20.0,
+                (detect.sum_x2)/20.0,
+                (detect.sum_y2)/20.0,
+            )
+            detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
+            init_num += 1
+            continue
+
+        # calculate params of the coords between cube and mycobot320
+        if nparams < 10:
+            if detect.get_calculate_params(frame) is None:
+                cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                nparams += 1
+                continue
+        elif nparams == 10:
+            nparams += 1
+            # calculate and set params of calculating real coord between cube and mycobot320
+            detect.set_params(
+                (detect.sum_x1+detect.sum_x2)/20.0,
+                (detect.sum_y1+detect.sum_y2)/20.0,
+                abs(detect.sum_x1-detect.sum_x2)/10.0 +
+                abs(detect.sum_y1-detect.sum_y2)/10.0
+            )
+            print("ok")
+            continue
+
+        # get detect result
+        # detect_result = detect.color_detect(frame)
+        # print('调用检测')
+        detect_result = detect.shape_detect(frame)
+        # print("完成检测")
+        if detect_result is None:
+            cv2.imshow("figure", frame)
+            continue
+        else:
+            x, y = detect_result
+            # calculate real coord between cube and mycobot320
+            real_x, real_y = detect.get_position(x, y)
+            if num == 20:
+                detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
+                detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
+                num = real_sx = real_sy = 0
+
+            else:
+                num += 1
+                real_sy += real_y
+                real_sx += real_x
+
+        cv2.imshow("figure", frame)
+
+        # close the window
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
--- a/mycobot_ai/aikit_320_pi/scripts/moving_utils.py
+++ b/mycobot_ai/aikit_320_pi/scripts/moving_utils.py
@ -0,0 +1,49 @@
+#encoding: UTF-8
+#!/usr/bin/env python3
+import rospy
+import time,os
+
+from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
+
+
+class Movement(object):
+	"""Tools class: Communication with mycobot."""
+	def __init__(self):
+		super(Movement, self).__init__()
+		self.angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=5)
+		self.coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5)
+
+		self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=10)
+
+		self.angles = MycobotSetAngles()
+		self.coords = MycobotSetCoords()
+		self.pump = MycobotPumpStatus()
+
+	def pub_coords(self, item, sp=20, m=1):
+		self.coords.x = item[0]
+		self.coords.y = item[1]
+		self.coords.z = item[2]
+		self.coords.rx = item[3]
+		self.coords.ry = item[4]
+		self.coords.rz = item[5]
+		self.coords.speed = sp
+		self.coords.model = m
+		self.coord_pub.publish(self.coords)
+
+
+	def pub_angles(self, item, sp):
+		self.angles.joint_1 = item[0]
+		self.angles.joint_2 = item[1]
+		self.angles.joint_3 = item[2]
+		self.angles.joint_4 = item[3]
+		self.angles.joint_5 = item[4]
+		self.angles.joint_6 = item[5]
+		self.angles.speed = sp
+		self.angle_pub.publish(self.angles)
+
+
+	def pub_pump(self, flag,Pin):
+		self.pump.Status = flag
+		self.pump.Pin1 = Pin[0]
+		self.pump.Pin2 = Pin[1]
+		self.pump_pub.publish(self.pump)
--- a/mycobot_ai/aikit_320_pi/scripts/test.py
+++ b/mycobot_ai/aikit_320_pi/scripts/test.py
@ -0,0 +1,18 @@
+from pymycobot.mycobot import MyCobot
+import time
+from pymycobot import PI_BAUD, PI_PORT
+
+mc = MyCobot(PI_PORT, PI_BAUD)
+
+coords = [ 
+        [135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
+        [136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
+    ]
+
+angles = [0, 0, 0, 0, 0, 0]
+
+mc.send_coords(coords[0], 20, 1)
+time.sleep(3)
+mc.send_coords(coords[1], 20, 1)
+
+
--- a/mycobot_description/urdf/new_320_pi/ai_kit320.dae
+++ b/mycobot_description/urdf/new_320_pi/ai_kit320.dae
--- a/mycobot_description/urdf/new_320_pi/new_mycobot_vision_v2.urdf
+++ b/mycobot_description/urdf/new_320_pi/new_mycobot_vision_v2.urdf
@ -0,0 +1,204 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="env">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04  1.5708 3.14159-->
+        <mesh filename="package://mycobot_description/urdf/new_320_pi/ai_kit320.dae"/>
+      </geometry>
+      <!-- <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/> -->
+      <origin xyz = "0 0 0.0 " rpy = "0 0 0"/>
+    </visual>
+  </link>
+
+  <link name="base">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/base.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 3.1415926"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/base.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 3.1415926"/>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link1.dae"/>
+      </geometry>
+
+    <origin xyz = "0.115 -0.172 -0.086 " rpy = " 0 0 3.1415926"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link1.dae"/>
+      </geometry>
+      <origin xyz = "0.115 -0.172 -0.09 " rpy = " 0 0 3.1415926"/>
+    </collision>
+  </link>
+
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link2.dae"/>
+      </geometry>
+      <origin xyz = "-0.075 -0.115 -0.022 " rpy = " 1.5708 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link2.dae"/>
+      </geometry>
+      <origin xyz = "-0.075 -0.115 -0.022 " rpy = " 1.5708 0 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link3.dae"/>
+      </geometry>
+    <origin xyz = "-0.22 -0.115 0.142 " rpy = " 1.5708 0 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link3.dae"/>
+      </geometry>
+      <origin xyz = "-0.22 -0.115 0.142 " rpy = " 1.5708 0 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link4.dae"/>
+      </geometry>
+    <origin xyz = "-0.115 0.338 -0.1435" rpy = " 1.5708 0 0"/>
+    </visual>
+    <collision>
+       <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link4.dae"/>
+      </geometry>
+      <origin xyz = "-0.115 0.338 -0.1435 " rpy = " 1.5708 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link5.dae"/>
+      </geometry>
+    <origin xyz = "-0.1149 -0.0322  -0.482 " rpy = " 0  0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link5.dae"/>
+      </geometry>
+    <origin xyz = "-0.1149 -0.0322  -0.482 " rpy = " 0  0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link6">
+    <visual>
+      <geometry>
+      <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link6.dae"/>
+      </geometry>
+    <origin xyz = "-0.11478 0.433 -0.15 " rpy = " 1.5708 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/new_320_pi/link6.dae"/>
+      </geometry>
+    <origin xyz = "-0.11478 0.433 -0.15 " rpy = " 1.5708 0 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
+    <parent link="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.162" rpy = "0 0 0"/> 
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+       <origin xyz= "0 0 0" rpy = "0 -1.5708 1.5708"/> 
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.13635 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+     <!-- <origin xyz = "0.12 0 0.09 " rpy = " 0 0 1.57080"/> -->
+     <origin xyz = "0.1205 0 0.082 " rpy = " 0 0 1.57080"/>
+  </joint>
+
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+     <origin xyz= "0 -0.084 0" rpy = "1.5708 0 0"/>
+  </joint>
+
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="link6"/>
+     <origin xyz= "0 0.06635 0" rpy = "-1.5708 0 0"/>
+  </joint>
+
+  <joint name="vision_joint" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="base"/>
+    <child link="env"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>
+  </joint>
+
+
+</robot>