# Intelligent route planning of MyCobot manipulator head

Posted by Labbat on Mon, 07 Mar 2022 12:46:58 +0100

I've been struggling whether to talk about the head movement first or the spatial coordinate system first. Later, I decided to talk about the head movement first, so that we can first feel how the manipulator is positioned in space, and then we will introduce in detail the spatial coordinates of the manipulator in the next section. Spatial coordinates is a very complex and deep understanding thing. Only by learning spatial coordinates well can we learn all kinds of movements of manipulator. In order to better experience the spatial positioning of the manipulator, let's start with the head movement of the manipulator and let's experience the spatial coordinates and motion.

(1)send_coords([x,y,z,rx,ry,rz],speed,model) is used to control the head of the manipulator to move to a specified point with a specified attitude. It is mainly used to realize the intelligent planning of the head of the manipulator from one position to another. 10. Y and Z represent the position of the manipulator head in space (the coordinate system is rectangular coordinate system), Rx, ry and RZ represent the posture of the manipulator head at this point (the coordinate system is Euler coordinate).

Function function: intelligently plan the route and move the head of the manipulator from the original point to the specified point.

Parameter Description: x,y,z are space rectangular coordinate system, [rx,ry,rz] represents the posture of the manipulator head, which is Euler coordinate system.

Speed: indicates the speed of the manipulator. The value range is 0 ~ 100. The higher the value, the faster the speed.

model: the value is limited to 0 and 1. 0 indicates the random planning of the manipulator head movement, as long as the manipulator head moves to the specified point.

1 indicates that the head of the manipulator allows the head of the manipulator to move to the specified point in a straight line.

(2)get_coords()

Function function: obtain the spatial coordinates and current posture of the manipulator head at this time.

Return value: the returned type is a list set containing six float elements. The first three coordinates are x, y and Z, representing the coordinates of the manipulator head, and the last three coordinates Rx, ry and RZ represent the posture of the manipulator head.

We are still the old method, experimenting and learning at the same time. Open a terminal window and input python. We must import API functions first.

```from pymycobot.mycobot import MyCobot
from pymycobot.genre import Coord
from pymycobot import PI_PORT, PI_BAUD
import time```

Let the head reach the coordinates of [59.9, - 65.8250.7] in a linear manner, and maintain the posture of [- 50.99,83.14, - 52.42]

```mc = MyCobot(PI_PORT, PI_BAUD)
mc.send_coords([59.9, -65.8, 250.7, -50.99, 83.14, -52.42], 80, 1)```

Maybe the shape you get from this piece of code is different from mine. It doesn't matter. As long as the head reaches this position, because it is the line planned by the head, even if each joint has various shapes, it doesn't matter. Let's do get_coords get the coordinate data at this time:

```coords = mc.get_coords()
print(coords)```

As shown below:

This value is different from the value we passed, mainly because it is the internal error of the machine, which is close to the value we passed.

Second example: we enter:

`mc.send_angles([88.68, -138.51, 155.65, -128.05, -9.93, -15.29], 50)`

The state becomes like this, and then we enter the previous head coordinates:

`mc.send_coords([59.9, -65.8, 250.7, -50.99, 83.14, -52.42], 80, 1)`

The manipulator has become like this. Let's take the coordinates of the manipulator again:

```coords = mc.get_coords()
print(coords)```

The coordinates we get are: [59.5, - 66.0, 251.1, - 49.56, 83.14, - 50.92] as shown in the figure below.

This shows that as long as the head spatial coordinates reach the position, the state of each joint may be different every time.

(3) Test applet provided in the manual:

```#!/usr/bin/python3
#-*- coding: UTF-8 -*-

from pymycobot.mycobot import MyCobot
from pymycobot.genre import Coord
from pymycobot import PI_PORT, PI_BAUD
import time

mc = MyCobot(PI_PORT, PI_BAUD)
# Get the angle and pose of the current head
coords = mc.get_coords()
print(coords)
# Intelligently plan the route, let the head reach the coordinates of [59.9, - 65.8250.7] in a linear way, and maintain the posture of [- 50.99,83.14, - 52.42]
mc.send_coords([59.9, -65.8, 250.7, -50.99, 83.14, -52.42], 80, 1)
# Set waiting time
time.sleep(1.5)
# Intelligently plan the route, let the head reach the coordinates of [59.9, - 65.8350.7] in a linear way, and maintain the posture of [- 50.99,83.14, - 52.42]
mc.send_coords([59.9, -65.8, 350.7, -50.99, 83.14, -52.42], 80, 1)
# Set waiting time
time.sleep(1.5)
# Only change the x coordinate of the head and set the x coordinate of the head to - 40. Let it plan the route intelligently and move the head to the changed position
mc.send_coord(Coord.X.value, -40, 70)```

Similarly, you can save this code as 4 Py, then you open a new window and execute:

`python 4.py`

You can run this code continuously.