diff --git a/.gitignore b/.gitignore index d4fabeb1..753f9079 100644 --- a/.gitignore +++ b/.gitignore @@ -65,3 +65,9 @@ controller.py Dator/db.sqlite3 workspace/src/barc/rosbag/ *.ropeproject/ +.DS_Store + +# recordings and images +images/ +recordings/ +predictions/ diff --git a/workspace/src/barc_gui/src/barc_gui/__init__.py b/.gitmodules similarity index 100% rename from workspace/src/barc_gui/src/barc_gui/__init__.py rename to .gitmodules diff --git a/README.md b/README.md index 862ae6dc..4acad3ba 100644 --- a/README.md +++ b/README.md @@ -1,63 +1,52 @@ -# Berkeley Autonomous Race Car (barc) Repo +# Learning Model Predictive Control (LMPC) for Multi-Agent Competitive Autonomous Car Racing -The Berkeley Autonomous Race Car is a development platform for autonomous driving to achieve complex maneuvers such as drifting, lane changes, and obstacle avoidance. A 1/10 scale RC car and an embedded Linux computer make up the hardware platform of the project. This project aims to be fully open-source. The data collection process is cloud-based and brings new dimensions to the Vehicle Dynamics and Control Theory teaching and research world. +This repository implements learning model predictive controllers for racing multiple agents competitively on a given race track in simulation or on the Berkeley Autonomus Race Car platform (BARC). -This site is home to the repository. The main site for the project is [here](http://www.barc-project.com/). - -## Organization -The primary folders in this repository include -* docs - * Overview about the mechanical, electrical, and software deign of the vehicle. Descriptions of the vehicle models used for some control algorithms -* CAD - * DWX files for fabricating the deck and side brackets for the chassis, and STL files for fabricating the sensor mounts (e.g. hall effect sensor, camera, ultrasound) and the cover for the odroid. -* Dator - * Web server for cloud robotics. Provides a standard way to record data and events from one or more local computers for later analysis. Based on [this repo](https://github.com/bwootton/Dator) from Bruce Wooton -* arduino - * Files to program the arduino to (1) send commands to the electronic speed control (ESC) unit and the servo, and to (2) acquire measurements from the encoders and ultrasound sensors -* scripts - * Bash programs that set up environment variables and launch the local server upon boot -* MATLAB - * Useful MATLAB scripts for processing ROS bag file. The bag files store all the message data (time stamped sensor measurements, actuator commands, etc) during an experiment -* workspace - * **Robotic Operating System (ROS)** workspace that contains the barc package. This package holds the source code to control the vehicle using the ROS framework - -All software to control the vehicle resides in the *Arduino* and *Workspace* folders. - - -## Getting started -1. Get the parts, [list](https://github.com/BARCproject/barc/blob/master/docs/BillofMaterials.md) -2. Flash the odroid, [instructions](https://github.com/BARCproject/barc/blob/master/docs/FlashingEMMC.md) -3. Assemble the car, [instructions](https://docs.google.com/document/d/1T8O4JhUlw09ALUGPSX7DlSO7Hc7vcKl_ahBeHncMguE/edit?usp=sharing) -4. Charge the battery, [instructions](https://github.com/BARCproject/barc/blob/master/docs/ChargingBattery.md) -5. Connect to the RC remotely, [instructions](https://github.com/BARCproject/barc/blob/master/docs/ConnectingToOdroid.md) - -You can also scroll through the material under our docs section to find other useful information +## Installing dependencies +After installing python and ROS, go ahead an install julia version 0.4. This package has only been tested using ROS kinetic and julia version 0.4.7. Older julia versions can be found [here](https://julialang.org/downloads/oldreleases.html). This package requires a couple of packages, which can be installed with the following command: +``` +julia /dependencies.jl +``` -## Useful resources +### Troubleshooting +Build Errors in the Gtk-package while installing the ProfileView-package can be resolved by running the following line in a separate terminal +``` +sudo apt-get install libgtk-3-dev +``` +followed by +``` +Pkg.build("Gtk") +``` +in the julia shell. -Recommended reading and resources -+ [A Gentle Introduction to ROS](https://cse.sc.edu/~jokane/agitr/), by Jason M. O'Kane, and this [basic tutorial](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29) for writing ROS nodes in python -+ [Julia JuMP](https://jump.readthedocs.io/en/latest/), Mathematical Programming using the Julia programming language -+ [Git - the simple guide](http://rogerdudler.github.io/git-guide/), or for a more in-depth reading, consider [this](https://www.atlassian.com/git/tutorials/ ) tutorial by Atlassian -+ [Electronic Speed Control (ESC) manual](http://propeleris.lt/failai/wp-s10c-rtr_manual.pdf), which details how to calibrate and program the ESC -+ [Open source ESC](http://vedder.se/2015/01/vesc-open-source-esc/), for building your own ESC -+ [Pulse Wide Modulation (PWM)](http://www.egr.msu.edu/classes/ece480/capstone/fall14/group07/PDFs/PWM_Application_Note.pdf), notes from Michigan State ECE480 class, explaining the concept of PWM and its usage in an arduino. -+ [Arduino Servo Library](http://makezine.com/2014/04/23/arduinos-servo-library-angles-microseconds-and-optional-command-parameters/), article by Make magazine, clarifying the usage of some functions within the library -+ [Mass moment of inertia calculation](http://www.mathworks.com/company/newsletters/articles/improving-mass-moment-of-inertia-measurements.html), a MATLAB newsletter explaining how to estimate the mass moment of inertia -+ [Computer Vision lecture series](https://www.youtube.com/watch?v=N_a6IP6KUSE), from Professor Peter Corke aat Queensland University of Technology +## Configuration +### Add HOME_DIR environment variable +Create the environment variable HOME_DIR, which is a string which points to your home directory/the directory which includes the barc folder. Create a julia configuration file, if you do not have one already: +``` +cd +vim .juliarc.jl +``` +and add the following line: +``` +ENV["HOME_DIR"] = +``` -## Student projects -+ [Lane Keeping and Obstacle Avoidance](https://github.com/ych09041/me131lane), with [ demo](https://www.youtube.com/watch?v=5HKu7AaSsoM), by Tony Abdo, Hohyun Song, Cheng Hao Yuan, UC Berkeley ME 131, Spring 2016 +### Config file +The config.yaml file allows for multiple different settings. +* mode: One of ["path_following", "learning", "racing"] +* initialization: One of ["inner", "center", "outer"] +* track_name: One of ["oval", "l_shape"] -## Potential issues -### Bad IMU magnetometer readings -The magnetic field around car, perhaps from the motor or aluminum deck, may interfere with the magnetometer readings, meaning the roll, pitch, yaw measurements may be off +### Launch file +The parameters delay_df and delay_a specifiy the delay for the steering and the acceleration in the controller and in the estimator. For the controller they are given in time steps (value="1") and for the estimator in seconds (value="0.1"). The parameters time_offset specify how long after the initialization the agent should wait before applying control inputs. This allows the two agents to start at different times. -### Wrong internal clock time / date -The time and date settings on the odroid may be incorrect. This may be because the [Real Time Clock (RTC) battery](http://www.hardkernel.com/main/products/prdt_info.php?g_code=G137508214939) is not connected or has a loose connection. To update the date / time settings, ensure the RTC battery is firmly connected, and ensure you are connected to the Internet via wifi or ethernet. Next, open a terminal and run the following network time protocol (ntp) commands +## Running the code +### Simulation Agent +To run agent 1 in simulation execute the following command: +``` +roslaunch barc sim_agent_1.launch +``` +For path following MPC set MODE to "path_following" and for learning set MODE to "learning". This runs the agent on the track and records the data. Notice that you have to run the command first in the path following mode to collect data for LMPC. To run agent 2 in simulation run the following command: ``` -sudo service ntp stop -sudo ntpdate -s time.nist.gov -sudo service ntp start +roslaunch barc sim_agent_2.launch ``` -If you have a "no server suitable for synchronization found", your hosting provider may be blocking ntp packets. Refer to [this](http://askubuntu.com/questions/429306/ntpdate-no-server-suitable-for-synchronization-found) community question for more information diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index c5a70d16..c2dfeb2d 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -21,12 +21,13 @@ WARNING: // include libraries #include -#include -#include +//#include +#include #include #include -#include "Maxbotix.h" #include +#include +#include /************************************************************************** CAR CLASS DEFINITION (would like to refactor into car.cpp and car.h but can't figure out arduino build process so far) @@ -48,6 +49,10 @@ class Car { int getEncoderFR(); int getEncoderBL(); int getEncoderBR(); + unsigned long getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) // Interrupt service routines void incFR(); void incFL(); @@ -55,6 +60,12 @@ class Car { void incBL(); void calcThrottle(); void calcSteering(); + float getVelocityEstimate(); + float vel_FL = 0; + float vel_FR = 0; + float vel_BL = 0; + float vel_BR = 0; + private: // Pin assignments const int ENC_FL_PIN = 2; @@ -68,13 +79,13 @@ class Car { // Car properties // unclear what this is for - const int noAction = 0; + const float noAction = 0.0; // Motor limits // TODO are these the real limits? - const int MOTOR_MAX = 120; - const int MOTOR_MIN = 40; - const int MOTOR_NEUTRAL = 90; + const float MOTOR_MAX = 150.0; + const float MOTOR_MIN = 40.0; + const float MOTOR_NEUTRAL = 90.0; // Optional: smaller values for testing safety /* const int MOTOR_MAX = 100; */ /* const int MOTOR_MIN = 75; */ @@ -84,10 +95,10 @@ class Car { // 120] judging from the sound of the servo pushing beyond a mechanical limit // outside that range. The offset may be 2 or 3 deg and the d_theta_max is then // ~31. - const int D_THETA_MAX = 30; - const int THETA_CENTER = 90; - const int THETA_MAX = THETA_CENTER + D_THETA_MAX; - const int THETA_MIN = THETA_CENTER - D_THETA_MAX; + const float D_THETA_MAX = 30.0; + const float THETA_CENTER = 90.0; + const float THETA_MAX = THETA_CENTER + D_THETA_MAX; + const float THETA_MIN = THETA_CENTER - D_THETA_MAX; // Interfaces to motor and steering actuators Servo motor; @@ -122,12 +133,33 @@ class Car { int BL_count = 0; int BR_count = 0; + // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) + // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + + volatile unsigned long FL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + + unsigned long FL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long FR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + + // Utility functions uint8_t microseconds2PWM(uint16_t microseconds); float saturateMotor(float x); float saturateServo(float x); }; +// Boolean keeping track of whether the Arduino has received a signal from the ECU recently +int received_ecu_signal = 0; + // Initialize an instance of the Car class as car Car car; @@ -137,6 +169,7 @@ Car car; // figure it out, please atone for my sins. void ecuCallback(const barc::ECU& ecu) { car.writeToActuators(ecu); + received_ecu_signal = 1; } void incFLCallback() { car.incFL(); @@ -160,29 +193,18 @@ void calcThrottleCallback() { // Variables for time step volatile unsigned long dt; volatile unsigned long t0; +volatile unsigned long ecu_t0; // Global message variables // Encoder, RC Inputs, Electronic Control Unit, Ultrasound barc::ECU ecu; -barc::ECU rc_inputs; -barc::Encoder encoder; -barc::Ultrasound ultrasound; +barc::Vel_est vel_est; + ros::NodeHandle nh; -ros::Publisher pub_encoder("encoder", &encoder); -ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); -ros::Publisher pub_ultrasound("ultrasound", &ultrasound); ros::Subscriber sub_ecu("ecu_pwm", ecuCallback); - - -// Set up ultrasound sensors -/* -Maxbotix us_fr(14, Maxbotix::PW, Maxbotix::LV); // front -Maxbotix us_bk(15, Maxbotix::PW, Maxbotix::LV); // back -Maxbotix us_rt(16, Maxbotix::PW, Maxbotix::LV); // right -Maxbotix us_lt(17, Maxbotix::PW, Maxbotix::LV); // left -*/ +ros::Publisher pub_vel_est("vel_est", &vel_est); // vel est publisher /************************************************************************** ARDUINO INITIALIZATION @@ -191,21 +213,27 @@ void setup() { // Set up encoders, rc input, and actuators car.initEncoders(); - car.initRCInput(); + //car.initRCInput(); car.initActuators(); // Start ROS node nh.initNode(); // Publish and subscribe to topics - nh.advertise(pub_encoder); - nh.advertise(pub_rc_inputs); - nh.advertise(pub_ultrasound); +// nh.advertise(pub_encoder); + //nh.advertise(pub_encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_rc_inputs); + //nh.advertise(pub_ultrasound); + nh.advertise(pub_vel_est); nh.subscribe(sub_ecu); // Arming ESC, 1 sec delay for arming and ROS car.armActuators(); t0 = millis(); + ecu_t0 = millis(); } @@ -217,29 +245,28 @@ void loop() { // compute time elapsed (in ms) dt = millis() - t0; + // kill the motor if there is no ECU signal within the last 1s + if( (millis() - ecu_t0) >= 200){ + if(!received_ecu_signal){ + car.killMotor(); + } else{ + received_ecu_signal = 0; + } + ecu_t0 = millis(); + } + if (dt > 50) { car.readAndCopyInputs(); - // TODO make encoder and rc_inputs private properties on car? Then - // car.publishEncoder(); and car.publishRCInputs(); - encoder.FL = car.getEncoderFL(); - encoder.FR = car.getEncoderFR(); - encoder.BL = car.getEncoderBL(); - encoder.BR = car.getEncoderBR(); - pub_encoder.publish(&encoder); - - rc_inputs.motor = car.getRCThrottle(); - rc_inputs.servo = car.getRCSteering(); - pub_rc_inputs.publish(&rc_inputs); - - // publish ultra-sound measurement - /* - ultrasound.front = us_fr.getRange(); - ultrasound.back = us_bk.getRange(); - ultrasound.right = us_rt.getRange(); - ultrasound.left = us_lt.getRange(); - pub_ultrasound.publish(&ultrasound); - */ + vel_est.vel_est = car.getVelocityEstimate(); + vel_est.vel_fl = car.vel_FL; + vel_est.vel_fr = car.vel_FR; + vel_est.vel_bl = car.vel_BL; + vel_est.vel_br = car.vel_BR; + vel_est.header.stamp = nh.now(); + pub_vel_est.publish(&vel_est); // publish estimated velocity + ////////////////////////////////////////////////!!!! + t0 = millis(); } @@ -279,10 +306,10 @@ void Car::initEncoders() { pinMode(ENC_FL_PIN, INPUT_PULLUP); pinMode(ENC_BR_PIN, INPUT_PULLUP); pinMode(ENC_BL_PIN, INPUT_PULLUP); - enableInterrupt(ENC_FR_PIN, incFRCallback, CHANGE); - enableInterrupt(ENC_FL_PIN, incFLCallback, CHANGE); - enableInterrupt(ENC_BR_PIN, incBRCallback, CHANGE); - enableInterrupt(ENC_BL_PIN, incBLCallback, CHANGE); + enableInterrupt(ENC_FR_PIN, incFRCallback, FALLING); //enables interrupts from Pin ENC_FR_PIN, when signal changes (CHANGE). And it call the function 'incFRCallback' + enableInterrupt(ENC_FL_PIN, incFLCallback, FALLING); + enableInterrupt(ENC_BR_PIN, incBRCallback, FALLING); + enableInterrupt(ENC_BL_PIN, incBLCallback, FALLING); } void Car::initRCInput() { @@ -306,7 +333,7 @@ void Car::armActuators() { void Car::writeToActuators(const barc::ECU& ecu) { float motorCMD = saturateMotor(ecu.motor); float servoCMD = saturateServo(ecu.servo); - motor.write(motorCMD); + motor.writeMicroseconds( (uint16_t) (1500 + (motorCMD-90.0)*1000.0/180.0)) steering.write(servoCMD); } @@ -339,21 +366,29 @@ void Car::calcSteering() { void Car::incFL() { FL_count_shared++; + FL_old_time = FL_new_time; //(ADDED BY TOMMI 7JULY2016) + FL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FL_FLAG; } void Car::incFR() { FR_count_shared++; + FR_old_time = FR_new_time; //(ADDED BY TOMMI 7JULY2016) + FR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FR_FLAG; } void Car::incBL() { BL_count_shared++; + BL_old_time = BL_new_time; //(ADDED BY TOMMI 7JULY2016) + BL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BL_FLAG; } void Car::incBR() { BR_count_shared++; + BR_old_time = BR_new_time; //(ADDED BY TOMMI 7JULY2016) + BR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BR_FLAG; } @@ -375,15 +410,19 @@ void Car::readAndCopyInputs() { } if(updateFlags & FL_FLAG) { FL_count = FL_count_shared; + FL_DeltaTime = FL_new_time - FL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & FR_FLAG) { FR_count = FR_count_shared; + FR_DeltaTime = FR_new_time - FR_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BL_FLAG) { BL_count = BL_count_shared; + BL_DeltaTime = BL_new_time - BL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BR_FLAG) { BR_count = BR_count_shared; + BR_DeltaTime = BR_new_time - BR_old_time; //(ADDED BY TOMMI 7JULY2016) } // clear shared update flags and turn interrupts back on updateFlagsShared = 0; @@ -410,3 +449,36 @@ int Car::getEncoderBL() { int Car::getEncoderBR() { return BR_count; } + +unsigned long Car::getEncoder_dTime_FL() { //(ADDED BY TOMMI 7JULY2016) + return FL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_FR() { //(ADDED BY TOMMI 7JULY2016) + return FR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016)+ +unsigned long Car::getEncoder_dTime_BL() { //(ADDED BY TOMMI 7JULY2016) + return BL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_BR() { //(ADDED BY TOMMI 7JULY2016) + return BR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) + +float Car::getVelocityEstimate() { + vel_FL = 0.0; + vel_FR = 0.0; + vel_BL = 0.0; + vel_BR = 0.0; + if(FL_DeltaTime > 0){ + vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime*1000000.0; + } + if(FR_DeltaTime > 0){ + vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime*1000000.0; + } + if(BL_DeltaTime > 0){ + vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime*1000000.0; + } + if(BR_DeltaTime > 0){ + vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime*1000000.0; + } + return ( vel_FL + vel_FR ) / 2.0; +} diff --git a/dependecies.jl b/dependecies.jl new file mode 100644 index 00000000..5c3e9d25 --- /dev/null +++ b/dependecies.jl @@ -0,0 +1,48 @@ +#!/usr/bin/env julia + +#= + File name: dependencies.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + + +# Check julia version +if string(VERSION)[1:3] != "0.4" + error("Julia version 0.4.x is needed. Please install a different julia + version.") +end + +# Install required packages +# Enable the use of python in julia +Pkg.add("PyCall") +# Enable loading and saving hdf5 files +Pkg.add("HDF5") +# Enable loading and saving variables in JLD-format +Pkg.add("JLD") +Pkg.pin("JLD", v"0.6.10") +# Enable visualization of profiling data +Pkg.add("ProfileView") +# Add Ipopt interface to julia +Pkg.add("Ipopt") +# Add JuMP for mathematical optimization in julia +Pkg.add("JuMP") +# Enable plotting with matplotlib in julia +Pkg.add("PyPlot") +# Add functionalities for polynomials in julia +# TODO: Can probably remove this dependency +Pkg.add("Polynomials") +# Enable efficient calculation of convex hulls +Pkg.add("QuickHull") +# Enable efficient calculation of distances +Pkg.add("Distances") +# Enable the use of ROS in julia +Pkg.add("RobotOS") +Pkg.pin("RobotOS", v"0.4.2") +# YAML-reader +Pkg.add("YAML") + + + + diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh new file mode 100755 index 00000000..45342d77 --- /dev/null +++ b/env_loader_odroid.sh @@ -0,0 +1,8 @@ +#!/bin/bash +source ~/barc/scripts/startup.sh +source /opt/ros/indigo/setup.bash +source ~/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/odroid/julia +export ROS_IP=192.168.10.213 +export ROS_MASTER_URI=http://192.168.10.74:11311 +exec "$@" diff --git a/env_loader_pc.sh b/env_loader_pc.sh new file mode 100755 index 00000000..5656d203 --- /dev/null +++ b/env_loader_pc.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source /opt/ros/kinetic/setup.bash +source /home/mpcubuntu/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia + +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 + +exec "$@" diff --git a/env_loader_yoga.sh b/env_loader_yoga.sh new file mode 100755 index 00000000..5dfc3b6d --- /dev/null +++ b/env_loader_yoga.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source ~/barc/scripts/startup.sh +source /opt/ros/kinetic/setup.bash +source ~/barc/workspace/devel/setup.bash + +export ROS_IP=192.168.10.74 +export ROS_MASTER_URI=http://192.168.10.74:11311 + +exec "$@" diff --git a/eval_sim/Kalman simulation/sim_kalman.jl b/eval_sim/Kalman simulation/sim_kalman.jl new file mode 100644 index 00000000..186ef945 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman.jl @@ -0,0 +1,224 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,10.0]) + +function main(code::AbstractString) + global Q, R + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/25 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,4) + u = zeros(sz,2) + + P = zeros(5,5) + x_est = zeros(length(t),5) + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] + y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + #a_x = imu_meas.z[imu_meas.t.>t[i],7][1] + y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + y[i,:] = [y_gps y_yaw y_vel_est] + y[:,3] = unwrap!(y[:,3]) + u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + #u[i,1] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1]-94.14)/2.7678 + #u[i,2] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1]-91.365)/105.6 + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1] == 90 + # u[i,1] = 0 + # end + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1] == 90 + # u[i,2] = 0 + # end + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + if gps_dist[i] > 0.3 + R[1,1] = 1+100*gps_dist[i]^2 + R[2,2] = 1+100*gps_dist[i]^2 + else + R[1,1] = 1 + R[2,2] = 1 + end + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + end + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + figure(1) + ax=subplot(5,1,1) + for i=1:4 + subplot(5,1,i,sharex=ax) + plot(t,y[:,i],t,x_est[:,i],"-*") + grid("on") + end + subplot(5,1,5,sharex=ax) + plot(cmd_pwm_log.t,cmd_pwm_log.z) + grid("on") + + + figure(2) + subplot(2,1,1) + title("Comparison raw GPS data and estimate") + plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["x_m","x_est"]) + grid("on") + subplot(2,1,2) + plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["y_m","y_est"]) + grid("on") + + figure(3) + plot(t,gps_dist) + title("GPS distances") + grid("on") + # figure() + # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # grid("on") + # title("x-y-view") + figure(4) + plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + title("Comparison simulation (-x) onboard-estimate (-*)") + grid("on") + legend(["x","y","psi","v","psi_drift"]) + + unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + figure(5) + plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + title("Comparison yaw") + grid("on") + + figure(6) + plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + grid("on") + title("Comparison of velocity measurement and estimate") + legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(4) zeros(4,1)] + C[4,4] = 0 + #C[3,3] = 0 + C[3,5] = 1 + return C*x +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl new file mode 100644 index 00000000..ec7a24f0 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman_imu.jl @@ -0,0 +1,353 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + + +function main(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + #cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,13) + q = zeros(sz,2) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(14,14) + x_est_gps_imu = zeros(length(t),14) + + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + y_gps_imu[1,4] = 0 + + qa = 1000 + qp = 1000 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) + #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,10.0, 10.0,10.0]) + # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y + + y_vel_prev = 0.0 + y_vel_est = 0.0 + + for i=2:length(t) + # Collect measurements and inputs for this iteration + # Interpolate GPS-measurements: + x_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1. 1 + c_x = [t_p.^2 t_p ones(sl,1)]\x_p + c_y = [t_p.^2 t_p ones(sl,1)]\y_p + x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x + y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y + else + x_int = 0 + y_int = 0 + end + + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + y_gps = [x_int y_int] + y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:] + + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + + att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] + acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = acc_f[1] + a_y = acc_f[2] + #y_yawdot = rot_f[3] + + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] + + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + y_vel_prev = y_vel_est + else + y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13]) + end + + + bta = atan(0.5*tan(u[i,2])) + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + println("Yaw0 = $yaw0") + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") + plot(t-t0,x_est_gps_imu[:,10:11],"-x") + #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+") + #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") + grid("on") + legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") + grid("on") + legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") + legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"]) + grid("on") + title("Velocity estimate and measurement") + + figure(4) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6]) + grid("on") + legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",]) + + # PLOT DIRECTIONS + figure(5) + plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11]) + grid("on") + title("x-y-view") + for i=1:10:size(pos_info.t,1) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end + for i=1:10:size(t,1) + bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) + dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)] # 7 = imu-yaw, 12 = model-yaw + lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-*") + end + axis("equal") + + # figure(6) + # plot(t-t0,q) + # grid("on") + # title("Errors") + figure(7) + plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8]) + legend(["psiDot_meas","psiDot_onboard","psiDot_est"]) + grid("on") + nothing +end + +function h_gps_imu(x,args) + y = zeros(13) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y + y[8] = x[10] # x + y[9] = x[11] # y + y[10] = x[12]+x[14] # psi + y[11] = x[13] # v + y[12] = x[3] # v_x + y[13] = x[4] # v_y + return y +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] + q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] + # if q_GPS[1] > 0.005 + # R[1,1] = 100 + # R[2,2] = 100 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # end + # if q_GPS2[1] > 0.005 + # R[8,8] = 100 + # R[9,9] = 100 + # else + # R[8,8] = 1 + # R[9,9] = 1 + # end + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1] +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*z[8] # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_psi + zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta)) # x + zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta)) # y + zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta)) # psi + zNext[13] = z[13] + dt*(u[1] - 0.5*z[13]) # v + zNext[14] = z[14] # psi_drift_2 + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end + +function plot_velocities(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + cmd_log = d_rec["cmd_log"] + + acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg) + acc = smooth(acc,5) + t0 = vel_est.t[1] + ax1=subplot(211) + plot(vel_est.t-t0,vel_est.z) + plot(vel_est.t_msg-t0,vel_est.z,"--") + legend(["vel","fl","fr","bl","br"]) + grid("on") + xlabel("t [s]") + title("Velocity") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--") + plot(vel_est.t_msg[1:end-1]-t0,acc) + grid("on") + legend(["u_a","u_a_sent","acceleration"]) + title("Commands") + xlabel("t [s]") + ylabel("") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/Other tests/NormalizeImu.jl b/eval_sim/Other tests/NormalizeImu.jl new file mode 100644 index 00000000..b3f6c0b9 --- /dev/null +++ b/eval_sim/Other tests/NormalizeImu.jl @@ -0,0 +1,44 @@ +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + t0 = imu_meas.t[1] + sz = size(imu_meas.t,1) + a_norm = zeros(sz,3) + for i=1:sz + a_norm[i,:] = (rotMatrix('y',-imu_meas.z[i,5])*rotMatrix('x',-imu_meas.z[i,4])*imu_meas.z[i,7:9]')' + end + figure() + plot(imu_meas.t-t0,imu_meas.z[:,7:8]) + grid("on") + figure() + plot(imu_meas.t-t0,a_norm[:,1:2]) + grid("on") + println("Mean raw: $(mean(imu_meas.z[:,8])), std raw: $(std(imu_meas.z[:,8]))") + println("Mean filtered: $(mean(a_norm[:,2])), std filtered: $(std(a_norm[:,2]))") +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl new file mode 100644 index 00000000..fe6dde2a --- /dev/null +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -0,0 +1,109 @@ + +using PyPlot +using JLD +using Optim + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function main(code::AbstractString) + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) + c = Optim.minimizer(res) + c2 = [1/c[1], c[2]/c[1], 1/c[4], c[5]/c[4]] + cost(cmd_opt,v_opt,c,1) + println("c = $c") + println("and c_2 = $c2") +end + +function v_over_u(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + L_b = 0.125 + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t),2) + psiDot = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i,:] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,:][end,:] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + delta = atan2(psiDot*0.25,v_x) + c = ((cmd[:,1]-91)/6.5-0.5*v)./delta.^2 + c = c[abs(c).<100] + plot(c) + grid("on") + figure() + plot(v) + grid("on") + #plot(cmd[:,1],v) +end + +function cost(cmd,v,c,p) + cost = 0 + v_s = zeros(length(v)) + v_s[1] = v[1] + v_s[2] = v[2] + for i=2:length(cmd)-1 + if cmd[i]-cmd[i-1] != 0 + v_s[i] = v[i] + end + if cmd[i] > 90 + v_s[i+1] = v_s[i] + 0.02*(c[1]*cmd[i] + c[2] - c[3]*v_s[i]) + else + v_s[i+1] = max(v_s[i] + 0.02*(c[4]*cmd[i] + c[5] - c[3]*v_s[i]),0) + end + end + if p==1 + plot((1:length(v_s))*0.02,v_s,(1:length(v))*0.02,v) + grid("on") + legend(["v_sim","v_meas"]) + title("Curve fitting acceleration") + xlabel("t [s]") + ylabel("v [m/s]") + end + cost = norm(v_s-v) + return cost +end diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl new file mode 100644 index 00000000..5c118c81 --- /dev/null +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -0,0 +1,66 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + #v2 = copy(v) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + + # idx = v_x.>1.1 + # psiDot = psiDot[idx] + # v_x = v_x[idx] + # cmd = cmd[idx] + delta = atan2(psiDot*0.25,v_x) + + figure(1) + ax2=subplot(211) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") + + figure(3) + plot(t-t0,delta,cmd_pwm_log.t-t0,(cmd_pwm_log.z[:,2]-90)/89.0) + grid("on") + xlabel("t [s]") + ylabel("d_f [rad]") + legend(["delta_true","delta_input"]) + + figure(2) + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl new file mode 100644 index 00000000..7de6122b --- /dev/null +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -0,0 +1,213 @@ + +using PyPlot +using JLD + +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +function main() + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = ("$(homedir())/open_loop/output-record-1486.jld",) + res_cmd = Float64[] + res_delta = Float64[] + res_t = Float64[] + L_b = 0.125 + for j=1:1 + d_rec = load(log_path_record[j]) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+0.5 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-0.5 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + res_cmd = cat(1,res_cmd,cmd) + res_delta = cat(1,res_delta,atan2(psiDot*0.25,v_x)) + res_t = cat(1,t) + end + res_delta = res_delta[res_cmd.>0] + res_cmd = res_cmd[res_cmd.>0] + res_delta = res_delta[res_cmd.<120] + res_cmd = res_cmd[res_cmd.<120] + res_delta = res_delta[res_cmd.>60] + res_cmd = res_cmd[res_cmd.>60] + sz = size(res_cmd,1) + + cmd_left = res_cmd[res_cmd.>103] + delta_left = res_delta[res_cmd.>103] + cmd_right = res_cmd[res_cmd.<80] + delta_right = res_delta[res_cmd.<80] + + coeff = [res_cmd ones(sz,1)]\res_delta + x = [1,200] + y = [1 1;200 1]*coeff + + coeff_r = [cmd_right ones(size(cmd_right,1),1)]\delta_right + xr = [1,200] + yr = [1 1;200 1]*coeff_r + + coeff_l = [cmd_left ones(size(cmd_left,1),1)]\delta_left + xl = [1,200] + yl = [1 1;200 1]*coeff_l + + ax1=subplot(211) + plot(res_t,res_delta) + grid("on") + subplot(212,sharex=ax1) + plot(res_t,res_cmd) + grid("on") + + figure(2) + plot(res_cmd,res_delta,"*",xl,yl,xr,yr) + + plot(vel_est.t,vel_est.z) + grid("on") + legend(["vel_avg","v_fl","v_fr","v_bl","v_br"]) +end + +function main_pwm(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + + L_b = 0.125 + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+1.0 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-1.0 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] == cmd_pwm_log.z[t[i]-1.0.>cmd_pwm_log.t,2][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + delta = atan2(psiDot*0.25,v_x) + + idx = copy( (delta.<1.0) & (delta.> -1.0) & (cmd .< 90) & (cmd .> 50) ) + delta = delta[idx] + cmd = cmd[idx] + + sz = size(cmd,1) + coeff = [cmd ones(sz,1)]\delta + x = [1,200] + y = [1 1;200 1]*coeff + + plot(cmd,delta,"*",x,y) + grid("on") + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") + +end + +function main_pwm2(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + #v2 = copy(v) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + + idx = v_x.>1.1 + psiDot = psiDot[idx] + v_x = v_x[idx] + cmd = cmd[idx] + delta = atan2(psiDot*0.25,v_x) + + c = [cmd ones(size(cmd,1))]\delta + + x = [60;120] + y = [x ones(2)]*c + + plot(cmd,delta,"+",x,y) + grid("on") + legend(["Measurements","Linear approximation"]) + xlabel("u_PWM") + ylabel("delta_F") + + figure(1) + ax2=subplot(211) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") + + figure(2) + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) +end + +function showV_V_F_relation() + L_F = 0.125 + L_R = 0.125 + delta = -0.3:.01:0.3 + v_F = 1.0 + v = L_R/L_F*cos(delta).*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + v2 = L_R/L_F*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + plot(delta,v,delta,v2) + grid("on") + xlabel("delta") + ylabel("v/v_F") +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl new file mode 100644 index 00000000..20c20862 --- /dev/null +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -0,0 +1,67 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + cmd_raw = zeros(length(t)) + + for i=1:length(t) + if cmd_log.z[t[i].>cmd_log.t,1][end] > 0 && vel_est.z[t[i].>vel_est.t,1][end] > 0.2 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + cmd[i] = cmd_log.z[t[i].>cmd_log.t,1][end] + cmd_raw[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + end + + idx = (v.>1) & (cmd_raw .> 95) + + sz = size(v[idx],1) + A = [cmd_raw[idx] ones(sz,1)] + b = v[idx] + coeff = A\b + + x = [90,110] + y = [90 1;110 1]*coeff + + #plot(cmd,v) + figure(1) + ax1=subplot(211) + plot(vel_est.t,vel_est.z) + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br"]) + subplot(212,sharex=ax1) + plot(cmd_pwm_log.t,cmd_pwm_log.z[:,1]) + grid("on") + + figure(2) + plot(cmd[cmd.>0],cmd[cmd.>0]./v[cmd.>0],"*") + xlabel("u_a") + ylabel("u_a/v") + grid("on") + + figure(3) + plot(cmd_raw[cmd_raw.>80],v[cmd_raw.>80],"*",x,y) + grid("on") + xlabel("PWM signal") + ylabel("v [m/s]") + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") +end diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl new file mode 100644 index 00000000..c2cb3a40 --- /dev/null +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -0,0 +1,101 @@ + +using PyPlot +using JLD + +function main(code::AbstractString,n::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + d_rec = load(log_path_record) + + delay_df = 5 + + #n = 2 + + #idx0 = (oldTraj.oldTraj[:,6,n].> 0) & (oldTraj.oldTraj[:,6,n].< 17.76) + #oldTraj.oldTraj = oldTraj.oldTraj[idx0,:,:] + #oldTraj.oldInput = oldTraj.oldInput[idx0,:,:] + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + sz = size(oldTraj.oldTraj[oldTraj.oldTraj[:,1,n].<1000,:,n],1) + oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],20) + #oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],2) + # plot(oldTraj.oldInput[1:sz,:,n]) + # for i=2:sz + # oldTraj.oldInput[i,:,n] = oldTraj.oldInput[i-1,:,n] + (oldTraj.oldInput[i,:,n]-oldTraj.oldInput[i-1,:,n])*0.5 + # end + # plot(oldTraj.oldInput[1:sz,:,n],"--") + #sz = 200 + idx_d = 20:5:sz-5 + idx = 20:5:sz-10 + sz1 = size(idx,1) + + y_psi = zeros(5*sz1) + A_psi = zeros(5*sz1,3) + y_vy = zeros(5*sz1) + A_vy = zeros(5*sz1,4) + y_vx = zeros(5*sz1) + A_vx = zeros(5*sz1,4) + + for i=0:4 + y_psi[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,3,n]) + A_psi[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_psi = (A_psi'*A_psi)\A_psi'*y_psi + + for i=0:4 + y_vy[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,2,n]) + A_vy[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,1,n].*oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_vy = (A_vy'*A_vy)\A_vy'*y_vy + + for i=0:4 + y_vx[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,1,n]) + A_vx[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n] oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i,1,n]] + end + c_vx = (A_vx'*A_vx)\A_vx'*y_vx + + println("psi:") + println(c_psi) + println("vy:") + println(c_vy) + println("vx:") + println(c_vx) + + figure(1) + plot(A_psi*c_psi) + plot(y_psi,"--") + #plot(A_psi,"--") + title("Psi") + + figure(2) + plot(A_vy*c_vy) + plot(y_vy,"--") + #plot(A_vy,"--") + title("Vy") + + figure(3) + plot(A_vx*c_vx) + plot(y_vx,"--") + #plot(A_vx,"--") + title("Vx") + + # figure(4) + # plot(oldTraj.oldTimes[:,1],oldTraj.oldTraj[:,3,1]) + # plot(pos_info.t,pos_info.z[:,11]) + # plot(imu_meas.t,imu_meas.z[:,3]) + nothing +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/PlotDataTutorial.pdf b/eval_sim/PlotDataTutorial.pdf new file mode 100644 index 00000000..7e33984d Binary files /dev/null and b/eval_sim/PlotDataTutorial.pdf differ diff --git a/eval_sim/README.md b/eval_sim/README.md new file mode 100644 index 00000000..e72bb0e5 --- /dev/null +++ b/eval_sim/README.md @@ -0,0 +1,78 @@ +Overview over evaluation functions +================================== +All evaluation functions use a code (String) which specifies an experiment. It is a 4-character combination which is created by the ROS master. All files that are saved during one experiment use this code. +Files that are saved: "output-record-(code).jld" from the recorder, "output-LMPC-(code).jld" from the LMPC node, and, if it is a simulation, "output-SIM-(code).jld" (contains real state information). +!! at the beginning denots most used functions. + +eval_data.jl +============ +Main file for all kinds of post-experiment evaluation + +eval_sim(code) +-------------- +Used for general evaluation of *simulations*. It uses 'real' simulation and 'measured' data and compares these. This can be helpful to develop a good state estimator. + +!! eval_run(code) +----------------- +Most general evaluation of experiments. Similar to eval_sim, but does not use simulation data. + +!! eval_LMPC(code) +------------------ +More detailed evaluation of specific LMPC data (MPC cost, sysID coefficients, curvature approximations, ...) + +plot_friction_circle(code, lap) +------------------------------- +Plots a_y over a_x. + +plot_v_ey_over_s(code, laps) +---------------------------- +Plots v and e_y over s for selected laps. Also plots lap times for all laps. + +plot_v_over_xy(code, lap) +------------------------- +Plots velocity in different colors on xy. + +eval_open_loop(code) +-------------------- +Similar to eval_run but for open-loop experiments (which are saved in a different folder). + +eval_predictions_kin(code) +------------------------- +Plots predictions for kinematic model (in path following mode) + +eval_predictions(code) +--------------------- +Plots predictions for dynamic model (in LMPC mode) + +eval_sysID(code) +--------------- +Plots sysID coefficients + +eval_oldTraj(code, lap) +----------------- +Plots data of one selected oldTrajectory (-> data of one specific lap +overlapping data before and after finish line) + +eval_LMPC_coeff(code, step) +-------------------- +Plots terminal constraints (both polynom and prediction) for one specific step in the LMPC process. + +!! anim_LMPC_coeff(code) +-------------------- +Animates predictions and terminal constraints (starting from first LMPC step) + +checkTimes(code) +---------------- +Check solving times and status (optimal/infeasible/user limit). + +Helper functions: +---------------- +create_track(half_track_width) -> used to create the track for plotting +initPlot() -> used to initialize plots for proper export and printing + +sim_kalman_imu.jl +================= +This file simulates a Kalman filter using real sensor data that was recorded during an experiment. It can be used to find the optimal structure and tuning values for the estimator. + +Parameter estimation folder +=========================== +Contains various files and functions to estimate BARC specific input mapping parameters (e.g. plot steering delay, least-square approximation for acceleration and steering, ...) \ No newline at end of file diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py new file mode 100644 index 00000000..2659b37a --- /dev/null +++ b/eval_sim/create_track.py @@ -0,0 +1,95 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.03 +theta = 0 +d_theta = 0 + +N = 40 + +def add_curve(theta, length, angle): + d_theta = 0 + curve = 2*sum(arange(1,length/2+1,1))+length/2 + for i in range(0,length): + if i < length/2+1: + d_theta = d_theta + angle / curve + else: + d_theta = d_theta - angle / curve + theta = hstack((theta,theta[-1] + d_theta)) + return theta + + +theta = array([0]) + +# SOPHISTICATED TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,60,-2*pi/3) +# theta = add_curve(theta,90,pi) +# theta = add_curve(theta,80,-5*pi/6) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,50,0) +# theta = add_curve(theta,40,-pi/4) +# theta = add_curve(theta,30,pi/4) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,25,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,28,0) + +# GOGGLE TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,30,pi/3) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,35,0) + +# SIMPLE GOGGLE TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,30,-pi/5) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,37,0) + +# SIMPLE GOGGLE TRACK +# theta = add_curve(theta,60,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,60,-pi/5) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,75,0) + +# SIMPLE TRACK +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,9,0) + +for i in range(0,size(theta)): + x = hstack((x, x[-1] + cos(theta[i])*ds)) + y = hstack((y, y[-1] + sin(theta[i])*ds)) + +print x +print y +plt.plot(x,y,'-o') +plt.axis('equal') +plt.grid() +plt.show() \ No newline at end of file diff --git a/eval_sim/create_track_bezier.py b/eval_sim/create_track_bezier.py new file mode 100644 index 00000000..c5fe3dd8 --- /dev/null +++ b/eval_sim/create_track_bezier.py @@ -0,0 +1,54 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.06 +theta = 0 +d_theta = 0 + +N = 40 + +def create_bezier(p0,p1,p2,dt): + t = arange(0,1+dt,dt)[:,None] + p = (1-t)**2*p0 + 2*(1-t)*t*p1+t**2*p2 + return p + +p0 = array([[0,0], + [2,0], + [2,-2], + [1,-4], + [3,-4], + [5,-5], + [3,-6], + [1,-6], + [-0.5,-5.5], + [-1.5,-5], + [-3,-4], + [-3,-2], + [0,0]]) +p1 = array([0,0,0.75,-1,1,inf,0,0,-1,0,inf,inf,0]) + +p = p0[0,:] +for i in range(0,size(p0,0)-1): + b1 = p0[i,1] - p1[i]*p0[i,0] + b2 = p0[i+1,1] - p1[i+1]*p0[i+1,0] + a1 = p1[i] + a2 = p1[i+1] + x = (b2-b1)/(a1-a2) + y = a1*x + b1 + if p1[i] == inf: + x = p0[i,0] + y = a2*x+b2 + elif p1[i+1] == inf: + x = p0[i+1,0] + y = a1*x+b1 + if a1 == a2: + p = vstack((p,p0[i+1,:])) + else: + p = vstack((p,create_bezier(p0[i,:],array([[x,y]]),p0[i+1,:],0.01))) + +plt.plot(p[:,0],p[:,1],'-',p0[:,0],p0[:,1],'*') +plt.grid() +plt.axis('equal') +plt.show() diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl new file mode 100644 index 00000000..fd6c94f7 --- /dev/null +++ b/eval_sim/eval_data.jl @@ -0,0 +1,2316 @@ +using JLD +using PyPlot +using PyCall +@pyimport matplotlib.animation as animation +@pyimport matplotlib.patches as patch +using JLD, ProfileView +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift +# pos_info[17] = a_x +# pos_info[18] = a_y + +include("../workspace/src/barc/src/barc_lib/classes.jl") + +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +# THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA) +# *********************************************************************************************** + +function eval_sim(code::AbstractString) + log_path_sim = "$(homedir())/simulations/output-SIM-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_sim = load(log_path_sim) + d_rec = load(log_path_record) + + imu_meas = d_sim["imu_meas"] + gps_meas = d_sim["gps_meas"] + z = d_sim["z"] + cmd_log = d_sim["cmd_log"] + slip_a = d_sim["slip_a"] + pos_info = d_rec["pos_info"] + vel_est = d_rec["vel_est"] + + t0 = pos_info.t[1] + track = create_track(0.4) + + figure() + ax1=subplot(311) + plot(z.t-t0,z.z,"-*") + title("Real states") + grid() + legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"]) + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-*") + title("Inputs") + grid() + legend(["u","d_f"]) + subplot(313,sharex=ax1) + plot(slip_a.t-t0,slip_a.z,"-*") + title("Slip angles") + grid() + legend(["a_f","a_r"]) + + figure() + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["Real state","GPS meas","Estimate"]) + + figure() + title("Comparison of psi") + plot(imu_meas.t-t0,imu_meas.z,"-x",z.t-t0,z.z[:,5:6],pos_info.t-t0,pos_info.z[:,10:11],"-*") + legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) + grid() + + figure() + title("Comparison of v") + plot(z.t-t0,z.z[:,3:4],z.t-t0,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t-t0,pos_info.z[:,8:9],"-*",pos_info.t-t0,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t-t0,vel_est.z) + legend(["real_xDot","real_yDot","real_v","est_xDot","est_yDot","est_v","v_x_meas"]) + grid() + + figure() + title("Comparison of x,y") + plot(z.t-t0,z.z[:,1:2],pos_info.t-t0,pos_info.z[:,6:7],"-*",gps_meas.t-t0,gps_meas.z) + legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) + grid() +end + +function eval_convhull(code::AbstractString,code2::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool) + + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_LMPC2 = "$(homedir())/simulations/output-LMPC-$(code2).jld" + + Data = load(log_path_LMPC) + Data2 = load(log_path_LMPC2) + + oldSS_xy = Data["oldSS_xy"] + oldSS_xy2 = Data2["oldSS_xy"] + + oldSS = Data["oldSS"] + selectedStates = Data["selectedStates"] + selStates = Data["selStates"] + statesCost = Data["statesCost"] + pred_sol = Data["pred_sol"] + pred_input = Data["pred_input"] + one_step_error = Data["one_step_error"] + lapStatus = Data["lapStatus"] + posInfo = Data["posInfo"] + eps_alpha = Data["eps_alpha"] + cvx = Data["cvx"] + cvy = Data["cvy"] + cpsi = Data["cpsi"] + input = Data["input"] + cost = Data["mpcCost"] + costSlack = Data["mpcCostSlack"] + obs_log = Data["obs_log"] + sol_u = Data["sol_u"] + final_counter = Data["final_counter"] + status = Data["sol_status"] + + Nl = selectedStates.Nl + Np = selectedStates.Np + buffersize = 5000 + currentIt = 200#lapStatus.currentIt + + flag = zeros(2) + + + track = create_track(0.45) + + pred_horizon = size(pred_sol)[1] - 1 + + println("prediction horizon N= ", pred_horizon) + + ospe1 = [] + ospe2 = [] + ospe3 = [] + ospe4 = [] + ospe5 = [] + ospe6 = [] + + slack1 = [] + slack2 = [] + slack3 = [] + slack4 = [] + slack5 = [] + slack6 = [] + + for l = laps + ospe1 = vcat(ospe1,one_step_error[1:currentIt,1,l]) + ospe2 = vcat(ospe2,one_step_error[1:currentIt,2,l]) + ospe3 = vcat(ospe3,one_step_error[1:currentIt,3,l]) + ospe4 = vcat(ospe4,one_step_error[1:currentIt,4,l]) + ospe5 = vcat(ospe5,one_step_error[1:currentIt,5,l]) + ospe6 = vcat(ospe6,one_step_error[1:currentIt,6,l]) + + slack1 = vcat(slack1,costSlack[1:currentIt,1,l]) + slack2 = vcat(slack2,costSlack[1:currentIt,2,l]) + slack3 = vcat(slack3,costSlack[1:currentIt,3,l]) + slack4 = vcat(slack4,costSlack[1:currentIt,4,l]) + slack5 = vcat(slack5,costSlack[1:currentIt,5,l]) + slack6 = vcat(slack6,costSlack[1:currentIt,6,l]) + end + + t_ospe = linspace(1,length(ospe1),length(ospe1)) + + figure() + plot(t_ospe,ospe1,t_ospe,ospe2,t_ospe,ospe3,t_ospe,ospe4,t_ospe,ospe5,t_ospe,ospe6) + legend(["vx","vy","psiDot","ePsi","eY","s"]) + grid("on") + title("One step prediction errors") + + figure() + plot(t_ospe,slack1,t_ospe,slack2,t_ospe,slack3,t_ospe,slack4,t_ospe,slack5,t_ospe,slack6) + legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + title("Slack costs") + grid("on") + + pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,11,track) + ellfig = figure(20) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,11],oldSS_xy[:,2,11],oldSS_xy2[:,1,22],oldSS_xy2[:,2,22]) + legend(["lap 13","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + ax[:add_artist](ell1) + title("X-Y view of laps 13 and 26") + + + ellfig = figure(21) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,5],oldSS_xy[:,2,5],oldSS_xy[:,1,26],oldSS_xy[:,2,26]) + legend(["lap 5","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + # ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + #ax[:add_artist](ell1) + title("X-Y view of laps 5 and 26") + + + ellfig = figure(22) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy2[:,1,10],oldSS_xy2[:,2,10],oldSS_xy2[:,1,22],oldSS_xy2[:,2,22]) + legend(["lap 14","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + ax[:add_artist](ell1) + title("X-Y view of laps 14 and 26") + + + + for i = laps + + pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,i,track) + + #println("pred sol= ",pred_sol_xy[:,1]) + # for index=1:buffersize + # if status[index,i] == :UserLimit + # flag[1]=index + # flag[2]= i + + # break + # end + # end + + + vx_alpha = eps_alpha[1,1:currentIt,i] + vy_alpha = eps_alpha[2,1:currentIt,i] + psiDot_alpha = eps_alpha[3,1:currentIt,i] + ePsi_alpha = eps_alpha[4,1:currentIt,i] + eY_alpha = eps_alpha[5,1:currentIt,i] + s_alpha = eps_alpha[6,1:currentIt,i] + + cvx1 = cvx[1:currentIt,1,i] + cvx2 = cvx[1:currentIt,2,i] + cvx3 = cvx[1:currentIt,3,i] + cvy1 = cvy[1:currentIt,1,i] + cvy2 = cvy[1:currentIt,2,i] + cvy3 = cvy[1:currentIt,3,i] + cvy4 = cvy[1:currentIt,4,i] + cpsi1 = cpsi[1:currentIt,1,i] + cpsi2 = cpsi[1:currentIt,2,i] + cpsi3 = cpsi[1:currentIt,3,i] + + + if obstacle == false + figure() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + elseif obstacle == true + + ellfig = figure(3) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + ax[:add_artist](ell1) + end + + + grid("on") + title("X-Y view of Lap $i") + + t = linspace(1,currentIt,currentIt) + + + + + + figure() + + plot(t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + title("Inputs in lap $i") + grid("on") + legend(["a","d_f"]) + + + #figure() + + # subplot(221) + # plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"]) + # title("One step prediction error for v_x in lap $i") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1]) + # legend(["ospe","a_x","d_f"]) + # grid("on") + + # subplot(222) + # plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for v_y in lap $i") + # grid("on") + + # subplot(223) + # plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for psiDot in lap $i") + # grid("on") + + # subplot(224) + # plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for ePsi in lap $i") + # grid("on") + + + # figure() + + # subplot(221) + # plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for eY in lap $i") + # grid("on") + + # subplot(222) + # plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for s in lap $i") + # grid("on") + + + # figure() + # plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) + # legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + # title("Slack costs") + # grid("on") + + + + # figure() + # plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i]) + # legend(["vx","vy","psiDot","ePsi","eY","s"]) + # grid("on") + # title("One step prediction errors") + + # println("one step prediction error= ",one_step_error[1:30,:,i]) + + + figure() + + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,1,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vx in lap $i") + grid("on") + + subplot(222) + plot(t,oldSS.oldSS[1:currentIt,2,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vy in lap $i") + grid("on") + + subplot(223) + plot(t,oldSS.oldSS[1:currentIt,3,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("psiDot in lap $i") + grid("on") + + subplot(224) + plot(t,oldSS.oldSS[1:currentIt,4,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("ePsi in lap $i") + grid("on") + + figure() + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,5,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("eY in lap $i") + grid("on") + + subplot(222) + plot(t,oldSS.oldSS[1:currentIt,6,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("s in lap $i") + grid("on") + + subplot(223) + velocity= sqrt(oldSS.oldSS[1:currentIt,2,i].^2 + oldSS.oldSS[1:currentIt,1,i].^2) + plot(t,velocity,"-*") + title("Overall velocity in lap $i") + grid("on") + + + + figure() + + subplot(221) + plot(t,cvx1,t,cvx2,t,cvx3) + legend(["cvx1","cvx2","cvx3"]) + title("C_Vx in lap $i") + grid("on") + + subplot(222) + plot(t,cvy1,t,cvy2,t,cvy3,t,cvy4) + legend(["cvy1","cvy2","cvy3","cvy4"]) + title("C_Vy in lap $i") + grid("on") + + subplot(223) + plot(t,cpsi1,t,cpsi2,t,cpsi3) + legend(["cpsi1","cpsi2","cpsi3"]) + title("C_Psi in lap $i") + grid("on") + + figure() + plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i]) + legend(["terminal Cost","control Cost","derivative Cost","lane Cost"]) + title("Costs of the Mpc in lap $i") + grid("on") + + + + + + + + if switch == true + + + + # for j = 2:2000 + for j = 54:60 + solution_status = status[final_counter[i-1]+j] + + vx_pred = pred_sol[:,1,j,i] + vy_pred = pred_sol[:,2,j,i] + psiDot_pred = pred_sol[:,3,j,i] + ePsi_pred = pred_sol[:,4,j,i] + eY_pred = pred_sol[:,5,j,i] + s_pred = pred_sol[:,6,j,i] + + + oldvx = selStates[1:Np,1,j,i] + oldvx2 = selStates[Np+1:2*Np,1,j,i] + #oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + oldvy = selStates[1:Np,2,j,i] + oldvy2 = selStates[Np+1:2*Np,2,j,i] + #oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + oldpsiDot = selStates[1:Np,3,j,i] + oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] + #oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + oldePsi = selStates[1:Np,4,j,i] + oldePsi2 = selStates[Np+1:2*Np,4,j,i] + #oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + oldeY = selStates[1:Np,5,j,i] + oldeY2 = selStates[Np+1:2*Np,5,j,i] + #oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + olds = selStates[1:Np,6,j,i] + olds2 = selStates[Np+1:2*Np,6,j,i] + #olds3 = selStates[2*Np+1:3*Np,6,j,i] + + + t = linspace(1,j,j) + + s_ey_pred = zeros(pred_horizon+1,2) + + s_ey_pred[:,1] = pred_sol[:,6,j,i] + s_ey_pred[:,2] = pred_sol[:,5,j,i] + + #println(s_ey_pred) + + + xy_predictions = xyPrediction(oldSS,s_ey_pred,track) + + #println("xyPredictions= ",xy_predictions) + + + if obstacle == false + figure(19) + clf() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i]) + plot(xy_predictions[1,:]',xy_predictions[2,:]',"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + #plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Prediction at iteration $j, status = $solution_status") + # end + + elseif obstacle == true + ellfig = figure(19) + clf() + # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + #plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i]) + + # plot([oldSS_xy[63:75,1,i-1],oldSS_xy[63:75,1,i-2]],[oldSS_xy[63:75,2,i-1],oldSS_xy[63:75,2,i-2]],"g")#,oldSS_xy[:,1,i-2],oldSS_xy[:,2,i-2]) + plot(oldSS_xy[59:73,1,i-1],oldSS_xy[59:73,2,i-1],"g") + legend(["SS^13"]) + plot(oldSS_xy[63:77,1,i-2],oldSS_xy[63:77,2,i-2],"g") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + plot(xy_predictions[1,:]',xy_predictions[2,:]',"k*") + + xlabel("x[m]") + ylabel("y[m]") + + #for i=1:4:size(x_est,1) + # for index=1:length(oldSS_xy[:,1,i]) + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[j,:,i] + # for j2=2:size(pred_sol)[1] + # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + # end + + + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + #plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + #plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,j+1]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j+1]-(pred_sol_xy[1,j-1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.4, 0.2, angle = 0) + rec1 = patch.Rectangle([xy_predictions[1,1]'-0.2,xy_predictions[2,1]'-0.1],0.4,0.2,angle=-2,fill=false) + ax[:add_artist](ell1) + ax[:add_artist](rec1) + + #plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + #grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Prediction in XY, lap 13") + # end + end + + figure(15) + clf() + subplot(221) + plot(s_pred,vx_pred,"or") + plot(olds,oldvx,"b") + plot(olds2,oldvx2,"b") + #plot(olds3,oldvx3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) + title("State vx in lap $i, iteration $j, status = $solution_status") + grid("on") + + subplot(222) + plot(s_pred,vy_pred,"or") + plot(olds,oldvy,"b") + plot(olds2,oldvy2,"b") + #plot(olds3,oldvy3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) + title("State vy in lap $i, iteration $j, status = $solution_status ") + grid("on") + + subplot(223) + plot(s_pred,psiDot_pred,"or") + plot(olds,oldpsiDot,"b") + plot(olds2,oldpsiDot2,"b") + #plot(olds3,oldpsiDot3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State psiDot in lap $i , iteration $j, status = $solution_status") + grid("on") + + subplot(224) + plot(s_pred,ePsi_pred,"or") + plot(olds,oldePsi,"b") + plot(olds2,oldePsi2,"b") + #plot(olds3,oldePsi3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State ePsi in lap $i, iteration $j, status = $solution_status ") + grid("on") + + + figure(16) + clf() + subplot(221) + plot(s_pred,eY_pred,"or") + plot(olds,oldeY,"b") + plot(olds2,oldeY2,"b") + #plot(olds3,oldeY3,"b") + + if obstacle == true + for obs_ind = 1:size(obs_log)[3] + if obs_log[j,1,obs_ind,i] < max(olds[end],olds2[end]) + plot(obs_log[j,1,obs_ind,i],obs_log[j,2,obs_ind,i],"ko") + end + end + end + + title("State eY in lap $i, iteration $j, status = $solution_status ") + grid("on") + + figure(17) + clf() + + subplot(121) + velocity= sqrt(oldSS.oldSS[1:j,2,i].^2 + oldSS.oldSS[1:j,1,i].^2) + plot(t,velocity,"-*") + legend(["velocity"]) + + + subplot(122) + #plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") + legend(["a_x","d_f"]) + + title("Comparison between velocity and inputs in lap $i, iteration $j ") + grid("on") + + figure(18) + clf() + + subplot(121) + plot(t,oldSS.oldSS[1:j,5,i],"-*") + legend(["e_y"]) + + subplot(122) + # plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") + legend(["a_x","d_f"]) + + title("Comparison between e_y and inputs in lap $i, iteration $j ") + grid("on") + + figure(20) + clf() + subplot(211) + plot(t,oldSS.oldSS[1:j,5,i],"-*") + title("eY in lap $i, iteration $j") + grid("on") + plot(linspace(j,j+size(pred_sol)[1],size(pred_sol)[1]),pred_sol[:,5,j,i],"-+") + + subplot(212) + plot(t,cost[1:j,6,i]) + title("lane cost in lap $i, iteration $j") + grid("on") + + + + + + sleep(0.5) + end + end + end + +end + +# THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL +# **************************************************************** +function eval_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = pos_info.t[1] + track = create_track(0.4) + + # Calculate accelerations + acc = smooth(diff(smooth(vel_est.z[:,1],10))./diff(vel_est.t),10) + figure(10) + plot(vel_est.t[1:end-1]-t0,acc,cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") + legend(["Acc","u_a"]) + + + figure() + plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["GPS meas","estimate"]) + + figure() + #plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + plot(pos_info.t-t0,pos_info.z[:,19:20]) + grid("on") + legend(["a_x","a_y"]) + ylabel("a [m/s^2]") + xlabel("t [s]") + title("Measured accelerations") + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-",gps_meas.t_msg-t0,gps_meas.z,"--") + grid(1) + title("GPS comparison") + xlabel("t [s]") + ylabel("Position [m]") + legend(["x_meas","y_meas","x_est","y_est"]) + + figure() + ax2=subplot(211) + title("Commands") + plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x") + grid("on") + xlabel("t [s]") + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z,"-*") + grid("on") + xlabel("t [s]") + + figure() + title("Comparison of psi") + plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*",pos_info.t-t0,pos_info.z[:,14]) + legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift","psi_raw"]) + grid() + + figure() + plot(pos_info.t-t0,pos_info.z[:,2:3]) + legend(["e_y","e_psi"]) + title("Deviations from reference") + grid("on") + xlabel("t [s]") + ylabel("eY [m], ePsi [rad]") + + # figure() + # title("Raw IMU orientation data") + # plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) + # grid("on") + # legend(["w_x","w_y","w_z","roll","pitch","yaw"]) + + figure() + title("v measurements and estimate") + plot(vel_est.t-t0,vel_est.z[:,1],"-x",pos_info.t-t0,pos_info.z[:,[8:9,4]],"-+") + legend(["v_raw","est_xDot","est_yDot","est_v"]) + grid() + + # figure() + # title("s, eY and inputs") + # ax1=subplot(211) + # plot(pos_info.t-t0,pos_info.z[:,2:3],"-*") + # grid("on") + # legend(["eY","ePsi"]) + # subplot(212,sharex=ax1) + # plot(cmd_log.t-t0,cmd_log.z,"-*") + # grid("on") + # legend(["u","d_f"]) + nothing +end + +function plot_friction_circle(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + idx = (pos_info.t.>t_start) & (pos_info.t.= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(smooth(oldTraj.oldTraj[idx,6,laps[i]],5),oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])") + end + legend() + grid("on") + xlabel("s [m]") + ylabel("v [m/s]") + xlim([0,19.11]) + ylim([0,3.2]) + + # plot e_y over s + figure(2) + for i=1:n_laps + idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,5,laps[i]],label="Lap $(laps[i])") + end + legend() + grid("on") + xlabel("s [m]") + ylabel("e_Y [m]") + xlim([0,19.11]) + ylim([-0.5,0.5]) + + # plot lap time over iterations + laps_run = size(oldTraj.oldCost[oldTraj.oldCost.>1],1) + lap_times = zeros(laps_run) + for i=1:laps_run + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].>=0,i][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].<=19.11,i][end] + lap_times[i] = t_end-t_start + end + figure(3) + #plot(1:size(lap_times,1)-2,lap_times[1:size(lap_times,1)-2],"-o") + plot(1:4,lap_times[1:4],"go",5:13,lap_times[5:13],"bo",14:size(lap_times,1),lap_times[14:size(lap_times,1)],"ro") + legend(["Path following","LMPC with Obstacle","LMPC without Obstacle"]) + grid("on") + xlabel("Lap number") + ylabel("Lap time [s]") + ylim([0,25]) +end + +function plot_v_over_xy(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + println("Laptime = $(t_end-t_start) s") + + idx = (pos_info.t.>=t_start) & (pos_info.t.<=t_end) + + track = create_track(0.4) + + figure() + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=1.5,vmax=2.8) + grid(1) + title("x-y-view") + axis("equal") + cb = colorbar() + cb[:set_label]("Velocity [m/s]") + println("Average v_x = ",mean(pos_info.z[idx,8])," m/s") + println("Max v_x = ",findmax(pos_info.z[idx,8])[1]," m/s") + +end + + +function eval_open_loop(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = pos_info.t[1] + + figure() + ax3=subplot(211) + title("Comparison speed and input") + plot(vel_est.t-t0,vel_est.z,"-*",vel_est.t_msg-t0,vel_est.z,"-x") + legend(["vel_est"]) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,1],cmd_pwm_log.t_msg-t0,cmd_pwm_log.z[:,1]) + grid("on") + + gps_speed_raw = diff(gps_meas.z)./diff(gps_meas.t) + gps_speed = [0;sqrt(gps_speed_raw[:,1].^2+gps_speed_raw[:,2].^2)] + figure() + title("Comparison GPS and encoder speed") + plot(vel_est.t-t0,vel_est.z,"-*",gps_meas.t-t0,gps_speed,"-x",pos_info.t_msg-t0,pos_info.z[:,4],"-+") + grid("on") + legend(["encoder","gps","estimator"]) + + figure() + title("Acceleration data") + plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") + legend(["a_x","a_y","a_z"]) + grid() + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,12:13],"-x",pos_info.t-t0,pos_info.z[:,6:7]) + legend(["x_gps","y_gps","x_filtered","y_filtered","x_est","y_est"]) + grid("on") + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*") + legend(["x_gps","y_gps"]) + xlabel("t [s]") + ylabel("Position [m]") + grid("on") +end + +# THIS FUNCTION EVALUATES MPC-SPECIFIC DATA +# ***************************************** +function eval_LMPC(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + + t0 = t[1] + + + figure(2) + ax1=subplot(311) + plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") + title("Comparison of publishing and receiving time") + legend(["x_dot_estimate","x_dot_MPC"]) + grid("on") + xlabel("t [s]") + ylabel("v_x [m/s]") + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*",t-t0,state[:,7]) + legend(["a_rec","d_f_rec","a_MPC","d_f_MPC","acc_filter"]) + grid("on") + subplot(313,sharex=ax1) + plot(t-t0,c_Vx) + title("System ID xDot coefficients") + grid("on") + legend(["1","2","3"]) + + figure(3) + plot(t,state) + grid("on") + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + + figure() + subplot(311) + title("c_Vx") + plot(t-t0,c_Vx) + legend(["1","2","3"]) + grid("on") + subplot(312) + title("c_Vy") + plot(t-t0,c_Vy) + legend(["1","2","3","4"]) + grid("on") + subplot(313) + title("c_Psi") + plot(t-t0,c_Psi) + legend(["1","2","3"]) + grid("on") + + # *********** CURVATURE ********************* + figure() + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,6] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(state[:,6],c,"-o") + for i=1:5:size(curv,1) + if isnan(sol_z[1,6,i]) + s = sol_z[:,1,i] + else + s = sol_z[:,6,i] + end + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() + + track = create_track(0.4) + figure() + hold(1) + plot(x_est[:,1],x_est[:,2],"-*") + title("Estimated position") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + axis("equal") + grid(1) + # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: + # CURRENT HEADING (PLOTTED BY A LINE) + # for i=1:10:size(pos_info.t,1) + # dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + # lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + # plot(lin[:,1],lin[:,2],"-+") + # end + + # PREDICTED PATH + for i=1:4:size(x_est,1) + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-*") + end + + # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM) + # for i=1:size(x_est,1) + # s = 0.4:.1:2.5 + # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + # x = ss*coeffX[i,:]' + # y = ss*coeffY[i,:]' + # plot(x,y) + # end + + # rg = 100:500 + # figure() + # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + # title("Comparison states and prediction") + # legend(["ey","epsi","v"]) + # grid(1) + # for i=100:5:500 + # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") + # end + + # figure() + # plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x") + # title("Old Trajectory") + # legend(["v_x","v_y","psiDot","ePsi","eY"]) + # xlabel("s") + # grid(1) + + figure() + ax1=subplot(211) + title("MPC states and cost") + plot(t-t0,state) + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + grid(1) + subplot(212,sharex = ax1) + plot(t-t0,cost) + grid(1) + legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) + + figure() + ax1=subplot(211) + title("States and inputs") + plot(t-t0,state[:,1:5]) + legend(["v_x","v_y","psiDot","ePsi","eY"]) + grid(1) + subplot(212,sharex = ax1) + plot(cmd_log.t-t0,cmd_log.z) + grid(1) + legend(["u","d_f"]) +end + +function eval_predictions_kin(code::AbstractString) + # This function helps to evaluate predictions of the *kinematic* model + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + z = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_lmpc = d_lmpc["cmd"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + sz = size(z,1) + N = size(sol_z,1)-1 # number of steps (prediction horizon) + figure(1) + plot(pos_info.t-t0,pos_info.z[:,2],"--g") + plot(t-t0,z[:,5],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,2,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_y") + + figure(2) + plot(pos_info.t-t0,pos_info.z[:,3],"--g") + plot(t-t0,z[:,4],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,3,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_psi") + + figure(3) + plot(pos_info.t-t0,(pos_info.z[:,8].^2+pos_info.z[:,9].^2).^0.5,"--g") + plot(t-t0,(z[:,1].^2+z[:,2].^2).^0.5,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*N,N+1),sol_z[:,4,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("v") + + figure(4) + plot(cmd_log.t-t0,cmd_log.z,"--g") + plot(t-t0,cmd_lmpc,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-2),N-1),sol_u[1:N-1,1,i],"--xr") + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-3),N-2),sol_u[1:N-2,2,i],"--xr") + end + grid("on") + + # Calculate one-step-errors: + one_step_err = zeros(sz,4) + for i=1:sz-1 + one_step_err[i,:] = sol_z[2,1:4,i] - [z[i+1,6] z[i+1,5] z[i+1,4] norm(z[i+1,1:2])] + end + + # Calculate 'real' d_f + L_b = 0.125 + v_x = real(sqrt(complex((pos_info.z[:,8].^2+pos_info.z[:,9].^2)-pos_info.z[:,11].^2*L_b^2))) + delta = atan2(pos_info.z[:,11]*0.25,v_x) + + figure(5) + ax1=subplot(211) + plot(t-t0,one_step_err) + grid("on") + legend(["s","ey","epsi","v"]) + subplot(212,sharex=ax1) + plot(t-t0,cmd_lmpc) + plot(cmd_log.t_msg-t0,cmd_log.z) + plot(cmd_log.t-t0,cmd_log.z,"--") + plot(pos_info.t-t0,delta) + grid("on") + +end +function eval_predictions(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + z = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + N = size(sol_z,1)-1 # number of steps (prediction horizon) + + #N = 7 + sz = size(t,1) + # Calculate one-step-errors: + ########### + # s + # eY + # ePsi + # v + ########### + # v_x + # v_y + # psiDot + # ePsi + # eY + # s + ########### + one_step_err = NaN*ones(sz,6) + for i=1:sz-1 + if isnan(sol_z[1,5,i]) # if we are in path following mode + one_step_err[i,1:4] = sol_z[2,1:4,i] - [z[i+1,[6,5,4]] norm(z[i+1,1:2])] + else + one_step_err[i,:] = sol_z[2,[6,5,4,1,2,3],i] - z[i+1,[6,5,4,1,2,3]] + end + end + i=1 + while isnan(sol_z[1,5,i]) + i = i+1 + end + figure(1) + plot(t-t0,step_diff) + grid("on") + title("One-step-errors") + legend(["xDot","yDot","psiDot","ePsi","eY"]) + + + figure(3) + plot(pos_info.t-t0,pos_info.z[:,11],"-o") + title("Open loop predictions psidot") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,3,i],"-x") + end + end + grid("on") + + figure(4) + plot(pos_info.t-t0,pos_info.z[:,9],"-o") + title("Open loop predictions v_y") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,2,i],"-x") + end + end + grid("on") + + + figure(2) + ax1=subplot(411) + title("Open loop predictions e_y") + plot(pos_info.t-t0,pos_info.z[:,2],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,2,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,5,i]) + end + end + grid("on") + + subplot(412,sharex=ax1) + title("Open loop predictions e_psi") + plot(pos_info.t-t0,pos_info.z[:,3],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,3,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,4,i]) + end + end + grid("on") + + subplot(413,sharex=ax1) + title("Open loop predictions v") + plot(pos_info.t-t0,pos_info.z[:,8],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,4,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,1,i]) + end + end + grid("on") + + N = 7 + subplot(414,sharex=ax1) + title("Open loop inputs") + for i=1:2:size(t,1)-N + plot(t[i:i+N-3]-t0,sol_u[1:N-2,2,i],t[i:i+N-1]-t0,sol_u[1:N,1,i]) + end + grid("on") +end + + +function eval_sysID(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + + t0 = imu_meas.t[1] + + figure(1) # longitudinal (xDot) + ax1=subplot(211) + title("Vx") + plot(t-t0,c_Vx) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") + + figure(2) # longitudinal (xDot) + ax2=subplot(211) + title("Psi") + plot(t-t0,c_Psi) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax2) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + + figure(3) # longitudinal (xDot) + ax3=subplot(211) + title("Vy") + plot(t-t0,c_Vy) + legend(["c1","c2","c3","c4"]) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") +end + + +function eval_oldTraj(code::AbstractString,i::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + #t = d["t"] + plot(oldTraj.oldTimes[:,i],oldTraj.oldTraj[:,:,i],"-x") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY","s"]) + figure() + plot(oldTraj.oldTimes[:,i],oldTraj.oldInput[:,:,i],"-x") + grid("on") + legend(["a","d_f"]) + figure() + plot(oldTraj.oldTraj[:,6,i],oldTraj.oldTraj[:,1:5,i],"-x") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY"]) +end + +function eval_LMPC_coeff(code::AbstractString,k::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + cost = d["cost"] + + s = sol_z[:,6,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) + grid() + title("Position = $(s[1]), k = $k") + xlabel("s") + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(313) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + xlabel("s") + ylabel("v_x") + println("Cost = $(cost[k,3])") +end + +function anim_LMPC_coeff(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + cost = d["cost"] + sol_status = d["sol_status"] + + i=1 + while isnan(sol_z[1,6,i]) + i = i+1 + end + for k=i:size(cost,1) + clf() + s = sol_z[:,6,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(511) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) + ylim([-0.4,0.4]) + grid() + title("Position = $(s[1]), k = $k, status = $(sol_status[k])") + xlabel("s") + ylabel("e_Y") + subplot(512) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(513) + plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("psiDot") + subplot(514) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + ylim([0.6,1.5]) + grid() + xlabel("s") + ylabel("v_x") + subplot(515) + plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("v_y") + println("Cost = $(cost[k,3])") + end +end + +function visualize_tdiff(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + t_diff = diff(pos_info.t_msg) + t_diff = t_diff[t_diff .< 0.08] + plt[:hist](t_diff,100) + grid("on") + xlabel("t_diff") + nothing +end + +function anim_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + L_a = 0.125 + w = 0.15 + alpha = atan(w/2/L_a) + l = sqrt(L_a^2+(w/2)^2) + t0 = pos_info.t[1] + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes(xlim = (-3,3),ylim=(-5,1)) + + track = create_track(0.4) + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid("on") + car = ax[:plot]([],[],"r-+")[1] + gps = ax[:plot]([],[],"g*")[1] + h_ti = ax[:title]("abc") + function init() + car[:set_data]([],[]) + return (car,None) + end + function animate(k) + i = k + 2000 + car_c = [pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l] + car[:set_data]([car_c[:,1]],[car_c[:,2]]) + gps[:set_data]([pos_info.z[max(1,i-100):i,12]],[pos_info.z[max(1,i-100):i,13]]) + ax[:set_title]("t = $(pos_info.t[i]-t0)") + #title(pos_info.t[i]-t0) + return (car,gps,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=1000, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end + +function anim_constraints(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + coeffCost = d_lmpc["coeffCost"] + coeffConst = d_lmpc["coeffConst"] + + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes() + + pred_vx = ax[:plot]([],[],"r-+")[1] + poly_vx1 = ax[:plot]([],[],"--")[1] + poly_vx2 = ax[:plot]([],[],"--")[1] + + function init() + #car[:set_data]([],[]) + return (None) + end + function animate(k) + i = k + 1000 + s = sol_z[:,6,i] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + pred_vx[:set_data]([s],[sol_z[:,1,i]]) + poly_vx1[:set_data]([s],[ss*coeffConst[:,1,1,i]]) + poly_vx2[:set_data]([s],[ss*coeffConst[:,2,1,i]]) + + return (pred_vx,poly_vx1,poly_vx2,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=100, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end + +# ***************************************************************** +# ****** HELPER FUNCTIONS ***************************************** +# ***************************************************************** + +function anim_MPC(z) + figure() + hold(0) + grid(1) + for i=1:size(z,3) + plot(z[:,:,i]) + xlim([1,11]) + ylim([-2,2]) + sleep(0.01) + end +end + +function anim_curv(curv) + s = 0.0:.05:2.0 + figure() + hold(0) + #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + for i=1:size(curv,1) + c = ss*curv[i,:]' + plot(s,c) + xlim([0,2]) + ylim([-1.5,1.5]) + sleep(0.1) + end +end + +# function eval_prof() +# Profile.clear() +# @load "$(homedir())/simulations/profile.jlprof" +# ProfileView.view(li, lidict=lidict) +# end + +function create_track(w) + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.03 + + theta = [0.0] + + # SOPHISTICATED TRACK + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) + + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # GOGGLE TRACK + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK + + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + add_curve(theta,60,0) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,40,pi/10) + add_curve(theta,60,-pi/5) + add_curve(theta,40,pi/10) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,75,0) + + # SIMPLE GOOGLE TRACK FOR 3110 + + # add_curve(theta,25,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,17,-pi/10) + # add_curve(theta,25,pi/5) + # add_curve(theta,17,-pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,32,0) + + # OVAL TRACK FOR TESTS IN VSD + + # add_curve(theta,80,0) + # add_curve(theta,110,-pi) + # add_curve(theta,160,0) + # add_curve(theta,110,-pi) + # add_curve(theta,80,0) + + # add_curve(theta,53,0) + # add_curve(theta,73,-pi) + # add_curve(theta,106,0) + # add_curve(theta,73,-pi) + # add_curve(theta,53,0) + + + # # SHORT SIMPLE track + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + #plot(track[:,1],track[:,2]) + return track + #plot(x,y,x_l,y_l,x_r,y_r) +end + +function create_track_eval(w) + println("CREATING THE TRACK") + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.03 + + theta = [0.0] + + # SOPHISTICATED TRACK + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) + + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # GOGGLE TRACK + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK + + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + add_curve(theta,60,0) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,40,pi/10) + add_curve(theta,60,-pi/5) + add_curve(theta,40,pi/10) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,75,0) + + # SIMPLE GOOGLE TRACK FOR 3110 + + # add_curve(theta,25,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,17,-pi/10) + # add_curve(theta,25,pi/5) + # add_curve(theta,17,-pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,32,0) + + # OVAL TRACK FOR TESTS IN VSD + + # add_curve(theta,80,0) + # add_curve(theta,110,-pi) + # add_curve(theta,160,0) + # add_curve(theta,110,-pi) + # add_curve(theta,80,0) + + # add_curve(theta,53,0) + # add_curve(theta,73,-pi) + # add_curve(theta,106,0) + # add_curve(theta,73,-pi) + # add_curve(theta,53,0) + + + # # SHORT SIMPLE track + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + #plot(track[:,1],track[:,2]) + return track + #plot(x,y,x_l,y_l,x_r,y_r) +end + +function add_curve(theta::Array{Float64}, length::Int64, angle) + d_theta = 0 + curve = 2*sum(1:length/2)+length/2 + for i=0:length-1 + if i < length/2+1 + d_theta = d_theta + angle / curve + else + d_theta = d_theta - angle / curve + end + push!(theta, theta[end] + d_theta) + end +end + +function initPlot() + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function simModel(z,u,dt,l_A,l_B) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.5 * z[4]) # v + + return zNext +end + +function checkTimes(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_lmpc = load(log_path_LMPC) + + t_solv = d_lmpc["t_solv"] + sol_status = d_lmpc["sol_status"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + sol_status_int = zeros(size(t_solv,1)) + t = d_lmpc["t"] + t0 = t[1] + + for i=1:size(sol_status,1) + if sol_status[i]==:Optimal + sol_status_int[i] = 1 + elseif sol_status[i]==:Infeasible + sol_status_int[i] = 2 + elseif sol_status[i]==:UserLimit + sol_status_int[i] = 3 + end + end + ax1=subplot(211) + plot(t,t_solv) + plot(t,sol_status_int,"*") + grid("on") + subplot(212,sharex=ax1) + plot(t,cmd,"-x") + grid("on") + + figure(2) + plot(t-t0,t_solv) + title("Solver time") + grid("on") + xlabel("t [s]") + ylabel("Solver time [s]") +end + +function checkConnectivity(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + + plot(gps_meas.t,gps_meas.z,"-x",gps_meas.t_msg,gps_meas.z,"--x",pos_info.t,pos_info.z[:,12:13],"-*",pos_info.t_msg,pos_info.z[:,12:13],"--*") + grid("on") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end + +function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,track::Array{Float64}) + + obs = obs_log[:,:,obstacle,lap] + + + + buffersize = size(obs)[1] + + + + # println("obs= ",obs) + + OrderXY = 10 + OrderThetaCurv = 8 + + + + ds = 0.03 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,buffersize,1) + xyPathAngle = [] + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:buffersize + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + #println("number of nodes= ",n_nodes) + s_start = (obs[i,1] - 1) + #println("s_start= ",s_start) + s_end = (obs[i,1] + 5) + #println("s_end= ",s_end) + s_nearest = obs[i,1] + + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) + + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end + + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) + + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = obs[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = obs[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* obs[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + obs[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - obs[i,2]*sin(xyPathAngle) + + + end + + return pred_sol_xy, xyPathAngle +end + + + + + +function xyPrediction(oldSS,s_ey_pred::Array{Float64},track::Array{Float64}) + + + pred_horizon = size(s_ey_pred)[1] + + + OrderXY = 10 + OrderThetaCurv = 8 + + + + ds = 0.03 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,pred_horizon,1) + xyPathAngle = [] + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:pred_horizon + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + #println("number of nodes= ",n_nodes) + s_start = (s_ey_pred[i,1] - 1) + #println("s_start= ",s_start) + s_end = (s_ey_pred[i,1] + 5) + #println("s_end= ",s_end) + s_nearest = s_ey_pred[i,1] + + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) + + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end + + + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) + + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = s_ey_pred[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = s_ey_pred[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* s_ey_pred[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + s_ey_pred[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - s_ey_pred[i,2]*sin(xyPathAngle) + + + end + + + return pred_sol_xy +end diff --git a/eval_sim/mapping_francesco.jl b/eval_sim/mapping_francesco.jl new file mode 100644 index 00000000..7fcd9bb5 --- /dev/null +++ b/eval_sim/mapping_francesco.jl @@ -0,0 +1,80 @@ + +using PyPlot +using JLD +#using Optim + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function data_analysis(code::AbstractString) + + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + # t_cmd = d_rec["t_cmd"] + # t_no_cmd = d_rec["t_no_cmd"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure() + plot(v_opt) + + # half = round(length(v_opt)/2) + # println(half) + + #v_mean = mean(v_opt[half:end]) + + #v_mean = mean(v_opt[length(v_opt)/2:end]) + + #a = v_mean/4 + + a = v_opt[end]/4 + + println("a= ",a) + + figure(2) + + plot(cmd_opt[1],a, "*") + + + # println(t_cmd) + # println(t_no_cmd) + # println(t0) + + # figure() + # plot(cmd,v) + + # figure() + # plot(t,cmd) + + # figure() + # plot(t,v) + + # figure() + # plot(v) + + # figure() + # plot(t) +end \ No newline at end of file diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl new file mode 100644 index 00000000..3a21b483 --- /dev/null +++ b/eval_sim/test_load.jl @@ -0,0 +1,217 @@ +using PyPlot +using JLD + +function test(path_acceleration::AbstractString,path_deceleration::AbstractString) + + + files_acc = readdir(path_acceleration) + files_dec = readdir(path_deceleration) + + a = zeros(length(files_acc) + length(files_dec)) + println("number of tests= ",length(a)) + + ## Loop for acceleration + + v_acceleration1 = [] #zeros(length(files)) + v_acceleration2 = [] + + commands_acc = [] #zeros(length(files)) + + for index = 1:length(files_acc) + + file = files_acc[index] + + log_path = "$(homedir())/open_loop/acceleration/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure(1) + + plot(v_opt) + grid("on") + + + # println("length= ",length(files)) + # println("v_opt= ",length(v_opt)) + # println("cmd_opt= ",length(cmd_opt)) + # println("v_opt[end]= ",v_opt[end]) + # println("a[i]= ",a[i]) + # println("v_opt[end]/4= ",v_opt[end]/4) + + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_acceleration1 = vcat(v_acceleration1,v_lms1) + v_acceleration2 = vcat(v_acceleration2,v_lms2) + a[index] = v_opt[end]/4 + commands_acc = vcat(commands_acc,cmd_opt[1:end-1]) + + + # println("a= ",a) + + + figure(2) + + plot(cmd_opt[1],a[index], "*") + grid("on") + + end + + ## Loop for deceleration + + v_deceleration1 = [] #zeros(length(files)) + v_deceleration2 = [] + + commands_dec = [] #zeros(length(files)) + + for index = 1:length(files_dec) + + file = files_dec[index] + + log_path = "$(homedir())/open_loop/deceleration/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.<100] + cmd_opt = cmd[cmd.<100] + + v_equal = 0 + flag = 0 + + for i = 2:length(v_opt) + + if v_opt[i]==v_opt[i-1] + v_equal = v_equal + 1 + else + v_equal = 0 + end + + if v_equal > 10 + flag = i + break + end + end + + v_opt = v_opt[1:flag] + cmd_opt = cmd_opt[1:flag] + + a[length(files_acc) + index] = (- v_opt[1])/4 + + # plot(cmd_opt[5],a[length(files_acc) + index], "*") + # grid("on") + + + + figure(3) + + plot(v_opt) + grid("on") + + + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_deceleration1 = vcat(v_deceleration1,v_lms1) + v_deceleration2 = vcat(v_deceleration2,v_lms2) + commands_dec = vcat(commands_dec,cmd_opt[1:end-1]) + + + + + end + + + + + + # Least Mean Square + + # println("length of v_acceleration1= ",length(v_acceleration1)) + # println("length of commands= ",length(commands)) + + + A = zeros(length(v_acceleration1)+length(v_deceleration1),5) + B = zeros(length(v_acceleration1)+length(v_deceleration1),1) + x = zeros(5,1) #[c1_+;c2_+,cM] + + A[1:length(commands_acc),1] = commands_acc + A[1:length(commands_acc),2] = 1 + A[1:length(v_acceleration2),3] = -v_acceleration2 + A[length(v_acceleration2)+1:end,3] = -v_deceleration2 + A[length(commands_acc)+1:end,4] = commands_dec + A[length(commands_acc)+1:end,5] = 1 + + B[1:length(v_acceleration1)] = v_acceleration1 + B[length(v_acceleration1)+1:end] = v_deceleration1 + + A = 0.02*A # 0.02 is the discretization + + + x = inv(A'*A)*A'*B + + println("the computed constants are: ", x) + + # x_axis = (findmin(commands)[1]):0.02:(findmax(commands)[1]) + # y_axis = x[1]*x_axis + x[2] + # plot(x_axis,y_axis) + + + + +end diff --git a/odroid_setup.sh b/odroid_setup.sh new file mode 100644 index 00000000..6201e3b0 --- /dev/null +++ b/odroid_setup.sh @@ -0,0 +1,2 @@ +export ROS_MASTER_URI=http://10.0.0.14:11311 +export ROS_IP=10.0.0.1 diff --git a/scripts/startup.sh b/scripts/startup.sh index f13ab2c3..4331836b 100755 --- a/scripts/startup.sh +++ b/scripts/startup.sh @@ -1,19 +1,36 @@ #!/bin/bash # local start up script when opening bash session +# set environment variable for python +# export WORKON_HOME=$HOME/.virtualenvs +# export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python +# export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv +# export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' +# source /usr/local/bin/virtualenvwrapper.sh +# workon barc -export WORKON_HOME=$HOME/.virtualenvs -export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python -export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv -export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' -source /usr/local/bin/virtualenvwrapper.sh -workon barc +# set environment variables for ROS +source $HOME/barc/workspace/devel/setup.bash +export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ -source /home/odroid/team_name.sh +# set environment variables for Amazon Web Server +# source $HOME/team_name.sh -alias nanorst='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' +# define commands +# * nanorst - resets the arduino nano from the command line (assuming the device is connected and on port /dev/ttyUSB0 +# * rebuild_system - rebuild all the ROS packages +alias flash_nano='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' alias rebuild_system='source ~/barc/scripts/rebuild_system.sh' +alias tmux='tmux -2' +alias reset_wifi_rules='sudo rm /etc/udev/rules.d/70-persistent-net.rules' +alias set_init_ap='sudo cp $HOME/barc/scripts/accesspoint.conf /etc/init/accesspoint.conf' -export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ - +# set configuration script for vim text editor cp ~/barc/scripts/vimrc ~/.vimrc + + +# added git branch listing (michael) +parse_git_branch() { + git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/ (\1)/' +} +export PS1="\u@\h \[\033[32m\]\w\[\033[33m\]\$(parse_git_branch)\[\033[00m\] $ " diff --git a/workspace/src/CMakeLists.txt b/workspace/src/CMakeLists.txt index 3703df4e..581e61db 120000 --- a/workspace/src/CMakeLists.txt +++ b/workspace/src/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 366e2c82..b7639c26 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -47,10 +47,13 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - Ultrasound.msg - Encoder.msg + pos_info.msg ECU.msg - Z_KinBkMdl.msg + Vel_est.msg + prediction.msg + selected_states.msg + xy_prediction.msg + theta.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_agent_1.launch b/workspace/src/barc/launch/barc_agent_1.launch new file mode 100644 index 00000000..670f061d --- /dev/null +++ b/workspace/src/barc/launch/barc_agent_1.launch @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/barc_agent_2.launch b/workspace/src/barc/launch/barc_agent_2.launch new file mode 100644 index 00000000..fd97d89a --- /dev/null +++ b/workspace/src/barc/launch/barc_agent_2.launch @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/barc_agent_multiple.launch b/workspace/src/barc/launch/barc_agent_multiple.launch new file mode 100644 index 00000000..9343a00f --- /dev/null +++ b/workspace/src/barc/launch/barc_agent_multiple.launch @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/closedLoop_mpc.launch b/workspace/src/barc/launch/closedLoop_mpc.launch deleted file mode 100644 index 8298e38a..00000000 --- a/workspace/src/barc/launch/closedLoop_mpc.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/estimation_DynBkMdl.launch b/workspace/src/barc/launch/estimation_DynBkMdl.launch deleted file mode 100644 index cab1838a..00000000 --- a/workspace/src/barc/launch/estimation_DynBkMdl.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/estimation_KinBkMdl.launch b/workspace/src/barc/launch/estimation_KinBkMdl.launch deleted file mode 100644 index c9cfb374..00000000 --- a/workspace/src/barc/launch/estimation_KinBkMdl.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/gps-remote.launch b/workspace/src/barc/launch/gps-remote.launch new file mode 100644 index 00000000..24642d62 --- /dev/null +++ b/workspace/src/barc/launch/gps-remote.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/workspace/src/barc/launch/imu-remote.launch b/workspace/src/barc/launch/imu-remote.launch new file mode 100644 index 00000000..c5e08b01 --- /dev/null +++ b/workspace/src/barc/launch/imu-remote.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/low_level_mapping.launch b/workspace/src/barc/launch/low_level_mapping.launch new file mode 100644 index 00000000..b2b08019 --- /dev/null +++ b/workspace/src/barc/launch/low_level_mapping.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/openLoop_circular.launch b/workspace/src/barc/launch/openLoop_circular.launch deleted file mode 100644 index 44d3d417..00000000 --- a/workspace/src/barc/launch/openLoop_circular.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/openLoop_straight.launch b/workspace/src/barc/launch/openLoop_straight.launch deleted file mode 100644 index 56142084..00000000 --- a/workspace/src/barc/launch/openLoop_straight.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/sensor_test.launch b/workspace/src/barc/launch/sensor_test.launch deleted file mode 100644 index ae8c0c29..00000000 --- a/workspace/src/barc/launch/sensor_test.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/sensor_test_barc.launch b/workspace/src/barc/launch/sensor_test_barc.launch new file mode 100644 index 00000000..ec09a023 --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_barc.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch new file mode 100644 index 00000000..724ef606 --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ` + + + + + + + diff --git a/workspace/src/barc/launch/sim_agent_1.launch b/workspace/src/barc/launch/sim_agent_1.launch new file mode 100644 index 00000000..e4405b4c --- /dev/null +++ b/workspace/src/barc/launch/sim_agent_1.launch @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sim_agent_2.launch b/workspace/src/barc/launch/sim_agent_2.launch new file mode 100644 index 00000000..2dbf884c --- /dev/null +++ b/workspace/src/barc/launch/sim_agent_2.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sim_agent_multiple.launch b/workspace/src/barc/launch/sim_agent_multiple.launch new file mode 100644 index 00000000..f9038281 --- /dev/null +++ b/workspace/src/barc/launch/sim_agent_multiple.launch @@ -0,0 +1,141 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/track_setup.launch b/workspace/src/barc/launch/track_setup.launch new file mode 100644 index 00000000..5bc97cfc --- /dev/null +++ b/workspace/src/barc/launch/track_setup.launch @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/track_setup_single.launch b/workspace/src/barc/launch/track_setup_single.launch new file mode 100644 index 00000000..86741645 --- /dev/null +++ b/workspace/src/barc/launch/track_setup_single.launch @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/ECU.msg b/workspace/src/barc/msg/ECU.msg index 2bffecbf..a37117f5 100644 --- a/workspace/src/barc/msg/ECU.msg +++ b/workspace/src/barc/msg/ECU.msg @@ -6,5 +6,6 @@ # # For modeling and state estimation, motors units are [N], and servo units are [rad] # For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle +Header header float32 motor float32 servo diff --git a/workspace/src/barc/msg/Encoder.msg b/workspace/src/barc/msg/Encoder.msg deleted file mode 100644 index 11fe3098..00000000 --- a/workspace/src/barc/msg/Encoder.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 FL -float32 FR -float32 BL -float32 BR diff --git a/workspace/src/barc/msg/Ultrasound.msg b/workspace/src/barc/msg/Ultrasound.msg deleted file mode 100644 index 2ffa3263..00000000 --- a/workspace/src/barc/msg/Ultrasound.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 front -float32 back -float32 right -float32 left diff --git a/workspace/src/barc/msg/Vel_est.msg b/workspace/src/barc/msg/Vel_est.msg new file mode 100644 index 00000000..69111051 --- /dev/null +++ b/workspace/src/barc/msg/Vel_est.msg @@ -0,0 +1,6 @@ +Header header +float32 vel_est +float32 vel_fl +float32 vel_fr +float32 vel_bl +float32 vel_br \ No newline at end of file diff --git a/workspace/src/barc/msg/Z_KinBkMdl.msg b/workspace/src/barc/msg/Z_KinBkMdl.msg deleted file mode 100644 index d76a6d56..00000000 --- a/workspace/src/barc/msg/Z_KinBkMdl.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 x -float32 y -float32 psi -float32 v diff --git a/workspace/src/barc/msg/mpc_solution.msg b/workspace/src/barc/msg/mpc_solution.msg new file mode 100644 index 00000000..7c03e33f --- /dev/null +++ b/workspace/src/barc/msg/mpc_solution.msg @@ -0,0 +1,14 @@ +# This is a message to hold data for pF/LMPC controller +Header header +float64[] z_x +float64[] z_y +float64[] z_vx +float64[] z_s +float64[] SS_x +float64[] SS_y +float64[] SS_vx +float64[] SS_s +float64[] z_fore_x +float64[] z_fore_y +float64[] z_iden_x +float64[] z_iden_y \ No newline at end of file diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg new file mode 100644 index 00000000..b106badc --- /dev/null +++ b/workspace/src/barc/msg/pos_info.msg @@ -0,0 +1,17 @@ + +# This is a message to hold data with position and trajectory information +Header header +float32 s +float32 ey +float32 epsi +float32 v +float32 x +float32 y +float32 v_x +float32 v_y +float32 psi +float32 psiDot +float32 a_x +float32 a_y +float32 u_a +float32 u_df diff --git a/workspace/src/barc/msg/pos_info64.msg b/workspace/src/barc/msg/pos_info64.msg new file mode 100644 index 00000000..b80ec9b4 --- /dev/null +++ b/workspace/src/barc/msg/pos_info64.msg @@ -0,0 +1,17 @@ + +# This is a message to hold data with position and trajectory information +Header header +float64 s +float64 ey +float64 epsi +float64 v +float64 x +float64 y +float64 v_x +float64 v_y +float64 psi +float64 psiDot +float64 a_x +float64 a_y +float64 u_a +float64 u_df diff --git a/workspace/src/barc/msg/prediction.msg b/workspace/src/barc/msg/prediction.msg new file mode 100644 index 00000000..a5577dae --- /dev/null +++ b/workspace/src/barc/msg/prediction.msg @@ -0,0 +1,9 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] s +float64[] ey +float64[] epsi +float64[] psidot +float64[] vx +float64[] vy +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/msg/selected_states.msg b/workspace/src/barc/msg/selected_states.msg new file mode 100644 index 00000000..4eb73937 --- /dev/null +++ b/workspace/src/barc/msg/selected_states.msg @@ -0,0 +1,6 @@ +# This is a message to hold data of the selected states in x and y coords +Header header +float64[] x +float64[] y +float64[] s +float64[] ey \ No newline at end of file diff --git a/workspace/src/barc/msg/theta.msg b/workspace/src/barc/msg/theta.msg new file mode 100644 index 00000000..dfed806f --- /dev/null +++ b/workspace/src/barc/msg/theta.msg @@ -0,0 +1,5 @@ +# This is a message to communicate the agent's theta parameters +Header header +float64[] theta_vx +float64[] theta_vy +float64[] theta_psi_dot diff --git a/workspace/src/barc/msg/xy_prediction.msg b/workspace/src/barc/msg/xy_prediction.msg new file mode 100644 index 00000000..b1db55de --- /dev/null +++ b/workspace/src/barc/msg/xy_prediction.msg @@ -0,0 +1,8 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] x +float64[] y +float64[] psi +float64[] acc +float64[] steering +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml b/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 00000000..a55e7a17 --- /dev/null +++ b/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/dictionaries/lukas.xml b/workspace/src/barc/src/.idea/dictionaries/lukas.xml new file mode 100644 index 00000000..b165ff6b --- /dev/null +++ b/workspace/src/barc/src/.idea/dictionaries/lukas.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml b/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 00000000..2ac8da62 --- /dev/null +++ b/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,51 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/misc.xml b/workspace/src/barc/src/.idea/misc.xml new file mode 100644 index 00000000..7ba73c25 --- /dev/null +++ b/workspace/src/barc/src/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/modules.xml b/workspace/src/barc/src/.idea/modules.xml new file mode 100644 index 00000000..f669a0e5 --- /dev/null +++ b/workspace/src/barc/src/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/src.iml b/workspace/src/barc/src/.idea/src.iml new file mode 100644 index 00000000..e98082ab --- /dev/null +++ b/workspace/src/barc/src/.idea/src.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/workspace.xml b/workspace/src/barc/src/.idea/workspace.xml new file mode 100644 index 00000000..9abdd1ab --- /dev/null +++ b/workspace/src/barc/src/.idea/workspace.xml @@ -0,0 +1,762 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + kinematic + set + un + agent + reference_input + adversarial + s_to_xy + s_tO_xy + xy_to_s + loop_rate + P + current_reference + reference + yaw0 + dt + Localization + ll + param + Locali + pub + 0.125 + psiDot + warn + unwrap + modelParams + u_lb + + + + + + + + + + + + + + + + + + Buildout + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +