GetForceAndFriction.py

"""
This scripts has the aim of determining a and b in equation dv/dt = a * Q - b * v (*),
    given we know a/b = v_sat_max (from v_saturation = v_sat_max * Q, see MotorCalibration.py for the details)
    In (*) we substitute a with b * v_sat_max -> dv/dt = b * (v_sat_max * Q - v) and integrate the equation.
    We try different values of b, till v(t) obtained by integration matches the results of a recorded experiment.
    The comparison is done by eye, by plotting the model and recorded velocity.
The second task of this script is to determine the two parameters used in CartPole simulation
    u_max (mapping from dimensionless Q to physical force) and M_fric (friction of the cart)
    We do it by comparing theoretically derived dv/dt = (u_max/M)*Q-(M_fric/M)*v
    and empirically obtained dv/dt = a * Q - b * v.
    From this follows:
    u_max = M * a
    M_fric = M * b
    Attention! In this case M is a total mass moved by the force generated by the motor
    In our experiment which we performed with pole attached M = mass of cart + pole + wire immobilizing the pole
    Adjust it accordingly if in future you perform a measurement with the pole not attached.
The end of the file is an old code attached in commented-out form.
    It contains some functions to try fitting the motor characteristics to more complex equations.
    It is not used now.
"""

import matplotlib.pyplot as plt

import numpy as np
import pandas as pd
import platform

# Change backend for matplotlib to plot interactively in pycharm
# This import must go before pyplot
from matplotlib import use

if platform.system() == 'Darwin':
    use('macOSX')
else:
    use('TkAgg')

# Define the variables
# FILE_NAME = 'Original.csv'
FILE_NAME = 'CPP_step_response-28-06-2024-POLOLU-check.csv'

v_max_sat = 0.55
b = 14.0

PATH_TO_DATA = 'DataAnalysis/MotorAndCartFriction/'
file_path = PATH_TO_DATA + FILE_NAME

def smooth(y, box_pts):
    box = np.ones(box_pts)/box_pts
    y_smooth = np.convolve(y, box, mode='same')
    return y_smooth

def acceleration_no_offset_formula(Q,v, parameters):
    a, b = parameters
    return a*Q-b*v

data: pd.DataFrame = pd.read_csv(file_path, comment='#')

data['dt'] = data['time'].shift(-1) - data['time']
data['positionD_last'] = data['positionD'].shift(1)
data['positionD_smoothed'] = smooth(data['positionD'], 30)

data = data.iloc[1:-1]
data = data.reset_index(drop=True)

time = data['time'].to_numpy()
dt = data['dt'].to_numpy()
Q = data['Q'].to_numpy()

a = v_max_sat * b
parameters = (a, b)

v = np.zeros_like(time)
for i in range(len(v) - 1):
    if i == 0:
        v[0] = 0.0
    v[i + 1] = v[i] + acceleration_no_offset_formula(Q[i], v[i], parameters) * dt[i]

fontsize_labels = 14
fontsize_ticks = 12

fig, axs = plt.subplots(2, 1, figsize=(16, 9), sharex=True)

axs[0].set_ylabel("Speed (m/s)", fontsize=fontsize_labels)
axs[0].plot(time, data['positionD'],
            'g', markersize=12, label='Velocity')
axs[0].plot(time, v,
            'orange', markersize=12, label='Velocity modeled')
axs[0].tick_params(axis='both', which='major', labelsize=fontsize_ticks)
axs[0].legend()

axs[1].set_ylabel("Motor Input Q (-)", fontsize=fontsize_labels)
axs[1].plot(time, Q,
            'r', markersize=12, label='Q')
axs[1].tick_params(axis='both', which='major', labelsize=fontsize_ticks)

axs[1].set_xlabel('Time (s)', fontsize=fontsize_labels)

fig.align_ylabels()
plt.tight_layout()
plt.show()

# Making m go to 0 and setting J_fric=0 (fine for pole without mass)
# positionDD = (u_max/M)*Q-(M_fric/M)*positionD
# Better instread of M we take M+m+0.001 - cart+pole+wire fixing the pole
# Compare this with positionDD = a*Q-b*positionD
# u_max = M*a
# M_fric = M*b

print()
print('a: {}'.format(a))
print('b: {}'.format(b))

m_total = 230.0/1000.0
u_max = m_total*a
M_fric = m_total*b

print()
print('Found values for Cartpole Simulator:')
print('u_max = {}'.format(u_max))
print('M_fric = {:.3}'.format(M_fric))




### Below some old code we used for fitting more complex dynaminal equations for motor
# Code is in this form probably not working but may serve as some inspiration, starting point

# # Second guess, including classincal friction:
#
# # Parameter guess
# b = 20.0
# a = 0.98*b
# c = 0.05*b
#
# print('a = {}'.format(a))
# print('b = {}'.format(b))
# print('c = {}'.format(c))
#
# v = np.zeros_like(time)
# for i in range(len(v)-1):
#     if i == 0:
#         v[0] = 0.0
#     v[i+1] = v[i]+(a*Q[i]-b*v[i]-c*np.sign(v[i]))*dt[i]
#
#
# fig, axs = plt.subplots(2, 1, figsize=(16, 9), sharex=True)
#
# axs[0].set_ylabel("Speed (m/s)", fontsize=fontsize_labels)
# # axs[0].plot(data['time'], data['positionD_smoothed'],
# #             'g', markersize=12, label='Velocity')
# axs[0].plot(data['time'], data['positionD'],
#             'g', markersize=12, label='Velocity')
# axs[0].plot(time, v,
#             'orange', markersize=12, label='Velocity modeled')
# axs[0].tick_params(axis='both', which='major', labelsize=fontsize_ticks)
#
# axs[1].set_ylabel("Motor Input Q (-)", fontsize=fontsize_labels)
# axs[1].plot(data['time'], data['Q'],
#             'r', markersize=12, label='Q')
# axs[1].tick_params(axis='both', which='major', labelsize=fontsize_ticks)
#
# axs[1].set_xlabel('Time (s)', fontsize=fontsize_labels)
#
# fig.align_ylabels()
#
# plt.tight_layout()
#
# plt.show()
#
#
#
#
#
# # Third guess, including correction with v^2
#
# # Parameter guess
# b = 20.0
# a = 1.4*b
# c = 0.08*b
# d = 10.0
#
#
#
#
# v = np.zeros_like(time)
# for i in range(len(v)-1):
#     if i == 0:
#         v[0] = 0.0
#     v[i+1] = v[i]+(a*Q[i]-b*v[i]-c*np.sign(v[i])-d*(v[i]**2)*np.sign(v[i]))*dt[i]
#
#
# # fig, axs = plt.subplots(2, 1, figsize=(16, 9), sharex=True)
# #
# # axs[0].set_ylabel("Speed (m/s)", fontsize=fontsize_labels)
# # # axs[0].plot(data['time'], data['positionD_smoothed'],
# # #             'g', markersize=12, label='Velocity')
# # axs[0].plot(data['time'], data['positionD'],
# #             'g', markersize=12, label='Velocity')
# # axs[0].plot(time, v,
# #             'orange', markersize=12, label='Velocity modeled')
# # axs[0].tick_params(axis='both', which='major', labelsize=fontsize_ticks)
# #
# # axs[1].set_ylabel("Motor Input Q (-)", fontsize=fontsize_labels)
# # axs[1].plot(data['time'], data['Q'],
# #             'r', markersize=12, label='Q')
# # axs[1].tick_params(axis='both', which='major', labelsize=fontsize_ticks)
# #
# # axs[1].set_xlabel('Time (s)', fontsize=fontsize_labels)
# #
# # fig.align_ylabels()
# #
# # plt.tight_layout()
# #
# # plt.show()