<jupyter_start><jupyter_text>BobbleBot Controller Analysis Report  Turning Control  <NAME>  11/11/18    IntroductionThis document outlines an analysis of BobbleBot controller performance as different gains are selected. At first, the BobbleBot simulator is used to capture the data and characterize the controller performance. BLAH BLAH TODO  Loading BobbleBot Simulation Data The BobbleBot simulator runs in Gazebo. Using the gazebo-ros packages, one can log data as the simulator runs and store it in a ros bag format. The simulation data can then be analyzed with Python using [Pandas](https://pandas.pydata.org/). This article discusses how to [load ROS bag files into Pandas](https://nimbus.unl.edu/2014/11/using-rosbag_pandas-to-analyze-rosbag-files/).<jupyter_code># Load anaylsis environment file. This file defines data directories
# and imports all needed Python packages for this notebook.
exec(open("env.py").read())
df.keys()<jupyter_output><empty_output><jupyter_text>Load Plot Config<jupyter_code># Set paths to relevant data directories and config files
tilt_tests_plot_config = os.path.abspath(os.path.join(os.getcwd(),'plots.yaml'))
# Load configs and data
pc = parse_config_file(tilt_tests_plot_config)<jupyter_output>Loading configs from file : /home/mike/Work/SOE/BobbleBot/src/analysis_tools/notebooks/TurningControlAnalysis/plots.yaml<jupyter_text>Pre-ProcessingThis section applies any and all pre-processing steps needed on the data set. Examples include trimming the data, removing bias, filtering, and applying transforms.<jupyter_code>max_time = 20.0
runs = ['turning_test', 'response_hw_test']
for run in runs:
    df[run] = df[run][df[run].index < max_time]<jupyter_output><empty_output><jupyter_text>Print sim data in tabular formAll the sim data was loaded when the analysis env file was sourced. We can get the data for a run in tabular form like so.<jupyter_code>n_rows = 5
df['turning_test'].head(n_rows)<jupyter_output><empty_output><jupyter_text>Search for a columnHere's how to search for a column(s) in a data frame.<jupyter_code>search_string = 'Vel'
found_data = df['turning_test'].filter(regex=search_string)
found_data.head()<jupyter_output><empty_output><jupyter_text>BobbleBot does not track its x,y world position internally because it is not needed for the controller. For this analysis, we want to look at some commanded trajectories in world coordinates. To do this, we use a kinematic model for a differential drive robot in order to compute translational and rotational state. Differential Drive Model      The Python code below uses the differential drive model to compute x, y position and velocity in world co-ordinates.<jupyter_code>import math
wheel_radius = 0.05
wheel_base_len = 0.2286 # 9in in m

def compute_auxiliary_terms(df):
    #left_motor_vel = df['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity']
    #right_motor_vel = df['bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity']
    #turn_rate = (wheel_radius/wheel_base_len)*(right_motor_vel - left_motor_vel)
    #heading = scipy.integrate.cumtrapz(y=turn_rate.values, x=turn_rate.index)
    #heading = np.append(heading, heading[-1])
    turn_rate = df['bobble_bobble_balance_controller_bb_controller_status__TurnRate']
    heading = scipy.integrate.cumtrapz(y=turn_rate.values, x=turn_rate.index)
    heading = np.append(heading, heading[-1])
    fwd_vel = df['bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity']
    
    x_vel = fwd_vel * np.cos(heading*np.pi/180.0)
    y_vel = fwd_vel * np.sin(heading*np.pi/180.0)
    
    x_pos = scipy.integrate.cumtrapz(y=x_vel.values, x=x_vel.index)
    x_pos = np.append(x_pos, x_pos[-1])
    y_pos = scipy.integrate.cumtrapz(y=y_vel.values, x=y_vel.index)
    y_pos = np.append(y_pos, y_pos[-1])
    
    desired_fwd_vel = df['bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity']
    desired_turn_rate = df['bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate']
    desired_heading = scipy.integrate.cumtrapz(y=desired_turn_rate.values, x=desired_turn_rate.index)
    desired_heading = np.append(desired_heading, desired_heading[-1])
    desired_x_vel = desired_fwd_vel * np.cos(desired_heading*np.pi/180.0)
    desired_y_vel = desired_fwd_vel * np.sin(desired_heading*np.pi/180.0)
    desired_x_pos = scipy.integrate.cumtrapz(y=desired_x_vel.values, x=desired_x_vel.index)
    desired_x_pos = np.append(desired_x_pos, desired_x_pos[-1])
    desired_y_pos = scipy.integrate.cumtrapz(y=desired_y_vel.values, x=desired_y_vel.index)
    desired_y_pos = np.append(desired_y_pos, desired_y_pos[-1])

    df['bobble_bobble_balance_controller_bb_controller_status__Heading'] = heading
    df['DerivedHeading'] = heading
    df['DesiredHeading'] = desired_heading
    df['DesiredXPos'] = desired_x_pos
    df['DesiredYPos'] = desired_y_pos
    df['DerivedXPos'] = x_pos
    df['DerivedYPos'] = y_pos

for run in runs:
   compute_auxiliary_terms(df[run])<jupyter_output><empty_output><jupyter_text>Plot TrajectoriesFirst define common plot configuration objects<jupyter_code>kp_legend = {
    'Kp 0.1': {
        'color': 'orange',
        'linestyle': '-'
    },
    'Kp 0.15': {
        'color': 'blue',
        'linestyle': '-'
    },
    'Kp 0.2': {
        'color': 'red',
        'linestyle': '-'
    },
}
ki_legend = {
    'Ki 0.0005': {
        'color': 'orange',
        'linestyle': '-'
    },
    'Ki 0.001': {
        'color': 'blue',
        'linestyle': '-'
    },
    'Ki 0.005': {
        'color': 'red',
        'linestyle': '-'
    },
}<jupyter_output><empty_output><jupyter_text>Velocity Plot<jupyter_code>%matplotlib inline
cfg = pc['velocity'].copy()
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'VelocityVsTime')<jupyter_output><empty_output><jupyter_text>Tilt Plot<jupyter_code>%matplotlib inline
cfg = pc['measured_tilt'].copy()
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'TiltVsTime')<jupyter_output><empty_output><jupyter_text>Turn Rate<jupyter_code>%matplotlib inline
cfg = pc['turn_rate'].copy()
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'TurnRateVsTime')<jupyter_output><empty_output><jupyter_text>Heading Plot Kp Runs<jupyter_code>%matplotlib inline
cfg = pc['heading'].copy()
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'DerivedHeadingVsTime')<jupyter_output><empty_output><jupyter_text>Desired Turn Rate vs Actual<jupyter_code>%matplotlib inline
cfg = pc['turning_control'].copy()
cfg['runs'] = runs
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'TurningControl', plot_func=desired_vs_actual_for_runs)<jupyter_output><empty_output><jupyter_text>Desired Heading vs Actual<jupyter_code>%matplotlib inline
cfg = pc['heading_control'].copy()
cfg['runs'] = runs
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'HeadingControl', plot_func=desired_vs_actual_for_runs)<jupyter_output><empty_output><jupyter_text>Position Plot Kp Runs<jupyter_code>%matplotlib inline
cfg = pc['position_derived'].copy()
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [-0.2, 0.2]
cfg['ylim'] = [-0.2, 0.2]
make_static_plot(df, cfg, 'DerivedPositionVsTime')<jupyter_output><empty_output><jupyter_text>Final PlotsThis section contains just the finalized summary plots that capture the primary result of the analysis.<jupyter_code>%matplotlib inline
cfg = pc['turning_control'].copy()
cfg['title'] = 'Simulated Turning Control'
cfg['runs'] = runs
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'TurningControl', plot_func=desired_vs_actual_for_runs)
%matplotlib inline
cfg = pc['heading_control'].copy()
cfg['title'] = 'Simulated Turning Control (Heading Response)'
cfg['runs'] = runs
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'HeadingControl', plot_func=desired_vs_actual_for_runs)
%matplotlib inline
cfg = pc['measured_tilt'].copy()
cfg['title'] = 'Simulated Turning Control (Tilt Response)'
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'TiltVsTime')
%matplotlib inline
cfg = pc['velocity'].copy()
cfg['title'] = 'Simulated Turning Control (Velocity Response)'
cfg['runs'] = runs
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 20.0]
make_static_plot(df, cfg, 'VelocityVsTime')<jupyter_output><empty_output><jupyter_text>HW Response<jupyter_code>%matplotlib inline
cfg = pc['turning_control'].copy()
cfg['title'] = 'HW Turning Test - Turning Control'
cfg['runs'] = ['response_hw_test']
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'TurningControl', plot_func=desired_vs_actual_for_runs)
%matplotlib inline
cfg = pc['heading_control'].copy()
cfg['title'] = 'HW Turning Test - Heading Response'
cfg['runs'] = ['response_hw_test']
cfg['xlim'] = [0, 20]
make_static_plot(df, cfg, 'HeadingControl', plot_func=desired_vs_actual_for_runs)
%matplotlib inline
cfg = pc['velocity'].copy()
cfg['title'] = 'HW Turning Test - Velocity Response'
cfg['runs'] = ['response_hw_test']
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 8.0]
make_static_plot(df, cfg, 'HwVelocityResponse')
%matplotlib inline
cfg = pc['measured_tilt'].copy()
cfg['title'] = 'HW Turning Test - Tilt Response'
cfg['runs'] = ['response_hw_test']
cfg['colors'] = ['red']
cfg['xlim'] = [0.0, 8.0]
make_static_plot(df, cfg, 'HwTiltResponse')
%matplotlib inline
cfg = pc['position_derived'].copy()
cfg['title'] = 'HW Turning Test - World X,Y Position'
cfg['runs'] = ['response_hw_test']
cfg['colors'] = ['red']
cfg['legend'] = {
    'Bobble-Bot Position': {
        'color': 'red',
        'linestyle': '-'
    },
}
cfg['legend_loc'] = 'upper right'
cfg['x_label'] = 'Position-X (m)'
cfg['y_label'] = 'Position-Y (m)'
cfg['xlim'] = [-0.5, 0.5]
cfg['ylim'] = [-0.5, 0.5]
make_static_plot(df, cfg, 'HwPositionResponse')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.14108885981684613, 'fraction_numerical': 0.014088759182851967, 'mean_word_length': 5.9545136459062284, 'pattern_counts': {'":': 0, '<': 66, '<?xml version=': 0, '>': 65, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '16', 'dir': 'jupyter-structured-clean-dedup', 'id': '258625', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 3724, 'n_tokens_neox': 3566, 'n_words': 744, 'path': 'analysis/notebooks/TurningControl/TurningControlAnalysis.ipynb', 'repo_name': 'jyothiswaroopkota/bobble_controllers', 'size': '9941'}