# [PYTHON] Digital twin with wheel moving robot

### Introduction

Wheel-moving robots (opposite two-wheel type) equipped with Microbit and Arduino are on sale as programming materials for children. (For example, a servo car kit using a 360-degree servo motor Move a 360-degree servo motor with Microbit) Sample programs are provided as teaching materials, such as various movements of the robot, wireless operation, and autonomous operation with sensors attached. Taking this one step further, there is something that connects cyber space (digital space) and physical space (real space) (digital twin) using data, such as simulating this movement in advance and reproducing the trajectory from the data actually moved. I thought it would be interesting. Therefore, I made a simulation that reproduces the trajectory from the information of the motor ON / OFF of the mobile robot. Retrieving ON / OFF information for the motor (or servo motor) is a future task, and that part is not included this time.

### Wheel moving robot

For wheel moving robots, the following site will be helpful, but the input is the speed of the left and right wheels, and the value to be set is the length between the wheels. The formula is as follows.



v: velocity \\

\omega: angular \ velocity \\

x, y: position\\
\theta: directonã€€\ angle\\


v _k = (v_r +v_l)/2 \\
\omega _k = (v_r +v_l)/t_d \\

x_{k+1} = x_k + \cos \theta _k\cdot\Delta K v_k \\
y_{k+1} = y_k + \sin \theta _k\cdot\Delta K v_k \\
\theta _{k+1} = \theta _k + \Delta K \omega _k \\



Figure: Coordinates and parameters of the wheel-moving robot (partially changed the figure in the referenced article)

Reference: Wheel moving robot principle [Moving Mecha / Robot and Basics of Control-Tohoku Gakuin University Information Processing ...](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RM Seminar_No20_s8.pdf ) Let's calculate the movement of a wheel robot (opposed two-wheel type)

### Simulation with Python

Referenced code Two wheel motion model sample

In this simulation, the speeds of the left and right wheels are constant and only ON / OFF information is provided.

Move Right wheel Left wheel
Straight line Previous ON Previous ON
Backward After ON After ON
Turn right OFF Previous ON
Turn left Previous ON OFF
Rotate on the spot Before ON (after ON) After ON (before ON)

#### Program environment

Win 10 Pro 64bit Anaconda python 3.7

#### Program code

"""
Opposing two-wheeled robot
Trajectory simulation

Input parameters
Wheel speed Right, Left: vr, vl [m/s]

output
Vehicle center of gravity speed: ver

Final output
Azimuth: thetat
x,y coordinate: xt ,yt

Referenced code
Two wheel motion model sample
change point:
Expand matrix format into ordinary expressions
Change from angular velocity of wheels to speed of wheels
Change simulation input

animation
Create graph (Matplotlib) animation with Python (Artist Animation)
https://water2litter.net/rum/post/python_matplotlib_animation_ArtistAnimation/

[Scientific / technical calculation by Python]Trajectory of parabolic movement animation(locus)Draw with, matplotlib
https://qiita.com/sci_Haru/items/278b6a50c4e9f4c07dcf

"""

import numpy as np
from math import cos, sin, pi
import math
import matplotlib.pyplot as plt
from matplotlib.animation import ArtistAnimation

def twe_wheel_fuc(v, state, delta, factor=1, td=2):
"""
Equation of state
Args:
v (tuple or list): velocity of each wheel unit m/s,(right velocity,left velocity)
state (list):ã€€state [x, y, thita] , x, y
delta (float): update time unit s
factor (float):velocity factor Defaults to 1
td (float): tread length between wheel unit m Defaults to 2.

Returns:
[list]: next state
"""
# vr: right wheel velocity, vl:left wheel velocity
# vel: Center of gravity velocity
# omega: Rotation angular velocity
vr = v[0]*factor
vl = v[1]*factor
vel = (vr + vl)/2
omega = (vr - vl)/(td)
# state[2]: theta
x_ = vel*delta*cos(state[2]+omega*delta/2)
y_ = vel*delta*sin(state[2]+omega*delta/2)
# x_ = vel*delta*cos(state[2])
# y_ = vel*delta*sin(state[2])
xt = state[0] + x_
yt = state[1] + y_
thetat = state[2]+omega*delta
update_state = [xt, yt, thetat]
return update_state

def simulation_twewheel(data,ini_state=[0,0,0],factor=1,td=6.36):
"""
data: list On/OFF data

"""
# simulation
#For drawing animation graphs
fig = plt.figure()
ims = []
#Storage of calculated data (coordinates)
st_x = []
st_y = []
st_theta = []
st_vec = ini_state

for i in data:
st_vec = twe_wheel_fuc(i, st_vec, delta=1,factor=factor,td=td)
xt, yt, thetat = st_vec
print("State:",st_vec)
print("Direction angle: ",math.degrees(thetat))
st_x.append(xt)
st_y.append(yt)
st_theta.append(thetat)

#Settings for Plot
plt.grid(True)
plt.axis("equal")
plt.xlabel("X")
plt.ylabel("Y")

#If only the position at time t
# im=plt.plot(xt,yt,'o', color='red',markersize=10, linewidth = 2)

#Create two pictures, the position at time t and the trajectory leading up to time t
plt.annotate('', xy=(xt+cos(thetat),yt+sin(thetat)), xytext=(xt,yt),
facecolor='blue', edgecolor='blue'))
im=plt.plot(xt,yt,'o',st_x,st_y, '--', color='red',markersize=10, linewidth = 2)

ims.append(im)

#Animation creation
anim = ArtistAnimation(fig, ims, interval=100, blit=True,repeat=False)
plt.show()
# plt.pause(10)

if __name__ == '__main__':
#Velocity data per second
#Switch on/Set to OFF and keep the speed constant. Forward rotation: 1, reverse rotation:-1, stop: 0
#(1,1): Forward, (0,1): Clockwise, (1,0): Counterclockwise,(-1,1) or (1,-1): In-situ rotation
input_lists =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(0,1),(0,1),(0,1),(0,1),(0,1),(0,1),
(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(1,0),(1,0),(1,0),(1,0),(1,0),(1,0),
(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(1,-1),(1,-1),(1,1),(1,1),
(1,-1),(1,-1),(1,1),(1,1),
(1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
(1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
(1,1),(1,1),]

input_lists2 =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),]

simulation_twewheel(data=input_lists,ini_state=[0,0,0],factor=1,td=6.36)