[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\\
	 t_d: tread

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.

Program environment

Win 10 Pro 64bit Anaconda python 3.7

Program code

"""
Opposing two-wheeled robot
Trajectory simulation

Input parameters
Length between two wheels: tread[m]
Wheel speed Right, Left: vr, vl [m/s]

output
Vehicle center of gravity speed: ver
Turning angular velocity: omega[rad/s]

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


Referenced code
Two wheel motion model sample
https://www.eureka-moments-blog.com/entry/2020/04/05/180844#%E5%AF%BE%E5%90%912%E8%BC%AA%E5%9E%8B%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88
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),
                    arrowprops=dict(shrink=0, width=1, headwidth=2, 
                    headlength=10, connectionstyle='arc3',
                    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)

We received advice from Kioto about the program code. thank you so much.

result

Result when input_list1 is read as data

Summary

Digital twins (cyberspace and physical space) can be connected via data. Data from physical space is the bridge, and in cyberspace, simulation by data or data by simulation is the bridge. I hope to expand from programming education to data utilization education.