cogmap_analyze.py

#!/usr/bin/env python3

import os
import glob
from tqdm import tqdm
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider


cogmap_folder: str = 'cogmaps'
cogmaps = glob.glob(cogmap_folder + '/' + "*intgain.csv")
#velmaps = glob.glob(cogmap_folder + '/' + "*veldata.csv")

cogmap_dict = {}

dirs = []
velgains = []
endspeeds = []
intgains = []


def lookup(dir, velgain, endspeed, intgain):
    return f"{dir}_{velgain}_{endspeed}_{intgain}"


for cogmap_file in tqdm(cogmaps):
    #velmap_file = [i for i in velmaps if i.split('intgain')[0] == cogmap_file.split('intgain')[0]]
    #assert len(velmap_file) == 1
    #velmap_file = velmap_file[0]
    with open(cogmap_file, 'r') as f:
        cogmap_txt = f.read()
    #with open(velmap_file, 'r') as f:
    #    velmap_txt = f.read()

    file_parse = cogmap_file.split('_')
    assert file_parse[1] == 'neg' or file_parse[1] == 'pos'
    dir = 1 if file_parse[1] == 'pos' else -1
    velgain = float(file_parse[2].split('velgain')[0])
    endspeed = float(file_parse[3].split('endspeed')[0])
    intgain = float(file_parse[4].split('intgain')[0])

    if dir not in dirs:
        dirs.append(dir)
    if velgain not in velgains:
        velgains.append(velgain)
    if endspeed not in endspeeds:
        endspeeds.append(endspeed)
    if intgain not in intgains:
        intgains.append(intgain)

    cogmap_parsed = [[float(k) for k in n.split(',')] for n in cogmap_txt.split('\n')[1:]]
    cogmap_dict[lookup(dir, velgain, endspeed, intgain)] = np.asarray(cogmap_parsed)

dirs =      np.asarray(sorted(dirs))
velgains =  np.asarray(sorted(velgains))
endspeeds = np.asarray(sorted(endspeeds))
intgains =  np.asarray(sorted(intgains))


# Create the figure and the line that we will manipulate
fig, ax = plt.subplots(figsize=(8, 6))
ax.set_ylim(-0.3, 0.3)
plt.subplots_adjust(bottom=0.25)

l, = ax.plot([], [], lw=2)

# Setting the axes
ax_slider1 = plt.axes([0.1, 0.01, 0.65, 0.03])
ax_slider2 = plt.axes([0.1, 0.05, 0.65, 0.03])
ax_slider3 = plt.axes([0.1, 0.09, 0.65, 0.03])
ax_slider4 = plt.axes([0.1, 0.13, 0.65, 0.03])

s_dir = Slider(ax_slider1, f'Dirs: {dirs[0]}', 1, len(dirs), valinit=1, valstep=1)
s_velgain = Slider(ax_slider2, f'Velgain: {velgains[0]}', 1, len(velgains), valinit=1, valstep=1)
s_endspeed = Slider(ax_slider3, f'Endspeed: {endspeeds[0]}', 1, len(endspeeds), valinit=1, valstep=1)
s_intgain = Slider(ax_slider4, f'Intgain: {intgains[0]}', 1, len(intgains), valinit=1, valstep=1)


def update(val):
    dir_val = dirs[int(s_dir.val) - 1]
    velgain_val = velgains[int(s_velgain.val) - 1]
    endspeed_val = endspeeds[int(s_endspeed.val) - 1]
    intgain_val = intgains[int(s_intgain.val) - 1]
    
    s_dir.label.set_text(f'Dirs: {dir_val}')
    s_velgain.label.set_text(f'Velgain: {velgain_val}')
    s_endspeed.label.set_text(f'Endspeed: {endspeed_val}')
    s_intgain.label.set_text(f'Intgain: {intgain_val}')

    key = lookup(dir_val, velgain_val, endspeed_val, intgain_val)
    data = cogmap_dict.get(key, np.zeros((0, 2)))
    l.set_xdata(data[:, 0])
    l.set_ydata(data[:, 1])
    ax.relim()
    ax.autoscale_view()
    fig.canvas.draw_idle()


s_dir.on_changed(update)
s_velgain.on_changed(update)
s_endspeed.on_changed(update)
s_intgain.on_changed(update)

plt.show()

cogmap_capture.py

#!/usr/bin/env python3

from typing import List

import odrive
from odrive.enums import *
import pickle as pkl
from tqdm import tqdm, trange
import time
import os
import itertools

import time
import pandas as pd
import numpy as np
from odrive.enums import AxisState, ProcedureResult, ControlMode, InputMode

# Control variables:
# - End speed (0.01 to 0.2)
# - Vel gain (0.2 to 1)
# - Sign (negative or positive)
# - Anticog integrator gain

# - Get+save cogmap

default_calib_start_vel = 0.5
anticog_max_torque = 0.5
anticog_integrator_gain = 10000

cogmap_folder = 'cogmaps'


def capture_cogmap(odrv, sign: int, vel_gain: float, end_speed: float, integrator_gain: float):
    filename = get_cogmap_filename(sign, vel_gain, end_speed, integrator_gain)
    if os.path.exists(filename):
        print(f"Cogmap {filename} already exists, skipping...")
        return

    print(
        f"Taking cogmap with a velocity sign of {sign}, vel_gain of {vel_gain}, end_speed of {end_speed}, integrator_gain of {integrator_gain}")

    # Setup dataframe
    df = pd.DataFrame(columns=['t', 'vel'])

    odrv.axis0.requested_state = AxisState.IDLE
    time.sleep(1)

    assert (sign == -1 or sign == 1)
    anticog_handle = odrv.axis0.config.anticogging
    t_total = int(anticog_handle.calib_coarse_tuning_duration + anticog_handle.calib_fine_tuning_duration)
    anticog_handle.calib_start_vel = sign * default_calib_start_vel
    anticog_handle.calib_end_vel = sign * end_speed
    anticog_handle.max_torque = anticog_max_torque
    anticog_handle.calib_coarse_integrator_gain = integrator_gain
    anticog_handle.calib_bidirectional = False

    odrv.axis0.controller.config.vel_gain = vel_gain

    start_time = time.monotonic()
    odrv.axis0.requested_state = AxisState.ANTICOGGING_CALIBRATION

    while odrv.axis0.requested_state == AxisState.ANTICOGGING_CALIBRATION or odrv.axis0.current_state == AxisState.ANTICOGGING_CALIBRATION:
        df.loc[len(df)] = [(time.monotonic() - start_time), odrv.axis0.pos_vel_mapper.vel]

    odrv.axis0.requested_state = AxisState.IDLE

    df.set_index('t', inplace=True)

    cogmap = []
    for i in range(1024):
        cogmap.append(odrv.axis0.config.anticogging.get_map(i))
    file_header = 'n,cogmap\n'
    file_data = ''.join([f"{i},{cogmap[i]}\n" for i in range(len(cogmap))])[:-1]
    file_str = file_header + file_data

    if odrv.axis0.procedure_result != ProcedureResult.SUCCESS:
        filename = filename[:filename.rfind('.')] + f'_FAILED_{odrv.axis0.procedure_result}.csv'

    filename_pd = filename[:filename.rfind('.')] + '_veldata.csv'
    pd_csv = df.to_csv(index=False)

    with open(filename, 'w') as f:
        f.write(file_str)
    with open(filename_pd, 'w') as f:
        f.write(pd_csv)


def get_cogmap_filename(sign: int, vel_gain: float, end_speed: float, integrator_gain: float):
    filename = f"cogmap_{'pos' if sign == 1 else 'neg'}_{vel_gain:.3f}velgain_{end_speed:.3f}endspeed_{integrator_gain:.3f}intgain.txt"
    return cogmap_folder + '/' + filename


def sweep_cogmaps(odrv,
                  vel_gains: List[float], end_speeds: List[float],
                  vel_integrators: List[float], directions: List[float]):
    permutations = [i for i in itertools.product(
        *[directions, vel_gains, end_speeds, vel_integrators]
    )]

    n_permutations = len(permutations)

    print(f"{n_permutations} total samples. Estimated completion: {n_permutations * 3 / 60} hours")

    try:
        for n in range(n_permutations):
            t_est_minutes = (n_permutations - n) * 3
            t_est_hours = t_est_minutes // 60
            t_est_minutes -= t_est_hours * 60
            print(f"{n}/{n_permutations}: est completion {t_est_hours}h {t_est_minutes}m")
            dir = permutations[n][0]
            vel_gain = permutations[n][1]
            end_speed = permutations[n][2]
            integrator_gain = permutations[n][3]
            capture_cogmap(odrv, dir, vel_gain, end_speed, integrator_gain)
    finally:
        odrv.axis0.requested_state = AxisState.IDLE


if __name__ == '__main__':
    odrv = odrive.find_any()

    #vel_gains = [0.50, 0.6, 0.7, 0.8, 0.9, 1]
    vel_gains = [0.8]
    end_speeds = [0.01, 0.025, 0.05, 0.075, 0.1, 0.125, 0.15, 0.175, 0.2, 0.225, 0.25]
    directions = [-1, 1]
    integrator_gains = [1000, 2500, 5000, 7500, 10000, 12500, 15000, 17500, 20000, 22500, 25000]
    sweep_cogmaps(odrv, vel_gains, end_speeds, integrator_gains, directions)